File content as of revision 31:d570f957e083:

/* 
    This file controls the three states of the car 
    
*/
#define MMA8451_I2C_ADDRESS (0x1d<<1)

InterruptIn stop_button(PTD0);
InterruptIn run_button(PTD5); 
InterruptIn wait_button(PTA13);  
DigitalOut red_led(PTC16);
DigitalOut green_led(PTC13);
DigitalOut yellow_led(PTC12);

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

MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
   
// Available states 
enum states {STOP, WAIT, RUN}; 

// Available LED colors 
enum led_colors {RED, YELLOW, GREEN};

void turn_leds_off(void){ yellow_led =0; green_led =0; red_led=0;};
enum fault_list {CLEAR,OFF_TRACK, COLLISION, LOW_VOLTAGE};
fault_list fault_type=CLEAR;
volatile bool _fault = false;
// turns an led 
void turn_led(led_colors Color){
       turn_leds_off();
        switch(Color) {
        case RED    :    {red_led = 1;    green_led=0;   yellow_led=0;};  break;
        case YELLOW :    {yellow_led = 1; green_led =0;  red_led=0;};     break;
        case GREEN  :    {green_led = 1;  yellow_led =0; red_led=0;};     break;
        } 
    };

void _stop(void){
   steering_enabled =false;
    enable_brakes();
    motor_enabled = false;  
    turn_led(RED); 
 
    }; 
void _run(void){
    // TODO: realease brakes, start the motors
    // 
    disable_brakes();
    motor_enabled = true; 
    turn_led(GREEN); 
    };
void _wait(void){
     // release brakes, turn on steering system, do not start the motor
     // disable_brakes();
     disable_brakes(); 
    motor_enabled = false;
    setpointLeft = 0.0;
    setpointRight = 0.0;
    steering_enabled = true;
     turn_led(YELLOW);
     };
     
states current_state = WAIT;    // By default the car waits
states prev_state = WAIT;

// This function gets called when a button is pushed
void state_update (void){ 
    current_state = (stop_button.read()==1 || _fault == true) ? STOP: ((run_button.read() ==1 && prev_state == WAIT) ? RUN : (wait_button.read() ==1 )? WAIT:  prev_state); 
    switch(current_state) {
        case STOP :  _stop();   break;
        case WAIT :  _wait();   break;
        case RUN  :  _run();    break;
        }
      prev_state = current_state;
    }

void fault_check(void){
        _fault = false; 
        fault_type = CLEAR;
    batteryVoltage = battInput * 3.3 * battDividerScalar;
    avgCellVoltage = batteryVoltage / 3.0;
    
    if (left_distance_sensor.read() < (0.400/3.3) && right_distance_sensor.read() < (0.400/3.3))
        {
          _fault = true;
          fault_type = OFF_TRACK;
        }
        
        
    if 
    
    (avgCellVoltage <= 3.4){
         _fault = true;
         fault_type = LOW_VOLTAGE;
         }      
    if (abs(acc.getAccX() > 1.48)){ // was 1.5
            _fault = true;
            fault_type = COLLISION;
        }    
                 
    
    state_update();
    };