/*************************************
 *     TIME:2023.3.10
 *     Development Team: Zhiyi Technology Co., Ltd.
 * 
 *  **************************************/
#include <Servo.h>
Servo myservo;// Set up a servo

//TMI8549 motor driver chip
#define ENA 5   // Enable, PWM can be used
#define INT1 7  //IN1 input
#define INT2 8  //IN2 input

#define sensor_L 10 // Left infrared sensor
#define sensor_R 9  // Right infrared sensor

// Ultrasonic sensor pin setup
int Echo = 12;//Pin to D12
int Trig = 13;//Pin to D13

int SensorLeft;
int SensorRight;
int rightDistance = 0, leftDistance = 0, middleDistance = 0;  // Define ultrasonic distances in three directions
int distance;

int T_Sensor1; // Trace sensor 1
int T_Sensor2; // Trace sensor 2
int T_Sensor3; // Trace sensor 3
int T_Sensor4; // Trace sensor 4

int Car_mode;
char speed_FB; // Mode 0 speed
char speed_LR; // Mode 1 speed 


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  myservo.attach(11,700,2400);// Set servo pin to D11
  myservo.write(80); // Initialize the steering Angle
  pinMode(ENA, OUTPUT);   // Pin sets output mode
  pinMode(INT1, OUTPUT);  // Pin sets output mode
  pinMode(INT2, OUTPUT);  // Pin sets output mode 
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT); 
  pinMode(A4, INPUT);
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT); 
  pinMode(sensor_L, INPUT);
  pinMode(sensor_R, INPUT);
  Car_mode = 0;  
  delay(100); 
}

//INT1,INT2输出控制逻辑
  //INT1   INT2   OUT1    OUT2    OUT3    OUT4
  //0      0      1       0       1       0     Forward
  //0      1      1       0       0       1     Turn right
  //1      0      0       1       1       0     Turn left
  //1      1      0       1       0       1     Back off

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available() > 0) {      //Determine whether the received data is greater than 0
    char ser_val = Serial.read();
    Serial.print("ser_val=");
    Serial.println(ser_val);
    switch(ser_val)
      {
          case 'F':
          forward();
          break;
          case 'B':
          back();
          break;
          case 'S':
          stop();
          break;
          case 'L':
          left();
          break;
          case 'R':
          right();
          break;
          case '0':
          Car_mode = 0;
          stop();
          break;
          case '1':
          Car_mode = 1;
          break;
          case '2':
          Car_mode = 2;
          break;
          case '3':
          Car_mode = 3;
          break;
    }      
  }
  if(Car_mode == 1){
    speed_FB = 120;  speed_LR = 150;
    Ultrasonic();
  }else if(Car_mode == 2){
    speed_FB = 120;  speed_LR = 120;    
    follow();
  }else if(Car_mode == 3){
    speed_FB = 120;  speed_LR = 200;
    Tracking_car();
  }else{
    speed_FB = 200;  speed_LR = 200;
  }
}

//前进
void forward(){
  analogWrite(ENA, speed_FB);  //The PWM wave from ENA input 0 to 255 is speed-regulated
  digitalWrite(INT1, LOW); 
  digitalWrite(INT2, LOW);  
}

//前进慢速
void forward_U(){
  analogWrite(ENA, 70);  //The PWM wave from ENA input 0 to 255 is speed-regulated
  digitalWrite(INT1, LOW); 
  digitalWrite(INT2, LOW);
}

//后退
void back() {
  analogWrite(ENA, speed_FB);  //The PWM wave from ENA input 0 to 255 is speed-regulated
  digitalWrite(INT1,HIGH);  //后退
  digitalWrite(INT2,HIGH);  
}

void stop(){
  analogWrite(ENA, 0);  //The PWM wave from ENA input 0 to 255 is speed-regulated,Set to 0 to stop
}

//左转
void left() {
  analogWrite(ENA, speed_LR);  //The PWM wave from ENA input 0 to 255 is speed-regulated  
  digitalWrite(INT1,HIGH);  //左转
  digitalWrite(INT2,LOW);
}

//左转大
void left_M() {
  analogWrite(ENA, 240);  //The PWM wave from ENA input 0 to 255 is speed-regulated  
  digitalWrite(INT1,HIGH);  //左转
  digitalWrite(INT2,LOW);
}

//右转
void right() {
  analogWrite(ENA, speed_LR);  //The PWM wave from ENA input 0 to 255 is speed-regulated
  digitalWrite(INT1,LOW);   //右转
  digitalWrite(INT2,HIGH);
}

//右转大
void right_M() {
  analogWrite(ENA, 240);  //The PWM wave from ENA input 0 to 255 is speed-regulated
  digitalWrite(INT1,LOW);   //右转
  digitalWrite(INT2,HIGH);
}

void Tracking_car(){  
  
  T_Sensor4 = digitalRead(A4);
  T_Sensor3 = digitalRead(A3);
  T_Sensor2 = digitalRead(A2);
  T_Sensor1 = digitalRead(A1);
  
  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();
  }
}

float GetDistance()
{
    float distance;
    // Send a low short pulse to Trig to trigger the ranging
    digitalWrite(Trig, LOW); //Send a low level to Trig
    delayMicroseconds(2); 
    digitalWrite(Trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(Trig, LOW);
    distance = pulseIn(Echo, HIGH) / 58.00;
    //Serial.print("Distance = ");
    //Serial.println(distance);//The serial output distance is converted into cm
	  return distance;
}

void Ultrasonic(){
    SensorLeft = digitalRead(sensor_L); 
    SensorRight = digitalRead(sensor_R);       
    middleDistance = GetDistance(); // Get the intermediate distance
    if(middleDistance < 25){
      stop();
      delay(200);
      myservo.write(145);// Turn left
      delay(300);  
      leftDistance = GetDistance();
      delay(100);
      myservo.write(35);// Turn right
      delay(300);          
      rightDistance = GetDistance();
      delay(100);
      myservo.write(90);// Back to positive
      delay(500);

      if(leftDistance > rightDistance){   // Turn left
        left();
        delay(380);        
      }else{   // Turn right
        right();
        delay(380);          
      }
    }else {    
      if((SensorLeft  == 0) && (SensorRight == 0)){
          back();
          delay(100);
      }else if((SensorLeft  == 0) && (SensorRight == 1)){
          right();
          delay(50);                 
      }else if((SensorLeft  == 1) && (SensorRight == 0)){
          left();
          delay(50);        
      }else if((SensorLeft  == 1) && (SensorRight == 1)){
        if(middleDistance > 50){
          forward();
        }else if(middleDistance <= 50){
          forward_U();
        }
      }
    }
}

void follow(){
    SensorLeft = digitalRead(sensor_L); 
    SensorRight = digitalRead(sensor_R);
    distance = GetDistance(); //Get Distance
    if(SensorLeft == LOW && SensorRight == HIGH){
      left();      
    }else if(SensorLeft == HIGH && SensorRight == LOW){
      right();
    }else if(SensorLeft == LOW && SensorRight == LOW){
      back();
    }else if(distance >=10 && distance <= 30){
      forward();      
    }else if(distance >30 || (distance >= 5 && distance <10)){
      stop();
    }else if(distance <5){
      back();
    }              
}
