day -5 游园（黑色寻迹加颜色识别） -arduino

//纯39弯道实现，能否实现纯凭小车质量，操作有一定难度，跑地图式
vint Left_f = 8;
int Left_s = 7;
int Right_f = 2;
int Right_s = 4;
int Left_go = 6;
int Right_go = 5;
int red = 11;
int green = 10;
int blue = 9;
int huidu;
int k;

void go_straight() {
  digitalWrite(Left_f, HIGH);
  digitalWrite(Left_s, LOW);
  digitalWrite(Right_f, HIGH);
  digitalWrite(Right_s, LOW);
  analogWrite(Left_go, 50);
  analogWrite(Right_go, 50);
}
void Stop() {
  digitalWrite(Left_f, LOW);
  digitalWrite(Left_s, LOW);
  digitalWrite(Right_f, LOW);
  digitalWrite(Right_s, LOW);
  analogWrite(Left_go, 0);
  analogWrite(Right_go, 0);
}
void turn_left() {
  digitalWrite(Left_f, LOW);
  digitalWrite(Left_s, HIGH);
  digitalWrite(Right_f, HIGH);
  digitalWrite(Right_s, LOW);
  analogWrite(Left_go, 7);
  analogWrite(Right_go, 50);
}
void turn_right() {
  digitalWrite(Left_f, HIGH);
  digitalWrite(Left_s, LOW);
  digitalWrite(Right_f, LOW);
  digitalWrite(Right_s, HIGH);
  analogWrite(Left_go, 50);
  analogWrite(Right_go, 7);
}
void back() {
  digitalWrite(Left_f, LOW);
  digitalWrite(Left_s, HIGH);
  digitalWrite(Right_f, LOW);
  digitalWrite(Right_s, HIGH);
  analogWrite(Left_go, 50);
  analogWrite(Right_go, 50);
}
void turn_left_stop() {
  digitalWrite(Left_f, LOW);
  digitalWrite(Left_s, HIGH);
  digitalWrite(Right_f, HIGH);
  digitalWrite(Right_s, LOW);
  analogWrite(Left_go, 50);
  analogWrite(Right_go, 50);
}
void turn_right_stop() {
  digitalWrite(Left_f, HIGH);
  digitalWrite(Left_s, LOW);
  digitalWrite(Right_f, LOW);
  digitalWrite(Right_s, HIGH);
  analogWrite(Left_go, 50);
  analogWrite(Right_go, 50);
}
void select_light(int k){
  switch(k){
    case 1:analogWrite(red,245);analogWrite(green,20);analogWrite(blue,195);break;
    case 2:analogWrite(red,10);analogWrite(green,21);analogWrite(blue,248);break;
    case 3:analogWrite(red,250);analogWrite(green,0);analogWrite(blue,0);break;
    case 4:analogWrite(red,40);analogWrite(green,200);analogWrite(blue,245);break;
    case 5:analogWrite(red,15);analogWrite(green,248);analogWrite(blue,10);break;
    case 6:analogWrite(red,227);analogWrite(green,246);analogWrite(blue,18);break;
    case 7:analogWrite(red,10);analogWrite(green,21);analogWrite(blue,248);break;
    case 8:analogWrite(red,40);analogWrite(green,200);analogWrite(blue,245);break;
    case 9:analogWrite(red,15);analogWrite(green,248);analogWrite(blue,10);break;
    default:break;
  }
}

void select_way(int choice) {       //小车动作
  switch (choice) {
    case 1: delay(200); turn_left_stop(); delay(800); break;
    case 2:  delay(200); turn_right_stop(); delay(800); break;
    case 3: delay(400);
      Stop();
      k++;
      select_light(k);
      delay(300);
      digitalWrite(red,LOW);
      digitalWrite(green,LOW);
      digitalWrite(blue, LOW);
      back();
      while(digitalRead(A2) || digitalRead(A1) ||digitalRead(A3) || digitalRead(A4)){
        delay(10);
      }
      turn_left_stop();
      delay(400);
       while((digitalRead(A1) ||digitalRead(A3)));
       delay(50);
      go_straight();
  break;
    case 4: go_straight(); delay(800); break;
  }
}
void Begin() {                   //按钮
  while (digitalRead(A0));
  while (!digitalRead(A0)) {
    delay(10);
    if (digitalRead(A0) == 0) {
      delay(100);
      while (!digitalRead(A0));
    }
  }
}
//1;左转   2：右转   3,：转身   4：直走
int a[39] = {4, 2, 1, 2, 3, 2, 4, 3, 2, 2, 2, 3, 4, 4, 3, 2, 1, 1, 3, 1, 3, 1, 1, 1, 2, 3, 2, 2, 3, 2, 2, 3, 2, 1, 3, 1, 2, 3};
int i = 0;


void setup() {
  // put your setup code here, to run once:
  pinMode(Left_f, OUTPUT);
  pinMode(Left_s, OUTPUT);
  pinMode(Right_f, OUTPUT);
  pinMode(Right_s, OUTPUT);
  pinMode(Left_go, OUTPUT);
  pinMode(Right_go, OUTPUT);
  pinMode(A0, INPUT);
  pinMode(A2, INPUT);
  pinMode(A1, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
  pinMode(red, OUTPUT);
  pinMode(green, OUTPUT);
  pinMode(blue, OUTPUT);
  Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly:
  Begin();
  delay(1000);
  while (1) {
    
    if (digitalRead(A2) && !digitalRead(A1) && !digitalRead(A3) && digitalRead(A4) ) {
      go_straight();
    }
    else if (digitalRead(A2) && !digitalRead(A1) &&  digitalRead(A3) && digitalRead(A4)) {
      turn_left();
    }
    else if (digitalRead(A2) && digitalRead(A1) && !digitalRead(A3) && digitalRead(A4)) {
      turn_right();

    }
    else if (digitalRead(A2) && digitalRead(A1) && digitalRead(A3) && !digitalRead(A4)) {
      turn_right();
      delay(30);
    }
    else if (!digitalRead(A2) && digitalRead(A1) && digitalRead(A3) && digitalRead(A4)) {
      turn_left();
      delay(30);
    }
    else if (!digitalRead(A2) && !digitalRead(A1) && digitalRead(A3) && digitalRead(A4)) {
      delay(40);
      turn_left_stop();
    }
    else if (!digitalRead(A2) && digitalRead(A1) && !digitalRead(A3) && digitalRead(A4)) {
      delay(40);
      turn_left_stop();
    }
    else if (digitalRead(A2) && !digitalRead(A1) && digitalRead(A3) && !digitalRead(A4)) {
      delay(40);
      turn_right_stop();
    }
    else  if (digitalRead(A2) && digitalRead(A1) && !digitalRead(A3) && !digitalRead(A4)) {
      delay(40);
      turn_right_stop();
    }
    else if (!digitalRead(A2) && !digitalRead(A1) && !digitalRead(A3) && digitalRead(A4)) {
      select_way(a[i]);
      i++;
    }
    else if (digitalRead(A2) && !digitalRead(A1) && !digitalRead(A3) && !digitalRead(A4)) {
      select_way(a[i]);
      i++;
    }
    else if (!digitalRead(A2) && !digitalRead(A1) && !digitalRead(A3) && !digitalRead(A4)) {
      select_way(a[i]);
      i++;
    }
    if (i == 38) {
      Stop();
    }
  }
}