File content as of revision 31:d570f957e083:

#define TI 0.001f //1kHz sample time
#define KP_LEFT 0.0379f //Proportional gain
#define KI_LEFT 0.7369f //Integral Gain

//#define KP_LEFT 16.1754f
//#define KI_LEFT 314.7632f

//#define KP_RIGHT 0.0379f
//#define KI_RIGHT 0.7369f

#define KP_RIGHT 0.0463f
#define KI_RIGHT 0.8981f

#define freq 25000.0f

#define fullBatScalar 0.5873f
#define speedSensorScalar 2.5f
#define battDividerScalar 4.2;

#define constSpeed = 0.17;

AnalogIn pot1(PTB2); //was PTB0
// Motor Left is PTE20 
// Motor right is pte 21
PwmOut motorLeft(PTE20); //was PTB1
PwmOut motorRight(PTE21); //was PTB2
// left is ptb3 
// right is ptc 2
AnalogIn speedSensorLeft(PTB3);
AnalogIn speedSensorRight(PTC2);
DigitalOut ledRed(LED1);
AnalogIn battInput(PTC1);
// left brake is pta12 
// right brake is pta4
DigitalOut brakeLeft(PTA12);
DigitalOut brakeRight(PTA4); //was PTD4

float pot1Voltage;
float dutyCycleLeft;
float tempDutyCycleLeft = 0;
float tempDutyCycleRight = 0;
float dutyCycleRight;
float speedLeftVolt;
float speedRightVolt;
float batteryVoltage;
float avgCellVoltage;

float errorAreaLeft = 0;
float errorAreaRight = 0;
float errorAreaLeftPrev = 0;
float errorAreaRightPrev = 0;
float errorLeft = 0;
float errorRight = 0;
float setpointLeft = 0.0; //target speed, 0.0 to 1.0
float setpointRight = 0.0;
float controllerOutputLeft = 0;
float controllerOutputRight = 0;
float x;

Timer t;
Timer laptimer;

bool clampLeft = false;
bool clampRight = false;
bool override = false;

bool brakeLeftBool = false;
bool brakeRightBool = false;

volatile bool are_brakes_activated; // Used for debugging 
void disable_brakes(void){
    are_brakes_activated = false;
    brakeLeft=0; 
    brakeRight=0;
    };
void enable_brakes(void) {
    are_brakes_activated = true;
    brakeLeft = 1;
    brakeRight = 1;
  //  errorAreaRight = 0.0;
  //  errorAreaLeft = 0.0;
    };
void _tempBraking(void){
    
    }


/* 
    Always set duty cycle to zero (disabling the motor), unless the motor is enabled 
    (motor_enabled = true);
    BY default, the motor is disabled (duty cycle is zero)
*/

