Hello,
Here's what I want to do : Use the PID library for arduino Arduino Playground - PIDLibrary , to tell my robot to stop at a certain (x,y) position.
Now, to test the library, I 'm trying to tell one wheel to stop when its encoder ticks reach a certain value.
The code :
#include <PID_v1.h>
#define encoderPinA 25 // Encoder A pin
#define encoderPinB 24 // Encoder B pin
int encoderStateA ; // Digital State of Encoder A
int encoderStateB ; // Digital State of Encoder B
int lastState=0; // Iast digital state of Encoder
int encoderTicks;
// PID Variables
#define SAMPLE_TIME 25 // Time interval in ms for PID sampling
#define POS_SETPOINT 79 // Analog value corresponding to 1000 ticks
double positionSetPoint, positionInput, positionOutput; // Define Variables we'll be connecting to
double aggKp=4, aggKi=0.2, aggKd=1; // aggressive PID parameters if there's a large gap between setpoint and input
double consKp=1, consKi=0.01, consKd=0.25; // Conservative PID parameters if input is close to setpoint
PID positionPID(&positionInput, &positionOutput, &positionSetPoint, aggKp, aggKi, aggKd, DIRECT); // Specify the links and initial tuning parameters
// Motor Driving variables
#define TOPSPEED 255
#define FORWARD_SPEED 150
#define TURN_SPEED 60
// motor A
int dir1PinA = 13;
int dir2PinA = 12;
int speedPinA = 10;
// motor B
int dir1PinB = 11;
int dir2PinB = 8;
int speedPinB = 9;
int speed[2]={0}; //speed[0] = left wheel ; speed[1] = right wheel
void setup(){
init_motors(); // Motor Pin intialization
init_positionPID(); // Pid initialization
init_encoders(); //Encoders Pin initialization
Serial.begin(38400);
}
void loop(){
//Reads encoders and increments or decrements ticks
readEncoder();
// The input has to be an analog Value (0 to 1023) so this function is to transfer the range of the ticks from 0->25875 to 0->1023
positionInput = map(encoderTicks, 0, 25875, 0, 1023);
//Decides whether to launch PID algorithm
positionPID.Compute();
// Sets the PID parameters
if(abs(positionInput-positionSetPoint)<5){
positionPID.SetTunings(consKp, consKi, consKd);}
else{
positionPID.SetTunings(aggKp, aggKi, aggKd);}
// Robot goes forward till it reaches the setpoint, backwards if it goes above Setpoint
if(positionInput<=positionSetPoint){
go_forward(0,positionOutput);
}else{
go_backwards(0,positionOutput);
}
}
void init_encoders(){
pinMode(encoderPinA, INPUT); // Encoder pin A INPUT
pinMode(encoderPinB, INPUT); // Encoder pin B INPUT
}
void readEncoder(){
encoderStateA = digitalRead(encoderPinA); // lit l'état de la broche en entrée
encoderStateB = digitalRead(encoderPinB);
if(encoderStateA!=lastState){
lastState=encoderStateA;
if(encoderStateA==1 && encoderStateB ==1) {
encoderTicks++;
Serial.println(encoderTicks);
}
else if(encoderStateA==1 && encoderStateB==0) {
encoderTicks--;
Serial.println(encoderTicks);
}
}
}
void go_forward(int left, int right){
speed[0]=left;
speed[1]=right;
analogWrite(speedPinA, speed[0]);
analogWrite(speedPinB, speed[1]);
digitalWrite(dir1PinA, HIGH);
digitalWrite(dir2PinA, LOW);
digitalWrite(dir1PinB, HIGH);
digitalWrite(dir2PinB, LOW);
}
void go_backwards(int left, int right){
analogWrite(speedPinA, speed[0]);
analogWrite(speedPinB, speed[1]);
digitalWrite(dir1PinA, LOW);
digitalWrite(dir2PinA, HIGH);
digitalWrite(dir1PinB, LOW);
digitalWrite(dir2PinB, HIGH);
}
void init_positionPID(){
// initialize the variables we're linked to
positionInput = 0;
positionSetPoint = POS_SETPOINT;
// turn the PID on
positionPID.SetMode(AUTOMATIC);
positionPID.SetSampleTime(SAMPLE_TIME);
}
void init_motors(){
pinMode(dir1PinA, OUTPUT);
pinMode(dir2PinA, OUTPUT);
pinMode(speedPinA, OUTPUT);
pinMode(dir1PinB, OUTPUT);
pinMode(dir2PinB, OUTPUT);
pinMode(speedPinB, OUTPUT);
}
In this code, as I said I'm only experimenting on the right wheel.
Now the problem is when I use the function readEncoders() alone it works perfectly : Increments when I manually turn the wheel forward, and decrements when I turn backwards. It also works when I tell the motor to turn at a constant speed(still without using PID).
But when I use the PID algorithm, on the Serial monitor I see the ticks going crazy (sometimes decrementing a few times then incrementing again) while the wheel is only going forward, and sometimes they go from like 300 to 540 
Missing 240 ticks is a little weird...don't you think? o_O
So if you have any ideas please share^^
Thanks
