File content as of revision 3:c21068661635:

#include "mbed.h"
#include "TFC.h"
#include "MotorControl.h"
#include "Vision.h"
#include "MMA8451Q.h"
#include "PIDcontroller.h"

Serial pc(USBTX,USBRX);
Serial debug(PTD3,PTD2);
Ticker convTicker,batLevelTicker;
Timeout camExpoTimeout;
Timer t;
Ticker test;
Motors motors;
Vision vision;
PID steeringPID;
PID speedPID;

uint32_t convCounter = 0;

float getSteeringValue(float pixelIndex)
{
    // return steering angle to send to servo function of pixel target
    return ((-5e-6*pixelIndex+0.0009)*pixelIndex-0.0248)*pixelIndex-0.9129;
    //return ((-5e-6*pixelIndex+0.0009)*pixelIndex-0.0224)*pixelIndex-0.9218;
}

float flt[4]={0,0,0,0};
float steeringFilter(float x)
{
    return 2.0*x;
    
/*  uint8_t i;
    
    for(i=3;i>0;i--)
    {
        flt[i] = flt[i-1];
    }
    flt[0] = x;
    
    return (flt[0]+flt[1]*0.75+flt[2]*0.5+flt[3]*0.25)/2.5;*/
}

void applySteeringCommand(float value)
{
    float clamp = 0.55;
    if(value > clamp)
        value = clamp;
    if(value < -clamp)
        value = -clamp;
    TFC_SetServo(0,value);
}

void stopCameraExposure()
{
}

void dataConvertionRoutine()
{
    uint8_t mask;
    
    // convertion of motor current is done at each tick
    mask = ADC_MASK_CAPTURE_MOTOR_CURRENT;
    
    // convertion of line scan is done each 5 ticks
    if((convCounter % 5) == 0)
        mask |= ADC_MASK_CAPTURE_LINE_SCAN;
    
    // convertion of pot and battery is done each 5 ticks
    if((convCounter % 5) == 2)
        mask |= ADC_MASK_CAPTURE_POT_0 | ADC_MASK_CAPTURE_POT_1 | ADC_MASK_CAPTURE_BATTERY_LEVEL;

    TFC_StartDataAcquisition(mask);
    
    convCounter++;
    if(convCounter >= 4000000000)
        convCounter = 0;
    
    //TFC_StartDataAcquisition(ADC_MASK_CAPTURE_ALL);
    //camExpoTimeout.attach(&stopCameraExposure,camExposure);
}

void updateBatteryLevel()
{
    float batteryVoltage;
    batteryVoltage = motors.getAverageBatteryVoltage();
    TFC_SetBatteryLED_Level(batteryVoltage*3.3-18.8);
}

void saveADCMeas()
{
    //TFC_BAT_LED2_TOGGLE;
    if(TFC_MotorCurrentReady)
    {
        TFC_MotorCurrentReady = 0;
        motors.saveMotorCurrentMeasure(TFC_ReadMotACurrentRaw(),TFC_ReadMotBCurrentRaw()); 
    }
    if(TFC_LineScanImageReady)
    {
        TFC_LineScanImageReady = 0; // clear counter to be able to detect next frame
        vision.saveData((uint16_t*)TFC_LineScanImage0);
    }
    if(TFC_BatteryVoltageReady)
    {
        TFC_BatteryVoltageReady = 0;
        motors.saveBatteryVoltageMeasure(TFC_ReadBatteryVoltageRaw());
    }
}

void printCarSpeed(Serial& output, Motors motors, uint32_t index)
{
    output.printf("%d %f %f\r\n",index,motors.getCarSpeed(),motors.getSpeedError());
}

void printAll(Serial& output, Motors motors, float PWM, float steering, uint32_t index)
{
    output.printf("%d %f %f %f\n",index,motors.getCarSpeed(), PWM ,steering);
}

void printEdges(Serial& output, struct lineScanData lineData)
{
    uint8_t i;
    
    output.printf("[");
    for(i=0;i<lineData.leftEdgeIndex;i++) output.printf(" ");
    output.printf("^");
    for(i=lineData.leftEdgeIndex;i<lineData.mostWhiteIndex;i++) output.printf(" ");
    output.printf("|");
    for(i=lineData.mostWhiteIndex;i<lineData.rightEdgeIndex;i++) output.printf(" ");
    output.printf("^");
    for(i=lineData.rightEdgeIndex;i<128;i++) output.printf(" ");
    output.printf("]\r\n");
}

void printLine(Serial& output)
{
    uint8_t i;
    
    for(i=0;i<128;i++)
    {
        output.printf("%d ",TFC_LineScanImage0[i]);
    }
    output.printf("\r\n");
}

bool dir = false;
void invertMot()
{
    if(dir)
        motors.setFixedPWMValue(1.0);
    else
        motors.setFixedPWMValue(-1.0);
    dir = !dir;
}

