Self drifting main project code

By -
Main Project Code

Flow chart for Main project

Code:

#include <Wire.h>
#include <Servo.h>
#include <TimerThree.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

//Speed Sensors
#define speed_r 2
#define speed_l 3
#define length_4pulse 8 //80 millimeter 8 centimeter
#define speed_mins 0
#define speed_maxs 100

//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

//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
#define ultrasonic_mins 6

//CMPS03 Compass Module
#define COMPASS 0x60 //defines address of compass

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

//Accelerometer
#define X_OUT A8
#define Y_OUT A9
#define Z_OUT A10
#define Voffset 337 // 3.3/2 = 1.65v -> 1.65*1023/5 = 337.59
#define sensitivity 63 // 0.308 -> 0.308*1023/5 = 63.0168

//PID Control
#define p_value 12
#define i_value 0.005 //0.5
#define d_value 8
#define p_value_speed 0
#define i_value_speed 0.1 //0.5
#define d_value_speed 0

Servo myservo;
int angle = center;
volatile int speed_pulse = 0;
volatile float current_speed = 0;
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;
float error_speed = 0;
float error_p_speed = 0;
float error_i_speed = 0;
float error_d_speed = 0;
float error_result_speed = 0;
float error_old_i_speed = 0;
float error_old_d_speed = 0;
float front_dist = 0;
float rear_dist = 0;
int front_left_dist = 0;
int front_right_dist = 0;
int back_left_dist = 0;
int back_right_dist = 0;
int front_sensor_dac = 0;
int back_sensor_dac = 0;
int speed_limit = 40; //cm/s
int default_speed = 0;
int speed_percent = default_speed;
uint16_t compass_value = 0;
float x_value = 0;
float y_value = 0;
float z_value = 0;
int send_to_rpi = 0;
volatile unsigned long time_start;
volatile unsigned long time_stop;
volatile int start_watch = 0;
int collision_flag = 0;

void setup() {
  //Serial Initial
  Serial.begin(115200);
  Serial1.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);

  //Accelerometer Initial
  pinMode(X_OUT, INPUT);
  pinMode(Y_OUT, INPUT);
  pinMode(Z_OUT, INPUT);

  //Speed Detect
  pinMode(speed_r, INPUT);
  pinMode(speed_l, INPUT);
  attachInterrupt(digitalPinToInterrupt(speed_r), speed_count, RISING);
  //Timer3.initialize(300000); // initialize timer3, and set a 1/10 second period
  //Timer3.attachInterrupt(speed_calculate);
}

