Robofun 機器人論壇

標題: 自走車結合循跡問題 [打印本頁]

---

作者: kittylove554    時間: 2018-6-22 19:05
標題: 自走車結合循跡問題


小妹是個Arduino新手，想請教以下這個問題
我們是使用Uno的Abb car
要讓自走車配合以下的圖做路線



紅色的線代表循跡膠帶，
而車子要根據藍芽輸入的貨櫃號走到那個貨櫃的前方停下
如果不搭配循跡膠帶，我的小車是可以根據我輸入的貨櫃號去找路線的，
但是路走得不會很直，所以要搭配循跡來校正 紅外線感測器我們使用TCRT5000

但是我的車子卻校正的不是很好 想請問有什麼比較好的結合方式嗎?

目前我是讓車子在前進的時候順便一起判斷是不是需要校正  
由於程式碼太多了，精簡成片段還是有點多
想請教有什麼比較好的邏輯嗎?我覺得車子邊走才邊校正好像...真的哪裡怪怪的  
而做出來確實車子雖然會校正，但有機會歪掉
#include <SoftwareSerial.h>
#include<Servo.h>
Servo servoRight;
Servo servoLeft;
int IRStatus=0;
int leftIn;
int center;
int rightIn;
void setup() {
   servoRight.attach(12);
  servoLeft.attach(13);
  Serial.begin(9600);
  BT.begin(38400);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
}

void loop() {

      for(int q=1;q<=AAnsy;q++) {
      if(q!=AAnsy) {
         
         forward(450);
          IRStatus=0;
          getSensorData();
          if(leftIn==1)IRStatus+=4;
          if(center==1)IRStatus+=2;
          if(rightIn==1)IRStatus+=1;
          driveMotor(IRStatus);
         //turnLeft(27);
      }
      else {
         
         forward(450);
         turnRight(750);
         IRStatus=0;
          getSensorData();
          if(leftIn==1)IRStatus+=4;
          if(center==1)IRStatus+=2;
          if(rightIn==1)IRStatus+=1;
          driveMotor(IRStatus);
         //turnLeft(27);
        
      }
      
    }

    for(int w=1;w<=AAnsx;w++) {
      if(w!=AAnsx) {
         
        forward(500);
        IRStatus=0;
          getSensorData();
          if(leftIn==1)IRStatus+=4;
          if(center==1)IRStatus+=2;
          if(rightIn==1)IRStatus+=1;
          driveMotor(IRStatus);
        //turnLeft(27);
      }
      else {
        //forward(500);
        //turnLeft(27);
         
          IRStatus=0;
          getSensorData();
          if(leftIn==1)IRStatus+=4;
          if(center==1)IRStatus+=2;
          if(rightIn==1)IRStatus+=1;
          driveMotor(IRStatus);
        stopward(3000);
      }
   
  }
}


void getSensorData(){
  
  leftIn=digitalRead(A1);
  center=digitalRead(A2);
  rightIn=digitalRead(A3);
  
  }

  void driveMotor(int IRStatus){
  switch(IRStatus){
    case 1:
         turnLeft(250);
         break;
    case 3:
         turnLeft(300);
         break;
    case 4:
         turnRight(250);
         break;
    case 6:
         turnRight(300);
         break;
    }
  }


int turnLeft(int delaytime){
  servoRight.writeMicroseconds(1300);
  servoLeft.writeMicroseconds(1300);
  delay(delaytime);
  }
int turnRight(int delaytime){
  servoRight.writeMicroseconds(1700);
  servoLeft.writeMicroseconds(1700);
  delay(delaytime);
  }
int forward(int delaytime){
  servoRight.writeMicroseconds(1700);
  servoLeft.writeMicroseconds(1300);
  delay(delaytime);
  }
int stopward(int delaytime){
  servoRight.writeMicroseconds(1500);
  servoLeft.writeMicroseconds(1500);
  delay(delaytime);
  }

---

作者: 超新手    時間: 2018-6-22 19:41


void driveMotor(int IRStatus)
有點奇怪？照理說，當IRStatus 等於 1 或 4 的時候
應該是要大轉彎， 值也要大一點
應該是
void driveMotor(int IRStatus){
  switch(IRStatus){
    case 1:
         turnLeft(300);
         break;
    case 3:
         turnLeft(250);
         break;

    case 4:
         turnRight(300);
         break;
    case 6:
         turnRight(250);
         break;
    }
  }

