File content as of revision 24:ba3428215006:

#include "Steering.h"

bool NOT_biSWBL_HELD;

void request_status_change(){
    
    char drive_status_request;
    ds_mutex.lock();
    drive_status_request = !drive_status;
    ds_mutex.unlock();
    char * status_string;

    if(drive_status_request){
        status_string = "ON";
    }
    else{
        status_string = "OFF";
    }

    CANMessage Txmsg_drive_status_request(0x501,&drive_status_request,1);
    for(int i = 0; i < 10; i++){
        CAN_Steering_Buffer.txWrite(Txmsg_drive_status_request);
    }

    display.ClearScreen();
    display.SelectFont(Arial12,BLACK,ReadData);
    display.GotoXY(26,16);
    display.PrintString("DRIVE STATUS REQUEST");

    printf("%s\n\r", status_string);
    return;
}

void reset()
{
    reset_body = 1;
    CANMessage Txmsg_reset(0x502,&reset_body,1);
    for(int i = 0; i < 10; i++){
        CAN_Steering_Buffer.txWriteDirect(Txmsg_reset);
    }
    NVIC_SystemReset();
    display.ClearScreen();
    display.SelectFont(Arial12,BLACK,ReadData);
    display.GotoXY(16,16);
    display.PrintString(" RESET INITIATED");
    printf("Reset Initiated\n\r");

    return;
}    

void Init()
{
    pc.baud(921600);
    drive_status = 0;
    drive_status_request = 1;
    reset_body = 0;
    ledstream.write(0);
    NOT_biSWBL_HELD = true;
}

void read_messages(void const *args) {
    
    while (true) {
        CANMessage Rxmsg;
     
        if(CAN_Steering_Buffer.rxRead(Rxmsg))
            if(Rxmsg.id == PCM_STATE_ID){
              
                // Mutex to protex shared variables
                ds_mutex.lock();
                drive_status = Rxmsg.data[0];
                ds_mutex.unlock();
            }
            
            if(Rxmsg.id == BATTERY_POWER_ID)
            {
                float power_ratio;
                ftc rcv;
                rcv.FLOAT=0.0;
                
                for(int i=0; i<4; i++){
                    rcv.C_FLOAT[i] = Rxmsg.data[i];
                }
                power_ratio=rcv.FLOAT/80000;
                ledstream.write(power_ratio);
            }
        }
    }


int main(){
    // Initialize, set all variables.
    Init();
    wait(0.1);
    
    //Init Display
    display.GotoXY(10,16);
    display.SelectFont(Arial_14,BLACK,ReadData);
    display.PrintString("Penn Electric Racing");
    CAN_Steering_Buffer.mode(NoAck);
    
     //New thread to read messages.
     Thread update_thread(read_messages);
     update_thread.set_priority(osPriorityLow);
     

    wait(1);

    // Start to read buttons on main thread
    while(1)
    {
        if(biSWBL.read() && NOT_biSWBL_HELD){
            request_status_change();
            NOT_biSWBL_HELD = false;
        }
        
        else if(!biSWBL.read()){
            NOT_biSWBL_HELD = true;
        }
        
        else{
            // ignore BiSWBL.read()
        }
        
        
        if(biSWBR.read()){ 
            reset();
        }
    }        
}