Cody Blakeney / TFC-TEST_TXSTATE_TEAM6v3

added distance function, added turnDir function, started function that completes left turn

Dependencies:   FRDM-TFC

Fork of TFC-TEST_TXSTATE_TEAM6v2 by Anthony Lamme

main.cpp@1:4d7f3b5b7463, 2015-04-04 (annotated)

Committer:
codestar
Date:
Sat Apr 04 19:41:57 2015 +0000
Revision:
1:4d7f3b5b7463
Parent:
0:6432166d0781
Child:
2:729063e39fb3
current

Who changed what in which revision?

UserRevisionLine numberNew contents of line
redxeth 0:6432166d0781 1 #include "mbed.h"
redxeth 0:6432166d0781 2 #include "TFC.h"
redxeth 0:6432166d0781 3
redxeth 0:6432166d0781 4
redxeth 0:6432166d0781 5 //This macro is to maintain compatibility with Codewarrior version of the sample. This version uses the MBED libraries for serial port access
redxeth 0:6432166d0781 6 Serial PC(USBTX,USBRX);
redxeth 0:6432166d0781 7
redxeth 0:6432166d0781 8 #define TERMINAL_PRINTF PC.printf
redxeth 0:6432166d0781 9
redxeth 0:6432166d0781 10
redxeth 0:6432166d0781 11 //This ticker code is used to maintain compability with the Codewarrior version of the sample. This code uses an MBED Ticker for background timing.
redxeth 0:6432166d0781 12
redxeth 0:6432166d0781 13 #define NUM_TFC_TICKERS 4
redxeth 0:6432166d0781 14
redxeth 0:6432166d0781 15 Ticker TFC_TickerObj;
redxeth 0:6432166d0781 16
redxeth 0:6432166d0781 17 volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
redxeth 0:6432166d0781 18
redxeth 0:6432166d0781 19 void TFC_TickerUpdate()
redxeth 0:6432166d0781 20 {
redxeth 0:6432166d0781 21 int i;
redxeth 0:6432166d0781 22
redxeth 0:6432166d0781 23 for(i=0; i<NUM_TFC_TICKERS; i++)
redxeth 0:6432166d0781 24 {
redxeth 0:6432166d0781 25 if(TFC_Ticker[i]<0xFFFFFFFF)
redxeth 0:6432166d0781 26 {
redxeth 0:6432166d0781 27 TFC_Ticker[i]++;
redxeth 0:6432166d0781 28 }
redxeth 0:6432166d0781 29 }
redxeth 0:6432166d0781 30 }
redxeth 0:6432166d0781 31
redxeth 0:6432166d0781 32
redxeth 0:6432166d0781 33
redxeth 0:6432166d0781 34
redxeth 0:6432166d0781 35 int main()
redxeth 0:6432166d0781 36 {
codestar 1:4d7f3b5b7463 37 uint32_t i,t,time = 0;
codestar 1:4d7f3b5b7463 38
codestar 1:4d7f3b5b7463 39 int data[128];
codestar 1:4d7f3b5b7463 40
codestar 1:4d7f3b5b7463 41 int black = 0;
redxeth 0:6432166d0781 42
codestar 1:4d7f3b5b7463 43 PC.baud(9600);
redxeth 0:6432166d0781 44 TFC_TickerObj.attach_us(&TFC_TickerUpdate,2000);
redxeth 0:6432166d0781 45
redxeth 0:6432166d0781 46 TFC_Init();
redxeth 0:6432166d0781 47
codestar 1:4d7f3b5b7463 48 bool timeTrue = true;
codestar 1:4d7f3b5b7463 49
redxeth 0:6432166d0781 50 for(;;)
redxeth 0:6432166d0781 51 {
redxeth 0:6432166d0781 52 //TFC_Task must be called in your main loop. This keeps certain processing happy (I.E. Serial port queue check)
redxeth 0:6432166d0781 53 // TFC_Task();
redxeth 0:6432166d0781 54
redxeth 0:6432166d0781 55 //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
redxeth 0:6432166d0781 56 //Let's look at the middle 2 switches
redxeth 0:6432166d0781 57 switch((TFC_GetDIP_Switch()>>1)&0x03)
redxeth 0:6432166d0781 58 {
redxeth 0:6432166d0781 59 default:
redxeth 0:6432166d0781 60 case 0 :
codestar 1:4d7f3b5b7463 61
codestar 1:4d7f3b5b7463 62 TFC_HBRIDGE_ENABLE;
codestar 1:4d7f3b5b7463 63 TFC_SetMotorPWM(.6,.6);
redxeth 0:6432166d0781 64
redxeth 0:6432166d0781 65
redxeth 0:6432166d0781 66 case 1:
redxeth 0:6432166d0781 67
redxeth 0:6432166d0781 68 //Demo mode 1 will just move the servos with the on-board potentiometers
redxeth 0:6432166d0781 69 if(TFC_Ticker[0]>=20)
redxeth 0:6432166d0781 70 {
redxeth 0:6432166d0781 71 TFC_Ticker[0] = 0; //reset the Ticker
redxeth 0:6432166d0781 72 //Every 20 mSeconds, update the Servos
redxeth 0:6432166d0781 73 TFC_SetServo(0,TFC_ReadPot(0));
redxeth 0:6432166d0781 74 TFC_SetServo(1,TFC_ReadPot(1));
codestar 1:4d7f3b5b7463 75
codestar 1:4d7f3b5b7463 76
redxeth 0:6432166d0781 77 }
codestar 1:4d7f3b5b7463 78
codestar 1:4d7f3b5b7463 79 if(TFC_PUSH_BUTTON_1_PRESSED)
codestar 1:4d7f3b5b7463 80 {
codestar 1:4d7f3b5b7463 81
codestar 1:4d7f3b5b7463 82 TERMINAL_PRINTF("%f", TFC_ReadPot(0));
codestar 1:4d7f3b5b7463 83 }
codestar 1:4d7f3b5b7463 84
codestar 1:4d7f3b5b7463 85
redxeth 0:6432166d0781 86 //Let's put a pattern on the LEDs
redxeth 0:6432166d0781 87 if(TFC_Ticker[1] >= 125)
redxeth 0:6432166d0781 88 {
redxeth 0:6432166d0781 89 TFC_Ticker[1] = 0;
redxeth 0:6432166d0781 90 t++;
redxeth 0:6432166d0781 91 if(t>4)
redxeth 0:6432166d0781 92 {
redxeth 0:6432166d0781 93 t=0;
redxeth 0:6432166d0781 94 }
redxeth 0:6432166d0781 95 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 96 }
redxeth 0:6432166d0781 97
codestar 1:4d7f3b5b7463 98 // TFC_SetMotorPWM(0,0); //Make sure motors are off
codestar 1:4d7f3b5b7463 99 // TFC_HBRIDGE_DISABLE;
redxeth 0:6432166d0781 100
redxeth 0:6432166d0781 101
redxeth 0:6432166d0781 102 break;
redxeth 0:6432166d0781 103
redxeth 0:6432166d0781 104 case 2 :
codestar 1:4d7f3b5b7463 105
codestar 1:4d7f3b5b7463 106
codestar 1:4d7f3b5b7463 107 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
codestar 1:4d7f3b5b7463 108 //Labview Application
redxeth 0:6432166d0781 109
redxeth 0:6432166d0781 110
codestar 1:4d7f3b5b7463 111 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
codestar 1:4d7f3b5b7463 112 {
codestar 1:4d7f3b5b7463 113 TFC_Ticker[0] = 0;
codestar 1:4d7f3b5b7463 114 TFC_LineScanImageReady=0;
codestar 1:4d7f3b5b7463 115 // TERMINAL_PRINTF("\r\n");
codestar 1:4d7f3b5b7463 116 // TERMINAL_PRINTF("L:");
codestar 1:4d7f3b5b7463 117
codestar 1:4d7f3b5b7463 118 if(t==0)
codestar 1:4d7f3b5b7463 119 t=4;
codestar 1:4d7f3b5b7463 120 else
codestar 1:4d7f3b5b7463 121 t--;
codestar 1:4d7f3b5b7463 122
codestar 1:4d7f3b5b7463 123 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 124
codestar 1:4d7f3b5b7463 125 // camera 1
codestar 1:4d7f3b5b7463 126
codestar 1:4d7f3b5b7463 127 if(TFC_PUSH_BUTTON_0_PRESSED)
codestar 1:4d7f3b5b7463 128 {
codestar 1:4d7f3b5b7463 129 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
codestar 1:4d7f3b5b7463 130 //Labview Application
codestar 1:4d7f3b5b7463 131 int highest = 0;
codestar 1:4d7f3b5b7463 132
codestar 1:4d7f3b5b7463 133 // camera 1
codestar 1:4d7f3b5b7463 134 for(i=0;i<128;i++)
codestar 1:4d7f3b5b7463 135 {
codestar 1:4d7f3b5b7463 136 if(i==0)
codestar 1:4d7f3b5b7463 137 {
codestar 1:4d7f3b5b7463 138 highest = TFC_LineScanImage0[i];
codestar 1:4d7f3b5b7463 139 }
codestar 1:4d7f3b5b7463 140 if(TFC_LineScanImage0[i]>highest)
codestar 1:4d7f3b5b7463 141 {
codestar 1:4d7f3b5b7463 142 highest = TFC_LineScanImage0[i];
codestar 1:4d7f3b5b7463 143 }
codestar 1:4d7f3b5b7463 144 }
codestar 1:4d7f3b5b7463 145
codestar 1:4d7f3b5b7463 146 black = highest;
codestar 1:4d7f3b5b7463 147
codestar 1:4d7f3b5b7463 148 TERMINAL_PRINTF("%i", black);
codestar 1:4d7f3b5b7463 149
codestar 1:4d7f3b5b7463 150
codestar 1:4d7f3b5b7463 151
redxeth 0:6432166d0781 152 break;
redxeth 0:6432166d0781 153
redxeth 0:6432166d0781 154 case 3 :
redxeth 0:6432166d0781 155
redxeth 0:6432166d0781 156
redxeth 0:6432166d0781 157 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
redxeth 0:6432166d0781 158 //Labview Application
redxeth 0:6432166d0781 159
redxeth 0:6432166d0781 160
redxeth 0:6432166d0781 161 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
redxeth 0:6432166d0781 162 {
redxeth 0:6432166d0781 163 TFC_Ticker[0] = 0;
redxeth 0:6432166d0781 164 TFC_LineScanImageReady=0;
codestar 1:4d7f3b5b7463 165 // TERMINAL_PRINTF("\r\n");
codestar 1:4d7f3b5b7463 166 // TERMINAL_PRINTF("L:");
redxeth 0:6432166d0781 167
redxeth 0:6432166d0781 168 if(t==0)
redxeth 0:6432166d0781 169 t=4;
redxeth 0:6432166d0781 170 else
redxeth 0:6432166d0781 171 t--;
redxeth 0:6432166d0781 172
redxeth 0:6432166d0781 173 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 174
redxeth 0:6432166d0781 175 // camera 1
codestar 1:4d7f3b5b7463 176
codestar 1:4d7f3b5b7463 177 if(TFC_PUSH_BUTTON_0_PRESSED)
codestar 1:4d7f3b5b7463 178 {
codestar 1:4d7f3b5b7463 179 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
codestar 1:4d7f3b5b7463 180 //Labview Application
codestar 1:4d7f3b5b7463 181
codestar 1:4d7f3b5b7463 182
codestar 1:4d7f3b5b7463 183 // camera 1
redxeth 0:6432166d0781 184 for(i=0;i<128;i++)
redxeth 0:6432166d0781 185 {
codestar 1:4d7f3b5b7463 186 if(TFC_LineScanImage0[i]<=black)
codestar 1:4d7f3b5b7463 187 {
codestar 1:4d7f3b5b7463 188 data[i]=0;
codestar 1:4d7f3b5b7463 189 }
codestar 1:4d7f3b5b7463 190 else
codestar 1:4d7f3b5b7463 191 {
codestar 1:4d7f3b5b7463 192 data[i]=1;
codestar 1:4d7f3b5b7463 193 }
codestar 1:4d7f3b5b7463 194 TERMINAL_PRINTF("%d", data[i]);
redxeth 0:6432166d0781 195 }
codestar 1:4d7f3b5b7463 196 TERMINAL_PRINTF("\r");
redxeth 0:6432166d0781 197
redxeth 0:6432166d0781 198 // camera 2
codestar 1:4d7f3b5b7463 199 /* for(i=0;i<128;i++)
redxeth 0:6432166d0781 200 {
redxeth 0:6432166d0781 201 if(i==127)
redxeth 0:6432166d0781 202 TERMINAL_PRINTF("%X\r\n",TFC_LineScanImage1[i]);
redxeth 0:6432166d0781 203 else
redxeth 0:6432166d0781 204 TERMINAL_PRINTF("%X,",TFC_LineScanImage1[i]);
redxeth 0:6432166d0781 205
codestar 1:4d7f3b5b7463 206 } */
codestar 1:4d7f3b5b7463 207
codestar 1:4d7f3b5b7463 208 }
redxeth 0:6432166d0781 209 }
redxeth 0:6432166d0781 210
redxeth 0:6432166d0781 211 break;
redxeth 0:6432166d0781 212 }
redxeth 0:6432166d0781 213 }
redxeth 0:6432166d0781 214
redxeth 0:6432166d0781 215
redxeth 0:6432166d0781 216 }
codestar 1:4d7f3b5b7463 217 }
codestar 1:4d7f3b5b7463 218 }