MODBUS-RS485从机程序

//用国产NANO时选择--工具--处理器0ld
////使用硬串口Serial(0, 1)收发Modbus数据
//舵机
#include <Servo.h>
Servo myservo;
Servo myservo1; //舵机实例化
Servo myservo2;
Servo myservo3;
Servo myservo4;
//舵机
#include <MsTimer2.h>
int dj1_time;//舵机1运行时间
int dj2_time;//舵机2运行时间
int dj3_time;//舵机3运行时间
int dj4_time;//舵机4运行时间
int dj5_time;//舵机5运行时间
#include <avr/wdt.h>//看门狗库
//-----ModbusRTU通讯
//基本参数
#define baudrate 38400  //定义通讯波特率
#define slaveID 2  //定义modbus RTU从站站号
#define modbusDataSize 20  //定义modbus数据库空间大小,可根据实际情况自行修改大小
unsigned int modbusData[modbusDataSize]={};   //建立modbus数据库

//系统参数
#define bufferSize 255  //一帧数据的最大字节数量
unsigned char frame[bufferSize];  //用于保存接收或发送的数据
HardwareSerial* ModbusPort;

#define tou_zz_out 2  //头正转输出脚
#define tou_fz_out 4  //头反转输出脚


#define tou_zz_xw A0  //头正转限位输入脚
#define tou_fz_xw A1  //头反转限位输入脚
#define tou_hz_xw A2  //头回中限位输入脚



//函数声明
unsigned int calculateCRC(unsigned char* _regs,unsigned char arraySize);  //声明CRC校验函数
void modbusRTU_slave();  //声明modbus RTU从站函数
void responseError(unsigned char ID,unsigned char function,unsigned char wrongNumber);  //声明错误信息返回函数
void modbusRTU_INI(HardwareSerial *SerialPort);  //声明modbus RTU端口初始化函数*SerialPort)--硬件串口

static boolean output = HIGH;//定义一个全局布尔变量
  static boolean dj1out = LOW,dj2out = LOW,dj3out = LOW,dj4out = LOW,dj5out = LOW;
  static boolean dj1yjs,dj2yjs,dj3yjs,dj4yjs,dj5yjs;//舵机延时已计数标志
  static boolean dj1ksjs,dj2ksjs,dj3ksjs,dj4ksjs,dj5ksjs;//舵机延时开始计数标志
  static int djwz1,djwz2,djwz3,djwz4,djwz5;//舵机位置，实际舵机角度值500-2500

  static boolean tou_zz,tou_fz,tou_hz;//头正转、头反转、头回中
  static boolean tou_zzxw_in,tou_fzxw_in,tou_hzxw_in;//头正转、头反转、头回中限位输入//int
  static boolean tou_zzhz_bz,tou_fzhz_bz;//头正转回中标志、头反转回中标志
