File content as of revision 4:369caebdf5dc:

#include "mbed.h"

#include "IRBehaviorController.h"


// Public methods

void
IRBehaviorController::setActiveThreshold(float threshold)
{
    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
    activationThreshold = threshold;
    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
}


void
IRBehaviorController::runSeeking(void)
{
    if (debug != NULL)
        debug->printf("IRController: Seeking Task Start.\r\n");
    
    scanIR();

    if (brightness > activationThreshold)
        output = pid->run(centeroid);
    else
        output = 0;
    
    if (debug != NULL)
        debug->printf("IRController: Seeking Task Complete.\r\n");
}

void
IRBehaviorController::runAvoidance(void)
{
    float tmp;
    
    if (debug != NULL)
        debug->printf("IRController: Avoidance Task Start.\r\n");
    
    scanIR();

    tmp = centeroid;
    
    // Centeroid value needs to be split for avoidance mode.
    if (tmp < 0)
        tmp += 3.0f;
    else
        tmp -= 3.0f;
        
    if (brightness > activationThreshold)
        output = pid->run(tmp);    
    else
        output = 0;
    
    if (debug != NULL)
        debug->printf("IRController: Avoidance Task Complete.\r\n");
}

float
IRBehaviorController::getPower(void)
{
    float tmp;
    
    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
    tmp = output;
    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
    
    return tmp;
}

float
IRBehaviorController::getBrightness(void)
{
    float tmp;
    
    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
    tmp = brightness;
    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
    
    return tmp;
}

float
IRBehaviorController::getCenteroid(void)
{
    float tmp;
    
    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
    tmp = centeroid;
    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
    
    return tmp;
}

void
IRBehaviorController::dumpDebug(Serial *debug)
{
    if (debug != NULL)
    {
        debug->printf("IRController: Centeroid  = %3.2f\r\n", centeroid);
        debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
        debug->printf("IRController: Power      = %3.2f\r\n", output);
    }   
}

// Private methods

void
IRBehaviorController::scanIR(void)
{
    if (debug != NULL)
        debug->printf("IRController: Scanning IR.\r\n");
    
    ird.scan();
    
    centeroid  = ird.get_centeroid();
    brightness = ird.get_weighted_avg_brightness();
    
    if (debug != NULL)
    {
        debug->printf("IRController: Scan complete.\r\n");
        debug->printf("IRController: Centeroid  = %3.2f\r\n", centeroid);
        debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
    }   
}