void loop() {
  // Read Infrared
  front_dist = read_analog_sensors(Front_sensor);//analogRead(Front_sensor);
  rear_dist = read_analog_sensors(Rear_sensor);//analogRead(Rear_sensor);
  
  // Read Accelerometer
  x_value = float(analogRead(X_OUT) - Voffset) / sensitivity;
  y_value = float(analogRead(Y_OUT) - Voffset) / sensitivity;
  z_value = float(analogRead(Z_OUT) - Voffset) / sensitivity;
  
  // Read Ultrasonic
  int front_left_dist_new = read_distance_ultrasonic(Front_Left);
  int front_right_dist_new = read_distance_ultrasonic(Front_Right);
  int back_left_dist_new = read_distance_ultrasonic(Back_Left);
  int back_right_dist_new = read_distance_ultrasonic(Back_Right);
  
  // Read Compass
  compass_value = read_compass();

  // Ultrasonic sensor error prevent 
  if (front_left_dist_new < ultrasonic_mins){
    front_left_dist = front_left_dist;
  }else{
    front_left_dist = front_left_dist_new;
  }
  if (front_right_dist_new < ultrasonic_mins){
    front_right_dist = front_right_dist;
  }else{
    front_right_dist = front_right_dist_new;
  }
  if (back_left_dist_new < ultrasonic_mins){
    back_left_dist = back_left_dist;
  }else{
    back_left_dist = back_left_dist_new;
  }
  if (back_right_dist_new < ultrasonic_mins){
    back_right_dist = back_right_dist;
  }else{
    back_right_dist = back_right_dist_new;
  }

  /////////////////////////////////////////////// Print Debug Start
  //Serial.print("Front: ");
  //Serial.print(front_dist);
  //Serial.print(" Rear: ");
  //Serial.print(rear_dist);
  //Serial.print("Front Left: ");
  //Serial.print(front_left_dist);
  //Serial.print(" Front Right: ");
  //Serial.print(front_right_dist);
  //Serial.print(" Left Side: ");
  //Serial.print(back_left_dist);
  //Serial.print(" Right Side: ");
  //Serial.print(back_right_dist);
  /////////////////////////////////////////////// Print Debug End

  // Prevent I part of controller still integrated after stop to prevent collision
  if(collision_flag == 0){

    // 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;
  
    /////////////////////////////////////////////// Print Debug Start
    //Serial.println(error_d);
    //Serial.print(" Error: ");
    //Serial.print(error);
    /////////////////////////////////////////////// Print Debug End
    
    error_result = error_p + error_i + error_d;
    angle = (error_result * half_steer / 100) + center;
    
    // Safety check for Angle of steering
    if (angle < mins){
      angle = mins;
    }else if(angle > maxs){
      angle = maxs;
    }

    /////////////////////////////////////////////// Print Debug Start
    //Serial.print(" Angle: ");
    //Serial.print(angle);
    /////////////////////////////////////////////// Print Debug End
  
    // Calculate error for speed
    error_speed = 40 - current_speed; //speed_limit
    //speed_pulse = 0;

    Serial.println(error_speed);
  
    // Use PID Controller to get Drive percent
    error_old_i_speed += error_speed;
    error_p_speed = error_speed * p_value_speed;
    error_i_speed = (error_old_i_speed * 0.1) * i_value_speed;
    error_d_speed = (error_speed - error_old_d_speed) * d_value_speed;
    error_old_d_speed = error_old_d_speed + (error_speed - error_old_d_speed)*0.5;
    error_result_speed = error_p_speed + error_i_speed + error_d_speed;
    speed_percent = error_result_speed;// * 5/100);
    //speed_percent = 0;
    
    // Safety check for speed command
    if (speed_percent < speed_mins){
      speed_percent = speed_mins;
    }else if(speed_percent > speed_maxs){
      speed_percent = speed_maxs;
    }
  }

  // Safety check for front collision
  if(front_dist <= 13.0){ // ADC value 450 is xx cm
    speed_percent = 0;
    collision_flag = 1;
  }else{
    collision_flag = 0;
  }

  /////////////////////////////////////////////// Print Debug Start
  /*Serial.print(" Error PID: ");
  Serial.print(error_result_speed);*/
  Serial.print(" Speed CMD: ");
  Serial.print(speed_percent);
  Serial.print(" Speed Car: ");
  Serial.println(current_speed);
  Serial.print("G x: ");
  Serial.print(x_value);
  Serial.print("G y: ");
  Serial.print(y_value);
  Serial.print("G z: ");
  Serial.println(z_value);
  /////////////////////////////////////////////// Print Debug End
  
  // Sent command to steering and speeding
  myservo.write(angle);
  analogWrite(PWM_PIN, (speed_percent * 255/100));

  // Received speed limit from RPi
  read_speed_limit();

  // Sent to RPi
  sent_to_rpi();
}

float read_analog_sensors(int Channel){
  int analog_value = analogRead(Channel);
  float result = 0.0;
  if(analog_value >= 310){
    result = (float(analog_value) * -1 / 22) + (375 / 11);
  }else if(analog_value >= 250 && analog_value <= 310){
    result = (float(analog_value) * -1 / 6) + (215 / 3);
  }else if(analog_value >= 210 && analog_value <= 250){
    result = (float(analog_value) * -1 / 2) + 155;
  }else if(analog_value <= 210){
    result = (float(analog_value) * -1) + 260;
  }
  return result;
}

void read_speed_limit(){
  //Serial.println(Serial1.readBytes());
  if (Serial1.available() > 0)
  {
    speed_limit = Serial1.readStringUntil(',').toInt();
    send_to_rpi = 1;
  }
}

void sent_to_rpi(){
  if(send_to_rpi == 1){
    Serial1.print(front_left_dist);
    Serial1.print(",");
    Serial1.print(front_right_dist);
    Serial1.print(",");
    Serial1.print(back_left_dist);
    Serial1.print(",");
    Serial1.print(back_right_dist);
    Serial1.print(",");
    Serial1.print(front_dist);
    Serial1.print(",");
    Serial1.print(rear_dist);
    Serial1.print(",");
    Serial1.print(current_speed);
    Serial1.print(",");
    Serial1.print(angle);
    Serial1.print(",");
    Serial1.print(compass_value);
    Serial1.print(",");
    Serial1.print(x_value);
    Serial1.print(",");
    Serial1.print(y_value);
    Serial1.print(",");
    Serial1.print(z_value);
    Serial1.print(",");
    Serial1.println(speed_limit);

    send_to_rpi = 0;
  }  
}

