/*************************************
 *     TIME:2023.3.9
 *     Development Team: Zhiyi Technology Co., Ltd.
 * 
 *  **************************************/
//TMI8549电机驱动芯片
#define ENA 5   //使能端，可使用PWM调速
#define INT1 7  //IN1输入端
#define INT2 8  //IN2输入端

int T_Sensor1; //寻迹传感器1
int T_Sensor2; //寻迹传感器2
int T_Sensor3; //寻迹传感器3
int T_Sensor4; //寻迹传感器4

void setup() {
  Serial.begin(9600);
  pinMode(ENA, OUTPUT);   //引脚设置输出模式
  pinMode(INT1, OUTPUT);  //引脚设置输出模式
  pinMode(INT2, OUTPUT);  //引脚设置输出模式
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT); 
  pinMode(A4, INPUT);
}

//INT1,INT2输出控制逻辑
  //INT1   INT2   OUT1    OUT2    OUT3    OUT4
  //0      0      1       0       1       0     前进
  //0      1      1       0       0       1     右转
  //1      0      0       1       1       0     左转
  //1      1      0       1       0       1     后退

void loop() {
  T_Sensor4 = digitalRead(A4);
  T_Sensor3 = digitalRead(A3);
  T_Sensor2 = digitalRead(A2);
  T_Sensor1 = digitalRead(A1);
  Serial.print(T_Sensor4);
  Serial.print("--");
  Serial.print(T_Sensor3);
  Serial.print("--");
  Serial.print(T_Sensor2);
  Serial.print("--");
  Serial.println(T_Sensor1);
  if(T_Sensor4 == LOW && T_Sensor3 == LOW && T_Sensor2 == LOW && T_Sensor1 == LOW){
    forward();
  }else if(T_Sensor4 == HIGH && T_Sensor3 == HIGH && T_Sensor2 == HIGH && T_Sensor1 == HIGH){
    stop();
  }else if(T_Sensor4 == HIGH && T_Sensor3 == LOW && T_Sensor2 == LOW && T_Sensor1 == LOW){
    left_M();
  }else if(T_Sensor4 == LOW && T_Sensor3 == HIGH && T_Sensor2 == LOW && T_Sensor1 == LOW){
    left();
  }else if(T_Sensor4 == LOW && T_Sensor3 == LOW && T_Sensor2 == HIGH && T_Sensor1 == LOW){
    right();
  }else if(T_Sensor4 == LOW && T_Sensor3 == LOW && T_Sensor2 == LOW && T_Sensor1 == HIGH){
    right_M();
  }
}

//前进
void forward(){
  analogWrite(ENA, 120);  //对ENA输入0~255的PWM波进行调速
  digitalWrite(INT1, LOW); 
  digitalWrite(INT2, LOW);
}

//后退
void back() {
  analogWrite(ENA, 120);  //对ENA输入0~255的PWM波进行调速
  digitalWrite(INT1,HIGH);  //后退
  digitalWrite(INT2,HIGH);  
}

void stop(){
  analogWrite(ENA, 0);  //对ENA输入0~255的PWM波进行调速,设置为0停车
}

//左转
void left() {
  analogWrite(ENA, 200);  //对ENA输入0~255的PWM波进行调速  
  digitalWrite(INT1,HIGH);  //左转
  digitalWrite(INT2,LOW);
}

//左转大
void left_M() {
  analogWrite(ENA, 240);  //对ENA输入0~255的PWM波进行调速
  digitalWrite(INT1,HIGH);  //左转
  digitalWrite(INT2,LOW);
}

//右转
void right() {
  analogWrite(ENA, 200);  //对ENA输入0~255的PWM波进行调速
  digitalWrite(INT1,LOW);   //右转
  digitalWrite(INT2,HIGH);
}

//右转大
void right_M() {
  analogWrite(ENA, 240);  //对ENA输入0~255的PWM波进行调速
  digitalWrite(INT1,LOW);   //右转
  digitalWrite(INT2,HIGH);
}