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