Code

// This is the sketch to drive a car using in an autonomous mode using sensors to follow lanes and detect obstacles.
// And also to drive it manually using control signals from an app over Bluetooth. The car can switch between modes (either one at a time).
// The car uses battery operated motors controlled by an L293D shield for the Arduino UNO.
// The left and right turns are managed by varying the speeds of the wheels or by changing their direction of rotation.
// For example, rotating right wheels backwards and left wheels forward will make the car turn right.
 
// The sketch reads data from sensors - two IR sensors for line detection and an ultrasound sensor for obstacle and distance detection.
// The ultrasound sensor is mounted on a servo to check for obstacles straight ahead, on the left, and on the right. 
 
 
/* LIBRARIES */
// Motor driver
#include <AFMotor.h>   //<https://github.com/adafruit/Adafruit-Motor-Shield-library>
// Servo
#include <Servo.h>     //<https://www.arduino.cc/reference/en/libraries/servo/>
 
// PIN DEFINITIONS
// Note: Analog pins can be used as digital pins but digital pins CANNOT be used as analog pins.
// Analog pins go A1, A2 and so on, while digital pins go 1, 2, and so on, equivalent to D1, D2, and so on.
// byte is an alias for uint8_t, either can be used.
const uint8_t IR_SENSOR_LEFT = A3;   //left side IR sensor 
const uint8_t IR_SENSOR_RIGHT = A2;   //right side IR sensor 
const uint8_t ECHO_PIN = A1;  //echo pin of determineDistanceFromUltrasound sensor
const uint8_t TRIGGER_PIN = A0;  //trigger pin of determineDistanceFromUltrasound sensor
// The L293D shield drives the servos and the motors.
const uint8_t SERVO_PIN = 9;   // The ultrasound sensor servo is connected to the motor driver shield pin which is connected to pin 9 of the Arduino UNO.
 
// CONTROL SIGNAL DEFINITIONS
// The car takes multiple single character inputs from an app over Classic Bluetooth in the manual control mode.
// The characters and actions are defined as constants below:
// const is preferred to #define
// But if you do use #define note that there is no semicolon at the end of the line.
const char signal_mode_rc = 'C';
const char signal_mode_auto = 'A';
const char signal_mode_neutral = 'N';
 
const char signal_move_forward = 'F';
const char signal_move_backward = 'B';
const char signal_turn_left = 'L'; // Capital L, this keeps on turning in place.
const char signal_turn_right = 'R'; // Capital R, this keeps on turning in place.
const char signal_nudge_left = 'l'; // Small l, a nudge is better for a regular path.
const char signal_nudge_right = 'r'; // Small r, a nudge is better for a regular path.
 
const char signal_stop = '0';
const char signal_low_speed = '1';
const char signal_medium_speed = '2';
const char signal_high_speed = '3';
 
// OTHER CONSTANTS
// Speed values can go from 0 to 255.
// The speed values have been determined after testing on a smooth floor and with constant weight and for optimum functioning. 
// The speed will vary accorinding to floor and weight on the car.
// The turning radius can be changed by changing the speed of the respective wheels in the left and right functions.
const uint8_t low_speed = 75;
const uint8_t medium_speed = 95;
const uint8_t high_speed = 115;
const uint8_t obstacle_threshold = 30; // In centimeters.
 
// This is to calculate the distance of an obstacle using the ultrasound sensor.
const float speed_of_sound = 0.034;
 
Servo myservo;  // Create a servo object to control a servo; twelve servo objects can be created on most boards.
 
// Initialising variables for programming logic and data.
bool mode_set = false;
int mode; // Default to control mode.
int M = 1 ; // control variable - WHAT IS THIS FOR?
int bt_data; // Data received over Bluetooth. Convert to char().
int ir_sensor_left_value;  // Left IR sensor value
int ir_sensor_right_value;  // Right IR sensor value 
 
 
// Keep speeds the same for RC or Auto mode.
int speed = low_speed; // default to low_speed on start.
 
// int speeed = 255; //speed varaiable for rc_mode
// int speed = 0; 
// For ultrasound and distance measurement, moved to the function as they are local variables.
// long duration; 
// float distanceCm;
// All distances are in centimeters.
float distance_forward, distance_left, distance_right;
// int distance_left, distance_right;
 
