- //定義變量程序段
- //把小車左輪電機編碼器碼盤的OUTA信號連接到Arduino控制器的數(shù)字端口2，
- //數(shù)字端口2是Arduino的外部中斷0的端口。
- #define PinA_left 2 //外部中斷0
- #define PinB_left 8 //小車左車輪電機編碼器碼盤的OUTB信號連接到數(shù)字端口8
- //把小車右車輪電機編碼器碼盤的OUTA信號連接到Arduino控制器的數(shù)字端口3，
- //數(shù)字端口3是Arduino的外部中斷1的端口。
- #define PinA_right 3 //外部中斷1
- #define PinB_right 9 //小車右車輪電機編碼器碼盤的OUTB信號連接到數(shù)字端口9
- int E_left =5; //L298P直流電機驅(qū)動板的左輪電機使能端口連接到數(shù)字接口5
- int M_left =4; //L298P直流電機驅(qū)動板的左輪電機轉(zhuǎn)向端口連接到數(shù)字接口4
- int E_right =6; //連接小車右輪電機的使能端口到數(shù)字接口6
- int M_right =7; //連接小車右輪電機的轉(zhuǎn)向端口到數(shù)字接口7
- int val_right; //小車右輪電機的PWM功率值
- int val_start;//上位機控制字節(jié)，用于控制電機是否啟動；
- int val_FB; //上位機控制字節(jié)，用于控制電機是正轉(zhuǎn)還是反轉(zhuǎn)；
- int val_left;//上位機控制字節(jié)，用于提供給左輪電機PWM功率值。
- int count1 = 0; //左輪編碼器碼盤脈沖計數(shù)值
- int count2= 0; //右輪編碼器碼盤脈沖計數(shù)值
- int rpm1 = 0; //左輪電機每分鐘(min)轉(zhuǎn)速(r/min)
- int rpm2 = 0; //右輪電機每分鐘(min)轉(zhuǎn)速(r/min)
- int rpm1_HIGH = 0;//左輪電機轉(zhuǎn)速分解成高、低兩個字節(jié)數(shù)據(jù)，以方便上傳給PC機
- int rpm1_LOW = 0;
- int rpm2_HIGH = 0;//右輪電機轉(zhuǎn)速分解成高、低兩個字節(jié)數(shù)據(jù)
- int rpm2_LOW = 0;
- int flag;//設置小車行車狀態(tài)，是前進、后退還是停止
- unsigned long time = 0, old_time = 0; // 時間標記
- unsigned long time1 = 0, time2 = 0; // 時間標記
- //初始化程序段
- void setup()
- {
- Serial.begin(9600); // 啟動串口通信，波特率為9600b/s
- pinMode(M_left, OUTPUT); //L298P直流電機驅(qū)動板的控制端口設置為輸出模式
- pinMode(E_left, OUTPUT);
- pinMode(M_right, OUTPUT);
- pinMode(E_right, OUTPUT);
- pinMode(PinA_left,INPUT); //伺服電機編碼器的OUTA和OUTB信號端設置為輸入模式
- pinMode(PinB_left,INPUT);
- pinMode(PinA_right,INPUT);
- pinMode(PinB_right,INPUT);
- //定義外部中斷0和1的中斷子程序Code(),中斷觸發(fā)為下跳沿觸發(fā)
- //當編碼器碼盤的OUTA脈沖信號發(fā)生下跳沿中斷時，
- //將自動調(diào)用執(zhí)行中斷子程序Code()。
- attachInterrupt(0, Code1, FALLING);//小車左車輪電機的編碼器脈沖中斷函數(shù)
- attachInterrupt(1, Code2, FALLING);//小車右車輪電機的編碼器脈沖中斷函數(shù)
- }
- //子程序程序段
- void advance()//小車前進
- {
- digitalWrite(M_left,HIGH);
- analogWrite(E_left,val_left);
- digitalWrite(M_right,LOW);
- analogWrite(E_right,val_right);
- }
- void back()//小車后退
- {
- digitalWrite(M_left,LOW);
- analogWrite(E_left,val_left);
- digitalWrite(M_right,HIGH);
- analogWrite(E_right,val_right);
- }
- void Stop()//小車停止
- {
- digitalWrite(E_right, LOW);
- digitalWrite(E_left, LOW);
- }
-
- //主程序段
- void loop()
- {
- if (Serial.available()>0) //如果Arduino控制器讀緩沖區(qū)中存在上位機下達的字節(jié)
- {
- val_start= Serial.read(); //從讀緩沖區(qū)中讀取上位機的三個控制字節(jié)
- delay(5);
- val_FB = Serial.read();
- delay(5);
- val_left= Serial.read();
- delay(5);
- if(val_start==0x11) //如果讀出的第一個字節(jié)為小車啟動標志字節(jié)0x11
- {
- if(val_FB ==0xAA) //如果讀出的第二個字節(jié)為小車前進標志字節(jié)0xAA
- {
- //讀出的第三個字節(jié)為小車左車輪電機的PWM功率值，把它賦值給右車輪電機功率變量
- val_right=val_left;
- advance(); //小車前進
- flag='a'; //設置小車前進標志字符
- count1 = 0; //恢復到編碼器測速的初始狀態(tài)
- count2 = 0;
- old_time= millis();
- }
- else if(val_FB ==0xBB) //如果讀出的第二個字節(jié)為小車后退標志字節(jié)0xBB
- {
- val_right=val_left;
- back(); //小車后退
- flag='b'; //設置小車后退標志字符
- count1 = 0; //恢復到編碼器測速的初始狀態(tài)
- count2 = 0;
- old_time= millis();
- }
- }
- else if(val_start==0x22) //如果讀出的第一個字節(jié)為小車停止標志字節(jié)0x22
- {
- Stop(); //小車停止
- flag='s'; //設置小車停止標志字符
- }
- }
-
- time = millis();//以毫秒為單位，計算當前時間
- //計算出每一秒鐘編碼器碼盤計得的脈沖數(shù)，
- if(abs(time - old_time) >= 1000) // 如果計時時間已達1秒
- {
- detachInterrupt(0); // 關閉外部中斷0
- detachInterrupt(1); // 關閉外部中斷1
- //把每一秒鐘編碼器碼盤計得的脈沖數(shù)，換算為當前轉(zhuǎn)速值
- //轉(zhuǎn)速單位是每分鐘多少轉(zhuǎn)，即r/min。這個編碼器碼盤為12個齒。
- rpm1 =(float)count1*60/12;//小車左車輪電機轉(zhuǎn)速
- rpm2 =(float)count2*60/12; //小車右車輪電機轉(zhuǎn)速
- rpm1_HIGH=rpm1/256;//把轉(zhuǎn)速值分解為高字節(jié)和低字節(jié)
- rpm1_LOW=rpm1%256;
- rpm2_HIGH=rpm2/256;
- rpm2_LOW=rpm2%256;
- //根據(jù)左右車輪轉(zhuǎn)速差rpm1-rpm2，乘以比例因子0.4，獲得比例調(diào)節(jié)后的右車輪電機PWM功率值
- val_right=(float)val_right+(rpm1-rpm2)*0.4;
- Serial.print(rpm1_HIGH,BYTE);//向上位計算機上傳左車輪電機當前轉(zhuǎn)速的高、低字節(jié)
- Serial.print(rpm1_LOW,BYTE);
- Serial.print(rpm2_HIGH,BYTE);//向上位計算機上傳右車輪電機當前轉(zhuǎn)速的高、低字節(jié)
- Serial.print(rpm2_LOW,BYTE);
- Serial.print(val_right,BYTE);// 向上位計算機上傳PID調(diào)節(jié)后的右輪電機PWM功率值
- if(flag=='a') //根據(jù)剛剛調(diào)節(jié)后的小車電機PWM功率值，及時修正小車前進或者后退狀態(tài)
- advance();
- if(flag=='b')
- back();
- //恢復到編碼器測速的初始狀態(tài)
- count1 = 0; //把脈沖計數(shù)值清零，以便計算下一秒的脈沖計數(shù)
- count2 = 0;
- old_time= millis(); // 記錄每秒測速時的時間節(jié)點
- attachInterrupt(0, Code1,FALLING); // 重新開放外部中斷0
- attachInterrupt(1, Code2,FALLING); // 重新開放外部中斷1
- }
- }
- // 左側(cè)車輪電機的編碼器碼盤計數(shù)中斷子程序
- void Code1()//算編碼器脈沖
- {
- //為了不計入噪音干擾脈沖，
- //當2次中斷之間的時間大于5ms時，計一次有效計數(shù)
- if((millis()-time1)>5)
- //當編碼器碼盤的OUTA脈沖信號下跳沿每中斷一次，
- count1 += 1; // 編碼器碼盤計數(shù)加一
- time1==millis();
- }
- // 右側(cè)車輪電機的編碼器碼盤計數(shù)中斷子程序
- void Code2()
- {
- if((millis()-time2)>5)
- //當編碼器碼盤的OUTA脈沖信號下跳沿每中斷一次，
- count2 += 1; // 編碼器碼盤計數(shù)加一
- time2==millis();
- }
|
[復制鏈接]
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