volatile bool motor_enabled = false;
void PI(void) { //motor PI interrupt

speedLeftVolt = (speedSensorLeft.read() * 3.3f);
speedRightVolt = (speedSensorRight.read() * 3.3f);

                
     //_steering();

    if(motor_enabled == true) {
     if(lap == 0 & counter < 7)
       {
           setpointLeft = 0.15;
           setpointRight = 0.15;
           }
        else if (counter == 7 & lap == 0)
        {
            setpointLeft = 0.32; //0.4 is too fast on sraightaway
            setpointRight = 0.32; //0.25 is good
        }    
        else if (counter == 1 & lap == 1)
        {
            setpointLeft = 0.24;
            setpointRight = 0.24;
            enable_brakes();  //uncomment out to brake on big turn
            t.start();
            if (t.read_ms() > 250)
            {
            disable_brakes();
            t.stop();    
            }  
        }
        else if (counter == 4 & lap == 1)
        {
            setpointLeft = 0.24;
            setpointRight = 0.24;    
        }
        
        else if(counter == 5 && lap == 1)
        {
            enable_brakes();
          t.start();
        
            if (t.read_ms() > 50)
            {
            disable_brakes();
            t.stop();    
            }    
            setpointLeft = 0.22;
           setpointRight = 0.22;
            }
        else if(counter == 3 && lap == 1 )
        {
          enable_brakes();
          t.start();
        
            if (t.read_ms() > 200)
            {
            disable_brakes();
            t.stop();    
            }    
        }
        else if(counter == 7 && lap == 1 )
        {
          enable_brakes();
          t.start();
        
            if (t.read_ms() > 80)
            {
            disable_brakes();
            t.stop();    
            }    
        }
        else if (lap >= 2)
        {
        setpointLeft = 0.15;
        setpointRight = 0.15;    
        }
        
        else {
        setpointLeft = 0.24;
        setpointRight = 0.24;    
            }
        
        
    
       //  setpointLeft = 0.1;
    //     setpointRight = 0.1;
        //--- Calculate Error ---
        errorLeft = setpointLeft - (speedLeftVolt / 3.3f); //error and setpoint is defined as a percentage, from 0 to 1
        errorRight = setpointRight - (speedRightVolt / 3.3f);
        //--- Steady State Error Tolerace ---
        if (abs(errorLeft) < 0.01){
            errorLeft = 0.0;
            }
        if (abs(errorRight) < 0.01){
        errorRight = 0.0;
            }
       //--- Calculate integral error ---
        if (clampLeft == false){
            errorAreaLeft = TI*errorLeft + errorAreaLeftPrev;
        }
         if (clampRight == false){ 
        errorAreaRight = TI*errorRight + errorAreaRightPrev;
        }
        errorAreaLeftPrev = errorAreaLeft;
        errorAreaRightPrev = errorAreaRight;
 
            /*
            if (errorAreaLeft > 0.1){
               errorAreaLeft = 0.0;
               }
               p
            if (errorAreaRight > 0.1){
            errorAreaRight = 0.0;
            }         
            */       
               
            //--- Calculate total error ---  
        controllerOutputLeft = KP_LEFT*errorLeft + KI_LEFT*errorAreaLeft;
        controllerOutputRight = KP_RIGHT*errorRight + KI_RIGHT*errorAreaRight;    
        tempDutyCycleLeft = fullBatScalar * controllerOutputLeft;
        tempDutyCycleRight = fullBatScalar * controllerOutputRight;
        //--- Motor over-voltage safety ---
        if (tempDutyCycleLeft > fullBatScalar){ //safety mechanism in case feedback breaks for any reason - 
            dutyCycleLeft = fullBatScalar; //will stop output from exceeding max duty cycle and damaging motor
            } else {                                            
                dutyCycleLeft = tempDutyCycleLeft;
            }
        if (tempDutyCycleRight > fullBatScalar){
              dutyCycleRight = fullBatScalar;
            } else {
                dutyCycleRight = tempDutyCycleRight;
               }                
        //--- integral anti-windup ---
        if (controllerOutputLeft > 1.0){
            if (errorAreaLeft > 0.0){
                clampLeft = true;
                }
            if (errorAreaLeft < 0.0){
                clampLeft = false;
              }
         } else {
             clampLeft = false;
            }
            
            if (controllerOutputRight > 1.0){
                if (errorAreaRight > 0.0){
                    clampRight = true;
                    }
                if (errorAreaRight < 0.0){
                    clampRight = false;
                    }
                } else {
                    clampRight = false;
            }
            
            
               
            //--- set motors to calculated output ---
            motorLeft.write(dutyCycleLeft); // 0.2 For debugging 
            motorRight.write(dutyCycleRight);
            
//--- motor braking ---
/*
if (controllerOutputLeft < 0.0){ 
brakeLeft = 1;
    } else {
        brakeLeft = 0;
}

if (controllerOutputRight < 0.0){ 
brakeRight = 1;
    } else {
        brakeRight = 0;
}
*/
      }
      else {
          motorLeft.write(0.0);
          motorRight.write(0.0);
          }
}