//初始化函数
void setup()
{

pinMode(tou_zz_xw, INPUT_PULLUP);// 设置引脚输入,上拉电阻有效
pinMode(tou_fz_xw, INPUT_PULLUP);// 设置引脚输入,上拉电阻有效
pinMode(tou_hz_xw, INPUT_PULLUP);// 设置引脚输入,上拉电阻有效


  pinMode(tou_zz_out, OUTPUT);
   pinMode(tou_fz_out, OUTPUT);

   pinMode(13, OUTPUT);
  wdt_enable(WDTO_2S); //开启看门狗，并设置溢出时间为两秒
  delay(100);
///*  //------NANO用2560板试验
   //Serial.begin(9600);  
   // modbusRTU_INI(&Serial1);  //定义modbus通讯端口 端口0:&Serial 端口1:&Serial1 端口2:&Serial2
  //------NANO用2560板试验
// */   
  modbusRTU_INI(&Serial);  //定义modbus通讯端口 端口0:&Serial 端口1:&Serial1 端口2:&Serial2
////舵机

  myservo.attach(5);
  myservo1.attach(6); //舵机PWM口
  myservo2.attach(9);
  myservo3.attach(10);
  myservo4.attach(11);
   myservo.writeMicroseconds(1500);//舵机恢复到初始位置
    myservo1.writeMicroseconds(1500);
    myservo2.writeMicroseconds(1500);
    myservo3.writeMicroseconds(1500);
    myservo4.writeMicroseconds(1500);
////舵机
  MsTimer2::set(10, flash); // 10ms 时间中断
  MsTimer2::start();
}
void flash() //时间中断输出
{  
  //wdt_reset(); //喂狗操作，使看门狗定时器复位

// static boolean output = HIGH;//定义一个全局布尔变量  
  //digitalWrite(13, output);//试验13脚中断脉冲输出
  output = !output;//取反
  if (output) {dj1yjs=LOW;dj2yjs=LOW;dj3yjs=LOW;dj4yjs=LOW;dj5yjs=LOW;}
}
void dj1js()   //dj_time 0.5秒+1---眼皮--2200/1500
{

  if (dj1ksjs && !dj1yjs && !output && dj1_time <modbusData[5]) { dj1_time++; dj1yjs=HIGH;}//Serial.print(ji_time);
  if (dj1_time>=modbusData[5]) //舵机1延时时间,必须大于等于10
  {
    dj1out = !dj1out;//取反
    if(dj1out) djwz1=modbusData[0];else djwz1=1500;
    dj1_time=0;
  }
// digitalWrite(13, dj1out && dj1ksjs);//试验13脚中断脉冲输出
}
void dj2js()   //dj_time 0.5秒+1-----眼球--1200/1800
{

  if (dj2ksjs && !dj2yjs && !output && dj2_time <modbusData[6]) { dj2_time++; dj2yjs=HIGH;} //else  dj2_time=0;
  if (dj2_time>=modbusData[6]) //舵机2延时时间,必须大于等于10
  {
    dj2out = !dj2out;//取反
    if(dj2out) djwz2=modbusData[1];else djwz2=1800;
    dj2_time=0;
  }
  //digitalWrite(13, dj2out && dj2ksjs);//试验13脚中断脉冲输出
}
void dj3js()   //dj_time 0.5秒+1-----眼皮--1500/800
{

  if (dj3ksjs && !dj3yjs && !output && dj3_time <modbusData[7]) { dj3_time++; dj3yjs=HIGH;} //else  dj3_time=0;
  if (dj3_time>=modbusData[7]) //舵机3延时时间,必须大于等于10
  {
    dj3out = !dj3out;//取反
    if(dj3out) djwz3=modbusData[2];else djwz3=1500;
    dj3_time=0;
  }
// digitalWrite(13, dj3out && dj3ksjs);//试验13脚中断脉冲输出
}
void dj4js()   //dj_time 0.5秒+1----眼球---1200/1800
{

  if (dj4ksjs && !dj4yjs && !output && dj4_time <modbusData[8]) { dj4_time++; dj4yjs=HIGH;} //else  dj4_time=0;
  if (dj4_time>=modbusData[8]) //舵机4延时时间,必须大于等于10
  {
    dj4out = !dj4out;//取反
    if(dj4out) djwz4=modbusData[3];else djwz4=1200;
    dj4_time=0;
  }
  //digitalWrite(13, dj4out && dj4ksjs);//试验13脚中断脉冲输出
}
void dj5js()   //dj_time 0.5秒+1
{

  if (dj5ksjs && !dj5yjs && !output && dj5_time <modbusData[9]) { dj5_time++; dj5yjs=HIGH;} //else  dj5_time=0;
  if (dj5_time>=modbusData[9]) //舵机5延时时间,必须大于等于10
  {
    dj5out = !dj5out;//取反
    if(dj5out) djwz5=modbusData[4];else djwz5=1500;
    dj5_time=0;
  }
// digitalWrite(13, dj5out && dj5ksjs);//试验13脚中断脉冲输出
}
/*-----MsTimer2.h官网例子
void flash() {
  static boolean output = HIGH;

  digitalWrite(13, output);
  output = !output;
}

void setup() {
  pinMode(13, OUTPUT);

  MsTimer2::set(500, flash); // 500ms period
  MsTimer2::start();
}

*/


//主循环
void loop()
{
  wdt_reset(); //喂狗操作，使看门狗定时器复位
  modbusRTU_slave();  //执行modbus函数

  gnm06_dz();//舵机

  //digitalWrite(tou_zz_out,HIGH);
  //digitalWrite(tou_fz_out,HIGH);
// digitalWrite(13,HIGH);
}


