Karl Schultheisz / Mbed 2 deprecated FRDM-TFC_TEST

Our version of TFC code.

Dependencies:   FRDM-TFC mbed

Fork of FRDM-TFC by Eli Hughes

main.cpp@8:4e9ce846f7c3, 2015-05-01 (annotated)

Committer:
kdsch
Date:
Fri May 01 15:44:59 2015 +0000
Revision:
8:4e9ce846f7c3
freescale cup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kdsch 8:4e9ce846f7c3 1 /* log
kdsch 8:4e9ce846f7c3 2 2/7/2015 2:18pm, Karl
kdsch 8:4e9ce846f7c3 3 - Pasted in http://developer.mbed.org/users/redxeth/code/TFC-TEST/file/6432166d0781/main.cpp
kdsch 8:4e9ce846f7c3 4 as directed in the video at https://community.freescale.com/videos/1591.
kdsch 8:4e9ce846f7c3 5 - Cleaned up code style for compactness and regularity.
kdsch 8:4e9ce846f7c3 6 2/15/2015 4:00pm, Karl
kdsch 8:4e9ce846f7c3 7 - Researched serial terminal connection. This was helpful: http://developer.mbed.org/handbook/SerialPC
kdsch 8:4e9ce846f7c3 8 2/17/2015 9:08am, Karl
kdsch 8:4e9ce846f7c3 9 - Played with different baud rates to improve frame rate. http://developer.mbed.org/forum/mbed/topic/893/
kdsch 8:4e9ce846f7c3 10 2/17/2015 3:14pm. Corbin
kdsch 8:4e9ce846f7c3 11 - Added function test_garage back in to view labview graphing of the camera data.
kdsch 8:4e9ce846f7c3 12 2/19/2015 6:34pm, Karl
kdsch 8:4e9ce846f7c3 13 - Moved the functions from control.cpp to main.cpp and deleted control.cpp and control.h.
kdsch 8:4e9ce846f7c3 14 This resolved errors of multiple definition. See http://suckless.org/style
kdsch 8:4e9ce846f7c3 15 endlog */
kdsch 8:4e9ce846f7c3 16
kdsch 8:4e9ce846f7c3 17 /* headers */
kdsch 8:4e9ce846f7c3 18 #include "mbed.h"
kdsch 8:4e9ce846f7c3 19 #include "TFC.h"
kdsch 8:4e9ce846f7c3 20 #include <stdlib.h>
kdsch 8:4e9ce846f7c3 21
kdsch 8:4e9ce846f7c3 22 /* macros */
kdsch 8:4e9ce846f7c3 23 #define NUM_TFC_TICKERS 4
kdsch 8:4e9ce846f7c3 24 #define IS 128
kdsch 8:4e9ce846f7c3 25
kdsch 8:4e9ce846f7c3 26 /* types */
kdsch 8:4e9ce846f7c3 27 enum State {STOP, GO, DONE, BRAKE};
kdsch 8:4e9ce846f7c3 28
kdsch 8:4e9ce846f7c3 29 typedef struct {
kdsch 8:4e9ce846f7c3 30 uint32_t x;
kdsch 8:4e9ce846f7c3 31 int y;
kdsch 8:4e9ce846f7c3 32 } Point;
kdsch 8:4e9ce846f7c3 33
kdsch 8:4e9ce846f7c3 34 /* global variables */
kdsch 8:4e9ce846f7c3 35 Serial PC(USBTX, USBRX);
kdsch 8:4e9ce846f7c3 36 Ticker TFC_TickerObj;
kdsch 8:4e9ce846f7c3 37 volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
kdsch 8:4e9ce846f7c3 38 void TFC_TickerUpdate()
kdsch 8:4e9ce846f7c3 39 {
kdsch 8:4e9ce846f7c3 40 for (int i = 0; i < NUM_TFC_TICKERS; i++)
kdsch 8:4e9ce846f7c3 41 if (TFC_Ticker[i]<0xFFFFFFFF)
kdsch 8:4e9ce846f7c3 42 TFC_Ticker[i]++;
kdsch 8:4e9ce846f7c3 43 }
kdsch 8:4e9ce846f7c3 44
kdsch 8:4e9ce846f7c3 45 /* function declarations */
kdsch 8:4e9ce846f7c3 46 void drive();
kdsch 8:4e9ce846f7c3 47 void stop();
kdsch 8:4e9ce846f7c3 48 void sortdesc(uint32_t x[], Point sx[]);
kdsch 8:4e9ce846f7c3 49 int comPoint(void *a, void *b);
kdsch 8:4e9ce846f7c3 50 void deriv2(volatile uint16_t x[], int ddx[]);
kdsch 8:4e9ce846f7c3 51 int min(int a, int b);
kdsch 8:4e9ce846f7c3 52 int max(int a, int b);
kdsch 8:4e9ce846f7c3 53
kdsch 8:4e9ce846f7c3 54 /* function definitions */
kdsch 8:4e9ce846f7c3 55
kdsch 8:4e9ce846f7c3 56 void drive(float s, float throt)
kdsch 8:4e9ce846f7c3 57 {
kdsch 8:4e9ce846f7c3 58 float l, r;
kdsch 8:4e9ce846f7c3 59 TFC_HBRIDGE_ENABLE;
kdsch 8:4e9ce846f7c3 60
kdsch 8:4e9ce846f7c3 61 // determine motor drive as a function of steering angle
kdsch 8:4e9ce846f7c3 62 /*
kdsch 8:4e9ce846f7c3 63 * ____
kdsch 8:4e9ce846f7c3 64 * /
kdsch 8:4e9ce846f7c3 65 * /
kdsch 8:4e9ce846f7c3 66 * /
kdsch 8:4e9ce846f7c3 67 */
kdsch 8:4e9ce846f7c3 68 l = (1 - 0*s*s) * throt * (1 - 1.6*s*(s>0));
kdsch 8:4e9ce846f7c3 69 r = (1 - 0*s*s) * throt * (1 + 1.6*s*(s<0));
kdsch 8:4e9ce846f7c3 70
kdsch 8:4e9ce846f7c3 71 TFC_SetMotorPWM(r, -l);
kdsch 8:4e9ce846f7c3 72 if (TFC_Ticker[2] >= 20) {
kdsch 8:4e9ce846f7c3 73 TFC_Ticker[2] = 0;
kdsch 8:4e9ce846f7c3 74 TFC_SetServo(0, s);
kdsch 8:4e9ce846f7c3 75 }
kdsch 8:4e9ce846f7c3 76 }
kdsch 8:4e9ce846f7c3 77
kdsch 8:4e9ce846f7c3 78
kdsch 8:4e9ce846f7c3 79 /* Stop the car. */
kdsch 8:4e9ce846f7c3 80 void stop()
kdsch 8:4e9ce846f7c3 81 {
kdsch 8:4e9ce846f7c3 82 TFC_SetMotorPWM(0, 0);
kdsch 8:4e9ce846f7c3 83 TFC_HBRIDGE_DISABLE;
kdsch 8:4e9ce846f7c3 84 }
kdsch 8:4e9ce846f7c3 85
kdsch 8:4e9ce846f7c3 86 /* compare the height of two points */
kdsch 8:4e9ce846f7c3 87 int comPoint(const void *a, const void *b)
kdsch 8:4e9ce846f7c3 88 {
kdsch 8:4e9ce846f7c3 89 Point *A = (Point *)a;
kdsch 8:4e9ce846f7c3 90 Point *B = (Point *)b;
kdsch 8:4e9ce846f7c3 91 if (A->y < B->y)
kdsch 8:4e9ce846f7c3 92 return 1;
kdsch 8:4e9ce846f7c3 93 else if (A->y > B->y)
kdsch 8:4e9ce846f7c3 94 return -1;
kdsch 8:4e9ce846f7c3 95 else
kdsch 8:4e9ce846f7c3 96 return 0;
kdsch 8:4e9ce846f7c3 97 }
kdsch 8:4e9ce846f7c3 98
kdsch 8:4e9ce846f7c3 99 /* sort an array of ints into an array of points in decreasing order */
kdsch 8:4e9ce846f7c3 100 void sortdesc(int x[], Point sx[])
kdsch 8:4e9ce846f7c3 101 {
kdsch 8:4e9ce846f7c3 102 for (uint32_t i = 0; i < IS; i++) {
kdsch 8:4e9ce846f7c3 103 sx[i].y = x[i];
kdsch 8:4e9ce846f7c3 104 sx[i].x = i;
kdsch 8:4e9ce846f7c3 105 }
kdsch 8:4e9ce846f7c3 106 qsort(sx, IS, sizeof(sx[0]), comPoint);
kdsch 8:4e9ce846f7c3 107 }
kdsch 8:4e9ce846f7c3 108
kdsch 8:4e9ce846f7c3 109 int maxarray(int x[])
kdsch 8:4e9ce846f7c3 110 {
kdsch 8:4e9ce846f7c3 111 int mem = 0; /* Typically this value would be zero */
kdsch 8:4e9ce846f7c3 112 for (int i = 0; i<IS; i++)
kdsch 8:4e9ce846f7c3 113 mem = max(x[i], mem);
kdsch 8:4e9ce846f7c3 114 return mem;
kdsch 8:4e9ce846f7c3 115 }
kdsch 8:4e9ce846f7c3 116
kdsch 8:4e9ce846f7c3 117 /* take a second derivative */
kdsch 8:4e9ce846f7c3 118 void deriv2(volatile uint16_t x[], int sx[])
kdsch 8:4e9ce846f7c3 119 {
kdsch 8:4e9ce846f7c3 120 int thres;
kdsch 8:4e9ce846f7c3 121 //PC.printf("clear\n");
kdsch 8:4e9ce846f7c3 122 for (int i = 2; i + 2 < IS; i++) {
kdsch 8:4e9ce846f7c3 123 sx[i] = x[i + 2] - 2 * x[i] + x[i - 2];
kdsch 8:4e9ce846f7c3 124 //PC.printf("%d %d\n", i, sx[i]);
kdsch 8:4e9ce846f7c3 125 }
kdsch 8:4e9ce846f7c3 126 thres = 1 * maxarray(sx) / 2; /* play with the coefficient to tune noise robustness */
kdsch 8:4e9ce846f7c3 127 for (int i = 1; i + 1 < IS; i++)
kdsch 8:4e9ce846f7c3 128 sx[i] = (sx[i] > thres && sx[i] > 700) ? 1 : 0;
kdsch 8:4e9ce846f7c3 129 //PC.printf("replot\n");
kdsch 8:4e9ce846f7c3 130 }
kdsch 8:4e9ce846f7c3 131
kdsch 8:4e9ce846f7c3 132 int min(int a, int b)
kdsch 8:4e9ce846f7c3 133 {
kdsch 8:4e9ce846f7c3 134 return (a < b) ? a : b;
kdsch 8:4e9ce846f7c3 135 }
kdsch 8:4e9ce846f7c3 136
kdsch 8:4e9ce846f7c3 137 int max(int a, int b)
kdsch 8:4e9ce846f7c3 138 {
kdsch 8:4e9ce846f7c3 139 return (a > b) ? a : b;
kdsch 8:4e9ce846f7c3 140 }
kdsch 8:4e9ce846f7c3 141
kdsch 8:4e9ce846f7c3 142 float sat(float x, float lim)
kdsch 8:4e9ce846f7c3 143 {
kdsch 8:4e9ce846f7c3 144 if (x > lim)
kdsch 8:4e9ce846f7c3 145 return lim;
kdsch 8:4e9ce846f7c3 146 else if (x < -lim)
kdsch 8:4e9ce846f7c3 147 return -lim;
kdsch 8:4e9ce846f7c3 148 else
kdsch 8:4e9ce846f7c3 149 return x;
kdsch 8:4e9ce846f7c3 150 }
kdsch 8:4e9ce846f7c3 151
kdsch 8:4e9ce846f7c3 152 int main()
kdsch 8:4e9ce846f7c3 153 {
kdsch 8:4e9ce846f7c3 154 float steer = 0;
kdsch 8:4e9ce846f7c3 155 int ddx[IS] = {0};
kdsch 8:4e9ce846f7c3 156 int edge[5];
kdsch 8:4e9ce846f7c3 157 float err = 0;
kdsch 8:4e9ce846f7c3 158 int nedges = 0;
kdsch 8:4e9ce846f7c3 159 PC.baud(115200);
kdsch 8:4e9ce846f7c3 160 TFC_TickerObj.attach_us(&TFC_TickerUpdate, 1000);
kdsch 8:4e9ce846f7c3 161 TFC_Init();
kdsch 8:4e9ce846f7c3 162 State s = STOP;
kdsch 8:4e9ce846f7c3 163 for (;;) {
kdsch 8:4e9ce846f7c3 164 switch (s) {
kdsch 8:4e9ce846f7c3 165
kdsch 8:4e9ce846f7c3 166 case STOP:
kdsch 8:4e9ce846f7c3 167 stop();
kdsch 8:4e9ce846f7c3 168 if (TFC_GetDIP_Switch() == 0)
kdsch 8:4e9ce846f7c3 169 s = GO;
kdsch 8:4e9ce846f7c3 170 TFC_SetBatteryLED(15);
kdsch 8:4e9ce846f7c3 171 break;
kdsch 8:4e9ce846f7c3 172 case GO:
kdsch 8:4e9ce846f7c3 173 if (TFC_Ticker[0] > 20 && TFC_LineScanImageReady > 0) {
kdsch 8:4e9ce846f7c3 174 TFC_Ticker[0] = 0;
kdsch 8:4e9ce846f7c3 175 TFC_LineScanImageReady = 0;
kdsch 8:4e9ce846f7c3 176 nedges = 0;
kdsch 8:4e9ce846f7c3 177 deriv2(TFC_LineScanImage0, ddx);
kdsch 8:4e9ce846f7c3 178
kdsch 8:4e9ce846f7c3 179 for (int i = 21; i+20<IS && nedges < 4; i++) {
kdsch 8:4e9ce846f7c3 180 //PC.printf("%c", ddx[i] == 0 ? ' ' : '|');
kdsch 8:4e9ce846f7c3 181 if (ddx[i - 1] < ddx[i])
kdsch 8:4e9ce846f7c3 182 edge[nedges++] = i;
kdsch 8:4e9ce846f7c3 183 }
kdsch 8:4e9ce846f7c3 184
kdsch 8:4e9ce846f7c3 185 //PC.printf("\n");
kdsch 8:4e9ce846f7c3 186 TFC_SetBatteryLED(nedges);
kdsch 8:4e9ce846f7c3 187 if (nedges == 0) {
kdsch 8:4e9ce846f7c3 188 err = 2;
kdsch 8:4e9ce846f7c3 189 } else if (nedges == 1 && edge[0] > 64) {
kdsch 8:4e9ce846f7c3 190 err = edge[0] - 90;
kdsch 8:4e9ce846f7c3 191 if(err > 0)
kdsch 8:4e9ce846f7c3 192 err = 0;
kdsch 8:4e9ce846f7c3 193 } else if (nedges == 1 && edge[0] <= 64) {
kdsch 8:4e9ce846f7c3 194 err = edge[0] - 37;
kdsch 8:4e9ce846f7c3 195 if(err < 0)
kdsch 8:4e9ce846f7c3 196 err = 0;
kdsch 8:4e9ce846f7c3 197 } else if (nedges == 2) {
kdsch 8:4e9ce846f7c3 198 err = (edge[0] + edge[1]) / 2.0 - 64;
kdsch 8:4e9ce846f7c3 199 } else if (nedges == 3 && edge[1] < 64) {
kdsch 8:4e9ce846f7c3 200 err = (edge[1] + edge[2]) / 2.0 - 64;
kdsch 8:4e9ce846f7c3 201 } else if (nedges == 3 && edge[1] >= 64) {
kdsch 8:4e9ce846f7c3 202 err = (edge[0] + edge[1]) / 2.0 - 64;
kdsch 8:4e9ce846f7c3 203 } else if (nedges == 4) {
kdsch 8:4e9ce846f7c3 204 s = BRAKE;
kdsch 8:4e9ce846f7c3 205 }
kdsch 8:4e9ce846f7c3 206 steer = sat(0.50 * steer + err * 0.01 * (TFC_ReadPot(1) + 1), 0.44);
kdsch 8:4e9ce846f7c3 207
kdsch 8:4e9ce846f7c3 208 drive(steer, 0.5 * (TFC_ReadPot(0) + 1));
kdsch 8:4e9ce846f7c3 209 }
kdsch 8:4e9ce846f7c3 210 if (TFC_GetDIP_Switch() != 0)
kdsch 8:4e9ce846f7c3 211 s = STOP;
kdsch 8:4e9ce846f7c3 212 break;
kdsch 8:4e9ce846f7c3 213 case BRAKE:
kdsch 8:4e9ce846f7c3 214 drive(0, -0.5);
kdsch 8:4e9ce846f7c3 215 wait_ms(400);
kdsch 8:4e9ce846f7c3 216 s = DONE;
kdsch 8:4e9ce846f7c3 217 break;
kdsch 8:4e9ce846f7c3 218 case DONE:
kdsch 8:4e9ce846f7c3 219 stop();
kdsch 8:4e9ce846f7c3 220 if (TFC_GetDIP_Switch() != 0)
kdsch 8:4e9ce846f7c3 221 s = STOP;
kdsch 8:4e9ce846f7c3 222 break;
kdsch 8:4e9ce846f7c3 223 default:
kdsch 8:4e9ce846f7c3 224 s = STOP;
kdsch 8:4e9ce846f7c3 225 break;
kdsch 8:4e9ce846f7c3 226 }
kdsch 8:4e9ce846f7c3 227 }
kdsch 8:4e9ce846f7c3 228 }