// Create motor objects.
// 1, 2, 3, 4 are the LD293 shield pins to which the motors are connected. 
AF_DCMotor motor1(1);
AF_DCMotor motor2(2); 
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
 
// Setup code 
void setup() {
  Serial.begin(9600); // Serial comm at 9600 bps
 
  // Note that Serial.printlm must enclose text in "", single '' do not work.
  Serial.println("Autonomous And Bluetooth Controlled Car setup...");
 
  myservo.attach(SERVO_PIN); // Initialise servos
 
  pinMode(IR_SENSOR_LEFT, INPUT);    
  pinMode(IR_SENSOR_RIGHT, INPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIGGER_PIN, INPUT);
}
 
/***** FUNCTIONS HERE *****/
 
// Function to calculate distance of objects using sensor, returns distance in cm.
float determineDistanceFromUltrasound(){
  long duration = 0;
  float distance_cm = 0;
 
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
 
  duration = pulseIn(ECHO_PIN, HIGH); 
  distance_cm = duration * speed_of_sound/2;
  return distance_cm;
}
 
/* Functions to move the car in autonomous mode */
 
// Break up the motor speed and direction in two generic functions.
// Takes values from 0 to 255, one for each of 4 motors for now.
void setMotorSpeeds(uint8_t speed1, uint8_t speed2, uint8_t speed3, uint8_t speed4){
  motor1.setSpeed(speed1);
  motor2.setSpeed(speed2);
  motor3.setSpeed(speed3);
  motor4.setSpeed(speed4);
}
 
// Takes values FORWARD or BACKWARD, one for each of 4 motors for now.
void setMotorDirections(uint8_t direction1, uint8_t direction2, uint8_t direction3, uint8_t direction4){
  motor1.run(direction1);
  motor2.run(direction2);
  motor3.run(direction3);
  motor4.run(direction4);
}
 
// Move forward at the selected speed.
void moveForward(){
  setMotorSpeeds(speed, speed, speed, speed);
  setMotorDirections(FORWARD, FORWARD, FORWARD, FORWARD);
  // DEBUG
  Serial.println("moving forward");
}
 
// Move backward at the selected speed.
void moveBackward(){
  setMotorSpeeds(speed, speed, speed, speed);
  setMotorDirections(BACKWARD, BACKWARD, BACKWARD, BACKWARD);
  // DEBUG
  Serial.println("moving backward");
}
 
// Nudge left
void nudgeLeft(){
  setMotorSpeeds(20, 170, 170, 20);
  setMotorDirections(BACKWARD, FORWARD, FORWARD, BACKWARD);
  // DEBUG
  Serial.println("nudging left");
}
// Nudge right
void nudgeRight(){
  setMotorSpeeds(170, 20, 20, 170);
  setMotorDirections(FORWARD, BACKWARD, BACKWARD, FORWARD);
  // DEBUG
  Serial.println("nudging right");
}
// to turn left 
void turnLeft(){
  setMotorSpeeds(0, 200, 200, 0);
  setMotorDirections(FORWARD, FORWARD, FORWARD, FORWARD);
  // DEBUG
  Serial.println("turning left");
  
}
// to turn right
void turnRight(){
  setMotorSpeeds(200, 0, 0, 200);
  setMotorDirections(FORWARD, FORWARD, FORWARD, FORWARD);
  // DEBUG
  Serial.println("turning right");
  
}
// To rotate 180 degrees in place.
void rotate(){
  setMotorSpeeds(255, 255, 255, 255);
  setMotorDirections(FORWARD, BACKWARD, BACKWARD, FORWARD);
  // DEBUG
  Serial.println("rotating in place");
}
 
// Function to stop all wheels and motors (commmon for both modes)
void stopAll(){
  setMotorSpeeds(0, 0, 0, 0);
  setMotorDirections(RELEASE, RELEASE, RELEASE, RELEASE);
 
  // DEBUG
  Serial.println("stopping");
}
 
// --------------- functions to maneuver the car in RC mode --------------- //
// WHY ARE THE TWO DIFFERENT? 
// void forward(){
 
//   motor.setSpeed(speed);
//   motor3.setSpeed(speed);
//   motor2.setSpeed(speed);
//   motor1.setSpeed(speed);
 
//   motor.run(FORWARD);
//   motor3.run(FORWARD);
//   motor2.run(FORWARD);
//   motor1.run(FORWARD);
 
