File content as of revision 0:2b269b9f417b:

#include "mbed.h"
#include <iostream>

#define KPS 2.0E-2f
#define KD 1.0e-4f
#define TI 0.001f
#define SET 0.0f
#define MINS 0.05f
#define MAXS 0.1f
#define BIAS 0.075f
#define TOL 0.02f
#define STEER_FREQ 0.02f //50 Hz
#define STEERUPDATEPERIOD 0.05

AnalogIn cameraIn1(PTB0);
DigitalOut si(PTA12);
DigitalOut clk(PTD4);

int sensorVal = 0;
float PixArray[128];
int maxVal = 9;

PwmOut servoSig(PTD2); //PWM output to control servo
DigitalOut myled(LED_BLUE);
Ticker control;
Timer ctrlTimer;

float data[4];

float fbPrev = 0.0f;
Serial bt(PTE0, PTE1); //COM12

void steer()
{
               
    float fb = ((float)maxVal)/64.0;  
    float e = SET - fb;
    float Controlleroutput = KPS * e - (KD * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
    if (Controlleroutput > MAXS) Controlleroutput = MAXS;
    else if (Controlleroutput < MINS) Controlleroutput = MINS;
    if (abs(Controlleroutput - servoSig.read()) > TOL || ctrlTimer.read() >= STEERUPDATEPERIOD) 
    {
        ctrlTimer.reset();
        servoSig.write(Controlleroutput);
    }
    fbPrev = fb;
    data[2] = Controlleroutput;
    data[3] = e;
}

int main()
{   
    //startup checks
    servoSig.period(STEER_FREQ);
    servoSig.write(0.05);
    wait(0.5);
    servoSig.write(0.1);
    wait(0.5);
    servoSig.write(0.075);
    wait(0.5);
    
    //Line-Scan camera
    //Initialize -- Clocks out indeterminate pixel data from power up
    si.write(1);
    clk.write(1);
    si.write(0);
    clk.write(0);
    
    for (int i = 0; i< 128; i++)
    {
        clk.write(1);
        clk.write(0);    
    }
//End Initializatio

    ctrlTimer.start();
    control.attach(&steer, TI);
    bt.baud(115200);
    while(1) {
        sensorVal = cameraIn1.read(); 
        
        //Start pixel count
        si.write(1);
        clk.write(1);
        si.write(0);
        clk.write(0);
        
        //Pixel Count and read
        for (int i = 0; i <128; i++)
        {
            wait_us(50); //saturation time
            PixArray[i] = cameraIn1.read();
            clk.write(1);
            clk.write(0);   
        }

        maxVal = 9;
        for(int i = 10; i < 118; i++){
            if(PixArray[i] > PixArray[maxVal]){
                maxVal = i;
            }
        }
        maxVal = maxVal- 64;
        myled = !myled;
        bt.printf("%f, ",data[0]);
        bt.printf("%f, ", data[1]);
        bt.printf("%f, ", data[2]);
        bt.printf("%f\r\n", data[3]);
    }
}