void gnm06_dz() //舵机//06功能码数据解析动作
{

   /////////////1
  if(modbusData[0]>500 && modbusData[5]>=10)
    {
     //myservo.attach(5);
     dj1ksjs=HIGH;
     //myservo.write(djwz1);//大概0-180角度控制
      myservo.writeMicroseconds(djwz1);//writeMicroseconds()--标准舵机500-2500角度控制
    }  
    else dj1ksjs=LOW;
    dj1js();
  if(modbusData[0]==1 && modbusData[5]==1) {djwz1=1500;myservo.writeMicroseconds(djwz1);} //舵机恢复到初始位置
  /////////////////==1恢复是为了避免==0时动作的不可控
    ////////////2
   if(modbusData[1]>500 && modbusData[6]>=10)
    {
    // myservo1.attach(6);
     dj2ksjs=HIGH;
     myservo1.writeMicroseconds(djwz2);

    }  
    else dj2ksjs=LOW;
    dj2js();
  if(modbusData[1]==1 && modbusData[6]==1) {djwz2=1500;myservo1.writeMicroseconds(djwz2);} //舵机恢复到初始位置     
    ///////////////////3
     if(modbusData[2]>500 && modbusData[7]>=10)
    {
    // myservo2.attach(9);
     dj3ksjs=HIGH;
     myservo2.writeMicroseconds(djwz3);

    }  
    else dj3ksjs=LOW;
    dj3js();
  if(modbusData[2]==1 && modbusData[7]==1) {djwz3=1500;myservo2.writeMicroseconds(djwz3);} //舵机恢复到初始位置
    //////////////////////4
     if(modbusData[3]>500 && modbusData[8]>=10)
    {
     //myservo3.attach(10);
     dj4ksjs=HIGH;
     myservo3.writeMicroseconds(djwz4);

    }  
    else dj4ksjs=LOW;
    dj4js();
  if(modbusData[3]==1 && modbusData[8]==1) {djwz4=1500;myservo3.writeMicroseconds(djwz4);} //舵机恢复到初始位置
    ///////////////////////5
     if(modbusData[4]>500 && modbusData[9]>=10)
    {
     //myservo4.attach(11);
     dj5ksjs=HIGH;
     myservo4.writeMicroseconds(djwz5);

    }  
    else dj5ksjs=LOW;
    dj5js();
  if(modbusData[4]==1 && modbusData[9]==1) {djwz5=1500;myservo4.writeMicroseconds(djwz5);} //舵机恢复到初始位置
    //////////////////////


}
//modbus RTU端口初始化函数
//参数:端口号
void modbusRTU_INI(HardwareSerial *SerialPort)//*SerialPort)--硬件串口
{
  ModbusPort = SerialPort;//SerialPort--硬件串口
  (*ModbusPort).begin(baudrate);
  (*ModbusPort).flush();
}


