/* 
   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();    
  }
}