File content as of revision 8:4e9ce846f7c3:

/* log
2/7/2015 2:18pm, Karl
 - Pasted in http://developer.mbed.org/users/redxeth/code/TFC-TEST/file/6432166d0781/main.cpp
   as directed in the video at https://community.freescale.com/videos/1591.
 - Cleaned up code style for compactness and regularity.
2/15/2015 4:00pm, Karl
 - Researched serial terminal connection. This was helpful: http://developer.mbed.org/handbook/SerialPC
2/17/2015 9:08am, Karl
 - Played with different baud rates to improve frame rate. http://developer.mbed.org/forum/mbed/topic/893/
2/17/2015 3:14pm. Corbin
 - Added function test_garage back in to view labview graphing of the camera data.
2/19/2015 6:34pm, Karl
 - Moved the functions from control.cpp to main.cpp and deleted control.cpp and control.h.
   This resolved errors of multiple definition. See http://suckless.org/style
endlog */

/* headers */
#include "mbed.h"
#include "TFC.h"
#include <stdlib.h>

/* macros */
#define NUM_TFC_TICKERS 4
#define IS 128

/* types */
enum State {STOP, GO, DONE, BRAKE};

typedef struct {
    uint32_t x;
    int y;
} Point;

/* global variables */
Serial PC(USBTX, USBRX);
Ticker TFC_TickerObj;
volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
void TFC_TickerUpdate()
{
    for (int i = 0; i < NUM_TFC_TICKERS; i++)
        if (TFC_Ticker[i]<0xFFFFFFFF) 
            TFC_Ticker[i]++;
}

/* function declarations */
void drive();
void stop();
void sortdesc(uint32_t x[], Point sx[]);
int comPoint(void *a, void *b);
void deriv2(volatile uint16_t x[], int ddx[]);
int min(int a, int b);
int max(int a, int b);

/* function definitions */

void drive(float s, float throt)
{
    float l, r;
    TFC_HBRIDGE_ENABLE;
    
    // determine motor drive as a function of steering angle
    /*
    *     ____
    *    /
    *   /
    *  /
    */
    l = (1 - 0*s*s) * throt * (1 - 1.6*s*(s>0));
    r = (1 - 0*s*s) * throt * (1 + 1.6*s*(s<0));

    TFC_SetMotorPWM(r, -l);
    if (TFC_Ticker[2] >= 20) {
        TFC_Ticker[2] = 0;
        TFC_SetServo(0, s);
    }   
}


/* Stop the car. */
void stop()
{
    TFC_SetMotorPWM(0, 0);
    TFC_HBRIDGE_DISABLE;
}

/* compare the height of two points */
int comPoint(const void *a, const void *b)
{
    Point *A = (Point *)a;
    Point *B = (Point *)b;
    if (A->y < B->y)
        return 1;
    else if (A->y > B->y)
        return -1;
    else
        return 0;
}

/* sort an array of ints into an array of points in decreasing order */
void sortdesc(int x[], Point sx[])
{
    for (uint32_t i = 0; i < IS; i++) {
        sx[i].y = x[i];
        sx[i].x = i;
    }
    qsort(sx, IS, sizeof(sx[0]), comPoint);
}

int maxarray(int x[])
{
    int mem = 0; /* Typically this value would be zero */
    for (int i = 0; i<IS; i++)
        mem = max(x[i], mem);
    return mem;
}

/* take a second derivative */
void deriv2(volatile uint16_t x[], int sx[])
{
    int thres;
    //PC.printf("clear\n");
    for (int i = 2; i + 2 < IS; i++) {
        sx[i] = x[i + 2] - 2 * x[i] + x[i - 2];
        //PC.printf("%d %d\n", i, sx[i]);
    }
    thres = 1 * maxarray(sx) / 2; /* play with the coefficient to tune noise robustness */
    for (int i = 1; i + 1 < IS; i++)
        sx[i] = (sx[i] > thres && sx[i] > 700) ? 1 : 0;
    //PC.printf("replot\n");
}

int min(int a, int b)
{
    return (a < b) ? a : b;
}

int max(int a, int b)
{
    return (a > b) ? a : b;
}

float sat(float x, float lim)
{
    if (x > lim)
        return lim;
    else if (x < -lim)
        return -lim;
    else
        return x;
}

int main()
{
    float steer = 0;
    int ddx[IS] = {0};
    int edge[5];
    float err = 0;
    int nedges = 0;
    PC.baud(115200);
    TFC_TickerObj.attach_us(&TFC_TickerUpdate, 1000);
    TFC_Init();
    State s = STOP;
    for (;;) {
        switch (s) {

        case STOP:
            stop();
            if (TFC_GetDIP_Switch() == 0)
                s = GO;
            TFC_SetBatteryLED(15);
            break;
        case GO:
            if (TFC_Ticker[0] > 20 && TFC_LineScanImageReady > 0) {
                TFC_Ticker[0] = 0;
                TFC_LineScanImageReady = 0;
                nedges = 0;
                deriv2(TFC_LineScanImage0, ddx);
                
                for (int i = 21; i+20<IS && nedges < 4; i++) {
                    //PC.printf("%c", ddx[i] == 0 ? ' ' : '|');
                    if (ddx[i - 1] < ddx[i])
                        edge[nedges++] = i;
                }
                
                //PC.printf("\n");
                TFC_SetBatteryLED(nedges);
                if (nedges == 0) {
                    err = 2;
                } else if (nedges == 1 && edge[0] > 64) {
                    err = edge[0] - 90;
                    if(err > 0)
                        err = 0;
                } else if (nedges == 1 && edge[0] <= 64) {
                    err = edge[0] - 37; 
                    if(err < 0)
                        err = 0;
                } else if (nedges == 2) {
                    err = (edge[0] + edge[1]) / 2.0 - 64;
                } else if (nedges == 3 && edge[1] < 64) {
                    err = (edge[1] + edge[2]) / 2.0 - 64;
                } else if (nedges == 3 && edge[1] >= 64) {
                    err = (edge[0] + edge[1]) / 2.0 - 64;
                } else if (nedges == 4) {
                    s = BRAKE;
                }
                steer = sat(0.50 * steer + err * 0.01 * (TFC_ReadPot(1) + 1), 0.44);
                
                drive(steer, 0.5 * (TFC_ReadPot(0) + 1));
            }
            if (TFC_GetDIP_Switch() != 0)
                s = STOP;
            break;
        case BRAKE:
            drive(0, -0.5);
            wait_ms(400);
            s = DONE;
            break;
        case DONE:
            stop();
            if (TFC_GetDIP_Switch() != 0)
                s = STOP;
            break;
        default:
            s = STOP;
            break;
        }
    }
}