File content as of revision 4:369caebdf5dc:

#include "mbed.h"
#include "m3pi.h"
#include "MODSERIAL.h"

#include "tlc5916.h"
#include "IRObjDetector.h"
#include "IRBehaviorController.h"
#include "Beacon.h"
#include "PID.h"

DigitalOut led1(LED1);
DigitalOut led2(LED2);

//Serial          blueTooth(p13, p14);
MODSERIAL       blueTooth(p13, p14); 
Ticker          taskTimer;
m3pi            base;

// PID terms
#define P_TERM    0.10f
#define I_TERM    0.00f
#define D_TERM    0.08f
#define THRESHOLD 0.20f

//PID                  pid(P_TERM, I_TERM, D_TERM, &blueTooth);
PID                  pid(P_TERM, I_TERM, D_TERM);
//IRObjDetector        ird(&blueTooth);
//IRBehaviorController irController(&pid, &blueTooth);
IRBehaviorController irController(&pid);
Beacon               beacon;


#define USR_POWERDOWN    (0x104)
int semihost_powerdown()
{
    uint32_t arg;
    return __semihost(USR_POWERDOWN, &arg);
}


void 
taskFunc(void)
{
//    blueTooth.printf("\e[1J");      // Clear terminal screen
//    blueTooth.printf("\e[H");       // Home terminal screen
//    blueTooth.printf("Task exec.\r\n");
    //irController.runSeeking();
    irController.runAvoidance();
    led1 = !led1;
}

void
findBeacon(void)
{
    uint8_t check[8]  = {0, 0, 0, 0, 0, 0, 0, 0 };
    uint8_t done      = 0;
    uint8_t threshold = 90;
    
    base.left_motor(0.2);
    base.right_motor(-0.2);

    // Search for the strongest signal, hopefully this will avoid reflections.  I need to add some delays in here to fix things a bit.
    while (!done)
    {
        int i;
        // Get 8 readings and put them in an array
        for (i = 0; i < 8; i++)
        {
            uint8_t tmp;
            beacon.scan();
        
            tmp = beacon.get_max_rssi();
            //blueTooth.printf("\r\nBeacon scan %d: rssi = 0x%2.2x\r\n", i, tmp);    
            check[i] = tmp;
            wait(0.1);
        }
        
        // Check all readings against threshold, stop turning and exit search loop when all readings are above threshold.
        if ((check[0] > threshold) && (check[1] > threshold) && (check[2] > threshold) && (check[3] > threshold) && (check[4] > threshold))
        {
            base.left_motor(0);
            base.right_motor(0);
            
            done = 1;
        }
    }
}

// PID terms
#define BEACON_P_TERM 1.0f
#define BEACON_I_TERM 0.0f
#define BEACON_D_TERM 0.0f
#define GO_SPEED 0.5f

PID beaconPID(BEACON_P_TERM, BEACON_I_TERM, BEACON_D_TERM);

int 
main() 
{
    float power  = 0.00f;
    float bpower = 0.00f;
    float ipower = 0.00f;
    float speed  = GO_SPEED;
    float max    = 1.00f;
    float right, left;
   
    float centeroid;
            
    uint16_t rssi      = 0x00;
    uint16_t prev_rssi = 0x00;
    

    
    //FunctionPointer ird_scan(&ird, &IRObjDetector::scan);
    
    wait(1);    
    base.reset();
    base.locate(0, 0);
    base.printf("IR Track");
    wait(4);

    //semihost_powerdown(); // Disable the secondary controller, this reduces both power and noise.
        
    blueTooth.baud(115200);
    blueTooth.printf("\r\nSystem start!\r\n");
 
    irController.setActiveThreshold(THRESHOLD);
    
    //beacon.calibrate(&blueTooth);
    
    //taskTimer.attach(&irController, &IRBehaviorController::runSeeking, 0.1);
    taskTimer.attach(&taskFunc, 0.10);
    
    blueTooth.printf("Find Beacon\r\n");
    //findBeacon();    
    blueTooth.printf("Beacon Found!\r\n");
    wait(1);
    blueTooth.printf("Start Main Loop.\r\n");        
    while(1)
    {
        uint16_t rssiL;
        uint16_t rssiR;
        float    irBrightness;
        float    irCenteroid;
        
        blueTooth.printf("\e[1J");      // Clear terminal screen
        blueTooth.printf("\e[H");       // Home terminal screen
        blueTooth.printf("Main Loop Start.\r\n");
        //blueTooth.printf("MAIN: Battery Voltage:\t%1.2f\r\n", base.battery());    
        
        //pid.dumpDebug(&blueTooth);
        //irController.dumpDebug(&blueTooth);
        
        blueTooth.printf("Getting power brightness and centeroid from IR.\r\n");
        ipower       = irController.getPower();
        irBrightness = irController.getBrightness();
        irCenteroid  = irController.getCenteroid();
        blueTooth.printf("IR:     power     = %3.2f\r\n", ipower);
        blueTooth.printf("IR:     brightness= %3.2f\r\n", irBrightness);
        blueTooth.printf("IR:     centeroid = %3.2f\r\n", irCenteroid);
        //ipower    = 0; // Temp disable.
        ipower = ipower * irBrightness; // We want more avoidance for closer objects
        //ipower = -ipower;
        
        beacon.scan();
        
        //NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
        centeroid = beacon.get_centeroid();
        rssi      = beacon.get_max_rssi();
        rssiR     = beacon.getR();
        rssiL     = beacon.getL();
        //NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
        
        bpower    = beaconPID.run(centeroid);
        //bpower = 0;
        
        blueTooth.printf("Beacon: centeroid = %3.2f\r\n", centeroid);
        blueTooth.printf("Beacon: power     = %3.2f\r\n", bpower);
        blueTooth.printf("Beacon: rssiL     = %5.5d\r\n", rssiL);
        blueTooth.printf("Beacon: rssiR     = %5.5d\r\n", rssiR);
        blueTooth.printf("Beacon: rssiD     = %5.5d\r\n", rssiR - rssiL);
        blueTooth.printf("Beacon: rssiM     = %5.5d\r\n", rssi);
                
        prev_rssi = rssi;

        if (ipower != 0)
            power = ipower;
        else
            power = bpower;
            
        if (rssi > 180)
        {
            speed = 0;
        }
        else
            speed = GO_SPEED;
                        
        right = speed - power;
        left  = speed + power;
        
        if (right < -max)
            right = -max;
        if (right > max)
            right = max;
            
        if (left < -max)
            left = -max;
        if (left > max)
            left = max;
        
        blueTooth.printf("MAIN: power = %3.2f\r\n", power);
        blueTooth.printf("MAIN: speed = %3.2f\r\n", speed);
        blueTooth.printf("MAIN: left  = %3.2f\r\n", left);
        blueTooth.printf("MAIN: right = %3.2f\r\n", right);
        
        base.left_motor(left);
        base.right_motor(right);
        
        blueTooth.printf("Main Loop End.\r\n");
        wait(0.25);
    };
    
//    testIRObjDet();
            
}