Karl Schultheisz
Our version of TFC code.
Fork of
FRDM-TFC
by 
Eli Hughes
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