//modbus RTU从站函数
//支持功能码03,06,16
void modbusRTU_slave()
{
  unsigned int characterTime; //字符时间
  unsigned char errorFlag=0;  //错误标志
  unsigned int crc16;  //校验位

  unsigned char address=0;

  if (baudrate > 19200)  //波特率大于19200时进入条件
  {
    characterTime = 750;
  }
  else
  {
    characterTime = 15000000/baudrate;  //1.5字符时间
  }
  while((*ModbusPort).available()>0)  //如果串口缓冲区数据量大于0进入条件
  {

    if(address<bufferSize)
    {
      frame[address]=(*ModbusPort).read();
      address++;
    }
    else  //条件不满足时直接清空缓冲区
    {
       (*ModbusPort).read();
    }
    delayMicroseconds(characterTime);  //等待1.5个字符时间
    if((*ModbusPort).available()==0)  //1.5个字符时间后缓冲区仍然没有收到数据,认为一帧数据已经接收完成,进入条件
    {
      unsigned char function=frame[1];  //读取功能码     
      if(frame[0]==slaveID||frame[0]==0)  //站号匹配或者消息为广播形式,进入条件
      {
        crc16 = ((frame[address - 2] << 8) | frame[address - 1]);
        if(calculateCRC(&frame[0],address - 2)==crc16)  //数据校验通过,进入条件
        {
          if (frame[0]!=0 && (function == 3))  //功能码03不支持广播消息
          {
            unsigned int startData=((frame[2] << 8) | frame[3]);  //读取modbus数据库起始地址         
            unsigned int dataSize=((frame[4] << 8) | frame[5]);  //需要读取的modbus数据库数据长度
            unsigned int endData=startData+dataSize;    //需要读取的modbus数据库数据的结束地址
            unsigned char responseSize=5+dataSize*2;  //计算应答的数据长度
            unsigned int temp1,temp2,temp3;

            if(dataSize>125 || endData>=modbusDataSize)  //读取数据的结束地址超过了modbus数据库的范围或单次读取的数据数量大于125
            {
              errorFlag=0x02;  //数据超过范围
              responseError(slaveID,function,errorFlag);  //返回错误消息
            }
            else
            {
              frame[0]=slaveID;  //设定站号
              frame[1]=function;  //设定功能码
              frame[2]=dataSize*2;  //设定数据长度
              temp3=3;
              for(temp1=startData;temp1<endData;temp1++)
              {
                temp2=modbusData[temp1];  //取出modbus数据库中的数据
                frame[temp3]=temp2>>8;
                temp3++;
                frame[temp3]=temp2 & 0xFF;
                temp3++;
              }
              crc16 = calculateCRC(&frame[0],responseSize-2);
              frame[responseSize-2] = crc16 >> 8;  //填写校验位
              frame[responseSize-1] = crc16 & 0xFF;
              (*ModbusPort).write(&frame[0],responseSize);  //返回功能码03的消息
            }
          }
          else if(function == 6)  //功能码为06时进入条件
          {
            unsigned int startData=((frame[2] << 8) | frame[3]);  //写入modbus数据库的地址         
            unsigned int setData=((frame[4] << 8) | frame[5]);  //写入modbus数据库的数值
            if(startData>=modbusDataSize)
            {
              errorFlag=0x02;  //数据超过范围
              responseError(slaveID,function,errorFlag);  //返回错误消息
            }
            else
            {
              modbusData[startData]=setData;  //写入数据到modbus数据库            
              frame[0]=slaveID;  //设定站号
              frame[1]=function;  //设定功能码
              frame[2] = startData >> 8;  //填写数据库地址
              frame[3] = startData & 0xFF;            
              frame[4] = modbusData[startData] >> 8;  //填写数据库数值
              frame[5] = modbusData[startData] & 0xFF;
              crc16 = calculateCRC(&frame[0],6);  //计算校验值
              frame[6] = crc16 >> 8;  //填写校验位
              frame[7] = crc16 & 0xFF;
              (*ModbusPort).write(&frame[0],8);  //返回功能码06的消息            
            }
          }
          else if(function == 16)  //功能码为16时进入条件
          {
            if(frame[6]!=address-9)  //校验数据长度
            {
              errorFlag=0x03;  //数据长度不符
              responseError(slaveID,function,errorFlag);  //返回错误消息
            }
            else  //校验数据长度正确
            {
              unsigned int startData=((frame[2] << 8) | frame[3]);  //写入modbus数据库起始地址         
              unsigned int dataSize=((frame[4] << 8) | frame[5]);  //需要写入的modbus数据库数据长度
              unsigned int endData=startData+dataSize;    //需要写入的modbus数据库数据的结束地址
              if(dataSize>125 || endData>=modbusDataSize)  //读取数据的结束地址超过了modbus数据库的范围或单次读取的数据数量大于125
              {
                errorFlag=0x02;  //数据超过范围
                responseError(slaveID,function,errorFlag);  //返回错误消息
              }
              else
              {              
                unsigned int temp1,temp2;
                temp2 = 7;  //从数据贞的第8个数据开始读取           
                for(temp1=startData;temp1<endData;temp1++)
                {
                  modbusData[temp1]=(frame[temp2]<<8|frame[temp2+1]);  //将数据写入modbus数据库中
                  temp2+=2;
                }
                frame[0]=slaveID;  //填写站号,frame[1]到frame[5]不变
                crc16 = calculateCRC(&frame[0],6);  //计算CRC校验
                frame[6] = crc16 >> 8;  //填写校验位
                frame[7] = crc16 & 0xFF;
                (*ModbusPort).write(&frame[0],8);  //发送功能码16的应答数据   
              }   
            }
          }
          else  //其他功能码
          {
            errorFlag = 0x01;  //不支持收到的功能码
            responseError(slaveID,function,errorFlag);  //返回错误消息
          }      
        }
        else //数据校验错误
        {
          errorFlag = 0x03;
          responseError(slaveID,function,errorFlag);  //返回错误消息
        }
      }
    }
  }
}



void responseError(unsigned char ID,unsigned char function,unsigned char wrongNumber)  //错误信息返回函数
{
  unsigned int crc16;  //校验位
  frame[0] = ID;  //设定站号
  frame[1] = function+0x80;
  frame[2] = wrongNumber;  //填写错误代码
  crc16 = calculateCRC(&frame[0],3);  //计算校验值
  frame[3] = crc16 >> 8;  //填写校验位
  frame[4] = crc16 & 0xFF;
  (*ModbusPort).write(&frame[0],5);  //返回错误代码        
}


//CRC校验函数
//参数1:待校验数组的起始地址
//参数2:待校验数组的长度
//返回值CRC校验结果,16位,低字节在前
unsigned int calculateCRC(unsigned char* _regs,unsigned char arraySize)
{
  unsigned int temp, temp2, flag;
  temp = 0xFFFF;
  for (unsigned char i = 0; i < arraySize; i++)
  {
    temp = temp ^ *(_regs+i);
    for (unsigned char j = 1; j <= 8; j++)
    {
      flag = temp & 0x0001;
      temp >>= 1;
      if (flag)
        temp ^= 0xA001;
    }
  }
  temp2 = temp >> 8;
  temp = (temp << 8) | temp2;
  temp &= 0xFFFF;
  return temp;
}