go_away

Author Topic: Help with PID loop and rounding error?  (Read 723 times)

0 Members and 1 Guest are viewing this topic.

Offline AKnickolaiTopic starter

  • Jr. Member
  • **
  • Posts: 9
  • Helpful? 0
Help with PID loop and rounding error?
« 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.

Code: [Select]
//*********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

Offline jkerns

  • Robot Overlord
  • ****
  • Posts: 214
  • Helpful? 11
Re: Help with PID loop and rounding error?
« Reply #1 on: March 07, 2012, 03:23:05 PM »
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

« Last Edit: March 07, 2012, 03:31:09 PM by jkerns »
I get paid to play with robots - can't beat that with a stick.

http://www.ltu.edu/engineering/mechanical/robotics_engineering.asp

 


Get Your Ad Here

data_list