reverse-turn

const int L1 = 4;

const int L2 = 5;

const int R2 = 6;

const int R1 = 7;

// 馬達與motoduino的腳位對應

const int MotorR = 11;   

const int MotorL = 10;

const int MotorPower = 50;

const int MotorControlL1 = 8;   

const int MotorControlL2 = 9;   

const int MotorControlR1 = 12;

const int MotorControlR2 = 13;

// 感測器狀態值

byte byteSensorStatus=0;

void setup() {

   //set up serial communications

   Serial.begin(57600);

  pinMode(L1, INPUT);

  pinMode(L2, INPUT);

  pinMode(R2, INPUT);

  pinMode(R1, INPUT);

  pinMode(MotorL, OUTPUT);

  pinMode(MotorR, OUTPUT);

}

//////////// 主程式 ////////

void loop(){

  int nIRStatus;  

  //清除感測器狀態值

  byteSensorStatus = 0;

  // 讀取左感測器狀態值

  nIRStatus = digitalRead(L1);

  if(nIRStatus == 1)

     byteSensorStatus = (byteSensorStatus | (0x01 << 3));

  // 讀取中間感測器狀態值   

  nIRStatus = digitalRead(L2);

  if(nIRStatus == 1)

     byteSensorStatus = (byteSensorStatus | (0x01 << 2));

  // 讀取中間感測器狀態值   

  nIRStatus = digitalRead(R2);

  if(nIRStatus == 1)

     byteSensorStatus = (byteSensorStatus | (0x01 << 1));

    // 讀取右邊感測器狀態值   

  nIRStatus = digitalRead(R1);

  if(nIRStatus == 1)

     byteSensorStatus = (byteSensorStatus | (0x01));

   drivemotor(byteSensorStatus);

   //delay(2000);

}

///////////////////////////

void drivemotor(byte nStatus){

  Serial.print(nStatus);

switch(nStatus){

// 感測器 黑色:1   白色:0

case 15: // SL:1 SM:1 SR:1  //黑黑黑黑

leftP();

break;

case 14: // SL:1 SM:1 SR:0  //黑黑黑白

leftP();

break;

case 13: // SL:1 SM:0 SR:1  //黑黑白黑

leftP();

break;

case 12: // SL:1 SM:0 SR:0  //黑黑白白

rightP();

break;

case 11: // SL:0 SM:1 SR:1 //黑白黑黑

leftP();

break;

case 10: // SL:0 SM:1 SR:0  //黑白黑白

leftP();

break;

case 9: // SL:0 SM:0 SR:1  //黑白白黑

leftP();

break;

case 8: // SL:0 SM:0 SR:0  //黑白白白

rightP();

break;

case 7: // SL:1 SM:1 SR:1  //白黑黑黑

leftP();

break;

case 6: // SL:1 SM:1 SR:0  //白黑黑白

forward();

break;

case 5: // SL:1 SM:0 SR:1  //白黑白黑

leftP();

break;

case 4: // SL:1 SM:0 SR:0  //白黑白白

rightP();

break;

case 3: // SL:0 SM:1 SR:1 // 白白黑黑

leftP();

break;

case 2: // SL:0 SM:1 SR:0  // 白白黑白

leftP();

break;

case 1: // SL:0 SM:0 SR:1  // 白白白黑

leftP();

break;

case 0: // SL:0 SM:0 SR:0  //白白白白

reverse();

break;

}

}

void motorstop(){

Serial.println("stop!");

digitalWrite( MotorControlR1, LOW);

digitalWrite( MotorControlR2, LOW);

digitalWrite( MotorControlL1, LOW);

digitalWrite( MotorControlL2, LOW);

analogWrite( MotorL, 0);

analogWrite( MotorR, 0);

}

void forward(){

Serial.println("forward");

digitalWrite( MotorControlR1, LOW);

digitalWrite( MotorControlR2, HIGH);

digitalWrite( MotorControlL1, LOW);

digitalWrite( MotorControlL2, HIGH);

analogWrite( MotorL, MotorPower);

analogWrite( MotorR, MotorPower);

}

void backward(){

Serial.println("backward");

digitalWrite( MotorControlR1, HIGH);

digitalWrite( MotorControlR2, LOW);

digitalWrite( MotorControlL1, HIGH);

digitalWrite( MotorControlL2, LOW);

analogWrite( MotorL, MotorPower);

analogWrite( MotorR, MotorPower);

}

void rightP(){

Serial.println("rightP");

digitalWrite( MotorControlR1, LOW);

digitalWrite( MotorControlR2, HIGH);

digitalWrite( MotorControlL1, LOW);

digitalWrite( MotorControlL2, HIGH);

analogWrite( MotorL, 0);

analogWrite( MotorR, MotorPower);

}

void leftP(){

Serial.println("leftP");

digitalWrite( MotorControlR1, LOW);

digitalWrite( MotorControlR2, HIGH);

digitalWrite( MotorControlL1, LOW);

digitalWrite( MotorControlL2, HIGH);

analogWrite( MotorL, MotorPower);

analogWrite( MotorR, 0);

}

void reverse(){

backward();

delay(200);

digitalWrite( MotorControlR1, LOW);

digitalWrite( MotorControlR2, HIGH);

digitalWrite( MotorControlL1, HIGH);

digitalWrite( MotorControlL2, LOW);

analogWrite( MotorL, MotorPower);

analogWrite( MotorR, MotorPower);

delay(400);

 motorstop();

}