Robofun 機器人論壇

標題: 程式問題請教 [打印本頁]
作者: ypyp3399    時間: 2013-12-11 09:39
標題: 程式問題請教
您好
我使用Arduino DUE來驅動馬達，我採用5個感測器，有32種狀態，可是馬達運動卻不是我程式所設定的狀態，舉例case 0(感測器狀態皆為0)，馬達程式預設皆停止，實際運行時馬達卻全速運行。
想請問我的程式是否有問題。
  1. const int IRPeople =10;   //紅外線感測器前方避障
  2. const int SLeft_1 = 5;   //左邊方向紅外線二訊號輸入
  3. const int SLeft = 6;      //左邊方向紅外線一訊號輸入
  4. const int SMiddle = 7;    //中間方向紅外線訊號輸入
  5. const int SRight = 11;     //右邊方向紅外線一訊號輸入
  6. const int SRight_1 = 12;  //右邊方向紅外線二訊號輸入

  8. const int PeopleLED = 13;  //紅外線感測前方避障狀態LED
  9. const int Motor_Left = 9;
  10. const int Motor_Right = 8;
  11. const int Motor_M1 = DAC0;     //右邊
  12. const int Motor_M2 = DAC1;     //左邊

  14. byte byteSensorStatus = 0;

  16. void setup(){
  17.   Serial.begin(9600);

  19.   pinMode(IRPeople,INPUT);  //紅外線感測器前方避障
  20.   pinMode(SLeft_1,INPUT);
  21.   pinMode(SLeft,INPUT);  
  22.   pinMode(SMiddle,INPUT);
  23.   pinMode(SRight,INPUT);
  24.   pinMode(SRight_1,INPUT);
  25.   
  26.   pinMode(Motor_M1,OUTPUT);
  27.   pinMode(Motor_M2,OUTPUT);
  28.   pinMode(Motor_Left,OUTPUT);
  29.   pinMode(Motor_Right,OUTPUT);
  30.   pinMode(PeopleLED,OUTPUT);  //紅外線感測前方避障狀態LED
  31.   
  32.   delay(5000);
  33. }

  35. void loop(){
  36.   int nIRStatus;
  37.   
  38.   byteSensorStatus = 0;
  39.   nIRStatus = digitalRead(IRPeople);
  40.   if(nIRStatus == 0){
  41.   
  42.     motorstop(0,0);
  43.     delay(5000);
  44.   }
  45.   else{
  46.   nIRStatus = digitalRead(SLeft_1);
  47.   if(nIRStatus == 1)
  48.   byteSensorStatus = (byteSensorStatus | (0x01 << 4));
  49.   
  50.   nIRStatus = digitalRead(SLeft);
  51.   if(nIRStatus == 1)
  52.   byteSensorStatus = (byteSensorStatus | (0x01 << 3));
  53.   
  54.   nIRStatus = digitalRead(SMiddle);
  55.   if(nIRStatus == 1)
  56.   byteSensorStatus = (byteSensorStatus | (0x01 << 2));
  57.   
  58.   nIRStatus = digitalRead(SRight);
  59.   if(nIRStatus == 1)
  60.   byteSensorStatus = (byteSensorStatus | (0x01 << 1));
  61.   
  62.   nIRStatus = digitalRead(SRight_1);
  63.   if(nIRStatus == 1)
  64.   byteSensorStatus = (byteSensorStatus | 0x01);
  65.   
  66.   drivemotor(byteSensorStatus);
  67.   }
  68. }

  70. void drivemotor(byte nStatus)
  71. {
  72.   switch(nStatus)
  73.   {
  74.     case 31:// SL1:1 SL:1 SM:1 SR:1 SR1:1//　黑黑黑黑黑
  75.     motorstop(0,0);
  76.     break;
  77.     case 30:// SL1:1 SL:1 SM:1 SR:1 SR1:0//　黑黑黑黑白
  78.     motorstop(0,0);
  79.     break;
  80.     case 29:// SL1:1 SL:1 SM:1 SR:0 SR1:1//　黑黑黑白黑
  81.     motorstop(0,0);
  82.     break;
  83.     case 28:// SL1:1 SL:1 SM:1 SR:0 SR1:0//　黑黑黑白白
  84.     right(0,0);
  85.     break;
  86.     case 27:// SL1:1 SL:1 SM:0 SR:1 SR1:1//　黑黑白黑黑
  87.     motorstop(0,0);
  88.     break;
  89.     case 26:// SL1:1 SL:1 SM:0 SR:1 SR1:1//　黑黑白黑白
  90.     motorstop(0,0);
  91.     break;
  92.     case 25:// SL1:1 SL:1 SM:0 SR:0 SR1:1//　黑黑白白黑
  93.     motorstop(0,0);
  94.     break;
  95.     case 24:// SL1:1 SL:1 SM:0 SR:0 SR1:0//　黑黑白白白
  96.     slowright_1(0,0);
  97.     break;
  98.     case 23:// SL1:1 SL:0 SM:1 SR:1 SR1:1//　黑白黑黑黑
  99.     motorstop(0,0);
  100.     break;
  101.     case 22:// SL1:1 SL:0 SM:1 SR:1 SR1:0//　黑白黑黑白
  102.     motorstop(0,0);
  103.     break;
  104.     case 21:// SL1:1 SL:0 SM:1 SR:0 SR1:1//　黑白黑白黑
  105.     motorstop(0,0);
  106.     break;
  107.     case 20:// SL1:1 SL:0 SM:1 SR:0 SR1:0//　黑白黑白白
  108.     motorstop(0,0);
  109.     break;
  110.     case 19:// SL1:1 SL:0 SM:0 SR:1 SR1:1//　黑白白黑黑
  111.     motorstop(0,0);
  112.     break;
  113.     case 18:// SL1:1 SL:0 SM:0 SR:1 SR1:0//　黑白白黑白
  114.     motorstop(0,0);
  115.     break;
  116.     case 17:// SL1:1 SL:0 SM:0 SR:0 SR1:1//　黑白白白黑
  117.     motorstop(0,0);
  118.     break;
  119.     case 16:// SL1:1 SL:0 SM:0 SR:0 SR1:0//　黑白白白白
  120.     slowright_1(0,0);
  121.     break;
  122.     case 15:// SL1:0 SL:1 SM:1 SR:1 SR1:1//　白黑黑黑黑
  123.     motorstop(0,0);
  124.     break;
  125.     case 14:// SL1:0 SL:1 SM:1 SR:1 SR1:0//　白黑黑黑白
  126.     forward(0,130);
  127.     break;
  128.     case 13:// SL1:0 SL:1 SM:1 SR:0 SR1:1//　白黑黑白黑
  129.     motorstop(0,0);
  130.     break;
  131.     case 12:// SL1:0 SL:1 SM:1 SR:0 SR1:0//　白黑黑白白
  132.     slowright(0,0);
  133.     break;
  134.     case 11:// SL1:0 SL:1 SM:0 SR:1 SR1:1//　白黑白黑黑
  135.     motorstop(0,0);
  136.     break;
  137.     case 10:// SL1:0 SL:1 SM:0 SR:1 SR1:0//　白黑白黑白
  138.     motorstop(0,0);
  139.     break;
  140.     case 9:// SL1:0 SL:1 SM:0 SR:0 SR1:1//　白黑白白黑
  141.     motorstop(0,0);
  142.     break;
  143.     case 8:// SL1:0 SL:1 SM:0 SR:0 SR1:0//　白黑白白白
  144.     slowright(0,0);
  145.     break;
  146.     case 7:// SL1:0 SL:0 SM:1 SR:1 SR1:1//　白白黑黑黑
  147.     left(0,0);
  148.     break;
  149.     case 6:// SL1:0 SL:0 SM:1 SR:1 SR1:0//　白白黑黑白
  150.     slowleft(0,0);
  151.     break;
  152.     case 5:// SL1:0 SL:0 SM:1 SR:0 SR1:1//　白白黑白黑
  153.     motorstop(0,0);
  154.     break;
  155.     case 4:// SL1:0 SL:0 SM:1 SR:0 SR1:0//　白白黑白白
  156.     forward(0,130);
  157.     break;
  158.     case 3:// SL1:0 SL:0 SM:0 SR:1 SR1:1//　白白白黑黑
  159.     slowleft_1(0,0);
  160.     break;
  161.     case 2:// SL1:0 SL:0 SM:0 SR:1 SR1:0//　白白白黑白
  162.     slowleft(0,0);
  163.     break;
  164.     case 1:// SL1:0 SL:0 SM:0 SR:0 SR1:1//　白白白白黑
  165.     slowleft_1(0,0);
  166.     break;
  167.     case 0:// SL1:0 SL:0 SM:0 SR:0 SR1:0//　白白白白白
  168.     motorstop(0,0);
  169.     break;
  170.   }
  171. }

  173. void motorstop(byte flag, byte motorspeed)
  174. {
  175.   
  176.   analogWrite(Motor_M1,motorspeed);
  177.   analogWrite(Motor_M2,motorspeed);
  178.   
  179. }

  181. void forward(byte flag, byte motorspeed)
  182. {
  183.   
  184.   digitalWrite(Motor_Left,LOW);
  185.   digitalWrite(Motor_Right,LOW);
  186.   analogWrite(Motor_M1,motorspeed);
  187.   analogWrite(Motor_M2,motorspeed);
  188. }

  190. void right(byte flag, byte motorspeed)
  191. {
  192.   digitalWrite(Motor_Left,LOW);
  193.   digitalWrite(Motor_Right,LOW);
  194.   analogWrite(Motor_M1,motorspeed);
  195.   analogWrite(Motor_M2,70);
  196. }

  198. void slowright(byte flag, byte motorspeed)
  199. {
  200.   digitalWrite(Motor_Left,LOW);
  201.   digitalWrite(Motor_Right,LOW);
  202.   analogWrite(Motor_M1,motorspeed);
  203.   analogWrite(Motor_M2,70);
  204. }

  206. void slowright_1(byte flag, byte motorspeed)
  207. {
  208.   digitalWrite(Motor_Left,LOW);
  209.   digitalWrite(Motor_Right,LOW);
  210.   analogWrite(Motor_M1,motorspeed);
  211.   analogWrite(Motor_M2,80);
  212. }

  214. void left(byte flag, byte motorspeed)
  215. {
  216.   digitalWrite(Motor_Left,LOW);
  217.   digitalWrite(Motor_Right,LOW);
  218.   analogWrite(Motor_M1,70);
  219.   analogWrite(Motor_M2,motorspeed);
  220. }

  222. void slowleft(byte flag, byte motorspeed)
  223. {
  224.   digitalWrite(Motor_Left,LOW);
  225.   digitalWrite(Motor_Right,LOW);
  226.   analogWrite(Motor_M1,70);
  227.   analogWrite(Motor_M2,motorspeed);
  228. }

  230. void slowleft_1(byte flag, byte motorspeed)
  231. {
  232.   digitalWrite(Motor_Left,LOW);
  233.   digitalWrite(Motor_Right,LOW);
  234.   analogWrite(Motor_M1,80);
  235.   analogWrite(Motor_M2,motorspeed);
  236. }
複製代碼
作者: vegewell    時間: 2013-12-21 03:25
回復 1# ypyp3399

是的,你這個程式有些問題,
如,三方紅外線訊號輸入同時是一,
那你的byteSensorStatus,應該選擇哪個?得到最好的動作,
你應該先測試一下 motorstop(0,0);
另外寫個簡單的測試程式,沒接紅外線感測器的,
據我瞭解,有些馬達驅動晶片,即使enable設定為0,也一樣照轉不誤.



歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/) Powered by Discuz! X3.2