I want to make a autonomous robotic car(it's just basic now) using arduino by implementing PID on distance as a input by ultrasonic sensor. I am just using one ultrasonic sensor which will control speed of my car in forward direction. I want it so that greater the distance, greater the PWM supplied to motor. I have written code but it is not responding smoothly.
I have set the set point as 15cm and I want PWM to be 0 when distance is also 15cm and increase accordingly with increase in distance. I have set the range of car to 400cm.
I checked it and PWM was 255 for distance greater than 50cm and for less than it was 0 means it is responding smoothly.
Do identify bugs and suggest if any improvement is possible.
#include <Ultrasonic.h>
#define mb1 8
#define mb2 9
#define mf1 10
#define mf2 11
#define en1 5
#define en2 6
#define trigPin 12
#define echoPin 13
int Output;
int Input;
long duration;
int Setpoint = 15;
int Error[10];
long Accumulator=0;
int PTerm;
int ITerm;
int DTerm;
byte Divider;
Ultrasonic ultrasonic(trigPin, echoPin);
void setup() {
Serial.begin(9600);
pinMode(mb1, OUTPUT);
pinMode(mb2, OUTPUT);
pinMode(mf1, OUTPUT);
pinMode(mf2, OUTPUT);
pinMode(en1, OUTPUT);
pinMode(en2, OUTPUT);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}
void forward(byte Output)
{
analogWrite(en1,Output);
analogWrite(en2,0);
digitalWrite(mb1, HIGH);
digitalWrite(mb2, LOW);
digitalWrite(mf1, LOW);
digitalWrite(mf2, LOW);
}
void GetError()
{
byte i = 0;
for(i=9;i>0;i--)
Error[i] = Error[i-1];
// load new error into top array spot
Error[0] = Setpoint-Input;
}
void CalculatePID()
{
// Set constants here
PTerm = 500;
ITerm = 10;
DTerm = 4;
Divider = 2;
Output = abs(Error[0])*PTerm; // start with proportional gain
Accumulator += abs(Error[0]); // accumulator is sum of errors
Output += ITerm*Accumulator; // add integral gain and error accumulation
Output += DTerm*(abs(Error[0])-abs(Error[9])); // differential gain comes next
Output = Output>>Divider;
if (Accumulator >1000) Accumulator = 1000;
if (Accumulator <-1000) Accumulator = -1000;
if (Output <255 & Output >0){
Output = Output;
}
else if (Output <= 0) {
Output = 0;
}
else if (Output >= 255 ) {
Output = 255;
}
}
void loop() // run over and over
{
Input = ultrasonic.distanceRead();
if (Input >400) {
}
else {
GetError(); // Get position error
CalculatePID(); // Calculate the PID output from the error
forward(Output);
Serial.print(Input);
Serial.print("cm\t");
Serial.print(Output);
Serial.print("PWM");
Serial.println("");
delay(1000);
}
}