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