Society of Robots - Robot Forum
Software => Software => Topic started by: AKnickolai on March 06, 2012, 12:51:03 PM
-
I am attempting to build a maze navigation robot that uses a PID loop to control its distance from the maze wall. The problem that I have is that if I make the gains on the control loop less than one, they appear to have no effect. If they are one or larger, the loop instantly saturates and hits the anti-windup code. Am I not using the correct class to store the variables for the PID controller? I can't understand why values less than one have no effect (almost like it is being rounded or truncated to zero). Any thoughts would be greatly appreciated.
//*********AVR LIBRARIES TO LOAD*********
#include <avr/io.h> // include I/O definitions (port names, pin names, etc)
#include <avr/interrupt.h> // include interrupt support
//*********DEFINE PORT FUNCTIONS*********
#define PORT_ON( port_letter, number ) port_letter |= (1<<number)
#define PORT_OFF( port_letter, number ) port_letter &= ~(1<<number)
#define PORT_ALL_ON( port_letter, number ) port_letter |= (number)
#define PORT_ALL_OFF( port_letter, number ) port_letter &= ~(number)
#define FLIP_PORT( port_letter, number ) port_letter ^= (1<<number)
#define PORT_IS_ON( port_letter, number ) ( port_letter & (1<<number) )
#define PORT_IS_OFF( port_letter, number ) !( port_letter & (1<<number) )
#define BV(bit) (1<<(bit))
#define cbi(reg,bit) reg &= ~(BV(bit))
#define sbi(reg,bit) reg |= (BV(bit))
//************DELAY FUNCTIONS************
//wait for X amount of cycles (23 cycles is about .992 milliseconds)
//to calculate: 23/.992*(time in milliseconds) = number of cycles
void delay_cycles(unsigned long int cycles)
{
while(cycles > 0)
cycles--;
}
//*********SOME VARIABLES***************
int FrontIR=0; //Left IR sensor
int LeftIR=0;//Front IR sensor
float Y=0; //Y will be the "input" variable for our PID, even though it is compare action
float Er=0; //Er is the current measured error between where we want to be and where we are (delta between PV & SP)
float Ero=0; //Ero is the previous measured error between where we want to be and where we are (delta between PV & SP)
float Ir=0; //Ir is the error associated with the I part of the controller
float Dr=0; //Dr is the error associated with the D part of the controller
float OP=0; //OP is the "output" result from our PID controller
//ALL VARIABLES BELOW NEED TO BE SET BY USER PER APPLICATION
float Kp=.1; //Kp is the gain that is proportional to the error
float Ki= .9; //Ki is the gain that is driven by integration of the error over time
float Kd= 0; //Kd is the gain relative to the derivative, or how fast we close on the set point
float SetPt=105; //SetPt is the specified set point for the robot
float Max= 500; //Max is the maximum output you will allow your controller to take, this prevents integrator wind up
float IrMax = 500;
//*******DEFINE SERVO FUNCTIONS**********
void steer(int OP){
if(OP == 0){ //go straight
OCR1B = 1000;
OCR1A = 2000;
}else if(OP > 0){ //go right
OCR1B = 1000; //full speed left
OCR1A = 2000-OP; //slow down right
}else if(OP < 0){ //go left
OCR1B = 1000-OP+100;//slow down left
OCR1A = 2000; //full speed right
}
}
//*******THIS IS THE CORE PROGRAM********
int main (void){
//CONFIGURE TIMER1
TCCR1A|=(1<<COM1A1)|(1<<COM1B1)|(1<<WGM11); //NON Inverted PWM
TCCR1B|=(1<<WGM13)|(1<<WGM12)|(1<<CS11)|(0<<CS10); //PRESCALER=8 MODE 14(FAST PWM)
//CONFIGURE PWM
ICR1=18868; //fPWM=50Hz (Period = 20ms Standard).
DDRD|=(1<<PD4)|(1<<PD5); //PWM Pins as Out
OCR1A=1520; //Set Servos to motionless position
OCR1B=1570; //OCR1B controls port4, OCR1A controls port5
//A2D CONFIGURATION
ADCSRA |= (1 << ADPS2) | (1 << ADPS1); // Set ADC prescalar to 64 - 125KHz sample rate @ 8MHz
ADMUX |= (1 << REFS0); // Set ADC reference to AVCC
ADMUX |= (1 << ADLAR); // Left adjust ADC result to allow easy 8 bit reading
ADCSRA |= (1 << ADEN); // Enable ADC
while(1) {
//get Right data from FrontIR
cbi(ADMUX, MUX2);sbi(ADMUX, MUX1); cbi(ADMUX, MUX0); //set a2d ch 2
sbi(ADCSRA, ADIF); //clear hardware "conversion complete" flag
sbi(ADCSRA, ADSC); //start conversion
while( bit_is_set(ADCSRA, ADSC) ); //wait for conversion to be done, patience grasshopper
FrontIR=ADCH;
//get Left data from LeftIR
cbi(ADMUX, MUX2);sbi(ADMUX, MUX1); sbi(ADMUX, MUX0); //set a2d ch 3
sbi(ADCSRA, ADIF); //clear hardware "conversion complete" flag
sbi(ADCSRA, ADSC); //start conversion
while( bit_is_set(ADCSRA, ADSC) ); //wait for conversion to be done, patience grasshopper
LeftIR=ADCH;
//perform PID control
Ero = Er; //hold the last round error for integrator
Er = (LeftIR)-SetPt;//calculate the error at this time through the loop
Ir = Ir + Er; //calculate the error with respec to the integrator
if (Ir >= IrMax){Ir = IrMax;}
if (Ir <= -IrMax){Ir = -IrMax;}
Dr = Er - Ero; //calculate the error with respect to the derivative
OP = (Kp*Er)+(Ki*Ir)/*+(Kd*Dr)*/; //calculate the PID output
//limit output to prevent integrator wind up and/or limit control out put
//if (OP >= Max){OP = Max;}
//if (OP <= -Max){OP = -Max;}
if(FrontIR >140){
steer(1000);
delay_cycles(100000);
}else{
steer(OP);
}
}
} //end main
-
Any warnings when you compile?
Does this work: (Ir <= -IrMax) or do you need (Ir <= -1*IrMax)?
Not solving the immediate problem, but in general - I wouldn't bother with the derivative control unless you really need it. If you do want to use it, you should run the input through a filter before you calculate the derivative to avoid problems with sample to sample noise.
I would also start out with the integral gain = 0. A simple proportional controller will be easier to tune as a first step. Add any I later (and take out some of the P). Your gains are going to end up funny since you are not accounting for the loop time - this will affect the magnitude of the numbers you use as a gain.
I use proportional only in this line follower: Two Wheel Lego Mindstorms NXT Balancing Robot at LTU (http://www.youtube.com/watch?v=u82w9CpHc0o#)