PID controller for Lego bot

bricx.txt

/* Techbricks.nl Line Follower
   Based on a PID controller, using the NXT 1.0 light sensor
   NXC firmware 1.28
   www.techbricks.nl
   last modified 03/03/2010                                    Breno version*/

/* Proportional gain, straight forward reaction of the controller
   Larger values typically mean faster response since the larger the error,
   the larger the proportional term compensation.
   An excessively large proportional gain will lead to process instability and oscillation.*/
#define Kproportional 1.3

/* Integral gain, improves the controller accuracy
   Larger values imply steady state errors are eliminated more quickly.
   The trade-off is larger overshoot: any negative error integrated during transient response
   must be integrated away by positive error before reaching steady state.*/
#define Kintegral 0.0

/* Derivative gain, improves the controller speed
   Larger values decrease overshoot, but slow down transient response
   and may lead to instability due to signal noise amplification in the differentiation of the error.*/
#define Kderivative 100

#define Kderivative2 100

// Sample time, determined the reaction rate
#define dt 25

// NXT 1.0 light sensor connected to port 2.
#define LIGHTSENSOR Sensor(IN_3)
#define range 0

task main()
{
int error = 0;
float error2=0;
float previous_error = 0;
float setpoint= 0;
float actual_position = 0;
float proportional = 0;
int integral = 0;
float previous_derivative=0;
float derivative = 0;
float derivative2 = 0;
float output = 0;
float left = 0;
float right = 0;


// Set the motor speed.
float speed=60;

// Set the sensor light on.
SetSensorLight(IN_3);

// Set Sensor type.
SENSOR_TYPE_LIGHT_ACTIVE;

// Read the value from the light sensor at the start position. The sensor must be place above the black line.
TextOut(1,LCD_LINE1,"Setpoint");
setpoint = LIGHTSENSOR;
NumOut(50,LCD_LINE1,setpoint);

// never ending loop.
while (true)
  {
   // Read the actual color sensor value.
   actual_position = LIGHTSENSOR;
   TextOut(1,LCD_LINE2,"Actual");
   NumOut(50,LCD_LINE2,actual_position);

   // Calculate the error, the differance between the setpoint and actual position.
   error = setpoint - actual_position;
      // Play a sound when the sensor is off the line
   if ((error < -10)||(error > 10)) PlayTone(TONE_B7, 1);

   // Proportional term makes a change to the output that is proportional to the current error value.
   proportional = Kproportional * error;

   // Integrate, sum of errors
   integral = integral + error;

   // Derivative, rate of change of the process error is calculated by determining the slope of the error over time.
   derivative = (error - error2) / dt;
   previous_derivative = (error2 - previous_error) / dt;
   derivative2 = (derivative-previous_derivative) / dt;

   // Calculate sum of Proportional, Integral and Derivative.
   output = proportional + Kintegral * dt * integral + Kderivative * derivative + Kderivative2*derivative2;
   /*
   if (error+previous_error<1)
   {
   speed=70;
   }
   else
   {
   speed=50;
   }
     */
   // save error value for period.
previous_error = error2;
error2=error;

while ((derivative>10)||(derivative<-10))
{
      OnFwd(OUT_A,right);
      actual_position = LIGHTSENSOR;
      until ( (actual_position <= setpoint +range) || (actual_position>= setpoint-range  )   )
     } ;

     
  // Controll left motor
   left = speed - output;

   // Controll right motor
   right = speed + output;


   // Adjust the left and right motor value.
   if (left >   100) left  =  100;
   if (left <  -100) left  = -100;
   if (right >  100) right =  100;
   if (right < -100) right = -100;


      OnFwd(OUT_A,right);

      OnFwd(OUT_B,left);
      if (speed+output>speed)
      {
      TextOut(1,LCD_LINE5,"Right");
      NumOut(55,LCD_LINE5,-right);
      }
      else
      {
       TextOut(1,LCD_LINE5,"Left");
      }

   // Wait sample time.
   Wait(dt);
   }
}

   /*
   // Controll left motor
   left = speed - output;

   // Controll right motor
   right = speed + output;

   // Adjust the left and right motor value.
   if (left >   100) left  =  100;
   if (left <  -100) left  = -100;
   if (right >  100) right =  100;
   if (right < -100) right = -100;

   if (left < 0 )
     {
      OnRev(OUT_A,-left);
      TextOut(1,LCD_LINE4,"Left  Rev");
      NumOut(55,LCD_LINE4,-left);
     }
   else
     {
      OnFwd(OUT_A,left);
      TextOut(1,LCD_LINE4,"Left  Fwd");
      NumOut(55,LCD_LINE4,left);
     }

   if (right < 0 )
     {
       OnFwd(OUT_B,-right);
       TextOut(1,LCD_LINE5,"Right Rev");
       NumOut(55,LCD_LINE5,-right);
      }
    else
      {
       OnRev(OUT_B,right);
       TextOut(1,LCD_LINE5,"Right Fwd");
       NumOut(55,LCD_LINE5,right);
       }
    *\
   // Wait sample time.
   Wait(dt);
   }
}