Revision 1:6dafc578e9f4, committed 2018-04-21

Comitter:

KDrainEE

Date:

Sat Apr 21 21:19:36 2018 +0000

Parent:

0:ff18c8dc7227

Commit message:

line scan camera implementation with pwm signal for clock

Changed in this revision

--- a/main.cpp	Wed Apr 18 20:37:15 2018 +0000
+++ b/main.cpp	Sat Apr 21 21:19:36 2018 +0000
@@ -1,62 +1,222 @@
 #include "mbed.h"
 
-
-AnalogIn cameraIn1(PTB0);
-DigitalOut si(PTA12);
-DigitalOut clk(PTD4);
+DigitalOut led(LED_GREEN);
 Serial bt(PTE0, PTE1);
 
-int sensorVal = 0;
+//*****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);
+    }
+}    
 
-float PixArray[128];
+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() {
-    
+int main() {  
     bt.baud(115200);
-    
-//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 Initialization
-
+//    bt.attach(&serCb);
+    cameraInit();
+    cameraDaq.attach(&acquire, 0.01f);
+//    ctrlTimer.start();
+//    control.attach(&controller, TI);
 
     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);   
-        }
-        
-        
-        for(int i = 0; i < 128; i++)
-        {
-            bt.printf("%f,", PixArray[i]);
-        }
-        bt.printf("\r\n");
-        
-        
+        led = !led;
+        bt.printf("%i\r\n", camMax-64);   
     }
 }