![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Our version of TFC code.
Fork of FRDM-TFC by
Revision 8:4e9ce846f7c3, committed 2015-05-01
- Comitter:
- kdsch
- Date:
- Fri May 01 15:44:59 2015 +0000
- Parent:
- 7:b34924a05d60
- Commit message:
- freescale cup
Changed in this revision
diff -r b34924a05d60 -r 4e9ce846f7c3 FRDM-TFC_TEST.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FRDM-TFC_TEST.lib Fri May 01 15:44:59 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/emh203/code/FRDM-TFC/#b34924a05d60
diff -r b34924a05d60 -r 4e9ce846f7c3 TFC.cpp --- a/TFC.cpp Sun Apr 20 00:14:53 2014 +0000 +++ b/TFC.cpp Fri May 01 15:44:59 2015 +0000 @@ -12,7 +12,7 @@ #define FTM0_CLOCK (SystemCoreClock/2) #define FTM0_CLK_PRESCALE (0) // Prescale Selector value - see comments in Status Control (SC) section for more details -#define FTM0_DEFAULT_SWITCHING_FREQUENCY (4000.0) +#define FTM0_DEFAULT_SWITCHING_FREQUENCY (9000.0) #define ADC_MAX_CODE (4095)
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