int read_compass(){
  uint16_t highByte1;
  uint16_t lowByte1;
  
  Wire.beginTransmission(COMPASS);
  Wire.write(2);
  Wire.endTransmission();

  Wire.requestFrom(COMPASS, 2);
  while(Wire.available() < 2);
  highByte1 = Wire.read();
  lowByte1 = Wire.read();
   
  return ((highByte1<<8 -="" 0="" 1="" 2="" 8="" addr="" delay="" distance="" ead_cen="" ead_pointer="" if="" int="" lowbyte1="" measure="" read_distance_ultrasonic="" reading="" return="" sensors="" speed_count="" start_watch="=" time_start="" time_stop="" to="" void="" wait="" wire.available="" wire.begintransmission="" wire.endtransmission="" wire.read="" wire.requestfrom="" wire.write=""> 8000){
      current_speed = ((length_4pulse / 4) * 1000000 / (time_stop));
      time_start = micros();
    }
  }
}

void speed_calculate(){
  //Serial.println(speed_pulse);
  //current_speed = (speed_pulse * length_4pulse / 4) * 10; // multiply 10 by time scaling
  //speed_pulse = 0;
  /*if(start_watch == 1){
    current_speed = 0;
    start_watch = 0;
  }*/
}

Different between driving around the track and drifting in the track is on PID value for steering back wheel, So for driving around the track is use wheel with rubber tire and PID value is 12, 0.005, 8 (P, I, D) but for drifting we remove a tire from the wheel and set PID to 5.5, 0, 0 (P, I, D).

Comments

Post a Comment

Popular posts from this blog

Sharp 2Y0A21 F 03

By -
Image
Sharp 2Y0A21 F 03 Distance Measuring Sensor Unit Sharp 2Y0A21 F 03 This sensor can measure a distance from 10 to 80 cm with analog output. The range of output voltage is defined as a graph below. Voltage output from 0.4 to 2.25 V (80 to 10 cm) Sample Code: void setup() { // put your setup code here, to run once: pinMode(A6, INPUT); pinMode(A7, INPUT); Serial.begin(115200); } void loop() { // put your main code here, to run repeatedly: Serial.println(analogRead(A6)); delay(1); Serial.println(analogRead(A7)); delay(1000); } Because the output of this sensor in analog output, So we can read data easily by using ADC. (analogRead() function in arduino) In this code, we read from 2 sharp sensors and print output to a serial port. Reference:  Datasheet
Keep reading

CMPS03 Compass Module

By -
Image
CMPS03 Compass Module CMPS03 connector description This module used I2C as a protocol to communicate with MCU and use 5V for power supply. Data from I2C is compass Bearing as a word, i.e. 0-3599 for a full circle, representing 0-359.9 degrees. Sample Code: /* CMPS03 with arduino I2C example This will display a value of 0 - 359 for a full rotation of the compass. The SDA line is on analog pin 4 of the arduino and is connected to pin 3 of the CMPS03. The SCL line is on analog pin 5 of the arduino and is conected to pin 2 of the CMPS03. Both SDA and SCL are also connected to the +5v via a couple of 1k8 resistors. A switch to callibrate the CMPS03 can be connected between pin 6 of the CMPS03 and the ground. */ #include <wire .h> #define ADDRESS 0x60 //defines address of compass void setup(){ Wire.begin(); //conects I2C Serial.begin(9600); } void loop(){ byte highByte; byte lowByte; Wire.beginTransmission(ADDRESS); //starts communication
Keep reading

Change I2C Address SRF10

By -
Change I2C Address of SRF10 Address Long Flash Short flashes Decimal Hex 224 E0 1 0 226 E2 1 1 228 E4 1 2 230 E6 1 3 232 E8 1 4 234 EA 1 5 236 EC 1 6 238 EE 1 7 240 F0 1 8 242 F2 1 9 244 F4 1 10 246 F6 1 11 248 F8 1 12 250 FA 1 13 252 FC 1 14 254 FE 1 15 Address Table For SRF10 can set ID for I2C up to 16 address as a table upper, So this post is about how to set ID for every SRF10. First, you need to have an only one SRF10 connect to I2C bus then use to the following code. Change Address Code: #include <Wire.h> #define ADDRESS byte(0x72) //defines address #define NEW_ADDRESS byte(0xEC) //defines address #define CMD byte(0x00) void setup() { // put your setup code here, to run once: Wire.begin(); //conects I2C Serial.begin(115200); } void loop() { // put your main code here, to run repeatedly: delay(5000); Serial.println("Begin"); Wire.beginTransmission(ADDRESS); Wire.write(CMD); Wire.write(byte(0xA0)); Wire.end
Keep reading