Hot Lapse

Hot Lapse Code

Flow chart for Hot Lapse Challenge

Code:

#include <Wire.h>
#include <Servo.h>

//For Steering
#define mins 45 //degree
#define maxs 120 //degree
#define center 83 //degree
#define half_steer 37
//Maximum speed for turn steering 10ms

//For Motor Drive
#define InA_PIN 5 //Yellow
#define InB_PIN 6 //Blue
#define PWM_PIN 4 //Green
//INA 1 INB 0 clockwise FORWARD
//INA 0 INB 1 counterclockwise REVERSE

//For Ultrasonic
//ADDRESS
#define Front_Left 112//0x70//0xE0
#define Front_Right 114//0x72//0xE4
#define Back_Left 116//0x74//0xE8
#define Back_Right 118//0x76//0xEC
#define CMD 0x00
#define RANGE_CMD 0x02
#define RANGE_SET 0x18
#define Read_inch 0x50
#define Read_cen 0x51
#define Read_micro_sec 0x52
#define Read_pointer 0x02

//Sharp Infrared sensors
#define Front_sensor A6
#define Rear_sensor A7

//PID Control
#define p_value 6
#define i_value 0 //0.5
#define d_value 0 //4

Servo myservo;
int angle = center;
float error = 0;
float error_p = 0;
float error_i = 0;
float error_d = 0;
float error_result = 0;
float error_old_i = 0;
float error_old_d = 0;
int front_dist = 0;
int front_left_dist = 0;
int front_right_dist = 0;
int default_speed = 35; //45
int speed_percent = default_speed;

void setup() {
  // put your setup code here, to run once:
  //Serial Initial
  Serial.begin(115200);
  //I2C Initial
  Wire.begin();

  //Set Range of Ultrasonic
  Wire.beginTransmission(Front_Left);
  Wire.write(RANGE_CMD);
  Wire.write(RANGE_SET);
  Wire.endTransmission();
  delay(10);
  Wire.beginTransmission(Front_Right);
  Wire.write(RANGE_CMD);
  Wire.write(RANGE_SET);
  Wire.endTransmission();
  delay(10);
  Wire.beginTransmission(Back_Left);
  Wire.write(RANGE_CMD);
  Wire.write(RANGE_SET);
  Wire.endTransmission();
  delay(10);
  Wire.beginTransmission(Back_Right);
  Wire.write(RANGE_CMD);
  Wire.write(RANGE_SET);
  Wire.endTransmission();
  
  //Servo Initial
  myservo.attach(9);

  //Motor Drive Initial
  pinMode(InA_PIN, OUTPUT);
  pinMode(InB_PIN, OUTPUT);
  pinMode(PWM_PIN, OUTPUT);
  analogWrite(PWM_PIN, 0);
  digitalWrite(InA_PIN, HIGH);
  digitalWrite(InB_PIN, LOW);
  //Default Speed
  digitalWrite(InA_PIN, HIGH);
  digitalWrite(InB_PIN, LOW);
  analogWrite(PWM_PIN, (speed_percent * 255/100));//25(27 * 255/100);

  //Sharp Infrared sensors Initial
  pinMode(Front_sensor, INPUT);
  pinMode(Rear_sensor, INPUT);
}

void loop() {
  // Read Infrared
  front_dist = analogRead(Front_sensor);

  // Read Ultrasonic
  int front_left_dist_new = read_distance_ultrasonic(Front_Left);
  int front_right_dist_new = read_distance_ultrasonic(Front_Right);

  // Ultrasonic read error prevention
  if (front_left_dist_new < 6){
    front_left_dist = front_left_dist;
  }else{
    front_left_dist = front_left_dist_new;
  }
  if (front_right_dist_new < 6){
    front_right_dist = front_right_dist;
  }else{
    front_right_dist = front_right_dist_new;
  }
  
  // Calculate error for steering
  error = front_right_dist - front_left_dist;

  // Use PID Controller to get Angle of steering
  error_old_i += error;
  error_p = error * p_value;
  error_i = (error_old_i * 0.1) * i_value;
  error_d = (error - error_old_d) * d_value;
  error_old_d = error_old_d + (error - error_old_d)*0.5;
  error_result = error_p + error_i + error_d;
  angle = (error_result * half_steer / 100) + 83;
  
  // Safety check for Angle of steering
  if (angle < mins){
    angle = mins;
  }else if(angle > maxs){
    angle = maxs;
  }
  
  // Safety check for front collision
  if(front_dist > 450){
    speed_percent = 0;
  }else{
    speed_percent = default_speed;
    // Speed up car to make it faster
    if(abs(error_result) < 20){
      speed_percent = default_speed + 5;
    }
  }
  
  // Send command to steering and speeding
  myservo.write(angle);
  analogWrite(PWM_PIN, (speed_percent * 255/100));//25(27 * 255/100);
}

int read_distance_ultrasonic(int addr){
  int reading = 0;
  Wire.beginTransmission(addr);
  Wire.write(CMD);
  Wire.write(Read_cen);
  Wire.endTransmission();
  delay(5);
  Wire.beginTransmission(addr);
  Wire.write(Read_pointer);
  Wire.endTransmission();
  Wire.requestFrom(addr, 2);
  if (2 <= Wire.available()) {
    reading = Wire.read();
    reading = reading << 8;
    reading |= Wire.read();
  }
  return reading;
}

This code do according to flow chart that we design for control part is used only P and the response is work well. So we separate this code to and another file because if we want to make a fastest lap time then we have to cut some part of program that’s not necessary.