Speed control loop for line following Robot

arduinoatmegaatxmegacontrol systemmicrocontroller

I am working to control the speed in my line following Robot (using arduino uno),by using proportional constant Kp in a closed loop, in my code I calculate the RPM from The Right Motor and the Left Motor,I used Timer2 interrupt every 50ms to calculate the speed (Rpm) from these Encoders.

I used Timer 0 and Timer 1 to generate pwm signals with a VARIETY duty-cycle=75 % to control the tow motors speed,
I measured The RPM for the Left Motor by using Tachometer it is 100 RPM,
I changed the Kp constant But my Encoder Output still Give me about 80 RPM (no change).

so my question is :
is there any problem in my code or the way I do the controlling loop or understanding the controlling loop.

This is the Code :

//encoders variable
volatile bool counting;
volatile unsigned long events0;//the number of pulses from Left Motor in 50 ms;
volatile unsigned long events1;//the number of pulses from Right Motor in 50 ms;
double n1=540;//the number of pulses in one turn for the Right Encoder;
double n2=540;//the number of pulses in one turn for the Left Encoder;
//end of encoders variable

  //PID controller Variable.
 double setpoint=100; // Reference speed;
 const int Kp=10;
 const int Ki=0;
 const int Kd=0;
 double measuredRightRpm_value;//speed in RPM from Right encoder/Motor;
 double measuredLeftRpm_value;//speed in RPM from Left encoder/Motor;
 volatile signed long erorR;//Eror from Right Encoder;
 volatile signed long erorL;//Eror from Left Encoder;
 volatile signed long Eror; 
 //End of PID controller Variable.

 //Timer 2 over flow interrupt
  int x = 0, flag = 0;

  ISR (TIMER2_OVF_vect)
  {
  TCNT2=0x64;//for 10 ms
  x++;
  if(x==5) //to get 50 ms
  { 
  flag = 1;
  x=0;
  }  
   }  // end of ISR

 void eventISR ()
 {
 if (counting)
 events0++;    
 }  // end of eventISR
 void eventISR1 ()
 {
 if (counting)
 events1++;    
 }  // end of eventISR1
void setup ()
{
//setup Timer 0 and Timer 1 to generate Pwm signals
TCCR0A=0xB3;
TCCR0B=0x02; // set Timer 0 to clk/8 HZ ,fast PWM mode,tow signal inverted to each  other.

TCCR1A=0xB1;// set Timer 1 to clk/8 HZ ,fast PWM mode,tow signal inverted to each other.
TCCR1B=0x0A;;

TCCR2A=0x00;//timer 2 overflow interrupt to calculate RPM from Encoder
TCCR2B=0x07;
TCNT2=0x64;

 pinMode(5,OUTPUT);
 pinMode(6,OUTPUT); 
 pinMode(9,OUTPUT);
 pinMode(10,OUTPUT);
 //end of Timers  setup for generating PWM and interrupts.


 //Encoder Setup    
 Serial.begin (9600);
 Serial.println ();
 pinMode (2, INPUT_PULLUP);
 attachInterrupt (0, eventISR, RISING);
 attachInterrupt (1, eventISR1, RISING);
 // end of Encoder setup
 counting = true;
 // Timer/Counter 2 Interrupt(s) initialization
 TIMSK2=0x01;
 interrupts ();
 }  // end of setup

 void showResults ()
 {

 Serial.print ("the speed (RPM) for the Left Motor = ");
 Serial.println ((events0/n1)*(60000/50));
 measuredLeftRpm_value=(events0/n1)*(60000/50);

 //Serial.print ("I counted for the second  Motor  ");
 //Serial.println (events1); 
 Serial.print ("the speed (RPM) for Right Motor = ");
 Serial.println ((events1/n2)*60000/50);

  }  // end of showResults

 void PID()
 {

 measuredRightRpm_value=(events0/n1)*(60000/50);//speed in Rpm from Right/encoder/Motor.
 erorR= setpoint - measuredRightRpm_value;//Erorr From_Right Encoder.

 measuredLeftRpm_value=(events1/n2)*60000/50;////speed in Rpm from Left/encoder/Motor.
 erorL= setpoint - measuredLeftRpm_value;////Erorr From_Left Encoder.


 }

void loop ()
{

counting = true; 
if(flag ==1)
{
  showResults();
  PID();
events0 = 0;
events1 = 0;
erorR=0;
erorL=0;
flag = 0;
}  


OCR0A=192+Kp*erorR; //75% DutyCycle  for Right Motor .
OCR0B=192+Kp*erorR;
OCR1A=-192-Kp*erorL;//75% DutyCycle  for Left Motor.
OCR1B=-192-Kp*erorL; 
}  // end of loop

Best Answer

I have found the Kp at the bottom. The problem is that you compute erorL,R in PID() presumably at timed interrupt (flag==1), but then you set everything to zero once returned from PID(). Also you will need some rearangement on number types, you should use floating point math to calculate error, Kp,...then to truncate in integer, then add or subtract. Also a limiting of output value is needed 0-100%. Not I am not C or C++ programmer so the pseudocode for simple P-regulator is as follows, all numbers are float type:

Y_regulator=(Setpoint-ProcessValue)*Kp + Y_static(Setpoint)
// ProcessValue...your measured value
// Y_static...this is a stored curve, in your case you have set this to 75%
if Y_regulator>100.0 then
Y_regulator=100.0
elseif Y_regulator<0.0 then
Y_regulator=0.0
endif

Then do noramlization, convert 0 to 100% into your registers numbers, by multiplying/dividing , rounding.

Y_Static(Setpoint) can be ommited, but you will need a PI-regulator, anyway with a P-regulator you will never reach the exact velocity, but close enough for the needed application.