|
|
//用國產(chǎn)NANO時(shí)選擇--工具--處理器0ld
////使用硬串口Serial(0, 1)收發(fā)Modbus數(shù)據(jù)
//舵機(jī)
#include <Servo.h>
Servo myservo;
Servo myservo1; //舵機(jī)實(shí)例化
Servo myservo2;
Servo myservo3;
Servo myservo4;
//舵機(jī)
#include <MsTimer2.h>
int dj1_time;//舵機(jī)1運(yùn)行時(shí)間
int dj2_time;//舵機(jī)2運(yùn)行時(shí)間
int dj3_time;//舵機(jī)3運(yùn)行時(shí)間
int dj4_time;//舵機(jī)4運(yùn)行時(shí)間
int dj5_time;//舵機(jī)5運(yùn)行時(shí)間
#include <avr/wdt.h>//看門狗庫
//-----ModbusRTU通訊
//基本參數(shù)
#define baudrate 38400 //定義通訊波特率
#define slaveID 2 //定義modbus RTU從站站號
#define modbusDataSize 20 //定義modbus數(shù)據(jù)庫空間大小,可根據(jù)實(shí)際情況自行修改大小
unsigned int modbusData[modbusDataSize]={}; //建立modbus數(shù)據(jù)庫
//系統(tǒng)參數(shù)
#define bufferSize 255 //一幀數(shù)據(jù)的最大字節(jié)數(shù)量
unsigned char frame[bufferSize]; //用于保存接收或發(fā)送的數(shù)據(jù)
HardwareSerial* ModbusPort;
#define tou_zz_out 2 //頭正轉(zhuǎn)輸出腳
#define tou_fz_out 4 //頭反轉(zhuǎn)輸出腳
#define tou_zz_xw A0 //頭正轉(zhuǎn)限位輸入腳
#define tou_fz_xw A1 //頭反轉(zhuǎn)限位輸入腳
#define tou_hz_xw A2 //頭回中限位輸入腳
//函數(shù)聲明
unsigned int calculateCRC(unsigned char* _regs,unsigned char arraySize); //聲明CRC校驗(yàn)函數(shù)
void modbusRTU_slave(); //聲明modbus RTU從站函數(shù)
void responseError(unsigned char ID,unsigned char function,unsigned char wrongNumber); //聲明錯(cuò)誤信息返回函數(shù)
void modbusRTU_INI(HardwareSerial *SerialPort); //聲明modbus RTU端口初始化函數(shù)*SerialPort)--硬件串口
static boolean output = HIGH;//定義一個(gè)全局布爾變量
static boolean dj1out = LOW,dj2out = LOW,dj3out = LOW,dj4out = LOW,dj5out = LOW;
static boolean dj1yjs,dj2yjs,dj3yjs,dj4yjs,dj5yjs;//舵機(jī)延時(shí)已計(jì)數(shù)標(biāo)志
static boolean dj1ksjs,dj2ksjs,dj3ksjs,dj4ksjs,dj5ksjs;//舵機(jī)延時(shí)開始計(jì)數(shù)標(biāo)志
static int djwz1,djwz2,djwz3,djwz4,djwz5;//舵機(jī)位置，實(shí)際舵機(jī)角度值500-2500
static boolean tou_zz,tou_fz,tou_hz;//頭正轉(zhuǎn)、頭反轉(zhuǎn)、頭回中
static boolean tou_zzxw_in,tou_fzxw_in,tou_hzxw_in;//頭正轉(zhuǎn)、頭反轉(zhuǎn)、頭回中限位輸入//int
static boolean tou_zzhz_bz,tou_fzhz_bz;//頭正轉(zhuǎn)回中標(biāo)志、頭反轉(zhuǎn)回中標(biāo)志
//初始化函數(shù)
void setup()
{
pinMode(tou_zz_xw, INPUT_PULLUP);// 設(shè)置引腳輸入,上拉電阻有效
pinMode(tou_fz_xw, INPUT_PULLUP);// 設(shè)置引腳輸入,上拉電阻有效
pinMode(tou_hz_xw, INPUT_PULLUP);// 設(shè)置引腳輸入,上拉電阻有效
pinMode(tou_zz_out, OUTPUT);
pinMode(tou_fz_out, OUTPUT);
pinMode(13, OUTPUT);
wdt_enable(WDTO_2S); //開啟看門狗，并設(shè)置溢出時(shí)間為兩秒
delay(100);
///* //------NANO用2560板試驗(yàn)
//Serial.begin(9600);
// modbusRTU_INI(&Serial1); //定義modbus通訊端口 端口0:&Serial 端口1:&Serial1 端口2:&Serial2
//------NANO用2560板試驗(yàn)
// */
modbusRTU_INI(&Serial); //定義modbus通訊端口 端口0:&Serial 端口1:&Serial1 端口2:&Serial2
////舵機(jī)
myservo.attach(5);
myservo1.attach(6); //舵機(jī)PWM口
myservo2.attach(9);
myservo3.attach(10);
myservo4.attach(11);
myservo.writeMicroseconds(1500);//舵機(jī)恢復(fù)到初始位置
myservo1.writeMicroseconds(1500);
myservo2.writeMicroseconds(1500);
myservo3.writeMicroseconds(1500);
myservo4.writeMicroseconds(1500);
////舵機(jī)
MsTimer2::set(10, flash); // 10ms 時(shí)間中斷
MsTimer2::start();
}
void flash() //時(shí)間中斷輸出
{
//wdt_reset(); //喂狗操作，使看門狗定時(shí)器復(fù)位
// static boolean output = HIGH;//定義一個(gè)全局布爾變量
//digitalWrite(13, output);//試驗(yàn)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]) //舵機(jī)1延時(shí)時(shí)間,必須大于等于10
{
dj1out = !dj1out;//取反
if(dj1out) djwz1=modbusData[0];else djwz1=1500;
dj1_time=0;
}
// digitalWrite(13, dj1out && dj1ksjs);//試驗(yàn)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]) //舵機(jī)2延時(shí)時(shí)間,必須大于等于10
{
dj2out = !dj2out;//取反
if(dj2out) djwz2=modbusData[1];else djwz2=1800;
dj2_time=0;
}
//digitalWrite(13, dj2out && dj2ksjs);//試驗(yàn)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]) //舵機(jī)3延時(shí)時(shí)間,必須大于等于10
{
dj3out = !dj3out;//取反
if(dj3out) djwz3=modbusData[2];else djwz3=1500;
dj3_time=0;
}
// digitalWrite(13, dj3out && dj3ksjs);//試驗(yàn)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]) //舵機(jī)4延時(shí)時(shí)間,必須大于等于10
{
dj4out = !dj4out;//取反
if(dj4out) djwz4=modbusData[3];else djwz4=1200;
dj4_time=0;
}
//digitalWrite(13, dj4out && dj4ksjs);//試驗(yàn)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]) //舵機(jī)5延時(shí)時(shí)間,必須大于等于10
{
dj5out = !dj5out;//取反
if(dj5out) djwz5=modbusData[4];else djwz5=1500;
dj5_time=0;
}
// digitalWrite(13, dj5out && dj5ksjs);//試驗(yàn)13腳中斷脈沖輸出
}
/*-----MsTimer2.h官網(wǎng)例子
void flash() {
static boolean output = HIGH;
digitalWrite(13, output);
output = !output;
}
void setup() {
pinMode(13, OUTPUT);
MsTimer2::set(500, flash); // 500ms period
MsTimer2::start();
}
*/
//主循環(huán)
void loop()
{
wdt_reset(); //喂狗操作，使看門狗定時(shí)器復(fù)位
modbusRTU_slave(); //執(zhí)行modbus函數(shù)
gnm06_dz();//舵機(jī)
//digitalWrite(tou_zz_out,HIGH);
//digitalWrite(tou_fz_out,HIGH);
// digitalWrite(13,HIGH);
}
void gnm06_dz() //舵機(jī)//06功能碼數(shù)據(jù)解析動(dòng)作
{
/////////////1
if(modbusData[0]>500 && modbusData[5]>=10)
{
//myservo.attach(5);
dj1ksjs=HIGH;
//myservo.write(djwz1);//大概0-180角度控制
myservo.writeMicroseconds(djwz1);//writeMicroseconds()--標(biāo)準(zhǔn)舵機(jī)500-2500角度控制
}
else dj1ksjs=LOW;
dj1js();
if(modbusData[0]==1 && modbusData[5]==1) {djwz1=1500;myservo.writeMicroseconds(djwz1);} //舵機(jī)恢復(fù)到初始位置
/////////////////==1恢復(fù)是為了避免==0時(shí)動(dòng)作的不可控
////////////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);} //舵機(jī)恢復(fù)到初始位置
///////////////////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);} //舵機(jī)恢復(fù)到初始位置
//////////////////////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);} //舵機(jī)恢復(fù)到初始位置
///////////////////////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);} //舵機(jī)恢復(fù)到初始位置
//////////////////////
}
//modbus RTU端口初始化函數(shù)
//參數(shù):端口號
void modbusRTU_INI(HardwareSerial *SerialPort)//*SerialPort)--硬件串口
{
ModbusPort = SerialPort;//SerialPort--硬件串口
(*ModbusPort).begin(baudrate);
(*ModbusPort).flush();
}
//modbus RTU從站函數(shù)
//支持功能碼03,06,16
void modbusRTU_slave()
{
unsigned int characterTime; //字符時(shí)間
unsigned char errorFlag=0; //錯(cuò)誤標(biāo)志
unsigned int crc16; //校驗(yàn)位
unsigned char address=0;
if (baudrate > 19200) //波特率大于19200時(shí)進(jìn)入條件
{
characterTime = 750;
}
else
{
characterTime = 15000000/baudrate; //1.5字符時(shí)間
}
while((*ModbusPort).available()>0) //如果串口緩沖區(qū)數(shù)據(jù)量大于0進(jìn)入條件
{
if(address<bufferSize)
{
frame[address]=(*ModbusPort).read();
address++;
}
else //條件不滿足時(shí)直接清空緩沖區(qū)
{
(*ModbusPort).read();
}
delayMicroseconds(characterTime); //等待1.5個(gè)字符時(shí)間
if((*ModbusPort).available()==0) //1.5個(gè)字符時(shí)間后緩沖區(qū)仍然沒有收到數(shù)據(jù),認(rèn)為一幀數(shù)據(jù)已經(jīng)接收完成,進(jìn)入條件
{
unsigned char function=frame[1]; //讀取功能碼
if(frame[0]==slaveID||frame[0]==0) //站號匹配或者消息為廣播形式,進(jìn)入條件
{
crc16 = ((frame[address - 2] << 8) | frame[address - 1]);
if(calculateCRC(&frame[0],address - 2)==crc16) //數(shù)據(jù)校驗(yàn)通過,進(jìn)入條件
{
if (frame[0]!=0 && (function == 3)) //功能碼03不支持廣播消息
{
unsigned int startData=((frame[2] << 8) | frame[3]); //讀取modbus數(shù)據(jù)庫起始地址
unsigned int dataSize=((frame[4] << 8) | frame[5]); //需要讀取的modbus數(shù)據(jù)庫數(shù)據(jù)長度
unsigned int endData=startData+dataSize; //需要讀取的modbus數(shù)據(jù)庫數(shù)據(jù)的結(jié)束地址
unsigned char responseSize=5+dataSize*2; //計(jì)算應(yīng)答的數(shù)據(jù)長度
unsigned int temp1,temp2,temp3;
if(dataSize>125 || endData>=modbusDataSize) //讀取數(shù)據(jù)的結(jié)束地址超過了modbus數(shù)據(jù)庫的范圍或單次讀取的數(shù)據(jù)數(shù)量大于125
{
errorFlag=0x02; //數(shù)據(jù)超過范圍
responseError(slaveID,function,errorFlag); //返回錯(cuò)誤消息
}
else
{
frame[0]=slaveID; //設(shè)定站號
frame[1]=function; //設(shè)定功能碼
frame[2]=dataSize*2; //設(shè)定數(shù)據(jù)長度
temp3=3;
for(temp1=startData;temp1<endData;temp1++)
{
temp2=modbusData[temp1]; //取出modbus數(shù)據(jù)庫中的數(shù)據(jù)
frame[temp3]=temp2>>8;
temp3++;
frame[temp3]=temp2 & 0xFF;
temp3++;
}
crc16 = calculateCRC(&frame[0],responseSize-2);
frame[responseSize-2] = crc16 >> 8; //填寫校驗(yàn)位
frame[responseSize-1] = crc16 & 0xFF;
(*ModbusPort).write(&frame[0],responseSize); //返回功能碼03的消息
}
}
else if(function == 6) //功能碼為06時(shí)進(jìn)入條件
{
unsigned int startData=((frame[2] << 8) | frame[3]); //寫入modbus數(shù)據(jù)庫的地址
unsigned int setData=((frame[4] << 8) | frame[5]); //寫入modbus數(shù)據(jù)庫的數(shù)值
if(startData>=modbusDataSize)
{
errorFlag=0x02; //數(shù)據(jù)超過范圍
responseError(slaveID,function,errorFlag); //返回錯(cuò)誤消息
}
else
{
modbusData[startData]=setData; //寫入數(shù)據(jù)到modbus數(shù)據(jù)庫
frame[0]=slaveID; //設(shè)定站號
frame[1]=function; //設(shè)定功能碼
frame[2] = startData >> 8; //填寫數(shù)據(jù)庫地址
frame[3] = startData & 0xFF;
frame[4] = modbusData[startData] >> 8; //填寫數(shù)據(jù)庫數(shù)值
frame[5] = modbusData[startData] & 0xFF;
crc16 = calculateCRC(&frame[0],6); //計(jì)算校驗(yàn)值
frame[6] = crc16 >> 8; //填寫校驗(yàn)位
frame[7] = crc16 & 0xFF;
(*ModbusPort).write(&frame[0],8); //返回功能碼06的消息
}
}
else if(function == 16) //功能碼為16時(shí)進(jìn)入條件
{
if(frame[6]!=address-9) //校驗(yàn)數(shù)據(jù)長度
{
errorFlag=0x03; //數(shù)據(jù)長度不符
responseError(slaveID,function,errorFlag); //返回錯(cuò)誤消息
}
else //校驗(yàn)數(shù)據(jù)長度正確
{
unsigned int startData=((frame[2] << 8) | frame[3]); //寫入modbus數(shù)據(jù)庫起始地址
unsigned int dataSize=((frame[4] << 8) | frame[5]); //需要寫入的modbus數(shù)據(jù)庫數(shù)據(jù)長度
unsigned int endData=startData+dataSize; //需要寫入的modbus數(shù)據(jù)庫數(shù)據(jù)的結(jié)束地址
if(dataSize>125 || endData>=modbusDataSize) //讀取數(shù)據(jù)的結(jié)束地址超過了modbus數(shù)據(jù)庫的范圍或單次讀取的數(shù)據(jù)數(shù)量大于125
{
errorFlag=0x02; //數(shù)據(jù)超過范圍
responseError(slaveID,function,errorFlag); //返回錯(cuò)誤消息
}
else
{
unsigned int temp1,temp2;
temp2 = 7; //從數(shù)據(jù)貞的第8個(gè)數(shù)據(jù)開始讀取
for(temp1=startData;temp1<endData;temp1++)
{
modbusData[temp1]=(frame[temp2]<<8|frame[temp2+1]); //將數(shù)據(jù)寫入modbus數(shù)據(jù)庫中
temp2+=2;
}
frame[0]=slaveID; //填寫站號,frame[1]到frame[5]不變
crc16 = calculateCRC(&frame[0],6); //計(jì)算CRC校驗(yàn)
frame[6] = crc16 >> 8; //填寫校驗(yàn)位
frame[7] = crc16 & 0xFF;
(*ModbusPort).write(&frame[0],8); //發(fā)送功能碼16的應(yīng)答數(shù)據(jù)
}
}
}
else //其他功能碼
{
errorFlag = 0x01; //不支持收到的功能碼
responseError(slaveID,function,errorFlag); //返回錯(cuò)誤消息
}
}
else //數(shù)據(jù)校驗(yàn)錯(cuò)誤
{
errorFlag = 0x03;
responseError(slaveID,function,errorFlag); //返回錯(cuò)誤消息
}
}
}
}
}
void responseError(unsigned char ID,unsigned char function,unsigned char wrongNumber) //錯(cuò)誤信息返回函數(shù)
{
unsigned int crc16; //校驗(yàn)位
frame[0] = ID; //設(shè)定站號
frame[1] = function+0x80;
frame[2] = wrongNumber; //填寫錯(cuò)誤代碼
crc16 = calculateCRC(&frame[0],3); //計(jì)算校驗(yàn)值
frame[3] = crc16 >> 8; //填寫校驗(yàn)位
frame[4] = crc16 & 0xFF;
(*ModbusPort).write(&frame[0],5); //返回錯(cuò)誤代碼
}
//CRC校驗(yàn)函數(shù)
//參數(shù)1:待校驗(yàn)數(shù)組的起始地址
//參數(shù)2:待校驗(yàn)數(shù)組的長度
//返回值CRC校驗(yàn)結(jié)果,16位,低字節(jié)在前
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;
}
|
評分
-
查看全部評分
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