//   Serial.println("moving forward");
// }
// void backward(){
//   motor.setSpeed(speed);
//   motor3.setSpeed(speed);
//   motor2.setSpeed(speed);
//   motor1.setSpeed(speed);
 
//   motor.run(BACKWARD);
//   motor3.run(BACKWARD);
//   motor2.run(BACKWARD);
//   motor1.run(BACKWARD);
 
//   Serial.println("moving backward");
 
// }
// Function to turn left on the spot - NOT USED
void hardLeft(){
  // // Set motor speeds
  // motor.setSpeed(speed);
  // motor3.setSpeed(speed);
  // motor2.setSpeed(speed);
  // motor1.setSpeed(speed);
  // // Set motor directions
  // motor.run(BACKWARD);
  // motor3.run(FORWARD);
  // motor2.run(FORWARD);
  // motor1.run(BACKWARD);
 
  setMotorSpeeds(speed, speed, speed, speed);
  setMotorDirections(BACKWARD, FORWARD, FORWARD, BACKWARD);
 
  // DEBUG
  Serial.println("hard left");
}
void left(){
  setMotorSpeeds(0, speed, speed, 0);
  setMotorDirections(RELEASE, FORWARD, FORWARD, RELEASE);
  // DEBUG
  Serial.println("moving left");
}
 
// Function to turn right on the spot - NOT USED
void hardRight(){
  // Set motor speeds
  motor4.setSpeed(speed);
  motor3.setSpeed(speed);
  motor2.setSpeed(speed);
  motor1.setSpeed(speed);
  // Set motor directions
  motor3.run(BACKWARD);
  motor4.run(FORWARD);
  motor1.run(FORWARD);
  motor2.run(BACKWARD);
}
 
void right(){
  setMotorSpeeds(speed, 0, 0, speed);
  setMotorDirections(FORWARD, RELEASE, RELEASE, FORWARD);
  // DEBUG  
  Serial.println("moving right");
}
 
// Function to compare distance between left and right and taking turn in the direction which has object at a greater distance.
void compareDistanceAndMove(){
  stopAll();
  Serial.println("comparing left and right distances");
 
  if (distance_left < obstacle_threshold && distance_right < obstacle_threshold ){  // if there is an obstacle on both sides then go back and rotate
    moveBackward();
    delay(1000);
    rotate();
    delay(1000);
    stopAll();
  }
  else if(distance_left > distance_right){ // if obstacle is on the right side then turn left
    moveForward();
    delay(400);
    turnLeft();
    delay(1000);
    stopAll();
    delay(100);
  }
  else{  // if obstacle is on the left side then turn right
    moveForward();
    delay(400);
    turnRight();
    delay(1000);
    stopAll();
    delay(100);
  }
}
 
// This function is to detect distance of objects on left and right side of the car. 
// The sensor is mounted on top of the servo and the servo rotated to point the ultrasound sensor in the desired direction.
void checkSideObstacles(){
    stopAll();
    delay(100);
    myservo.write(30);  // turn sensor to the right.
    delay(500);
    distance_right = determineDistanceFromUltrasound();
    Serial.print("distance right: ");
    Serial.println(distance_right);
    delay(100);
    myservo.write(150); // turn sensor to the left.
    delay(500);
    distance_left = determineDistanceFromUltrasound();
    Serial.print("distance left: ");
    Serial.println(distance_left);
    delay(100);
    myservo.write(95);
    delay(500);
    // Decide which side has no obstacle and turn and move forward in that direction.
    compareDistanceAndMove();
}
 
