File content as of revision 1:6dafc578e9f4:

#include "mbed.h"

DigitalOut led(LED_GREEN);
Serial bt(PTE0, PTE1);

//*****Line-Scan Camera******//
AnalogIn cameraIn(PTB3);
DigitalOut si(PTA12);
PwmOut clk(PTD4);
InterruptIn edgeDetector(PTD5);
Ticker cameraDaq;

int sensorVal;
int dataCount;
float pixArray[128];
int camMax;
bool rejectReading;

void cameraInit()
{
    clk.period(5e-5);
    clk.write(0.5f);
}

void getValue(){
    if(dataCount == 0){
        si.write(0);
    }
    if(dataCount < 128){
        pixArray[dataCount] = cameraIn.read();
        dataCount++;
    }
    else{
        //end aquisition cycle
        edgeDetector.rise(NULL);
    }
}    

void beginAcq(){
    si.write(1);
    edgeDetector.fall(NULL);
    edgeDetector.rise(&getValue);  
}

void acquire()
{
    dataCount = 0;
    edgeDetector.fall(&beginAcq);  
    for(int i = 9; i < 118;i++){
        if(pixArray[i] > pixArray[camMax]){
            camMax = i;
        }
    }     
}

//*****Controller******//
//#define SCALAR 0.53f
//#define MINM 0.0f
//#define MAXM 0.53f
//#define KPM 0.15f //0.1414f
//#define KI 19.7408f
//
//#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
//PwmOut servoSig(PTA13);
//Ticker control;
//Timer ctrlTimer;
//float fbPrev = 0.0f;
//
//AnalogIn _right(PTB1);
//AnalogIn _left(PTB3);
//AnalogIn speed(PTB2);
//PwmOut gateDrive(PTA4);
//DigitalOut brake(PTD0);
//
//float Setpoint = 0.08f;
//float spHolder;
//float errSum = 0.0;
//float Q[2] = {0.5,0.5};
//bool offTrackState = 0;
//bool waitState = 1;
//
//void offTrack(){
//    
//    spHolder = Setpoint;
//    Setpoint = 0;
//    applyBrake();
//
//}
//
//void wait(){
//    while(waitState){
//        
//        }
//    
//}
//    
//
//inline void applyBrake()
//{  
//    //spHolder = Setpoint;
////    Setpoint = 0.0;
//    gateDrive.write(0);
//    brake.write(1); 
//}
//
//inline void releaseBrake()
//{
//    brake.write(0);
//    //Setpoint = spHolder;
//    gateDrive.write();
//}
//
//void statePriori(){
//    f
//
//void controller()
//{
//    float L = _left.read();
//    float R = _right.read();
//    if (L == 0.0 && R == 0.0 && !offTrackState)
//    {
//        offTrack();
//        
//    }
//    else{
//        releaseBrake();
//    }
//    //if(rejectCamera)
////    {
////        Q[0] = 0.95;
////        Q[1] = 0.05;
////    }
//               
//    float fb = Q[0]*(camMax-64)/64.0 + Q[1]*(L-R);  
//    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;
//    
//    float error = Setpoint-speed.read();
//    errSum +=(error * TI);
//    float iTerm = KI*errSum;
//    if(iTerm > MAXM) iTerm = MAXM;
//    if(iTerm < MINM) iTerm = MINM; 
//    float output = KPM*error + iTerm;
//    if(output > MAXM) output = MAXM;
//    if(output < MINM) output = MINM;        
//
//    gateDrive.write(output);
//}
//
////****User input****//
//void display()
//{
//    bt.printf("Setpoint = %f, Brake = %f\r\n", Setpoint, brake.read());
//}
//
//void serCb()
//{
//    char x = bt.getc();
//    if (x == 'u')
//    {
//        Setpoint += 0.025;
//        display();
//    }
//    else if(x == 'h')
//    {
//        Setpoint -= 0.025;
//        display();
//    }
//    else if (x == 'b')
//    {
//        applyBrake();
//        display();
//       
//    }
//    else if (x == 'n')
//    {
//        releaseBrake();
//        display();
//    }
//    else if (x == 'p')
//    {
//        display();
//    }
//    else
//    {
//        bt.printf("Invalid input");
//    }
//    if(Setpoint > MAXM) Setpoint = MAXM;
//    if(Setpoint < MINM) Setpoint = MINM;    
//}

int main() {  
    bt.baud(115200);
//    bt.attach(&serCb);
    cameraInit();
    cameraDaq.attach(&acquire, 0.01f);
//    ctrlTimer.start();
//    control.attach(&controller, TI);

    while(1)
    {
        led = !led;
        bt.printf("%i\r\n", camMax-64);   
    }
}