/*
This file implements a pid controller used to simulator cruise control in a car
The input is a throtle value between 0 - 100 ( read on P1 )
The output is the car's speed ( P2 - P0 )
*/
#pragma SMALL DB OE
#include <reg51.h>
/* PID controller code */
double vehicle_accel;
double vehicle_speed;
unsigned char Numbers[17] = { 0xC0, 0xF3, 0xA4, 0xA1, 0x93, 0x89,
0x88, 0xE3, 0x80, 0x81, 0x82, 0x98,
0xCC, 0xB0, 0x8C, 0x8E };
typedef struct
{
double dState; // Last position input
double iState; // Integrator state
/* double iMax, iMin; Maximum and minimum allowable integrator state */
double iGain; // integral gain
double pGain; // proportional gain
double dGain; // derivative gain
} SPid;
double UpdatePID(SPid *pid, double error, double position)
{
double pTerm, dTerm, iTerm;
pTerm = pid -> pGain * error; // calculate the proportional term
// calculate the integral state with appropriate limiting
pid -> iState += error;
/*
if ( pid -> iState > pid -> iMax )
pid -> iState = pid -> iMax;
else if ( pid -> iState < pid -> iMin )
pid -> iState = pid -> iMin;
*/
iTerm = pid -> iGain * pid -> iState; // calculate the integral term
dTerm = pid -> dGain * ( pid -> dState - position );
pid -> dState = position;
return pTerm + dTerm + iTerm;
}
void init_pid( SPid *pid )
{
pid -> dState = 0.0;
pid -> iState = 0.0;
pid -> iGain = 0.0001;
pid -> pGain = 102.0;
pid -> dGain = 0.0;
}
void init_car()
{
vehicle_accel = 0.0;
vehicle_speed = 0.0;
}
void one_tic(void) {
double accel;
accel = vehicle_accel - (vehicle_speed / 10);
vehicle_speed += (accel * .1);
vehicle_speed = vehicle_speed < 0 ? 0 : vehicle_speed;
}
void main(){
unsigned char pedal;
int index;
double desiredSpeed;
double throtle;
SPid plantPID;
init_pid( &plantPID );
init_car();
while( 1 ){
desiredSpeed = 65.0;
throtle = UpdatePID( &plantPID, desiredSpeed - vehicle_speed, vehicle_speed );
pedal = throtle;
if ( pedal < 0x00 ){
pedal = 0x00;
}
else if ( pedal > 0x64 ){
pedal = 0x64;
}
vehicle_accel = pedal * .18; //miles/sec^2
index = vehicle_speed;
P0 = Numbers[ index/10 ];
P2 = Numbers[ index%10 ];
one_tic();
}
}