而且直接尋線即可
加上
case 2:
forward(200);
break;
不知道為什麼你不直接這樣做，
然後有些地方還寫的怪怪的

---

作者: kittylove554    時間: 2018-6-24 17:29

超新手 發表於 2018-6-22 19:41
void driveMotor(int IRStatus)
有點奇怪？照理說，當IRStatus 等於 1 或 4 的時候
應該是要大轉彎， 值也 ...

謝謝你幫我解答
不過可能是我沒把程式碼打清楚才會覺得怪怪的吧
其實是做下面這個東西

因為邊邊有點難放 我放在中間
假設貨櫃為 A、B、C、D...區
排列是
D1 D2 D3 D4 E1 E2 E3 E4
A5 A6 A7 A8 B5 B6 B7 B8
A1 A2 A3 A4 B1 B2 B3 B4

其實要做的事情只是在手機輸入指定的貨櫃前停下

https://www.youtube.com/watch?v=bHe-WqrkT5U&feature=youtu.be

好像因為我影片隱私設定，嵌入不進來Orz

程式碼如下(未添加循跡)
...不曉得高人能否提點一下，需要增加循跡令車子能在黑色膠帶上行走的話應該怎麼寫比較好呢?
我一開始的想法是半循跡半自走，讓車子自己走，然後在依循跡校正，但是事實證明，車子是會亂跑的。
這部分邏輯真的卡到生鏽了

---

作者: kittylove554    時間: 2018-6-24 17:31