// This is the function for autonomous mode. 
// Once the car is in this mode it will start moving forward.
// If black colored lane lines are set up the car will stay within those lines.
// The IR sensors on both front sides of the car detect the black line when the car reaches the edge and the car nudges itself back inside. 
// The car will stop if it detects an object with the ultrasound sensor in front of it at less than 30cm. 
// The servo will be trigerred to move the ultrasound sensor right and left to check for obstacles on either side.
// The car will turn to the side which there are no obstacles.
// If both sides have obstacles the car will rotate in place and go back where it came from.
void startAutonomousMode() {
  // DEBUG
  // Serial.println("autonomous mode");
 
  // Before starting, check if there is an obstacle.
  distance_forward = determineDistanceFromUltrasound();  //to check distance of object in front
  // DEBUG
  Serial.print("distance forward: " );
  Serial.println(distance_forward);
  
  if(distance_forward <= obstacle_threshold){
    Serial.print("obstacle detected");
    // Send indicator data to app when transmit circuit is ready.
    stopAll();
    delay(3000);  // wait for 3 seconds to measure again, in case the obstacle has moved.
    distance_forward = determineDistanceFromUltrasound();
    if (distance_forward <= obstacle_threshold){  //if it detects an object at a distance less than the threshold again it will call the check side function to turn accordingly
      checkSideObstacles();
    }
  }
 
  //Logic to keep inside lanes.
  ir_sensor_left_value = digitalRead(IR_SENSOR_LEFT);  //left IR value
  ir_sensor_right_value = digitalRead(IR_SENSOR_RIGHT); // right IR value
 
  Serial.print("Left IR : " );
  Serial.println(ir_sensor_left_value);
  Serial.print("Right IR : " );
  Serial.println(ir_sensor_right_value);
 
  // 
  if (ir_sensor_left_value == 1 && ir_sensor_right_value == 1){  // if both values are 0 then it means neither sensor is picking up black line , means the car is inside the lane and can move forward
    Serial.println("within lane moving forward");
    moveForward(); 
  }
  else if (ir_sensor_left_value == 0 && ir_sensor_right_value == 1){  //left sensor value is 1 and right sensor value is 0 , the car is on the left side border of the lane and should nudge towards the right
    stopAll(); //stopAll and delay is required in order to slow down the car so that it does not go off track and is able to take turn
    delay(50);
    Serial.println("nudging right");
    nudgeRight();  
  }
  else if (ir_sensor_left_value == 1 && ir_sensor_right_value == 0) { //left sensor value is 0 and right sensor value is 1 , the car is on the left side border of the lane and should nudge towards the left
    stopAll();
    delay(50);
    Serial.println("nudging left");
    nudgeLeft();
  }
  else if (ir_sensor_left_value == 0 && ir_sensor_right_value == 0) { // optional condition if both detect black line
    stopAll();
    Serial.println("unknown terrain, stopping.");
    delay(100);
  }
 
  // Check for control signals, the only ones to respond to are speed change, stop, and mode change.
  if(Serial.available() > 0){
 
    bt_data = Serial.read();
 
    switch(char(bt_data)){
      case signal_low_speed:
        speed = low_speed;
        setMotorSpeeds(speed, speed, speed, speed); // This should change the speed while maintaining the direction.
        Serial.println("low speed");
        break;
      case signal_medium_speed:
        speed = medium_speed;
        setMotorSpeeds(speed, speed, speed, speed); // This should change the speed while maintaining the direction.
        Serial.println("medium speed");
        break;
      case signal_high_speed:
        speed = high_speed;
        setMotorSpeeds(speed, speed, speed, speed); // This should change the speed while maintaining the direction.
        Serial.println("high speed");
        break;
      case signal_stop:
        stopAll();
        Serial.println("stopped");
        break;
      case signal_mode_neutral:
        Serial.println("exiting autonomous mode");
        stopAll();
        mode = signal_mode_neutral;
        mode_set = false;
        break;
    }
    
    
    // if (bt_data == signal_low_speed){ //low speed , the speed variable will be set to 75 so that the motors run at that speed
    //     speed = low_speed;
    //     Serial.println("low speed");  
    //   }
    // if (bt_data == signal_medium_speed){ //medium speed , the speed variable will be set to 95 so that the motors run at that speed
    //     speed  = medium_speed;
    //     Serial.println("medium speed"); 
    //   }
    // if (bt_data == signal_high_speed){//high speed , the speed variable will be set to 115 so that the motors run at that speed
    //     speed = high_speed;
    //     Serial.println("high speed");
    //   }  
 
      // these values of speed have been chosen since for any higher value the car will not be able to follow the lane and go off-track 
      // higher speed can be made possible by increasing the number of IR sensors and increasing lane line width.
 
    // if (bt_data == 'E'){  //to exit the autonomous mode
    //   Serial.println("exiting mode auto");
    //   stopAll();
    //   M = 1; //set control to 1
    // }
  }
}
 
