Kurt Nalty
Austin Community College initial line follower program for Freescale car. This is based upon the FRDM-TFC code at mbed.org.
Fork of
FRDM-TFC
by 
Eli Hughes
Austin Community College - Initial Specs for FRDM-TFC line following car.
This uses the fine library by ELI HUGHES.
Our goal here is to provide the simplest line-follower as a quick car checkout.
First, we limit the run duration for the car, so that you can catch it, if necessary. Right DIP switch (4) sets run time. 0 => 5 sec, 1 => 10 sec.
We provide simple speed selection with the other three DIP switches. I recommend 001 as the slowest real speed, (000 is stop, of course). DIP switches 1 2 3 make a 3 bit speed. 1 is msb, 3 is lsb
The car won't start until you press and release the driver's side PB. Left PB (TFC_PUSH_BUTTON_1) is permissive GO on release using left 3 DIPs for speed 0-7.
The car will stop when the passenger side PB is pressed. Right PB is STOP - TFC_PUSH_BUTTON_0
TFC_Ticker[3] is our run time counter. Like the Code Warrior edition, we use msec tickers.
Left (Driver side) trim pot 1 can be used to trim static steering servo Right trim pot 0 can be used to offset line camera
Back LED reflects drive motor status. The top three are not currently used.
Revision 5:9d36b4c896e1, committed 2013-11-11
- Comitter:
- knalty
- Date:
- Mon Nov 11 03:04:42 2013 +0000
- Parent:
- 4:8a4a3fc59e57
- Commit message:
- Initial Line Following Code, Nov 10, 2013
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 8a4a3fc59e57 -r 9d36b4c896e1 main.cpp
--- a/main.cpp Wed Sep 04 02:48:20 2013 +0000
+++ b/main.cpp Mon Nov 11 03:04:42 2013 +0000
@@ -1,13 +1,29 @@
#include "mbed.h"
#include "TFC.h"
-
-//This macro is to maintain compatibility with Codewarrior version of the sample. This version uses the MBED libraries for serial port access
-Serial PC(USBTX,USBRX);
+
+/* Austin Community College Initial Specs
+
+Left (Driver side) trimpot 1 can be used to trim static steering servo
+
+Right trimpot 0 can be used to offset line camera
+
+Left PB (TFC_PUSH_BUTTON_1) is permissive GO using left 3 DIPs for speed 0-7
-#define TERMINAL_PRINTF PC.printf
+Right DIP switch (4) sets run time. 0 => 5 sec, 1 => 10 sec.
+DIP switches 1 2 3 make a 3 bit speed. 1 is msb, 3 is lsb
+TFC_Ticker[3] is our run time counter
+
+Right PB is STOP and CENTER_SERVO. TFC_PUSH_BUTTON_0
-
+Back LED reflects drive motor status
+
+*/
+
+
+ //This ticker code is used to maintain compability with the Codewarrior version of the sample.
+ // This code uses an MBED Ticker for background timing.
+
//This ticker code is used to maintain compability with the Codewarrior version of the sample. This code uses an MBED Ticker for background timing.
#define NUM_TFC_TICKERS 4
@@ -29,169 +45,137 @@
}
}
-
-
-
-int main()
-{
- uint32_t i,j,t = 0;
- PC.baud(115200);
- TFC_TickerObj.attach_us(&TFC_TickerUpdate,2000);
-
- TFC_Init();
-
- for(;;)
- {
- //TFC_Task must be called in your main loop. This keeps certain processing happy (I.E. Serial port queue check)
- // TFC_Task();
-
- //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
- //Let's look at the middle 2 switches
- switch((TFC_GetDIP_Switch()>>1)&0x03)
- {
- default:
- case 0 :
- //Demo mode 0 just tests the switches and LED's
- if(TFC_PUSH_BUTTON_0_PRESSED)
- TFC_BAT_LED0_ON;
- else
- TFC_BAT_LED0_OFF;
-
- if(TFC_PUSH_BUTTON_1_PRESSED)
- TFC_BAT_LED3_ON;
- else
- TFC_BAT_LED3_OFF;
-
-
- if(TFC_GetDIP_Switch()&0x01)
- TFC_BAT_LED1_ON;
- else
- TFC_BAT_LED1_OFF;
-
- if(TFC_GetDIP_Switch()&0x08)
- TFC_BAT_LED2_ON;
- else
- TFC_BAT_LED2_OFF;
-
- break;
-
- case 1:
-
- //Demo mode 1 will just move the servos with the on-board potentiometers
- if(TFC_Ticker[0]>=20)
- {
- TFC_Ticker[0] = 0; //reset the Ticker
- //Every 20 mSeconds, update the Servos
- TFC_SetServo(0,TFC_ReadPot(0));
- TFC_SetServo(1,TFC_ReadPot(1));
- }
- //Let's put a pattern on the LEDs
- if(TFC_Ticker[1] >= 125)
- {
- TFC_Ticker[1] = 0;
- t++;
- if(t>4)
- {
- t=0;
- }
- TFC_SetBatteryLED_Level(t);
- }
-
- TFC_SetMotorPWM(0,0); //Make sure motors are off
- TFC_HBRIDGE_DISABLE;
-
- break;
-
- case 2 :
-
- //Demo Mode 2 will use the Pots to make the motors move
- TFC_HBRIDGE_ENABLE;
-
- TFC_SetMotorPWM(TFC_ReadPot(0),TFC_ReadPot(1));
-
-
- //Let's put a pattern on the LEDs
- if(TFC_Ticker[1] >= 125)
- {
- TFC_Ticker[1] = 0;
- t++;
- if(t>4)
- {
- t=0;
- }
- TFC_SetBatteryLED_Level(t);
- }
- break;
-
- case 3 :
-
- uint32_t NumCameras = 1; // Enter here how many line scan cameras hooked up
-
- //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
- //Labview Application
-
-
- if(TFC_Ticker[0]>1000 && TFC_LineScanImageReady>0)
- {
- TFC_Ticker[0] = 0;
- TFC_LineScanImageReady=0;
- TERMINAL_PRINTF("\r\n");
- TERMINAL_PRINTF("L:");
-
- if(t==0)
- t=4;
- else
- t--;
-
- TFC_SetBatteryLED_Level(t);
-
- // camera 1
- for(i=0;i<8;i++)
- {
- for(j=0;j<16;j++)
- {
- TERMINAL_PRINTF("%X",TFC_LineScanImage0[(i*16)+j]);
-
- if (NumCameras == 1)
- {
- if((i==7)&&(j==15))
- TERMINAL_PRINTF("\r\n",TFC_LineScanImage0[(i*16)+j]);
- else
- TERMINAL_PRINTF(",",TFC_LineScanImage0[(i*16)+j]);
- }
-
- }
- wait_ms(10);
- }
-
- // camera 2
- if (NumCameras == 2)
- {
- for(i=0;i<8;i++)
- {
- for(j=0;j<16;j++)
- {
- TERMINAL_PRINTF("%X",TFC_LineScanImage1[(i*16)+j]);
-
- if((i==7)&&(j==15))
- TERMINAL_PRINTF("\r\n",TFC_LineScanImage1[(i*16)+j]);
- else
- TERMINAL_PRINTF(",",TFC_LineScanImage1[(i*16)+j]);
- }
- wait_ms(10);
- }
- }
-
- }
-
- break;
+int main(void)
+{
+ uint32_t i=0;
+ uint32_t MaxRunTime;
+ uint8_t Motor_ON;
+ float Motor_Speed;
+ float Servo_Position; // -1.0 .. +1.0
+ float Servo_Offset; // -1.0 to +1.0
+ float Camera_Offset;
+ uint8_t Start_PB_Pressed; // flag to detect release of Start_PB
+ uint8_t DIP_Speed;
+ uint8_t scratch;
+ uint16_t Current_Location;
+ uint16_t Current_Min;
+ float DarkSpot;
+
+// Initialization
+
+ TFC_Init();
+ TFC_TickerObj.attach_us(&TFC_TickerUpdate,1000);
+
+ TFC_HBRIDGE_DISABLE; // Explicitly turn off motor
+ TFC_SetMotorPWM(0.0, 0.0 ); // Explicitly zero motor setpoints
+ TFC_SetServo(0, 0.0); // Explicitly center steering servo
+ Motor_ON = 0;
+ Motor_Speed = 0;
+ Start_PB_Pressed = 0;
+ Servo_Position = 0.0;
+ TFC_BAT_LED0_OFF;
+ TFC_BAT_LED1_OFF;
+ TFC_BAT_LED2_OFF;
+ TFC_BAT_LED3_OFF;
+ DIP_Speed = 0;
+ TFC_Ticker[3] = 0; // clear run timer
+
+// Main Event Loop
+
+ for(;;)
+ {
+
+ //TFC_Task coordinates with communications and periodic interrupt routines
+
+ // TFC_Task();
+
+ // check for stop button pressed
+
+ if (TFC_PUSH_BUTTON_0_PRESSED)
+ {
+ TFC_HBRIDGE_DISABLE; // Explicitly turn off motor
+ TFC_SetMotorPWM(0.0, 0.0 ); // Explicitly zero motor setpoints
+ TFC_SetServo(0, 0.0); // Explicitly center steering servo
+ Motor_ON = 0;
+ Motor_Speed = 0.0;
+ Start_PB_Pressed = 0;
+ Servo_Position = 0.0;
+ TFC_BAT_LED0_OFF; // turn off motor power indicator LED
+ TFC_BAT_LED1_OFF;
+ TFC_BAT_LED2_OFF;
+ TFC_BAT_LED3_OFF;
+
+ }
+
+ // read trimpots to establish servo and camera offsets
+
+ Camera_Offset = TFC_ReadPot(0); // returns -1.0 to 1.0
+ Servo_Offset = TFC_ReadPot(1); // returns -1.0 to 1.0
+
+ // read SW4 and update MaxRunTime
+
+ if (TFC_GetDIP_Switch() & 8) MaxRunTime = 10000; else MaxRunTime = 5000; // 10 sec, or 5 sec
+
+ // check run status, and turn off motors if run time exceeded
+
+ if (Motor_ON == 0) TFC_Ticker[3] = 0; // clear run timer
+ else if(TFC_Ticker[3] > MaxRunTime)
+ {
+ TFC_HBRIDGE_DISABLE; // Explicitly turn off motor
+ TFC_SetMotorPWM(0.0, 0.0 ); // Explicitly zero motor setpoints
+ TFC_SetServo(0, 0.0); // Explicitly center steering servo
+ Motor_ON = 0;
+ Motor_Speed = 0.0;
+ Start_PB_Pressed = 0;
+ Servo_Position = 0.0;
+ TFC_BAT_LED0_OFF; // turn off motor power indicator LED
+ TFC_BAT_LED1_OFF;
+ TFC_BAT_LED2_OFF;
+ TFC_BAT_LED3_OFF;
+ }
+
+ //check for the start button (1) pressed, then released.
+
+ if (TFC_PUSH_BUTTON_1_PRESSED) Start_PB_Pressed = 1;
+ if ((!TFC_PUSH_BUTTON_1_PRESSED) & (Start_PB_Pressed)) // Falling edge detected
+ {
+ TFC_BAT_LED0_ON; // turn on back LED
+ scratch = TFC_GetDIP_Switch(); // read DIP switches for initial speed
+ DIP_Speed = 0;
+ if (scratch & 1) DIP_Speed += 4; // SW1
+ if (scratch & 2) DIP_Speed += 2; // SW2
+ if (scratch & 4) DIP_Speed += 1; // SW3
+ Motor_Speed = 0.125*DIP_Speed; // scale 0..7 to 0.0 .. 0.875
+
+ Motor_ON = 1; // set flag used by run duration logic
+ TFC_SetMotorPWM(Motor_Speed, Motor_Speed); // acts like Motor_Speed is zero
+ TFC_HBRIDGE_ENABLE; // enable motors
+ }
+
+ // scan through the camera array, and find smallest value in the zone 32..96
+
+ if(Motor_ON)
+ {
+ Current_Min = TFC_LineScanImage0[32]; // TFC_LineScanImage0
+ Current_Location = 32;
+ for (i=32;i<96;i++)
+ {
+ if(TFC_LineScanImage0[i] < Current_Min)
+ {
+ Current_Min = TFC_LineScanImage0[i];
+ Current_Location = i;
+ }
}
+
+ DarkSpot = (64.0 - Current_Location)*0.02; // gain will need to be played with.
+
+ Servo_Position = DarkSpot + Servo_Offset + Camera_Offset;
+ TFC_SetServo(0,Servo_Position);
+ }
+
}
-
-
}
-