//用国产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;
}
////使用硬串口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;
}
评分
