File content as of revision 20:202e0046527e:

#include "core.h"
namespace display{
    //Display on MBED text display: odometer value, average speed
    const float freq = 2.0f; //hz
    MCP23017 *port;
    WattBob_TextLCD *lcd;
    
    void init(){
        port = new MCP23017(p9, p10, 0x40); 
        lcd = new WattBob_TextLCD(port);
        port->write_bit(1,BL_BIT); // LCD backlight on.
        lcd->cls();
    }
    static inline void hotLoop(){
        lcd->cls();
        
        runTimeParams::liveAccess.lock();
        float odometer = runTimeParams::odometer;
        float avgSpeed = runTimeParams::avgSpeed;
        runTimeParams::liveAccess.unlock();
        
        lcd->locate(0,0);   //located col, row.
        lcd->printf("Odo=%.2f",odometer);
        
        lcd->locate(1,0);   //located col, row.
        lcd->printf("Speed=%.2f",avgSpeed);
    }
}
namespace brakeIndicator{
    //Show use of the brake on a LED on the RedBox Unit
    const float freq = 2.0f; //hz
    DigitalOut led2(PORT_REDBOX_LED1);
    static inline void init(){
//        led2.period_ms(50);
    }
    static inline void hotLoop(){
            runTimeParams::liveAccess.lock();
            if (runTimeParams::brakeForce>0.3f){
//                led2.write(runTimeParams::brakeForce*100.0f);
                    led2 = 1;
                    }
                else{
                    led2 = 0;
                }
            runTimeParams::liveAccess.unlock();

    }
}
namespace speedIndicator{
    //Monitor speed and if it goes over 88 mph switch on a LED on
        //the RedBox unit
    static const float freq = 1; //hz
    DigitalOut led(PORT_REDBOX_LED2);
    static inline void init(){
        led = 0;
    }
    static inline void hotLoop(){
            runTimeParams::liveAccess.lock();
            if (runTimeParams::avgSpeed>88.0f){
            led = 1;
            }
            else{
                led = 0;    
                }
            runTimeParams::liveAccess.unlock();
            
    }
}
namespace sideLights{
    //Read a single side light switch and set side lights accordingly
    static const float freq = 1; //hz
    DigitalIn lightSwitch(PORT_BRAKE);
    DigitalOut led(PORT_SIDE_LIGHTS);
    static inline void hotLoop(){
        led = lightSwitch;
        }
        
    }
namespace turnSignal{
    //Read the two turn indicator switches and flash appropriate
        //indicator LEDs at a rate of 1Hz
    static const float freq = 0.5; //hz
    DigitalIn lSwitch(PORT_TURN_SIGNAL_SWITCH_LEFT);
    DigitalIn rSwitch(PORT_TURN_SIGNAL_SWITCH_RIGHT);
    
    PwmOut lLed(PORT_TURN_SIGNAL_LED_LEFT);
    PwmOut rLed(PORT_TURN_SIGNAL_LED_RIGHT);
    
    static inline void hotLoop(){
        int a = lSwitch.read();
        int b = rSwitch.read();
        
        //If both switches are switched on
        //then flash both indicator LEDs at a rate of 2Hz (hazard mode)
        if(a&&b){
             lLed.period(0.5f);
             rLed.period(0.5f);
        }
        else{
            lLed.period(1.0f);
            rLed.period(1.0f);
        }
        a ? lLed.write(0.5f) : lLed.write(0.0f);
        b ? rLed.write(0.5f) : rLed.write(0.0f);
    }
    
}



namespace task_group_1{
    Thread thread;
    const float freq = 2.0f; //hz
    

    void runTask(){
        Timer executionTimer,sleepTimer;
        executionTimer.reset();
        sleepTimer.reset();
        
        display::init();
        brakeIndicator::init();
        speedIndicator::init();
        
        const int const_delay = int((1000.0f/freq)+0.5f); //ideal sched delay
        int dynamic_delay = const_delay; //real sched delay (updated in loop)
        
        int tick = 0; //freq subsampler
        #if DEBUG_MODE
        int max_exec_time = 0; //for logging
        #endif
        
        while(true){
            //Determine scheduling compensators:
                //1: release time drift 
                //2: execution time 
            sleepTimer.stop();
            executionTimer.start();
            int sleepTime = sleepTimer.read_ms();
            const int drift = ((sleepTime - dynamic_delay) > 0)?
                                        (sleepTime - dynamic_delay) : 0;
            
            
            // Run all tasks--------------
            brakeIndicator::hotLoop();
            
            static const int tick_interval_sIndicator = int((freq/speedIndicator::freq)+0.5f);
            if (!(tick%tick_interval_sIndicator)){
                speedIndicator::hotLoop();
                sideLights::hotLoop();
            }
            static const int tick_interval_tSignal = int((freq/turnSignal::freq)+0.5f);
            if (!(tick%tick_interval_tSignal)){
                turnSignal::hotLoop();
            }
            display::hotLoop();
            //--------------Completed tasks
            
            tick++;
            executionTimer.stop();
            int exec_time = executionTimer.read_ms();
            
            
            #if DEBUG_MODE
            //Debug Logs (once per dequeue call to avoid memory issues)
            if (exec_time > max_exec_time) max_exec_time=exec_time;
            static const int tick_interval_debug_log = int((freq/dequeueMail::freq)+0.5f);
            if (!(tick%tick_interval_debug_log)){
                runTimeParams::debugAccess.lock();
                *runTimeParams::debugLog += "task_group_1," + to_string(max_exec_time) + ","
                            + to_string(sleepTime) + ","
                            + to_string(drift) + "\n\r";
                runTimeParams::debugAccess.unlock();
            }
            #else
            static const int tick_interval_debug_log = 1;
            #endif
            //Reset tick count
            static const int tick_LCM = 
                tick_interval_debug_log*
                tick_interval_sIndicator*
                tick_interval_tSignal;
                
            if (tick==tick_LCM) tick=0;
            
            
            executionTimer.reset();
            sleepTimer.reset();
            sleepTimer.start();
            //compensate for delays
            dynamic_delay = const_delay - (exec_time + drift);
            Thread::wait(dynamic_delay);
        }   
    }
}