Diff: main.cpp

Revision:

0:2b269b9f417b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 18 22:14:31 2018 +0000
@@ -0,0 +1,110 @@
+#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]);
+    }
+}