File content as of revision 35:b42afc973902:

#include "Steering.h"

bool NOT_biSWBL_HELD;
bool NOT_biSWTR_HELD;

void update_display(void const *args){
    
    while(true){
        
        if(screen_flags[curr_screen] || should_redraw){
        screen_flags[curr_screen] = 0;
        should_redraw = false;
        switch(curr_screen){
    
            case HOME_SCREEN:{
                
                display.ClearScreen();
                display.SelectFont(Arial10,BLACK,ReadData);
                display.GotoXY(37,0);
                display.PrintString(" Home Screen");
              
                display.GotoXY(22,30);
                if(vars_list->get_value(PCM_STATE_ID) == 0){
                    display.PutString(17, 30, "Drive Status: OFF");
                }
                else{
                    display.PrintString("Drive Status: ON");
                }
                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(){
    should_redraw = true;
    curr_screen = (curr_screen+1) % NUM_SCREEN; 
}

void request_status_change(){
    
    char drive_status_request;
    ds_mutex.lock();
    drive_status_request = !(vars_list->get_value(PCM_STATE_ID));

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

    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);
    printf("Reset Initiated\n\r");

    return;
}    

void Init()
{
    should_redraw = true;
    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;
    
    vars_list = new variables();
    
    vars_list->add(PCM_STATE_ID, HOME_SCREEN);
    vars_list->add(BATTERY_VOLTAGE_ID, BATTERY_SCREEN);
    vars_list->add(BATTERY_POWER_ID, BATTERY_SCREEN);
    vars_list->add(BATTERY_CURRENT_ID, BATTERY_SCREEN);
    
}

void read_messages(void const *args) {
    
    while (true) {
        
        CANMessage Rxmsg;
        if(CAN_Steering_Buffer.rxRead(Rxmsg)){
            id_node = vars_list->get_node(Rxmsg.id);
            if(id_node != NULL){
                if(id_node->value != Rxmsg.data[0]){
                    screen_flags[id_node->screen] = 1;  
                }
                id_node->value = Rxmsg.data[0];
            }
        }
    }
}

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