int main()
{
    uint8_t r,l;
    float motorPWM,steering,steeringFiltered;
    float pixelTarget;
    struct lineScanData lineData;
    uint32_t lastcount = 0;
    bool debugon = false;
    
    //MMA8451Q acc(PTE25, PTE24, (0x1d<<1));
    
    TFC_Init();
    
    pc.baud(115200);
    debug.baud(115200);
    
//    test.attach(&invertMot,2);
    
    // run convertion routine each 2ms
    convTicker.attach_us(&dataConvertionRoutine, 2000);
    // record data when convertion done
    TFC_ConvertionReadyCallback.attach(&saveADCMeas);
    
    batLevelTicker.attach(&updateBatteryLevel,0.2);
    
    motors.setFixedPWMMode();
    motors.setSpeedTargetValue(2.0);
    motors.setFixedPWMValue(0.45);
    
    steeringPID.setReference(0);
    steeringPID.setCoefficients(1.2,0,0);
    steeringPID.setOutputLimits(-0.6,0.6);
    
    speedPID.setReference(1.0);
    speedPID.setCoefficients(0.6,3,0);
    speedPID.setOutputLimits(-1.0,1.0);

    while(1)
    {
        if(TFC_ReadPushButton(0))
        {
            motors.stop();
            debugon = false;
        }
        
        if(TFC_ReadPushButton(1))
        {
            motors.start();
            debugon = true;
        }
        
        vision.processTasks();
        lineData = vision.getRoadData();
        r = lineData.rightEdgeIndex;
        //if(r>=115)
        //    r = 128;
        l = lineData.leftEdgeIndex;
        //if(l<=13)
        //    l = 0;
        pixelTarget = (r+l)/2.0;

        steering = getSteeringValue(pixelTarget);
        
        // previous
        //applySteeringCommand(steeringFilter(steering));
        
        
        // new
        steeringFiltered = steeringPID.processNewValue(-steering);
        applySteeringCommand(steeringFiltered);
        
        // drive proportional to steering
        motorPWM = 0.5-abs(steeringFiltered)/2.0;
        motors.setFixedPWMValue(motorPWM);
        
        // speed is regulated
        //speedPID.setReference(1.8-abs(steeringFiltered)*2.5); // set speed to 2m/s max and decrease to 0.5m/s if steering is at max
        //motorPWM = speedPID.processNewValue(motors.getCarSpeed());
        //motors.setFixedPWMValue(motorPWM);

        motors.processTasks();
        //motorPWM = TFC_ReadPot(0);
        //motorPWM = (TFC_GetDIP_Switch()*0.05+0.25);
        //motors.setFixedPWMValue(motorPWM);
        
        /*if(TFC_LineScanImageReady)
        {
            TFC_LineScanImageReady = 0; // clear counter to be able to detect next frame

            // calculation time measure
            t.reset();
            t.start();
            
            // stop calculation time measurement
            t.stop();
            
            //pc.printf("Bat: %fV, PWM:%f, MA:%frps, MB:%frps\r",motors.getAverageBatteryVoltage(),motorPWM,motors.getWheelRPS('A'),motors.getWheelRPS('B'));
            //pc.printf("Bat: %fV, PWM:%f, MA:%fV, MB:%fV\r",motors.getAverageBatteryVoltage(),motorPWM,motors.getAverageMotCurrent('A'),motors.getAverageMotCurrent('B'));
            //pc.printf("Bat: %fV, PWM:%f, MotA:%f, MotB:%f, speedA:%f, speedB:%f\r",TFC_ReadBatteryVoltage(),motorPWM,TFC_ReadMotACurrent(),TFC_ReadMotBCurrent(),getMotSpeed('A',motorPWM),getMotSpeed('B',motorPWM));
            //pc.printf("Bat: %fV, d=%f, time:%dus max light:%d @ %d    \r",TFC_ReadBatteryVoltage(),steering,t.read_us(),lineData.maxLightValue,lineData.mostWhiteIndex);
        }*/
        
        if((convCounter % 100) == 0) // each 0.2 second
        {
            //printEdges(pc);
            //printLine(pc);
            
            //pc.printf("%d %d => %f\r\n",l,r,steering);
            //pc.printf("%f %f\r",motors.getAverageMotCurrent('A'),motors.getAverageMotCurrent('B'));
        }
        
        //pc.printf("%d %f %f %f    \r",lastcount,acc.getAccX(),acc.getAccY(),acc.getAccZ());
        if(debugon && (convCounter != lastcount))
        {
            lastcount = convCounter;
            //printAll(debug, motors, motorPWM, steeringFiltered, lastcount);
            //printCarSpeed(debug, motors, lastcount);
            //pc.printf("%d %f %f %f\n",lastcount,motors.getMotPWM('A'),motors.getMotCurrent('A'),motors.getWheelSpeed('A'));
            //pc.printf("%f\r",speedPID.getIntegralTerm());
            //debug.printf("%d %f %f %f\n",lastcount,motors.getMotCurrent('A'),motors.getMotCurrent('B'),motors.getCarSpeed(),);
            //debug.printf("%d %f %f %f\n",lastcount,motors.getMotPWM('A'),motors.getMotPWM('B'),steering);
            //debug.printf("%d %f %f %f\n",lastcount,acc.getAccX(),acc.getAccY(),steering);
        }
    }
}