// In the RC mode the car will move based on control signals received from an app over Classic Bluetooth.
void startRCMode(){
  // DEBUG
  // Serial.println("starting RC mode");
 
  // Listen continuously for control signals.
  if(Serial.available() > 0){
    bt_data = Serial.read(); // Will only read one char, that's all that is expected.
 
    // DEBUG
    Serial.println(bt_data);
      
    // Always stop everything between control signals. WHY?
    // stopAll(); 
 
    // Decode the control signal received and call the required function.
    // Use switch instead of if-else
    switch(char(bt_data)){
      case signal_move_forward:
        stopAll(); 
        moveForward();
        break;
      case signal_move_backward:
        stopAll();
        moveBackward();
        break;
      case signal_turn_right: 
        turnRight();
        break;  
      case signal_turn_left: 
        turnLeft();
        break;
      case signal_nudge_right: 
        nudgeRight();
        break;  
      case signal_nudge_left: 
        nudgeLeft();
        break;
      case signal_stop: 
        stopAll();
        break;
      case signal_low_speed: 
        speed = low_speed;
        break;
      case signal_medium_speed: 
        speed = medium_speed;
        break;
      case signal_high_speed: 
        speed = high_speed;
        break;
      case signal_mode_neutral:
        Serial.println("exiting RC mode");
        stopAll();
        mode = signal_mode_neutral;
        mode_set = false;
        break; 
    }
    // if (bt_data == 'F'){
    //   moveForward();
    // }
 
    // if (bt_data == 'B'){ //backward movement 
    //   backward();
    // }
 
    // if (bt_data == 'L'){ //left
    //   left();
    // }
 
    // if (bt_data == 'R'){ //right
    //   right();
    // }
    // if (bt_data == 'S'){ //stop
    //   stopAll();
    // }
    // if (bt_data == 'l'){ //low speed , the speed variable will be set to 100 so that the motors run at that speed
    //   speed = 100;
    //   Serial.println("low speed"); 
    // }
    // if (bt_data == 'm'){ //medium speed , the speed variable will be set to 180 so that the motors run at that speed
    //   speed  = 180;
    //   Serial.println("medium speed"); 
    // }
    // if (bt_data == 'h'){//high speed , the speed variable will be set to 255 so that the motors run at that speed
    //   speed = 255;
    //   Serial.println("high speed");
    // }
    // if (bt_data == 'E'){ //exit mode
    //   Serial.println("exiting mode rc");
    //   stopAll();
    //   M = 1;  // set control to 1
    //   Serial.println(M);
    // }
  }
}
 
/* The loop function */
// Listen for a control signal over BT in the loop function to switch to a mode only when mode is not set.
// Once in a mode listen there to switch to another mode or to neutral.
void loop(){
    // M = 1 (control is 1) , means that the car is not in any mode right now and is open for an input for modes
    // if( M == 1 && Serial.available() > 0 ){   // this if loop listens (only when control is 1) for the message coming via bluetooth to set the mode. it will only run when no mode is selected
    //   mode = Serial.read();
    //   Serial.println(mode);
    //   if (mode == '1' || mode == '2'){
    //     M = 0; // sets control to 0 indicating it has been shifted to some mode.
    //   }
    // }
 
    // while (mode == '1' && M == 0){     // if mode 1 has been selected the  its the rc mode
    //   startRCMode();
    // }
    // while (mode == '2' && M == 0) { // if mode 2 has been selected then its the autonomous mode
    //   startAutonomousMode();
    // }
    
    if(!mode_set){
      Serial.println("In neutral mode, send A or C to move to auto or remote control mode.");
      delay(15000);
    }
    
    if(!mode_set && Serial.available() > 0){
      bt_data = Serial.read(); // Will only read one char, that's all that is expected.
      // DEBUG
      Serial.println(char(bt_data)); // Char value
      // Serial.println(bt_data); // ASCII value
      if(char(bt_data) == signal_mode_rc || char(bt_data) == signal_mode_auto){
        mode = char(bt_data); // Set mode
        mode_set = true;
      }
    }
 
    // Mode is set so run the code for that mode where it will listen to and process control signals.
    // Both modes will have an exit option that will move to mode_set = false.
    if (mode == signal_mode_auto){
      startAutonomousMode();
    } else if (mode == signal_mode_rc){
      startRCMode();
    }
}