I need to control my robot to move in a straight line by distance 'x'. x will be in multiples of 10cm.
My robot is a 2 wheeled robot and I am using DualVNH5019MotorShield to drive 2 motors.
In order to maintain accuracy of movement and so that it stops within the correct distance, we need to implement PID.
I have never used PID before and am very new to this concept.
Please give me suggestions how should i go about it/
To start with, you'll need some means of measuring how much your wheels have turned - encoders for example.
Yes i have quadrature encoders, which give me 2249 counts per revolution.
The diameter of my wheels is 6cm, so by calculations in 10cm it should give 1194 counts assuming that both wheels move at the same speed and in a straight line.
But the problem is that they are not moving with the same speed. Right one is faster than the left one.
Hence, I am using PID.
I am using the arduino PID library available online in my code.
Now the question is, how should I use my PID?
One for each wheel? Like one PID controller for left wheel, one for right?
So far this is what I have done:
#include "DualVNH5019MotorShield.h"
#include "PinChangeInt.h"
#include <PID_v1.h>
#include <math.h>
DualVNH5019MotorShield md;
//Ultrasonic Sensor
#define URPWM 7
#define URTRIG 6
unsigned int Distance=0;
uint8_t EnPwmCmd[4]={0x44,0x02,0xbb,0x01};
//Unclassified
boolean moveAhead=true;
int state=0;
double currentTime=0,lastTime=0;
const float Pi = 3.14159;
//Speeds
double leftS=0,rightS=0;
double lastCountLeft=0,lastCountRight=0;
//Pins that are input for motors must not be used as interrupt pins.
//Encoder handling
#define motorL_A 11
#define motorL_B 5
#define motorR_A 3
#define motorR_B 13
volatile int motorL_oldA=0,motorR_oldA=0,motorL_newB=0,motorR_newB=0;
volatile long encoderLeft=0,encoderRight=0;
//PID variables
int pwml=300,pwmR=300;
double fkp=0.3,fki=1.2,fkd=0;
double forward_input,forward_output,forward_setCnt;
PID straight(&forward_input,&forward_output,0,fkp,fkd,fki,DIRECT);
double rotationLeft, rotationRight;
void setup(){
Serial.begin(115200);
md.init();
reset();
forward_setCnt=0;
rotation_setCnt=0;
//Encoders
pinMode(motorL_A, INPUT);
pinMode(motorL_B, INPUT);
pinMode(motorR_A, INPUT);
pinMode(motorR_B, INPUT);
digitalWrite(motorL_A,HIGH);
digitalWrite(motorL_B,HIGH);
digitalWrite(motorR_A,HIGH);
digitalWrite(motorR_B,HIGH);
PCintPort::attachInterrupt(motorL_A, MotorLeftA, CHANGE);
PCintPort::attachInterrupt(motorL_B, MotorLeftB, CHANGE);
PCintPort::attachInterrupt(motorR_A, MotorRightA, CHANGE);
PCintPort::attachInterrupt(motorR_B, MotorRightB, CHANGE);
//PID
straight.SetMode(AUTOMATIC);
straight.SetOutputLimits(-400,400);
straight.SetSampleTime(200);
//US sensor
PWM_Mode_Setup();
}
/*-------------------------------------Interrupt Handling------------------------------------------*/
void MotorLeftA(){
motorL_newB ^ motorL_oldA ? encoderLeft++: encoderLeft--;
motorL_oldA = digitalRead(motorL_A);
}
void MotorLeftB(){
motorL_newB=digitalRead(motorL_B);
motorL_newB ^ motorL_oldA ? encoderLeft++ : encoderLeft--;
}
void MotorRightA(){
motorR_newB ^ motorR_oldA ? encoderRight--: encoderRight++;
motorR_oldA = digitalRead(motorR_A);
}
void MotorRightB(){
motorR_newB=digitalRead(motorR_B);
motorR_newB ^ motorR_oldA ? encoderRight-- : encoderRight++;
}
/*---------------------Motion(forward, backward, left right)---------------------------------*/
void forward(){ //where 1 block=10cm
reset();
md.setM1Speed(pwml);
md.setM2Speed(pwmR);
Speed();
forward_input=rightS-leftS;
straight.Compute();
md.setM1Speed(pwm1-forward_output);
md.setM2Speed(pwm2+forward_output);
}
void reset(){
rotationLeft=rotationRight=0;
encoderRight=encoderLeft=0;
forward_setCnt=rotation_setCnt=0;
}
/*------------------------------------Speeds----------------------------------------------------*/
void Speed(){
double currentLEncoder=encoderLeft;
double currentREncoder=encoderRight;
currentTime=millis();
double time=currentTime-lastTime;
leftS=(currentLEncoder-lastCountLeft)*60.0*1000.0/time;
rightS=(currentREncoder-lastCountRight)*60.0*1000.0/time;
lastCountLeft=currentLEncoder;
lastCountRight=currentREncoder;
lastTime=currentTime;
}
/*---------------------Main Loop--------------------------------------------------------------*/
void loop(){
pwml=300; pwmR=300;
distance=1194; //10cm make 1194 rotations
if(encoderRight<distance && encoderLeft<distance)
forward();
md.setBreakes(0,0);
}
Here's an example using two pids to control wheel speed by reading encoders: PID example
Looks like the principle would work for your setup too.
Except you want to control distance and speed, which makes it a bit more complex.
You can either use the control loop for position, and then feed it a profile of the movement
that stays within your velocity constraints, or you can control velocity in an inner loop and
have an outer loop that drives that given the distance / steering error.
MarkT:
Except you want to control distance and speed, which makes it a bit more complex.You can either use the control loop for position, and then feed it a profile of the movement
that stays within your velocity constraints, or you can control velocity in an inner loop and
have an outer loop that drives that given the distance / steering error.
So i modified my code a bit, and used a different approach for PID.
This is what I have done, using two PIDs one for distance and one for ensuring right wheel turns=left wheel turns.
#include "DualVNH5019MotorShield.h"
#include "PinChangeInt.h"
#include <PID_v1.h>
#include <math.h>
DualVNH5019MotorShield md;
//Ultrasonic Sensor
#define URPWM 7
#define URTRIG 6
unsigned int Distance=0;
uint8_t EnPwmCmd[4]={0x44,0x02,0xbb,0x01};
//Unclassified
boolean moveAhead=true;
int state=0;
double currentTime=0,lastTime=0;
const float Pi = 3.14159;
//Speeds
double leftS=0,rightS=0;
double lastCountLeft=0,lastCountRight=0;
//Pins that are input for motors must not be used as interrupt pins.
//Encoder handling
#define motorL_A 11
#define motorL_B 5
#define motorR_A 3
#define motorR_B 13
volatile int motorL_oldA=0,motorR_oldA=0,motorL_newB=0,motorR_newB=0;
volatile long encoderLeft=0,encoderRight=0;
//PID variables
double forward_curSetCnt, forward_curRate,rotation_curSetCnt, rotation_curRate;
double fkp=0.3,fki=1.2,fkd=0;
double rkp=0.65,rki=1.1,rkd=0;
double forward_input,rotation_input,forward_output,rotation_output,forward_setCnt, rotation_setCnt;
PID straight(&forward_input,&forward_output,&forward_setCnt,fkp,fkd,fki,DIRECT);
PID rotn(&rotation_input,&rotation_output,&rotation_setCnt,rkp,rkd,rki,DIRECT);
double rotationLeft, rotationRight;
void setup(){
Serial.begin(115200);
md.init();
reset();
forward_setCnt=0;
rotation_setCnt=0;
//Encoders
pinMode(motorL_A, INPUT);
pinMode(motorL_B, INPUT);
pinMode(motorR_A, INPUT);
pinMode(motorR_B, INPUT);
digitalWrite(motorL_A,HIGH);
digitalWrite(motorL_B,HIGH);
digitalWrite(motorR_A,HIGH);
digitalWrite(motorR_B,HIGH);
PCintPort::attachInterrupt(motorL_A, MotorLeftA, CHANGE);
PCintPort::attachInterrupt(motorL_B, MotorLeftB, CHANGE);
PCintPort::attachInterrupt(motorR_A, MotorRightA, CHANGE);
PCintPort::attachInterrupt(motorR_B, MotorRightB, CHANGE);
//PID
straight.SetMode(AUTOMATIC);
rotn.SetMode(AUTOMATIC);
straight.SetOutputLimits(-250,250);
rotn.SetOutputLimits(-250,250);
straight.SetSampleTime(200);
rotn.SetSampleTime(200);
forward_curRate=2;
//US sensor
PWM_Mode_Setup();
}
/*-------------------------------------Interrupt Handling------------------------------------------*/
void MotorLeftA(){
motorL_newB ^ motorL_oldA ? encoderLeft++: encoderLeft--;
motorL_oldA = digitalRead(motorL_A);
}
void MotorLeftB(){
motorL_newB=digitalRead(motorL_B);
motorL_newB ^ motorL_oldA ? encoderLeft++ : encoderLeft--;
}
void MotorRightA(){
motorR_newB ^ motorR_oldA ? encoderRight--: encoderRight++;
motorR_oldA = digitalRead(motorR_A);
}
void MotorRightB(){
motorR_newB=digitalRead(motorR_B);
motorR_newB ^ motorR_oldA ? encoderRight-- : encoderRight++;
}
/*---------------------Motion(forward, backward, left right)---------------------------------*/
void forward(int block){ //where 1 block=10cm
reset();
straight.SetTunings(30,0.053,0.001);
forward_setCnt=block*1193.13;
rotation_setCnt=0;
}
void leftTurn(){
reset();
forward_setCnt=0;
rotation_setCnt=1510; //VERIFY
}
void rightTurn(){
reset();
forward_setCnt=0;
rotation_setCnt=-1510; //VERIFY
}
void reset(){
rotationLeft=rotationRight=0;
encoderRight=encoderLeft=0;
forward_setCnt=rotation_setCnt=0;
forward_curSetCnt=rotation_curSetCnt=0;
}
/*------------------------------------Speeds----------------------------------------------------*/
void currentSpeed(){
double currentLEncoder=encoderLeft;
double currentREncoder=encoderRight;
currentTime=millis();
double time=currentTime-lastTime;
leftS=(currentLEncoder-lastCountLeft)*60.0*1000.0/(2249*time);
rightS=(currentREncoder-lastCountRight)*60.0*1000.0/(2249*time);
lastCountLeft=currentLEncoder;
lastCountRight=currentREncoder;
lastTime=currentTime;
}
/*--------------------------------------PID-----------------------------------------------------*/
void updatePID(){
double fwd, rtn;
rotationLeft=encoderLeft;
rotationRight=encoderRight;
forward_input=rotationLeft+rotationRight;
rotation_input=rotationLeft-rotationRight;
if(straight.Compute()){
//STOP?
fwd=forward_output;
if(forward_setCnt>0){ //Moving forward
forward_curSetCnt+=forward_curRate;
if(forward_curSetCnt>=forward_setCnt)
forward_curSetCnt=forward_setCnt;
}
else{ //Moving backward
forward_curSetCnt-=forward_curRate;
if(forward_curSetCnt<=forward_setCnt)
forward_curSetCnt=forward_setCnt;
}
if(rotn.Compute()){
rtn=rotation_output;
if(rotation_setCnt>0){
rotation_curSetCnt+=rotation_curRate;
if(rotation_curSetCnt>=rotation_setCnt)
rotation_curSetCnt=rotation_setCnt;
}
else{ //Moving backward
rotation_curSetCnt-=rotation_curRate;
if(rotation_curSetCnt<=rotation_setCnt)
rotation_curSetCnt=rotation_setCnt;
}
}
}
md.setM2Speed(-(fwd+rtn)); //right wheel
md.setM1Speed(-(fwd-rtn)); //Left Wheel
}
/*-----------------------------------DISTANCE-------------------------------------------------*/
void findDistance(){
// a low pull on pin COMP/TRIG triggering a sensor reading
digitalWrite(URTRIG, LOW);
digitalWrite(URTRIG, HIGH); // reading Pin PWM will output pulses
unsigned long DistanceMeasured=pulseIn(URPWM,LOW);
if(DistanceMeasured>=10200) // the reading is invalid.
Serial.println("Invalid");
else
Distance=DistanceMeasured/50; // every 50us low level stands for 1cm
if(Distance<20 && Distance >=10)
moveAhead=false;
}
void PWM_Mode_Setup(){
pinMode(URTRIG,OUTPUT); // A low pull on pin COMP/TRIG
digitalWrite(URTRIG,HIGH); // Set to HIGH
pinMode(URPWM, INPUT); // Sending Enable PWM mode command
for(int i=0;i<4;i++){
Serial.write(EnPwmCmd[i]);
}
}
/*---------------------Main Loop--------------------------------------------------------------*/
void loop(){
if(moveAhead)
updatePID();
if(encoderRight+encoderLeft!=0){
Serial.print("Right ");
Serial.println(encoderRight);
Serial.println("Left ");
Serial.print(encoderLeft);
Serial.println(encoderRight + encoderLeft);
Serial.println(forward_output-rotation_output); //Left
Serial.println(forward_output+rotation_output);//Right
}
delay(2500);
Serial.println(forward_setCnt);
Serial.println();
forward(2);
}
Sorry because this is off-topic question but it also related with PID...
How can I reset Output value of PID? I want to make a regulation of temperature in room, and output of PID is making regulation of windows (how much open/close are them). I want that PID start making calculation every 3 minutes from begin (Output = 0). I read that if I put PID in manual mode and than back in auto it doesn't change value of output so i need to find other solution...