File content as of revision 31:d570f957e083:

//Timer t;
#include "MMA8451Q.h"
#include "mbed.h"
#include "steering_header.h"
#include "steering_methods.h"
#include "driving.h"

#include "state_control.h"

#include <iostream>
#include "bluetooth.h"


Serial pc(USBTX, USBRX); // tx, rx



Ticker motorLoop;
Ticker fault_detector; 
Ticker controlUpdate; 



    

int main() { 
    
    state_update();
    wait(2.5);
    //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);
    motorLeft.period(1/freq);
    motorRight.period(1/freq);
    //controlUpdate.attach(&PID, TI);
    //Center the servo motor
    servo_motor_pwm.write(CENTER_DUTY_CYCLE);
   // controlUpdate.attach(&PID, TI);
    //Start the control systm using a Ticker object
      motorLoop.attach(&PI,TI);
    steering_control_ticker.attach(&steering_control, TI_STEERING);
     fault_detector.attach(&fault_check, 0.008);
      //bt.attach(&btReceive);
    
    // 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");
//Timer t;
//t.start();

 while(1) {
     wait(0.2);
     pc.printf("\n\n");
  /*  
     bt.printf("\n\n");
     bt.printf("\n\rMotor enabled ? : %d ", motor_enabled );
     bt.printf("\n\rSteering enabled ? : %d ", steering_enabled );
     bt.printf("\n\rCurrent duty cycle : %f", current_duty_cycle);
     bt.printf("\n\Brake enabled ? : %d ", are_brakes_activated );
     bt.printf("\n\rCurrent duty cycle? : %f ", current_duty_cycle );
     */
      //wait(0.5); //commented out the wait bc it slows down the fault_check, and it breaks the analogIn readings for the driving input
                    //the driving input ticker is faster than the analog.read() function, so all analog.read() methods must be in the main loop
     switch(current_state){
         case STOP :    bt.printf("\r\nCurrent state is stop"); break; 
         case RUN:      bt.printf("\r\nCurrent state is RUN"); break; 
         case WAIT :    bt.printf("\r\nCurrent state is WAIT"); break; 
         };
     switch(fault_type) {
         case CLEAR :       bt.printf("\r\nFault: CLEAR"); break; 
         case OFF_TRACK:    bt.printf("\r\nFault: OFF TRACK"); break;
         case COLLISION :      bt.printf("\r\nFault: COLLISION"); break;
         case LOW_VOLTAGE : bt.printf("\r\nFault: LOW VOLTAGE");  break;
          }
         
        //  bt.printf("\n\n----");
       //   bt.printf("%d", t.read_ms());
         // bt.printf("\n\rLeft distance sens (ADC) : %1.5f", left_sens);
         // bt.printf("\n\rRight distance sens (ADC): %1.5f",right_sens);
         // bt.printf("\n\rDifference (ADC): %1.5f",left_sens - right_sens);
          bt.printf("\n%1.5f", feedback);
         // bt.printf("\n\rLap: %d", lap);
         // bt.printf("\n\ravgCellVolt;ag : %1.5f", avgCellVoltag);
//pot1Voltage = pot1 * 3.3f;
//dutyCycleLeft = (pot1 * fullBatScalar);
//dutyCycleRight = (pot1 * fullBatScalar);

//speedLeft = (speedSensorLeft * speedSensorScalar);
//speedRight = (speedSensorRight * speedSensorScalar);
/*
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);
pc.printf("Output: %1.2f     ", controllerOutputLeft);

pc.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);
        }
    }
}