#include <SoftwareSerial.h>
#include<Servo.h>
Servo servoRight;
Servo servoLeft;
SoftwareSerial BT(9,10);   // 接收腳(RX), 傳送腳(TX)；
char val;
String vals;
int count1=0;
char cd[2];
int leftOut;
int leftIn;
int center;
int rightIn;
int rightOut;
int IRStatus=0;
int count=0;
int number1=0;
void setup() {
  // put your setup code here, to run once:
   servoRight.attach(12);
  servoLeft.attach(13);
  Serial.begin(9600); //for arduino serial port mointor
  BT.begin(38400);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
if(BT.available()){
  val=BT.read();
  if(val==65) vals+='A';if(val==66) vals+='B';if(val==67) vals+='C';
  if(val==68) vals+='D';if(val==69) vals+='E';if(val==70) vals+='F';
  if(val==71) vals+='G';if(val==72) vals+='H';if(val==73) vals+='I';
  if(val==49) vals+='1';if(val==50) vals+='2';if(val==51) vals+='3';
  if(val==52) vals+='4';if(val==53) vals+='5';if(val==54) vals+='6';
  if(val==55) vals+='7';if(val==56) vals+='8';
  count1++;
  if(count1==2){
    Serial.println(vals);
    int x=0,y=0; //貨櫃作標
    int Ax=0,Ay=0;// 車子起點作標
    int Ansx=0,Ansy=0;//要拿貨的座標
    int AAnsx=0,AAnsy=0;//車子要走幾步

     ↓這邊因為當初一些設計意外，結果變成這樣麻煩的狀況Q_Q早知道會寫成這樣我就用數字編號貨櫃就好了
    if(vals=="A1")number1=1;if(vals=="A2")number1=2;if(vals=="A3")number1=3;if(vals=="A4")number1=4;
    if(vals=="A5")number1=5;if(vals=="A6")number1=6;if(vals=="A7")number1=7;if(vals=="A8")number1=8;
   
    if(vals=="B1")number1=9;if(vals=="B2")number1=10;if(vals=="B3")number1=11;if(vals=="B4")number1=12;
    if(vals=="B5")number1=13;if(vals=="B6")number1=14;if(vals=="B7")number1=15;if(vals=="B8")number1=16;

    if(vals=="C1")number1=17;if(vals=="C2")number1=18;if(vals=="C3")number1=19;if(vals=="C4")number1=20;
    if(vals=="C5")number1=21;if(vals=="C6")number1=22;if(vals=="C7")number1=23;if(vals=="C8")number1=24;

    if(vals=="D1")number1=25;if(vals=="D2")number1=26;if(vals=="D3")number1=27;if(vals=="D4")number1=28;
    if(vals=="D5")number1=29;if(vals=="D6")number1=30;if(vals=="D7")number1=31;if(vals=="D8")number1=32;

    if(vals=="E1")number1=33;if(vals=="E2")number1=34;if(vals=="E3")number1=35;if(vals=="E4")number1=36;
    if(vals=="E5")number1=37;if(vals=="E6")number1=38;if(vals=="E7")number1=39;if(vals=="E8")number1=40;

    if(vals=="F1")number1=41;if(vals=="F2")number1=42;if(vals=="F3")number1=43;if(vals=="F4")number1=44;
    if(vals=="F5")number1=45;if(vals=="F6")number1=46;if(vals=="F7")number1=47;if(vals=="F8")number1=48;

    if(vals=="G1")number1=49;if(vals=="G2")number1=50;if(vals=="G3")number1=51;if(vals=="G4")number1=52;
    if(vals=="G5")number1=53;if(vals=="G6")number1=54;if(vals=="G7")number1=55;if(vals=="G8")number1=56;
   
    if(vals=="H1")number1=57;if(vals=="H2")number1=58;if(vals=="H3")number1=59;if(vals=="H4")number1=60;
    if(vals=="H5")number1=61;if(vals=="H6")number1=62;if(vals=="H7")number1=63;if(vals=="H8")number1=64;

    if(vals=="I1")number1=65;if(vals=="I2")number1=66;if(vals=="I3")number1=67;if(vals=="I4")number1=68;
    if(vals=="I5")number1=69;if(vals=="I6")number1=70;if(vals=="I7")number1=71;if(vals=="I8")number1=72;

    switch(number1){//放入貨櫃作標
      case 1:
      x=2;y=1;
      break;
      case 2:
      x=3;y=1;
      break;
      case 3:
      x=4;y=1;
      break;
      case 4:
      x=5;y=1;
      break;
      case 5:
      x=2;y=2;
      break;
      case 6:
      x=3;y=2;
      break;
      case 7:
      x=4;y=2;
      break;
      case 8:
      x=5;y=2;
      break;
      case 9:
      x=8;y=1;
      break;
      case 10:
      x=9;y=1;
      break;
      case 11:
      x=10;y=1;
      break;
      case 12:
      x=11;y=1;
      break;
      case 13:
       x=8;y=2;
      break;
      case 14:
      x=9;y=2;
      break;
      case 15:
      x=10;y=2;
      break;
      case 16:
      x=11;y=2;
      break;
      case 17:
       x=14;y=1;
      break;
      case 18:
      x=15;y=1;
      break;
      case 19:
      x=16;y=1;
      break;
      case 20:
      x=17;y=1;
      break;
      case 21:
       x=14;y=2;
      break;
      case 22:
      x=15;y=2;
      break;
      case 23:
      x=16;y=2;
      break;
      case 24:
      x=17;y=2;
      break;
      case 25:
      x=2;y=5;
      break;
      case 26:
      x=3;y=5;
      break;
      case 27:
      x=4;y=5;
      break;
      case 28:
      x=5;y=5;   
      break;
      case 29:
       x=2;y=6;
      break;
      case 30:
      x=3;y=6;
      break;

      case 31:
      x=4;y=6;
      break;
      case 32:
      x=5;y=6;
      break;
      case 33:
       x=8;y=5;
      break;
      case 34:
      x=9;y=5;
      break;
      case 35:
      x=10;y=5;
      break;
      case 36:
      x=11;y=5;   
      break;
      case 37:
      x=8;y=6;
      break;
      case 38:
      x=9;y=6;
      break;
      case 39:
      x=10;y=6;
      break;
      case 40:
      x=11;y=6;
      break;

      case 41:
      x=14;y=5;
      break;
      case 42:
      x=15;y=5;
      break;
      case 43:
      x=16;y=5;
      break;
      case 44:
      x=17;y=5;   
      break;
      case 45:
      x=14;y=6;
      break;
      case 46:
      x=15;y=6;
      break;
      case 47:
      x=16;y=6;
      break;
      case 48:
      x=17;y=6;
      break;
      case 49:
       x=2;y=9;
      break;
      case 50:
      x=3;y=9;
      break;
      case 51:
      x=4;y=9;
      break;
      case 52:
      x=5;y=9;   
      break;
      case 53:
       x=2;y=10;
      break;
      case 54:
      x=3;y=10;
      break;
      case 55:
      x=4;y=10;
      break;
      case 56:
      x=5;y=10;
      break;
      case 57:
      x=8;y=9;
      break;
      case 58:
      x=9;y=9;
      break;
      case 59:
      x=10;y=9;
      break;
      case 60:
      x=11;y=9;   
      break;
      case 61:
       x=8;y=10;
      break;
      case 62:
      x=9;y=10;
      break;
      case 63:
      x=10;y=10;
      break;
      case 64:
      x=11;y=10;
      break;
      case 65:
      x=14;y=9;
      break;
      case 66:
      x=15;y=9;
      break;
      case 67:
      x=16;y=9;
      break;
      case 68:
      x=17;y=9;   
      break;
      case 69:
       x=14;y=10;
      break;
      case 70:
      x=15;y=10;
      break;

      case 71:
      x=16;y=10;
      break;
      case 72:
      x=17;y=10;
      break;
      }
    Serial.println(x);
    Serial.println(y);
    if(vals.charAt(1)=='1'||vals.charAt(1)=='2'||vals.charAt(1)=='3'||vals.charAt(1)=='4'){
        //下面拿貨 ex 貨櫃A1(2,1)
        Ansx=x-Ax;//拿貨x   Ax起點(0) x貨櫃(2) Ansx拿貨(2)
        Ansy=y-Ay-1;//拿貨y Ay起點(0) y貨櫃(1) Ansy拿貨(0)
        }
      else{//上面拿貨 ex 貨櫃A5(2,2)
        Ansx=x-Ax;//拿貨x   Ax起點(0) x貨櫃(2) Ansx拿貨(2)
        Ansy=y-Ay+2;//拿貨y Ay起點(0) y貨櫃(2) Ansy拿貨(4)
        }
        //要去(2,4)座標
        AAnsx=Ansx-(Ax); //Ansx拿貨座標(2) Ax起點(0) AAnsx步數(2)
        AAnsy=Ansy-(Ay); //Ansy拿貨座標(4) Ay終點(0) AAnsy步數(4)
        Serial.println(AAnsx);
        Serial.println(AAnsy);
      for(int q=0;q<=AAnsy;q++) {//直走
      if(q!=AAnsy) {
         forward(450);
         turnLeft(27);//車子會右偏，所以往左校正一些
      }
      else {//右轉
         forward(450);
         turnRight(650);
         turnLeft(27);   
      }
    }
    for(int w=1;w<=AAnsx;w++) {//右轉後直走
      if(w!=AAnsx) {
        forward(500);
        //turnRight(50);
      }
      else {//停下
        stopward(3000);
      }
    }
    count1=0;
    vals="";
    stopward(100);
    }
  }
}
↓目前使用3燈，不知是否使用5燈會好點?
void getSensorData(){
  leftIn=digitalRead(A1);
  center=digitalRead(A2);
  rightIn=digitalRead(A3);
  }
   switch(IRStatus){
    case 0:
         stopward(100);
    break;
    case 1:
         turnLeft(250);
         break;
    case 2:
          stopward(100);
    break;
    case 3:
         turnLeft(300);
         break;
    case 4:
         turnRight(250);
         break;
    case 5:
         forward(450);
    break;
    case 6:
         turnRight(300);
         break;
    case 7:
          stopward(100);
    break;
    }
  }
int backward(int delaytime){
  servoRight.writeMicroseconds(1300);
  servoLeft.writeMicroseconds(1700);
  delay(delaytime);
  }
int turnLeft(int delaytime){
  servoRight.writeMicroseconds(1300);
  servoLeft.writeMicroseconds(1300);
  delay(delaytime);
  }
int turnRight(int delaytime){
  servoRight.writeMicroseconds(1700);
  servoLeft.writeMicroseconds(1700);
  delay(delaytime);
  }
int forward(int delaytime){
  servoRight.writeMicroseconds(1700);
  servoLeft.writeMicroseconds(1300);
  delay(delaytime);
}
  }
int stopward(int delaytime){
  servoRight.writeMicroseconds(1500);
  servoLeft.writeMicroseconds(1500);
  delay(delaytime);
  }

---

作者: 超新手    時間: 2018-6-27 20:17
要循跡, 只要把
1) void driveMotor(int IRStatus)
的 case 寫對即可
2) 在 void driveMotor(int IRStatus)
中增加 case 2
3) 拿掉其他 半自走 的部份

---

歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/)

Powered by Discuz! X3.2