Hello
I am now trying for the first time to use the PID. The thing I am trying to build is a Maze solving and obstacle avoiding robot. The code that I have now doesn't have any kind of PID. And it looks like this:
int enaA = 3;
int in1a = 10;
int in2a = 11;
int enaB = 5;
int in1b = 9;
int in2b = 6;
void setup(){
pinMode(enaA, OUTPUT);
pinMode(enaB, OUTPUT);
pinMode(in1a, OUTPUT);
pinMode(in1b, OUTPUT);
pinMode(in2a, OUTPUT);
pinMode(in2b, OUTPUT);
Serial.begin(9800);
}
void loop(){
SharpIR(0,1,2);
}
void SharpIR(int a, int b, int c){
int cita[5];
int citb[10];
int citc[5];
int suma = 0;
int avga = 0;
int sumb = 0;
int avgb = 0;
int sumc = 0;
int avgc = 0;
if(a<=15){ // reads a 5 times and makes the average to get e more correct reading
for(int i = 0; i<5;i++){
cita[i] = analogRead(a);
delay(1);
}
for(int i = 0; i<5; i++){
suma += cita[i];
}
avga = suma/5;
}
if(b<=15){// reads b 5 times and makes the average to get e more correct reading
for(int i = 0; i<10;i++){
citb[i] = analogRead(b);
delay(5);
}
for(int i = 0; i<10; i++){
sumb += citb[i];
}
avgb = sumb/10;
}
if(c<=15){// reads c 5 times and makes the average to get e more correct reading
for(int i = 0; i<5;i++){
citc[i] = analogRead(c);
delay(1);
}
for(int i = 0; i<5; i++){
sumc += citc[i];
}
avgc = sumc/5;
}
Serial.print("A: ");
Serial.print(avga);
Serial.print("; B: ");
Serial.print(avgb);
Serial.print("; C:");
Serial.print(avgc);
Serial.println("");
if(avgc >=300 && avgb <=400){
if(avgc <=350){
analogWrite(enaB, 255);
analogWrite(enaA, 255);
analogWrite(in1a, 0);
analogWrite(in2a, 255);
analogWrite(in1b, 0);
analogWrite(in2b, 255);
Serial.println("inainte");// inainte = forward in romanian
}
}
if(avgc <=300 && avgc >=250 && avgb <=400){
analogWrite(enaB, 255);
analogWrite(enaA, 255);
analogWrite(in1a, 0);
analogWrite(in2a, 255);
analogWrite(in1b, 0);
analogWrite(in2b, 200);
Serial.println("dreapta");// dreapta = right in romanian
}
if(avgc <=250 && avgc >=200 && avgb <=400){
analogWrite(enaB, 255);
analogWrite(enaA, 255);
analogWrite(in1a, 0);
analogWrite(in2a, 255);
analogWrite(in1b, 0);
analogWrite(in2b, 150);
Serial.println("dreapta1");
}
if(avgc <=200 && avgc >=0 && avgb <=400){
analogWrite(enaB, 255);
analogWrite(enaA, 255);
analogWrite(in1a, 0);
analogWrite(in2a, 255);
analogWrite(in1b, 0);
analogWrite(in2b, 0);
Serial.println("dreapta2");
}
if(avgc >=350 && avgc <=400 && avgb <=400){
analogWrite(enaB, 255);
analogWrite(enaA, 255);
analogWrite(in1a, 0);
analogWrite(in2a, 200);
analogWrite(in1b, 0);
analogWrite(in2b, 255);
Serial.println("stanga");// stanga = left in romanian
}
if(avgc >=400 && avgc <=450 && avgb <=400){
analogWrite(enaB, 255);
analogWrite(enaA, 255);
analogWrite(in1a, 0);
analogWrite(in2a, 150);
analogWrite(in1b, 0);
analogWrite(in2b, 255);
Serial.println("stanga1");
}
if(avgc >=450 && avgc <=500 && avgb <=400){
analogWrite(enaB,255);
analogWrite(enaA, 255);
analogWrite(in1a, 0);
analogWrite(in2a, 0);
analogWrite(in1b, 0);
analogWrite(in2b, 255);
Serial.println("stanga2");
}
if(avgc >=500 && avgc <= 700 && avgb <=400){
analogWrite(enaB,255);
analogWrite(enaA, 255);
analogWrite(in1a, 0);
analogWrite(in2a, 0);
analogWrite(in1b, 0);
analogWrite(in2b, 255);
Serial.println("stanga3");
}
if(avgb >=400){
// if(avgc >=300){
analogWrite(enaB, 255);
analogWrite(enaA, 255);
analogWrite(in1a, 0);
analogWrite(in2a, 0);
analogWrite(in1b, 0);
analogWrite(in2b, 255);
Serial.println("Stanga obstructie");
// }
}
}
The setup that i use is 3 Sharp IR distance sensors and a Tamyia double gearbox double DC motors. It also uses the L298 motor driver.
My questions are:
- If I have the 3 sensors how do I store the readings and make the difference? see code below.
- I have 3 sensors and only 2 motors... to many output variables?
- How can I correct the mistakes made in the code below?
#include <PID_v1.h>
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
double Setpoint1, Input1, Output1;
double Setpoint2, Input2, Output2;
....
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
PID myPID1(&Input1, &Output1, &Setpoint1,2,5,1, DIRECT);
PID myPID2(&Input1, &Output1, &Setpoint1,2,5,1, DIRECT);
void setup()
{
//initialize the variables we're linked to
Input = analogRead(0);
...
Setpoint = 100;
...
//turn the PID on
myPID.SetMode(AUTOMATIC);
...
}
void loop(){
SharpIR(0,1,2);
}
SharpIR(int a, int b, int c){
Input = analogRead(a);
Input1 = analogRead(b);
...
if(Input >=300 && Input1 <=400){
if(Input <=350){
// speed control from enable
analogWrite(enaB, Output);
analogWrite(enaA, Output);
// Motors are always going forward
analogWrite(in1a, 0);
analogWrite(in2a, 255);
analogWrite(in1b, 0);
analogWrite(in2b, 255);
Serial.println("inainte");// inainte = forward in romanian
}
}
....
}
Thank You