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@3:04f91137660a, 2015-04-04 (annotated)

Committer:
anthonylamme
Date:
Sat Apr 04 22:21:19 2015 +0000
Revision:
3:04f91137660a
Parent:
2:729063e39fb3
Child:
4:61edb416e860
added init

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
codestar 2:729063e39fb3 32 //finds light intensity values of lines.
redxeth 0:6432166d0781 33
codestar 2:729063e39fb3 34 int FindBlack();
codestar 2:729063e39fb3 35
codestar 2:729063e39fb3 36 // finds and sets by reference the values of the left and right bounds
codestar 2:729063e39fb3 37 void Bounds(int &, int &, int);
anthonylamme 3:04f91137660a 38 int distance(int &, int &, int);
redxeth 0:6432166d0781 39
redxeth 0:6432166d0781 40 int main()
redxeth 0:6432166d0781 41 {
codestar 1:4d7f3b5b7463 42 uint32_t i,t,time = 0;
codestar 1:4d7f3b5b7463 43
anthonylamme 3:04f91137660a 44 int a = 200, b = 200, d;
codestar 2:729063e39fb3 45
codestar 1:4d7f3b5b7463 46 int data[128];
codestar 1:4d7f3b5b7463 47
codestar 1:4d7f3b5b7463 48 int black = 0;
redxeth 0:6432166d0781 49
codestar 1:4d7f3b5b7463 50 PC.baud(9600);
redxeth 0:6432166d0781 51 TFC_TickerObj.attach_us(&TFC_TickerUpdate,2000);
redxeth 0:6432166d0781 52
redxeth 0:6432166d0781 53 TFC_Init();
redxeth 0:6432166d0781 54
codestar 1:4d7f3b5b7463 55 bool timeTrue = true;
codestar 1:4d7f3b5b7463 56
redxeth 0:6432166d0781 57 for(;;)
redxeth 0:6432166d0781 58 {
redxeth 0:6432166d0781 59 //TFC_Task must be called in your main loop. This keeps certain processing happy (I.E. Serial port queue check)
redxeth 0:6432166d0781 60 // TFC_Task();
redxeth 0:6432166d0781 61
redxeth 0:6432166d0781 62 //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
redxeth 0:6432166d0781 63 //Let's look at the middle 2 switches
redxeth 0:6432166d0781 64 switch((TFC_GetDIP_Switch()>>1)&0x03)
redxeth 0:6432166d0781 65 {
redxeth 0:6432166d0781 66 default:
anthonylamme 3:04f91137660a 67 case 0 ://initilazation case
anthonylamme 3:04f91137660a 68 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
anthonylamme 3:04f91137660a 69 {
anthonylamme 3:04f91137660a 70 TFC_Ticker[0] = 0;
anthonylamme 3:04f91137660a 71 TFC_LineScanImageReady=0;
anthonylamme 3:04f91137660a 72 if(TFC_PUSH_BUTTON_0_PRESSED)
anthonylamme 3:04f91137660a 73 {
anthonylamme 3:04f91137660a 74 black = FindBlack();
anthonylamme 3:04f91137660a 75 d=distance(a,b,black);
anthonylamme 3:04f91137660a 76 }
anthonylamme 3:04f91137660a 77 }
anthonylamme 3:04f91137660a 78 TFC_SetServo(0,0.0);
anthonylamme 3:04f91137660a 79 break;
redxeth 0:6432166d0781 80
redxeth 0:6432166d0781 81 case 1:
redxeth 0:6432166d0781 82
redxeth 0:6432166d0781 83 //Demo mode 1 will just move the servos with the on-board potentiometers
redxeth 0:6432166d0781 84 if(TFC_Ticker[0]>=20)
redxeth 0:6432166d0781 85 {
redxeth 0:6432166d0781 86 TFC_Ticker[0] = 0; //reset the Ticker
redxeth 0:6432166d0781 87 //Every 20 mSeconds, update the Servos
redxeth 0:6432166d0781 88 TFC_SetServo(0,TFC_ReadPot(0));
redxeth 0:6432166d0781 89 TFC_SetServo(1,TFC_ReadPot(1));
codestar 1:4d7f3b5b7463 90
codestar 1:4d7f3b5b7463 91
redxeth 0:6432166d0781 92 }
codestar 1:4d7f3b5b7463 93
codestar 1:4d7f3b5b7463 94 if(TFC_PUSH_BUTTON_1_PRESSED)
codestar 1:4d7f3b5b7463 95 {
codestar 1:4d7f3b5b7463 96
codestar 1:4d7f3b5b7463 97 TERMINAL_PRINTF("%f", TFC_ReadPot(0));
codestar 1:4d7f3b5b7463 98 }
codestar 1:4d7f3b5b7463 99
codestar 1:4d7f3b5b7463 100
redxeth 0:6432166d0781 101 //Let's put a pattern on the LEDs
redxeth 0:6432166d0781 102 if(TFC_Ticker[1] >= 125)
redxeth 0:6432166d0781 103 {
redxeth 0:6432166d0781 104 TFC_Ticker[1] = 0;
redxeth 0:6432166d0781 105 t++;
redxeth 0:6432166d0781 106 if(t>4)
redxeth 0:6432166d0781 107 {
redxeth 0:6432166d0781 108 t=0;
redxeth 0:6432166d0781 109 }
redxeth 0:6432166d0781 110 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 111 }
redxeth 0:6432166d0781 112
codestar 1:4d7f3b5b7463 113 // TFC_SetMotorPWM(0,0); //Make sure motors are off
codestar 1:4d7f3b5b7463 114 // TFC_HBRIDGE_DISABLE;
redxeth 0:6432166d0781 115
redxeth 0:6432166d0781 116
redxeth 0:6432166d0781 117 break;
redxeth 0:6432166d0781 118
redxeth 0:6432166d0781 119 case 2 :
codestar 1:4d7f3b5b7463 120
codestar 1:4d7f3b5b7463 121
codestar 1:4d7f3b5b7463 122 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
codestar 1:4d7f3b5b7463 123 //Labview Application
redxeth 0:6432166d0781 124
redxeth 0:6432166d0781 125
codestar 1:4d7f3b5b7463 126 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
codestar 1:4d7f3b5b7463 127 {
codestar 1:4d7f3b5b7463 128 TFC_Ticker[0] = 0;
codestar 1:4d7f3b5b7463 129 TFC_LineScanImageReady=0;
codestar 1:4d7f3b5b7463 130 // TERMINAL_PRINTF("\r\n");
codestar 1:4d7f3b5b7463 131 // TERMINAL_PRINTF("L:");
codestar 1:4d7f3b5b7463 132
codestar 1:4d7f3b5b7463 133 if(t==0)
codestar 1:4d7f3b5b7463 134 t=4;
codestar 1:4d7f3b5b7463 135 else
codestar 1:4d7f3b5b7463 136 t--;
codestar 1:4d7f3b5b7463 137
codestar 1:4d7f3b5b7463 138 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 139
codestar 1:4d7f3b5b7463 140 // camera 1
codestar 1:4d7f3b5b7463 141
codestar 1:4d7f3b5b7463 142 if(TFC_PUSH_BUTTON_0_PRESSED)
codestar 1:4d7f3b5b7463 143 {
codestar 2:729063e39fb3 144
codestar 1:4d7f3b5b7463 145
codestar 2:729063e39fb3 146 black = FindBlack();
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 2:729063e39fb3 188 // zero is black
codestar 2:729063e39fb3 189
codestar 1:4d7f3b5b7463 190 data[i]=0;
codestar 1:4d7f3b5b7463 191 }
codestar 1:4d7f3b5b7463 192 else
codestar 1:4d7f3b5b7463 193 {
codestar 2:729063e39fb3 194 // one is white
codestar 1:4d7f3b5b7463 195 data[i]=1;
codestar 1:4d7f3b5b7463 196 }
codestar 1:4d7f3b5b7463 197 TERMINAL_PRINTF("%d", data[i]);
redxeth 0:6432166d0781 198 }
codestar 1:4d7f3b5b7463 199 TERMINAL_PRINTF("\r");
redxeth 0:6432166d0781 200
redxeth 0:6432166d0781 201 // camera 2
codestar 1:4d7f3b5b7463 202 /* for(i=0;i<128;i++)
redxeth 0:6432166d0781 203 {
redxeth 0:6432166d0781 204 if(i==127)
redxeth 0:6432166d0781 205 TERMINAL_PRINTF("%X\r\n",TFC_LineScanImage1[i]);
redxeth 0:6432166d0781 206 else
redxeth 0:6432166d0781 207 TERMINAL_PRINTF("%X,",TFC_LineScanImage1[i]);
redxeth 0:6432166d0781 208
codestar 1:4d7f3b5b7463 209 } */
codestar 1:4d7f3b5b7463 210
codestar 1:4d7f3b5b7463 211 }
redxeth 0:6432166d0781 212 }
redxeth 0:6432166d0781 213
redxeth 0:6432166d0781 214 break;
redxeth 0:6432166d0781 215 }
redxeth 0:6432166d0781 216 }
redxeth 0:6432166d0781 217
redxeth 0:6432166d0781 218
redxeth 0:6432166d0781 219 }
codestar 1:4d7f3b5b7463 220 }
codestar 2:729063e39fb3 221 }
codestar 2:729063e39fb3 222
codestar 2:729063e39fb3 223 // finds and sets by reference the values of the left and right bounds
codestar 2:729063e39fb3 224
codestar 2:729063e39fb3 225 void bounds(int &a,int &b,int black)
codestar 2:729063e39fb3 226 {
codestar 2:729063e39fb3 227
codestar 2:729063e39fb3 228 for(int i=0; i<128; i++)
codestar 2:729063e39fb3 229 {
codestar 2:729063e39fb3 230 if(TFC_LineScanImage0[i]==black&& i==0)
codestar 2:729063e39fb3 231 {
codestar 2:729063e39fb3 232 a = i;
codestar 2:729063e39fb3 233 }
codestar 2:729063e39fb3 234
codestar 2:729063e39fb3 235 else if(TFC_LineScanImage0[i]==black)
codestar 2:729063e39fb3 236 {
codestar 2:729063e39fb3 237 //if there are to black values next to eachother it sets a to newer inside value.
codestar 2:729063e39fb3 238
codestar 2:729063e39fb3 239 if( (i-a) == 1 )
codestar 2:729063e39fb3 240 {
codestar 2:729063e39fb3 241 a = i;
codestar 2:729063e39fb3 242 }
codestar 2:729063e39fb3 243
codestar 2:729063e39fb3 244 // if there is a space between black values the next black value is the right bound.
codestar 2:729063e39fb3 245 else
codestar 2:729063e39fb3 246 {
codestar 2:729063e39fb3 247 // sets the right inside bound and ends the loop.
codestar 2:729063e39fb3 248
codestar 2:729063e39fb3 249 b = i;
codestar 2:729063e39fb3 250 break;
codestar 2:729063e39fb3 251 }
codestar 2:729063e39fb3 252 }
codestar 2:729063e39fb3 253 }
codestar 2:729063e39fb3 254 }
codestar 2:729063e39fb3 255
codestar 2:729063e39fb3 256
codestar 2:729063e39fb3 257
codestar 2:729063e39fb3 258
codestar 2:729063e39fb3 259 int FindBlack()
codestar 2:729063e39fb3 260 {
codestar 2:729063e39fb3 261 int low1=2000,low2=2000;
codestar 2:729063e39fb3 262 for(int i=2;i<64;i++)//first half of line scan
codestar 2:729063e39fb3 263 {
codestar 2:729063e39fb3 264 int ave=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
codestar 2:729063e39fb3 265 if(i==2)
codestar 2:729063e39fb3 266 {
codestar 2:729063e39fb3 267 low1=ave;//first loop sets lowest average
codestar 2:729063e39fb3 268 }
codestar 2:729063e39fb3 269 if(low1>ave)
codestar 2:729063e39fb3 270 {
codestar 2:729063e39fb3 271 low1=ave;
codestar 2:729063e39fb3 272 }
codestar 2:729063e39fb3 273 }
codestar 2:729063e39fb3 274 for(int i=66;i<128;i++)//second half of line scan
codestar 2:729063e39fb3 275 {
codestar 2:729063e39fb3 276 int ave2=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
codestar 2:729063e39fb3 277 if(i==66)
codestar 2:729063e39fb3 278 {
codestar 2:729063e39fb3 279 low2=ave2;
codestar 2:729063e39fb3 280 }
codestar 2:729063e39fb3 281 if(low2>ave2)
codestar 2:729063e39fb3 282 {
codestar 2:729063e39fb3 283 low2=ave2;
codestar 2:729063e39fb3 284 }
codestar 2:729063e39fb3 285 if(low1==low2)
codestar 2:729063e39fb3 286 {
codestar 2:729063e39fb3 287 return low1; //confirms bothe lines and breaks loop returns black
codestar 2:729063e39fb3 288 }
codestar 2:729063e39fb3 289 }
codestar 2:729063e39fb3 290 if(low1>low2)
codestar 2:729063e39fb3 291 {
codestar 2:729063e39fb3 292 return low1;
codestar 2:729063e39fb3 293 }
codestar 2:729063e39fb3 294 else
codestar 2:729063e39fb3 295 {
codestar 2:729063e39fb3 296 return low2;
codestar 2:729063e39fb3 297 }
anthonylamme 3:04f91137660a 298 }
anthonylamme 3:04f91137660a 299 int distance(int &a, int &b, int black)
anthonylamme 3:04f91137660a 300 {
anthonylamme 3:04f91137660a 301 bounds(a,b,black);
anthonylamme 3:04f91137660a 302 return (a-b);
anthonylamme 3:04f91137660a 303 }
anthonylamme 3:04f91137660a 304