Diff: main.cpp

Revision:

8:4e9ce846f7c3

diff -r b34924a05d60 -r 4e9ce846f7c3 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 01 15:44:59 2015 +0000
@@ -0,0 +1,228 @@
+/* log
+2/7/2015 2:18pm, Karl
+ - Pasted in http://developer.mbed.org/users/redxeth/code/TFC-TEST/file/6432166d0781/main.cpp
+   as directed in the video at https://community.freescale.com/videos/1591.
+ - Cleaned up code style for compactness and regularity.
+2/15/2015 4:00pm, Karl
+ - Researched serial terminal connection. This was helpful: http://developer.mbed.org/handbook/SerialPC
+2/17/2015 9:08am, Karl
+ - Played with different baud rates to improve frame rate. http://developer.mbed.org/forum/mbed/topic/893/
+2/17/2015 3:14pm. Corbin
+ - Added function test_garage back in to view labview graphing of the camera data.
+2/19/2015 6:34pm, Karl
+ - Moved the functions from control.cpp to main.cpp and deleted control.cpp and control.h.
+   This resolved errors of multiple definition. See http://suckless.org/style
+endlog */
+
+/* headers */
+#include "mbed.h"
+#include "TFC.h"
+#include <stdlib.h>
+
+/* macros */
+#define NUM_TFC_TICKERS 4
+#define IS 128
+
+/* types */
+enum State {STOP, GO, DONE, BRAKE};
+
+typedef struct {
+    uint32_t x;
+    int y;
+} Point;
+
+/* global variables */
+Serial PC(USBTX, USBRX);
+Ticker TFC_TickerObj;
+volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
+void TFC_TickerUpdate()
+{
+    for (int i = 0; i < NUM_TFC_TICKERS; i++)
+        if (TFC_Ticker[i]<0xFFFFFFFF) 
+            TFC_Ticker[i]++;
+}
+
+/* function declarations */
+void drive();
+void stop();
+void sortdesc(uint32_t x[], Point sx[]);
+int comPoint(void *a, void *b);
+void deriv2(volatile uint16_t x[], int ddx[]);
+int min(int a, int b);
+int max(int a, int b);
+
+/* function definitions */
+
+void drive(float s, float throt)
+{
+    float l, r;
+    TFC_HBRIDGE_ENABLE;
+    
+    // determine motor drive as a function of steering angle
+    /*
+    *     ____
+    *    /
+    *   /
+    *  /
+    */
+    l = (1 - 0*s*s) * throt * (1 - 1.6*s*(s>0));
+    r = (1 - 0*s*s) * throt * (1 + 1.6*s*(s<0));
+
+    TFC_SetMotorPWM(r, -l);
+    if (TFC_Ticker[2] >= 20) {
+        TFC_Ticker[2] = 0;
+        TFC_SetServo(0, s);
+    }   
+}
+
+
+/* Stop the car. */
+void stop()
+{
+    TFC_SetMotorPWM(0, 0);
+    TFC_HBRIDGE_DISABLE;
+}
+
+/* compare the height of two points */
+int comPoint(const void *a, const void *b)
+{
+    Point *A = (Point *)a;
+    Point *B = (Point *)b;
+    if (A->y < B->y)
+        return 1;
+    else if (A->y > B->y)
+        return -1;
+    else
+        return 0;
+}
+
+/* sort an array of ints into an array of points in decreasing order */
+void sortdesc(int x[], Point sx[])
+{
+    for (uint32_t i = 0; i < IS; i++) {
+        sx[i].y = x[i];
+        sx[i].x = i;
+    }
+    qsort(sx, IS, sizeof(sx[0]), comPoint);
+}
+
+int maxarray(int x[])
+{
+    int mem = 0; /* Typically this value would be zero */
+    for (int i = 0; i<IS; i++)
+        mem = max(x[i], mem);
+    return mem;
+}
+
+/* take a second derivative */
+void deriv2(volatile uint16_t x[], int sx[])
+{
+    int thres;
+    //PC.printf("clear\n");
+    for (int i = 2; i + 2 < IS; i++) {
+        sx[i] = x[i + 2] - 2 * x[i] + x[i - 2];
+        //PC.printf("%d %d\n", i, sx[i]);
+    }
+    thres = 1 * maxarray(sx) / 2; /* play with the coefficient to tune noise robustness */
+    for (int i = 1; i + 1 < IS; i++)
+        sx[i] = (sx[i] > thres && sx[i] > 700) ? 1 : 0;
+    //PC.printf("replot\n");
+}
+
+int min(int a, int b)
+{
+    return (a < b) ? a : b;
+}
+
+int max(int a, int b)
+{
+    return (a > b) ? a : b;
+}
+
+float sat(float x, float lim)
+{
+    if (x > lim)
+        return lim;
+    else if (x < -lim)
+        return -lim;
+    else
+        return x;
+}
+
+int main()
+{
+    float steer = 0;
+    int ddx[IS] = {0};
+    int edge[5];
+    float err = 0;
+    int nedges = 0;
+    PC.baud(115200);
+    TFC_TickerObj.attach_us(&TFC_TickerUpdate, 1000);
+    TFC_Init();
+    State s = STOP;
+    for (;;) {
+        switch (s) {
+
+        case STOP:
+            stop();
+            if (TFC_GetDIP_Switch() == 0)
+                s = GO;
+            TFC_SetBatteryLED(15);
+            break;
+        case GO:
+            if (TFC_Ticker[0] > 20 && TFC_LineScanImageReady > 0) {
+                TFC_Ticker[0] = 0;
+                TFC_LineScanImageReady = 0;
+                nedges = 0;
+                deriv2(TFC_LineScanImage0, ddx);
+                
+                for (int i = 21; i+20<IS && nedges < 4; i++) {
+                    //PC.printf("%c", ddx[i] == 0 ? ' ' : '|');
+                    if (ddx[i - 1] < ddx[i])
+                        edge[nedges++] = i;
+                }
+                
+                //PC.printf("\n");
+                TFC_SetBatteryLED(nedges);
+                if (nedges == 0) {
+                    err = 2;
+                } else if (nedges == 1 && edge[0] > 64) {
+                    err = edge[0] - 90;
+                    if(err > 0)
+                        err = 0;
+                } else if (nedges == 1 && edge[0] <= 64) {
+                    err = edge[0] - 37; 
+                    if(err < 0)
+                        err = 0;
+                } else if (nedges == 2) {
+                    err = (edge[0] + edge[1]) / 2.0 - 64;
+                } else if (nedges == 3 && edge[1] < 64) {
+                    err = (edge[1] + edge[2]) / 2.0 - 64;
+                } else if (nedges == 3 && edge[1] >= 64) {
+                    err = (edge[0] + edge[1]) / 2.0 - 64;
+                } else if (nedges == 4) {
+                    s = BRAKE;
+                }
+                steer = sat(0.50 * steer + err * 0.01 * (TFC_ReadPot(1) + 1), 0.44);
+                
+                drive(steer, 0.5 * (TFC_ReadPot(0) + 1));
+            }
+            if (TFC_GetDIP_Switch() != 0)
+                s = STOP;
+            break;
+        case BRAKE:
+            drive(0, -0.5);
+            wait_ms(400);
+            s = DONE;
+            break;
+        case DONE:
+            stop();
+            if (TFC_GetDIP_Switch() != 0)
+                s = STOP;
+            break;
+        default:
+            s = STOP;
+            break;
+        }
+    }
+}
\ No newline at end of file