File content as of revision 2:c857935f928e:

#include "mbed.h"
#include "driving.h"
#include "steering_header.h"
#include "steering_methods.h"
#include "state_control.h"
#include "MMA8451Q.h"
#include <iostream>
#define BLUETOOTHBAUDRATE 115200 //Communication rate of the micro-controller
 //to the Bluetooth module
#define SERIALTXPORT PTE0 //Tx Pin (Sends information to serial device)
#define SERIALRXPORT PTE1 //Rx Pin (Receives information from serial

#define MMA8451_I2C_ADDRESS (0x1d<<1)

Serial pc(USBTX, USBRX); // tx, rx
Serial bt(SERIALTXPORT, SERIALRXPORT); //(TX, RX) Serial declaration for new serial

PinName const SDA = PTE25;
PinName const SCL = PTE24;

MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
Ticker motorLoop;
Timer t;

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;

char newInputChar;
int newInputInt;
bool clampLeft = false;
bool clampRight = false;

bool brakeLeftBool = false;
bool brakeRightBool = false;

volatile bool newData = false;
 
void btReceive() { //comment this out if it's fucked

 static char buffer[6];
 static int serialCount = 0;
 
{
        char byteIn = bt.getc();
      //  bt.printf("Got %c",byteIn);
        if (byteIn == 'n') {
            buffer[serialCount] = 0;
            //bt.printf("Got endl %c",byteIn);
            int speed;
            char type;
            if (sscanf(buffer,"%c%i",&type,&speed) == 2) {
                newInputChar = type;
               // bt.printf("char: %c", type);
                newInputInt = speed;
               // bt.printf("speed: %i", speed);
                newData = true;
                }
            serialCount = 0;
            } else {
                buffer[serialCount] = byteIn;
                if (serialCount < 6)
                serialCount++;
                }
        }
    }
    
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;
}
*/
        
      
}

int main() {
 

    state_update();
   
    //Delcare Onboard LED with blue color
    DigitalOut led_b(LED_BLUE);
    //Set the period of the servo motor control signal
    servo_motor_pwm.period(1/SERVO_MOTOR_FREQ);
    //Center the servo motor
    servo_motor_pwm.write(CENTER_DUTY_CYCLE);

    //Start the control systm using a Ticker object
      steering_control_ticker.attach(&steering_control, TI);
     
    
    // call landmark_counter wjen a landmark is detected 
    left_landmark_sensor.rise(&landmark_counter);
    right_landmark_sensor.rise(&landmark_counter);
    // update status when the button is pressed
    stop_button.rise(&state_update);
    run_button.rise(&state_update);
    wait_button.rise(&state_update);
 
 
 bt.baud(BLUETOOTHBAUDRATE);
 //Sets the communication rate of the micro-controller to the Bluetooth module.
pc.printf("Hello World!\n");
bt.printf("Hello World!\n");

t.start();
float time = t.read();

//bt.attach(&btReceive);
motorLoop.attach(&PI,TI);

motorLeft.period(1/freq);
motorRight.period(1/freq);

//setpointLeft = 0.0; //target speed, 0.0 to 1.0
//setpointRight = 0.0;

setpointLeft = 0.7; //target speed, 0.0 to 1.0
setpointRight = 0.7;

 //prints the string to the Tera-Term Console using the Bluetooth object ‘bt’.
 while(1) {
     
     pc.printf("\n\n");

     pc.printf("\r\nSteering enabled? : %d",steering_enabled);
     pc.printf("\r\nCurrent duty cycle : %f ", current_duty_cycle);
     pc.printf("\r\nLeft duty cycle : %f ", left_distance_sensor.read());
     pc.printf("\r\nRight voltage : %f"      , right_distance_sensor.read());
     pc.printf("\r\nCurrent duty cycle : %f ", current_duty_cycle);
       fault_check();
      wait(0.5);
     switch(current_state ){
         case STOP :    pc.printf("\r\nCurrent state is stop"); break; 
         case RUN:      pc.printf("\r\nCurrent state is RUN"); break; 
         case WAIT :    pc.printf("\r\nCurrent state is WAIT"); break; 
         };
     switch(fault_type) {
         case CLEAR :       pc.printf("\r\nFault: CLEAR"); break; 
         case OFF_TRACK:    pc.printf("\r\nFault: OFF TRACK"); break;
         case BUMPER :      pc.printf("\r\nFault: OBSTECALE"); break;
         case LOW_VOLTAGE : pc.printf("Fault: LOW VOLTAGE");  break;
          }
          
x = abs(acc.getAccX());
pot1Voltage = pot1 * 3.3f;
batteryVoltage = battInput * 3.3 * battDividerScalar;
avgCellVoltage = batteryVoltage / 3.0;

//dutyCycleLeft = (pot1 * fullBatScalar);
//dutyCycleRight = (pot1 * fullBatScalar);

//speedLeft = (speedSensorLeft * speedSensorScalar);
//speedRight = (speedSensorRight * speedSensorScalar);

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

bt.printf("Duty Cycle = %1.2f     ", dutyCycleLeft);
bt.printf("Speed (V) L,R = %1.2f", speedLeftVolt);
bt.printf(", %1.2f     ", speedRightVolt);

bt.printf("Error L,R: %5.2f", errorLeft);
bt.printf(", %5.2f     ", errorRight);

bt.printf("ErrorArea: %1.2f     ", errorAreaLeft);
bt.printf("Output: %1.2f     ", controllerOutputLeft);

bt.printf("Batt Voltage: %1.2f \n",avgCellVoltage);

//setpointLeft = pot1;
//setpointRight = pot1;


if (t.read() > 5){
    setpointLeft = 0.0;
    setpointRight = 0.0;
    }
    
    if (t.read_ms() > 5100){
        setpointLeft = 0.2;
        setpointRight = 0.2;
        }

 if (newData){  
      newData = false;
  //    bt.printf("Got %c %3i\n",newInputChar, newInputInt);
      
      if (newInputChar == 'L')
      setpointLeft = (newInputInt / 100.0f);
      if (newInputChar == 'R')
      setpointRight = (newInputInt / 100.0f);
      

//wait(0.1);
        }
    }
}