File content as of revision 3:25c6bf0abc00:

#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 20000.0f

#define fullBatScalar 0.5873f
#define speedSensorScalar 2.5f
#define battDividerScalar 4.0;
PwmOut motorLeft(PTB1); 
PwmOut motorRight(PTB2);
AnalogIn pot1(PTB0);
AnalogIn speedSensorLeft(PTB3);
AnalogIn speedSensorRight(PTC2);
DigitalOut ledRed(LED1);
AnalogIn battInput(PTC1);
DigitalOut brakeLeft(PTA12);
DigitalOut brakeRight(PTD4);

DigitalIn enableBrakeLeft(PTA4);
DigitalIn enableBrakeRight(PTA5);

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;

bool clampLeft = false;
bool clampRight = false;

bool brakeLeftBool = false;
bool brakeRightBool = false;


 
    
void PI() { //motor PI interrupt

//--- 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;
}

//--- fucked up manual braking stuff ---
if (setpointLeft == 0.0 || brakeLeftBool == true)
{
    errorAreaLeft = 0.0;
    brakeLeft = 1;
} else {
    brakeLeft = 0;
}

if (setpointRight == 0.0 || brakeRightBool == true)
{
    errorAreaRight = 0.0;
    brakeRight = 1;
} else {
    brakeRight = 0;
}

//--- set motors to calculated output ---    
motorLeft.write(dutyCycleLeft);
motorRight.write(dutyCycleRight);


//--- motor braking ---
/*
if (controllerOutputLeft < 0.0){ 
brakeLeft = 1;
    } else {
        brakeLeft = 0;
}

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