File content as of revision 43:47a3ac562be8:

#include "Steering.h"

bool NOT_biSWBL_HELD;
bool NOT_biSWTR_HELD;

void update_display(void const *args){


    while(true){
        
        switch(curr_screen){
    
            case HOME_SCREEN:
            
                display.ClearScreen();
                display.SelectFont(Arial10,BLACK,ReadData);
                display.GotoXY(37,0);
                display.PrintString("Home Screen");
                break;
            
            case BATTERY_SCREEN:
            
                display.ClearScreen();
                display.SelectFont(Arial10,BLACK,ReadData);
                display.GotoXY(33,0);
                display.PrintString("Battery Screen");
                break;
        
            default:
                break;    
        
        }
        wait(2);   
    }
}

void toggle_screen(){
    curr_screen = (curr_screen+1) % 2; 
}

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);
    curr_screen = HOME_SCREEN;
    drive_status = 0;
    drive_status_request = 1;
    reset_body = 0;
    ledstream.write(0);
    NOT_biSWBL_HELD = true;
    NOT_biSWTR_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 main2(){
    // 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);
    
    wait(2);
     
     //New thread to read messages.
    Thread update_thread(read_messages);
    
    // display the screen.
    Thread display_thread(update_display);
      

    // 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(biSWTR.read() && NOT_biSWTR_HELD){
            printf("ff\r\n");
            toggle_screen();
            NOT_biSWTR_HELD = false;
        }
        
        else if(!biSWTR.read()){
            NOT_biSWTR_HELD = true;
        }
        
        else{
            // ignore BiSWTR.read()
        }
        
        
        if(biSWBR.read()){ 
            reset();
        }
    }        
}