小车游华广 - 比赛

//by Power Lin @ wiki-power.com
//12:52

#include <AFMotor.h> //L293D 驱动库

int max = 250; //速度快
int mid = 0; //速度中
int min = -80; //速度慢
int vertical_delay = 250; //左右转 90 °的延时，待调试校正

#define S1 Sx_state[0]
#define S2 Sx_state[1]
#define S3 Sx_state[2]
#define S4 Sx_state[3]
#define S5 Sx_state[4]

AF_DCMotor M1(1); //定义电机对象，命名为 M1，接于 1 号端口
AF_DCMotor M2(4);
AF_DCMotor M3(2);
AF_DCMotor M4(3);

//function: 红外光电循迹
#define S1_pin 22
#define S2_pin 24
#define S3_pin 26
#define S4_pin 28
#define S5_pin 30
#define CLP_pin 32 //微动开关
#define Near_pin 34 //红外壁障

int Sx_state[5] = {0, 0, 0, 0, 0}; //5 个红外光电的状态
int CLP_state = 0; //微动开关状态
int CLP_count = 0; //微动开关状态计数
int Near_state = 0; //红外壁障的状态
int error_state = 0; //偏移状态
int decide_state = 0; //交叉路口选择状态
int vertical_state = 0; //直角转弯的状态
int vertical_state_left = 0; //直角转弯的状态
int stop_state = 0; //停下状态

void car_stop() { //停止
  M1.run(RELEASE);
  M2.run(RELEASE);
  M3.run(RELEASE);
  M4.run(RELEASE);
}
void car_forward() { //前进
  motor_run(max, max, max, max);
}
void car_backward() { //后退
  motor_run(-max, -max, -max, -max);
}
void car_left_light() { //小左转
  motor_run(mid, max, mid, max);
}
void car_left_heavy() { //大左转
  motor_run(min, max, min, max);
}
void car_right_light() { //小右转
  motor_run(max, mid, max, mid);
}
void car_right_heavy() { //大右转
  motor_run(max, min, max, min);
}
void car_left_vertical() { //左转 90°
  motor_run(-max, max, -max, max);
  delay(vertical_delay);
}
void car_right_vertical() { //右转 90°
  motor_run(max, -max, max, -max);
  delay(vertical_delay);
}

void motor_run(int m1, int m2, int m3, int m4) { //设置速度
  M1.setSpeed(m1);
  M2.setSpeed(m2);
  M3.setSpeed(m3);
  M4.setSpeed(m4);
  if (m1 > 0) M1.run(FORWARD);
  else M1.run(BACKWARD);
  if (m2 > 0) M2.run(FORWARD);
  else M2.run(BACKWARD);
  if (m3 > 0) M3.run(FORWARD);
  else M3.run(BACKWARD);
  if (m4 > 0) M4.run(FORWARD);
  else M4.run(BACKWARD);
}

void setup() {
  //循迹初始化
  pinMode(S1_pin, INPUT);
  pinMode(S2_pin, INPUT);
  pinMode(S3_pin, INPUT);
  pinMode(S4_pin, INPUT);
  pinMode(S5_pin, INPUT);
  pinMode(CLP_pin, INPUT);
  pinMode(Near_pin, INPUT);

  //蓝牙初始化
  Serial.begin(9600);
  Serial.println("hello world");
  car_stop(); //初始化先关闭电机
}

void loop() {
  //循迹检测
  Sx_state[0] = digitalRead(S1_pin);
  Sx_state[1] = digitalRead(S2_pin);
  Sx_state[2] = digitalRead(S3_pin);
  Sx_state[3] = digitalRead(S4_pin);
  Sx_state[4] = digitalRead(S5_pin);
  CLP_state = digitalRead(CLP_pin);
  Near_state = digitalRead(Near_pin);
  for (int i = 0; i <= 4; i++ ) {
    if (Sx_state[i] == 0) {
      Sx_state[i] = 1;
    } else {
      Sx_state[i] = 0;
    }
  }
  car_control(); //运动算法
  //  Serial.println(CLP_count);
}

