main.cpp

00001 #include "mbed.h"
00002 #include "TFC.h"
00003 
00004 
00005 /*          Austin Community College Initial Specs
00006 
00007 Left (Driver side) trimpot 1 can be used to trim static steering servo
00008 
00009 Right trimpot 0 can be used to offset line camera
00010 
00011 Left PB (TFC_PUSH_BUTTON_1) is permissive GO using left 3 DIPs for speed 0-7
00012 
00013 Right DIP switch (4) sets run time. 0 => 5 sec,   1 => 10 sec.
00014 DIP switches 1 2 3 make a 3 bit speed. 1 is msb, 3 is lsb
00015 TFC_Ticker[3] is our run time counter
00016 
00017 Right PB is STOP and CENTER_SERVO. TFC_PUSH_BUTTON_0
00018 
00019 Back LED reflects drive motor status
00020 
00021 */
00022 
00023 
00024  //This ticker code is used to maintain compability with the Codewarrior version of the sample.  
00025  // This code uses an MBED Ticker for background timing.
00026 
00027  //This ticker code is used to maintain compability with the Codewarrior version of the sample.   This code uses an MBED Ticker for background timing.
00028  
00029 #define NUM_TFC_TICKERS 4
00030 
00031 Ticker TFC_TickerObj;
00032  
00033 volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
00034  
00035 void TFC_TickerUpdate()
00036 {
00037     int i;
00038  
00039     for(i=0; i<NUM_TFC_TICKERS; i++)
00040      {
00041         if(TFC_Ticker[i]<0xFFFFFFFF) 
00042         {
00043             TFC_Ticker[i]++;
00044         }
00045     }
00046 }
00047  
00048   
00049 
00050 
00051 
00052 int main(void)
00053 {
00054     uint32_t i=0;
00055     uint32_t MaxRunTime;
00056     uint8_t Motor_ON;
00057     float Motor_Speed;
00058     float Servo_Position;   // -1.0 .. +1.0
00059     float Servo_Offset;     // -1.0 to +1.0
00060     float Camera_Offset;
00061     uint8_t Start_PB_Pressed;   // flag to detect release of Start_PB
00062     uint8_t DIP_Speed;
00063     uint8_t scratch;
00064     uint16_t Current_Location;
00065     uint16_t Current_Min;
00066     float DarkSpot;
00067 
00068 //  Initialization
00069 
00070     TFC_Init();
00071     TFC_TickerObj.attach_us(&TFC_TickerUpdate,1000);
00072 
00073     TFC_HBRIDGE_DISABLE;    // Explicitly turn off motor
00074     TFC_SetMotorPWM(0.0, 0.0 ); // Explicitly zero motor setpoints
00075     TFC_SetServo(0, 0.0);       // Explicitly center steering servo
00076     Motor_ON = 0;
00077     Motor_Speed = 0;
00078     Start_PB_Pressed = 0;
00079     Servo_Position = 0.0;
00080     TFC_BAT_LED0_OFF;
00081     TFC_BAT_LED1_OFF;
00082     TFC_BAT_LED2_OFF;
00083     TFC_BAT_LED3_OFF;
00084     DIP_Speed = 0;
00085     TFC_Ticker[3] = 0;  // clear run timer
00086 
00087 //  Main Event Loop
00088 
00089     for(;;)
00090     {
00091 
00092     //TFC_Task coordinates with communications and periodic interrupt routines
00093 
00094   //      TFC_Task();
00095 
00096         // check for stop button pressed
00097 
00098         if (TFC_PUSH_BUTTON_0_PRESSED)
00099         {
00100             TFC_HBRIDGE_DISABLE;    // Explicitly turn off motor
00101             TFC_SetMotorPWM(0.0, 0.0 ); // Explicitly zero motor setpoints
00102             TFC_SetServo(0, 0.0);       // Explicitly center steering servo
00103             Motor_ON = 0;
00104             Motor_Speed = 0.0;
00105             Start_PB_Pressed = 0;
00106             Servo_Position = 0.0;
00107             TFC_BAT_LED0_OFF;   // turn off motor power indicator LED
00108             TFC_BAT_LED1_OFF;
00109             TFC_BAT_LED2_OFF;
00110             TFC_BAT_LED3_OFF;
00111 
00112         }
00113 
00114         // read trimpots to establish servo and camera offsets
00115 
00116         Camera_Offset = TFC_ReadPot(0);     // returns -1.0 to 1.0
00117         Servo_Offset = TFC_ReadPot(1);      // returns -1.0 to 1.0
00118 
00119         // read SW4 and update MaxRunTime
00120 
00121         if (TFC_GetDIP_Switch() & 8) MaxRunTime = 10000; else MaxRunTime = 5000;   // 10 sec, or 5 sec
00122 
00123         // check run status, and turn off motors if run time exceeded
00124 
00125         if (Motor_ON == 0) TFC_Ticker[3] = 0;  // clear run timer
00126         else if(TFC_Ticker[3] > MaxRunTime)
00127         {
00128             TFC_HBRIDGE_DISABLE;    // Explicitly turn off motor
00129             TFC_SetMotorPWM(0.0, 0.0 ); // Explicitly zero motor setpoints
00130             TFC_SetServo(0, 0.0);       // Explicitly center steering servo
00131             Motor_ON = 0;
00132             Motor_Speed = 0.0;
00133             Start_PB_Pressed = 0;
00134             Servo_Position = 0.0;
00135             TFC_BAT_LED0_OFF;   // turn off motor power indicator LED
00136             TFC_BAT_LED1_OFF;
00137             TFC_BAT_LED2_OFF;
00138             TFC_BAT_LED3_OFF;
00139         }
00140 
00141         //check for the start button (1) pressed, then released.
00142 
00143         if (TFC_PUSH_BUTTON_1_PRESSED) Start_PB_Pressed = 1;
00144         if ((!TFC_PUSH_BUTTON_1_PRESSED) & (Start_PB_Pressed)) // Falling edge detected
00145         {
00146             TFC_BAT_LED0_ON;    // turn on back LED
00147             scratch = TFC_GetDIP_Switch();  // read DIP switches for initial speed
00148             DIP_Speed = 0;
00149             if (scratch & 1) DIP_Speed += 4;    // SW1
00150             if (scratch & 2) DIP_Speed += 2;    // SW2
00151             if (scratch & 4) DIP_Speed += 1;    // SW3
00152             Motor_Speed = 0.125*DIP_Speed;    // scale 0..7 to 0.0 .. 0.875
00153 
00154             Motor_ON = 1;   // set flag used by run duration logic
00155             TFC_SetMotorPWM(Motor_Speed, Motor_Speed);  // acts like Motor_Speed is zero
00156             TFC_HBRIDGE_ENABLE; // enable motors
00157         }
00158 
00159         // scan through the camera array, and find smallest value in the zone 32..96
00160 
00161         if(Motor_ON)
00162         {
00163             Current_Min = TFC_LineScanImage0[32];   // TFC_LineScanImage0
00164             Current_Location = 32;
00165             for (i=32;i<96;i++)
00166             {
00167                 if(TFC_LineScanImage0[i] < Current_Min)
00168                 {
00169                     Current_Min = TFC_LineScanImage0[i];
00170                     Current_Location = i;
00171                 }
00172             }
00173 
00174             DarkSpot = (64.0 - Current_Location)*0.02;    // gain will need to be played with.
00175 
00176             Servo_Position = DarkSpot + Servo_Offset + Camera_Offset;
00177             TFC_SetServo(0,Servo_Position);
00178         }
00179 
00180     }
00181 }