File content as of revision 0:aebd709fc6bb:

#include "mbed.h"
#include "TFC.h"

//Serial pc(USBTX, USBRX);

float servoTrim();
float motorTrim();
float avgPos();
int readCamCenter();

int cam[122],
    peaks[2];

int filter[]={-1,-1,-1,6,-1,-1,-1}; //size must not change

float servBuf[100] = {0},
      weight[100];

int main(){
    float pos = 0,
          speed = 0;
          
    int prevState1 = 0,
        prevState2 = 0;
        
        
    weight[0] = 1;
    for(int i = 1; i < 100; i++){
        weight[i] = weight[i-1] * 0.95;
    }

    //pc.baud(115200);
    TFC_Init();
    
    while(TFC_ReadPushButton(0) == 0){
    }
    
    wait(3);
    
    TFC_HBRIDGE_ENABLE;
    
    while(1){        
        if(TFC_ReadPushButton(0) != 0 && prevState1 != TFC_ReadPushButton(0)){
            speed -= 0.05;
        }
        else if(TFC_ReadPushButton(1) != 0 && prevState2 != TFC_ReadPushButton(1)){
            speed += 0.05;
        }
        
        prevState1 = TFC_ReadPushButton(0);
        prevState2 = TFC_ReadPushButton(1);
        
        TFC_SetMotorPWM(speed, speed);
            
        pos = float(readCamCenter() - 61) / 61;
    
        pos *= (TFC_ReadPot(1) + 1) / 2;
        
        pos += TFC_ReadPot(0);
        
        if((TFC_ServoTicker % 5) == 0){
            for(int i = 99; i > 0; i--){
                servBuf[i] = servBuf[i-1];
            }
            servBuf[0] = pos;
        }
    
        TFC_SetServo(0, avgPos());
    }
}

float servoTrim(){
    return TFC_ReadPot(0);
}

float motorTrim(){
    return TFC_ReadPot(1);
}

int readCamCenter(){
    int temp;
    int peaksval[] ={0, 0};
    
    if(TFC_LineScanImageReady != 0){
        
        TFC_LineScanImageReady = 0;
        for(int i = 0; i < 122; i++){
            temp = 0;
            for(int h = 0; h < 7; h++){
                temp = temp + (TFC_LineScanImage0[i+h] * filter[h]);
            }
            cam[i] = temp;
        }
    }

    temp = 0;
    
    for(int i = 0; i < 2; i++){
        peaksval[i] = 0;
    }
    
    for(int i = 0; i < 122; i++){
        if(cam[i] > peaksval[0]){
            peaksval[0] = cam[i];
            peaks[0] = i;
        }
    }
    
    for(int i = 0; i < 122; i++){
        if(cam[i] > peaksval[1] && peaks[0] != i){
            peaksval[1] = cam[i];
            peaks[1] = i;
        }
    }

    temp = ((peaks[0]+1)+(peaks[1]+1))/2;
    
    return  temp;
}

float avgPos(){
    float temp = 0;
    
    for(int i = 0; i < 100; i++){
        temp += servBuf[i] * weight[i];
    }
    
    return (temp / 19.8816);
}