This is how the buggy looks like
Software
I made the C++ library which you can use to assign pins of the Arduino to outputs of the sensors and motors.
buggy.h
buggy.c
There are three important functions there:
//sends a 10ms signal to Trig pin of ultrasonic sensors then waits for 8 signals from the echo pin of sensor and returns the distance
long Buggy::CountDistance(int pin_trigger, int pin_echo)
{
digitalWrite(pin_trigger, HIGH);
delayMicroseconds(10);
digitalWrite(pin_trigger, LOW);
//sending 8 pulses
long distance=pulseIn(pin_echo, HIGH)/58;
return distance;
}
//measures the distance from three sensors
void Buggy::AssignDistance(void)
{
distance_left=CountDistance(sonar_pin_trigg_left, sonar_pin_echo_left);
distance_center=CountDistance(sonar_pin_trigg_center, sonar_pin_echo_center);
distance_right=CountDistance(sonar_pin_trigg_right, sonar_pin_echo_right);
}
//returns the distance from sensors
//for n=1 -> left sensor, n=2 ->central sensor, n=3->right sensor
long Buggy::GetDistance(int n)
{
switch(n)
{
case 1:
return distance_left;
break;
case 2:
return distance_center;
break;
case 3:
return distance_right;
break;
default:
break;
}
}
The Arduino code is included in the following source file:
Arduino code
Main function:
void loop()
{
//-------------------READING SENSORS---------------------
ReadSensors();
L_distance=robot->GetDistance(1);//distance on left sensor
//robot->GetDistance(2));//distance on center sensor
R_distance=robot->GetDistance(3);//distance on right sensor
//--------------------------------------------------------
if(L_distance>50)
L_distance=50;
if(R_distance>50)
R_distance=50;
L=50-L_distance;
R=50-R_distance;
PID=PID_value(L,R);
Motor_control(PID);
}//end of loop()
It reads the distance between the buggy and anything around from all three sensors.
This part of code 'saturates' the distance so that it is only in range of 0-50 cm
//-----------------------
if(L_distance>50)
L_distance=50;
if(R_distance>50)
R_distance=50;
L=50-L_distance;
R=50-R_distance;
//-----------------------
After that PID controller sends the value to the motors according to the distance read from sensors.
Motor control(double) function is the motor controlling function and it sends values to both left and right motors.
Lets take a look at the function which operates right motor:
//-------------------------------------------------------
void Right_Motor(double PID)
{
if(av_PWM+PID>max_PWM)
analogWrite(right_forward, max_PWM);
else if(av_PWM+PID<0)
analogWrite(right_forward, 0);
else
analogWrite(right_forward, av_PWM+PID);
}
//--------------------------------------------------------
First of all PID values is calculated in 'double PID_value(int L, int R)'. When Right_Motor(double) receives the value it checks if PWM value is in range of 0-255, which is minimum-maximum range for Arduino's analogWrite(double) function. If it exceed it then Right_motor function saturates it either to 0 or 255. Otherwise it uses PID value to move the motor.
Here is the youtube link with the buggy avoiding obstacles. It only uses proportional part at the moment. I will make another video with the upgraded version of the controller in the future.
P controller
Later on I will update it with the circuit diagram, hardware and upgraded version of controller...