//小车运动算法
void car_control() {
  if (CLP_state == 1 ) {
    CLP_count++; //微动开关计数自加一
    if (CLP_count >= 1) { //撞到挡板往回走
      Serial.println("State 1");
      car_stop(); // 歇一歇
      delay(500);
      car_backward(); //后退
      delay(300);
      car_left_heavy(); //转弯
      delay(950);
    }
  }
  else {
    if (S1 == 0 && S2 == 0 && S3 == 1 && S4 == 0  && S5 == 0)
      error_state = 0; // 无偏移
    if (S1 == 0 && S2 == 1 && S3 == 0 && S4 == 0  && S5 == 0)
      error_state = -1;
    if (S1 == 0 && S2 == 0 && S3 == 0 && S4 == 1  && S5 == 0)
      error_state = 1;
    if (S1 == 1 && S2 == 0 && S3 == 0 && S4 == 0  && S5 == 0)
      error_state = -2;
    if (S1 == 0 && S2 == 0 && S3 == 0 && S4 == 0  && S5 == 1)
      error_state = 2;
    if (S1 == 1 && S2 == 1 && S3 == 1 && S4 == 0  && S5 == 1)
      error_state = -3;
    if (S1 == 0 && S2 == 0 && S3 == 1 && S4 == 1  && S5 == 1) {
      /*vertical_state++;
        if (vertical_state == 1 && vertical_state == 2) {
        error_state = 9; //转弯进机关
        }*/
      error_state = 3;
    }
    if (S1 == 0 && S2 == 1 && S3 == 1 && S4 == 1  && S5 == 0)
      error_state = 111;
    if (S1 == 1  && S2 == 1 && S3 == 1 && S4 == 1  && S5 == 1)
      error_state = 111;
    if (S1 == 0  && S2 == 1 && S3 == 1 && S4 == 1  && S5 == 1)
      error_state = 111;
    if (S1 == 1  && S2 == 1 && S3 == 1 && S4 == 1  && S5 == 0)
      error_state = 111;
    if (S1 == 1  && S2 == 1 && S3 == 0 && S4 == 1  && S5 == 1)
      error_state = 222;

    if (S1 == 1  && S2 == 0 && S3 == 1 && S4 == 1  && S5 == 1)
      error_state = 222;
    if (S1 == 1  && S2 == 1 && S3 == 1 && S4 == 0  && S5 == 1)
      error_state = 222;

    switch (error_state) {
      case 222: // T 路口
        //        if (stop_state == 9) {
        //          car_left_heavy();
        //          delay(400);
        //        } else {
        car_forward();
        //        }
        //        stop_state++;
        break;
      case 111: //交叉路口
        car_forward(); // 直走
        break;
      case -3:
        if (error_state == -3) {
          car_stop();
          delay(400);
          //car_left_vertical(); //11100 左直角 - 左转 90°
          //while (1)car_stop();
          if (vertical_state == 1) {
            car_left_heavy(); //00111 右直角 - 右转 90°
            delay(1000);
          } else {
            car_left_heavy();
            delay(400);
          }
          vertical_state_left++;
        } else {
          car_forward();
        }
        break;
      case 3:

        if (error_state == 3) {
          car_stop();
          delay(400);

          if (vertical_state == 0) {
            car_right_heavy(); //00111 右直角 - 右转 90°
            delay(500);
          } else {
            car_right_heavy();
            //          car_forward();
          }
          car_forward(); // 直走
          vertical_state++;
        } else {
          car_forward();
        }
        break;

      case 0:
        car_forward(); //00100 直走
        break;
      case -1:
        car_left_light(); //01000 小左转
        break;
      case 1:
        car_right_light(); //00010 小右转
        break;
      case -2:
        car_left_heavy(); //10000 大左转
        break;
      case 2:
        car_right_heavy(); //00001 大右转
        break;

      //case 9:
      //car_right_heavy();
      //delay(500);
      //break;
      //      case 222:
      //        car_stop();
      //        break;
      default:
        if (error_state == -2) {
          car_left_heavy();
        }
        else if (error_state == 2) {
          car_right_heavy();
        }
        else
          car_forward();
        //car_stop();
    }

  }
}