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@4:61edb416e860, 2015-04-05 (annotated)

Committer:
codestar
Date:
Sun Apr 05 01:57:47 2015 +0000
Revision:
4:61edb416e860
Parent:
3:04f91137660a 
added function to find the distance. added function to determine the direction of an approaching turn. began work on a function to make the car complete a left turn

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"
codestar 4:61edb416e860 3 #include <cmath>
redxeth 0:6432166d0781 4
redxeth 0:6432166d0781 5
redxeth 0:6432166d0781 6 //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 7 Serial PC(USBTX,USBRX);
redxeth 0:6432166d0781 8
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);
codestar 4:61edb416e860 38
codestar 4:61edb416e860 39 // finds distance between bounds
anthonylamme 3:04f91137660a 40 int distance(int &, int &, int);
codestar 4:61edb416e860 41
codestar 4:61edb416e860 42 // returns interger for turn direction 0 for left 1 for right
codestar 4:61edb416e860 43
codestar 4:61edb416e860 44 int turnDir( int &, int &, int, int);
codestar 4:61edb416e860 45
codestar 4:61edb416e860 46 //void turnLeft(int &, int &, int, int); needs to be finished
codestar 4:61edb416e860 47
redxeth 0:6432166d0781 48
redxeth 0:6432166d0781 49 int main()
redxeth 0:6432166d0781 50 {
codestar 1:4d7f3b5b7463 51 uint32_t i,t,time = 0;
codestar 1:4d7f3b5b7463 52
codestar 4:61edb416e860 53 int a = 200, b = 200, d = 200;
codestar 2:729063e39fb3 54
codestar 1:4d7f3b5b7463 55 int data[128];
codestar 1:4d7f3b5b7463 56
codestar 1:4d7f3b5b7463 57 int black = 0;
redxeth 0:6432166d0781 58
codestar 1:4d7f3b5b7463 59 PC.baud(9600);
redxeth 0:6432166d0781 60 TFC_TickerObj.attach_us(&TFC_TickerUpdate,2000);
redxeth 0:6432166d0781 61
redxeth 0:6432166d0781 62 TFC_Init();
redxeth 0:6432166d0781 63
codestar 4:61edb416e860 64 bool didInit = false ; // flag for track calibration
codestar 1:4d7f3b5b7463 65
redxeth 0:6432166d0781 66 for(;;)
redxeth 0:6432166d0781 67 {
redxeth 0:6432166d0781 68 //TFC_Task must be called in your main loop. This keeps certain processing happy (I.E. Serial port queue check)
redxeth 0:6432166d0781 69 // TFC_Task();
redxeth 0:6432166d0781 70
redxeth 0:6432166d0781 71 //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
redxeth 0:6432166d0781 72 //Let's look at the middle 2 switches
redxeth 0:6432166d0781 73 switch((TFC_GetDIP_Switch()>>1)&0x03)
redxeth 0:6432166d0781 74 {
redxeth 0:6432166d0781 75 default:
anthonylamme 3:04f91137660a 76 case 0 ://initilazation case
anthonylamme 3:04f91137660a 77 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
anthonylamme 3:04f91137660a 78 {
anthonylamme 3:04f91137660a 79 TFC_Ticker[0] = 0;
anthonylamme 3:04f91137660a 80 TFC_LineScanImageReady=0;
codestar 4:61edb416e860 81 if(TFC_PUSH_BUTTON_0_PRESSED&& didInit == false)
anthonylamme 3:04f91137660a 82 {
anthonylamme 3:04f91137660a 83 black = FindBlack();
anthonylamme 3:04f91137660a 84 d=distance(a,b,black);
codestar 4:61edb416e860 85
codestar 4:61edb416e860 86 PC.printf("%d \n" , a );
codestar 4:61edb416e860 87 PC.printf("%d \n", b );
codestar 4:61edb416e860 88 PC.printf("%d \n", black);
codestar 4:61edb416e860 89 PC.printf("%d \n" , d );
codestar 4:61edb416e860 90
codestar 4:61edb416e860 91 didInit = true; // sets initialzation to true
anthonylamme 3:04f91137660a 92 }
anthonylamme 3:04f91137660a 93 }
anthonylamme 3:04f91137660a 94 TFC_SetServo(0,0.0);
codestar 4:61edb416e860 95
codestar 4:61edb416e860 96 // this is test code to have the car print if a turn case happened.
codestar 4:61edb416e860 97
codestar 4:61edb416e860 98 if(didInit) // check to see if initalized
codestar 4:61edb416e860 99 {
codestar 4:61edb416e860 100 if(d < distance(a,b,black))
codestar 4:61edb416e860 101 {
codestar 4:61edb416e860 102 int whichTurn = 2; // impossible for function to return
codestar 4:61edb416e860 103
codestar 4:61edb416e860 104 whichTurn = turnDir(a,b,black,d); // sets which turn to 0 for left and 1 for right
codestar 4:61edb416e860 105
codestar 4:61edb416e860 106 PC.printf("%d \n", whichTurn);
codestar 4:61edb416e860 107 }
codestar 4:61edb416e860 108 }
anthonylamme 3:04f91137660a 109 break;
redxeth 0:6432166d0781 110
codestar 4:61edb416e860 111 case 1: // moves servo and prints pot value.
redxeth 0:6432166d0781 112
codestar 4:61edb416e860 113
redxeth 0:6432166d0781 114 if(TFC_Ticker[0]>=20)
redxeth 0:6432166d0781 115 {
redxeth 0:6432166d0781 116 TFC_Ticker[0] = 0; //reset the Ticker
redxeth 0:6432166d0781 117 //Every 20 mSeconds, update the Servos
redxeth 0:6432166d0781 118 TFC_SetServo(0,TFC_ReadPot(0));
redxeth 0:6432166d0781 119 TFC_SetServo(1,TFC_ReadPot(1));
codestar 1:4d7f3b5b7463 120
codestar 1:4d7f3b5b7463 121
redxeth 0:6432166d0781 122 }
codestar 1:4d7f3b5b7463 123
codestar 1:4d7f3b5b7463 124 if(TFC_PUSH_BUTTON_1_PRESSED)
codestar 1:4d7f3b5b7463 125 {
codestar 1:4d7f3b5b7463 126
codestar 4:61edb416e860 127 PC.printf("%f", TFC_ReadPot(0));
codestar 1:4d7f3b5b7463 128 }
codestar 1:4d7f3b5b7463 129
codestar 1:4d7f3b5b7463 130
redxeth 0:6432166d0781 131 //Let's put a pattern on the LEDs
redxeth 0:6432166d0781 132 if(TFC_Ticker[1] >= 125)
redxeth 0:6432166d0781 133 {
redxeth 0:6432166d0781 134 TFC_Ticker[1] = 0;
redxeth 0:6432166d0781 135 t++;
redxeth 0:6432166d0781 136 if(t>4)
redxeth 0:6432166d0781 137 {
redxeth 0:6432166d0781 138 t=0;
redxeth 0:6432166d0781 139 }
redxeth 0:6432166d0781 140 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 141 }
redxeth 0:6432166d0781 142
codestar 1:4d7f3b5b7463 143 // TFC_SetMotorPWM(0,0); //Make sure motors are off
codestar 1:4d7f3b5b7463 144 // TFC_HBRIDGE_DISABLE;
redxeth 0:6432166d0781 145
redxeth 0:6432166d0781 146
redxeth 0:6432166d0781 147 break;
redxeth 0:6432166d0781 148
redxeth 0:6432166d0781 149 case 2 :
codestar 1:4d7f3b5b7463 150
codestar 1:4d7f3b5b7463 151
codestar 1:4d7f3b5b7463 152 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
codestar 1:4d7f3b5b7463 153 //Labview Application
redxeth 0:6432166d0781 154
redxeth 0:6432166d0781 155
codestar 1:4d7f3b5b7463 156 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
codestar 1:4d7f3b5b7463 157 {
codestar 1:4d7f3b5b7463 158 TFC_Ticker[0] = 0;
codestar 1:4d7f3b5b7463 159 TFC_LineScanImageReady=0;
codestar 4:61edb416e860 160 // PC.printf("\r\n");
codestar 4:61edb416e860 161 // PC.printf("L:");
codestar 1:4d7f3b5b7463 162
codestar 1:4d7f3b5b7463 163 if(t==0)
codestar 1:4d7f3b5b7463 164 t=4;
codestar 1:4d7f3b5b7463 165 else
codestar 1:4d7f3b5b7463 166 t--;
codestar 1:4d7f3b5b7463 167
codestar 1:4d7f3b5b7463 168 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 169
codestar 1:4d7f3b5b7463 170 // camera 1
codestar 1:4d7f3b5b7463 171
codestar 1:4d7f3b5b7463 172 if(TFC_PUSH_BUTTON_0_PRESSED)
codestar 1:4d7f3b5b7463 173 {
codestar 2:729063e39fb3 174
codestar 1:4d7f3b5b7463 175
codestar 2:729063e39fb3 176 black = FindBlack();
codestar 1:4d7f3b5b7463 177
codestar 4:61edb416e860 178 PC.printf("%i", black);
codestar 1:4d7f3b5b7463 179
codestar 1:4d7f3b5b7463 180
codestar 1:4d7f3b5b7463 181
redxeth 0:6432166d0781 182 break;
redxeth 0:6432166d0781 183
redxeth 0:6432166d0781 184 case 3 :
redxeth 0:6432166d0781 185
redxeth 0:6432166d0781 186
redxeth 0:6432166d0781 187 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
redxeth 0:6432166d0781 188 //Labview Application
redxeth 0:6432166d0781 189
redxeth 0:6432166d0781 190
redxeth 0:6432166d0781 191 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
redxeth 0:6432166d0781 192 {
redxeth 0:6432166d0781 193 TFC_Ticker[0] = 0;
redxeth 0:6432166d0781 194 TFC_LineScanImageReady=0;
codestar 4:61edb416e860 195 // PC.printf("\r\n");
codestar 4:61edb416e860 196 // PC.printf("L:");
redxeth 0:6432166d0781 197
redxeth 0:6432166d0781 198 if(t==0)
redxeth 0:6432166d0781 199 t=4;
redxeth 0:6432166d0781 200 else
redxeth 0:6432166d0781 201 t--;
redxeth 0:6432166d0781 202
redxeth 0:6432166d0781 203 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 204
redxeth 0:6432166d0781 205 // camera 1
codestar 1:4d7f3b5b7463 206
codestar 1:4d7f3b5b7463 207 if(TFC_PUSH_BUTTON_0_PRESSED)
codestar 1:4d7f3b5b7463 208 {
codestar 1:4d7f3b5b7463 209 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
codestar 1:4d7f3b5b7463 210 //Labview Application
codestar 1:4d7f3b5b7463 211
codestar 1:4d7f3b5b7463 212
codestar 1:4d7f3b5b7463 213 // camera 1
redxeth 0:6432166d0781 214 for(i=0;i<128;i++)
redxeth 0:6432166d0781 215 {
codestar 1:4d7f3b5b7463 216 if(TFC_LineScanImage0[i]<=black)
codestar 1:4d7f3b5b7463 217 {
codestar 2:729063e39fb3 218 // zero is black
codestar 2:729063e39fb3 219
codestar 1:4d7f3b5b7463 220 data[i]=0;
codestar 1:4d7f3b5b7463 221 }
codestar 1:4d7f3b5b7463 222 else
codestar 1:4d7f3b5b7463 223 {
codestar 2:729063e39fb3 224 // one is white
codestar 1:4d7f3b5b7463 225 data[i]=1;
codestar 1:4d7f3b5b7463 226 }
codestar 4:61edb416e860 227 PC.printf("%d", data[i]);
redxeth 0:6432166d0781 228 }
codestar 4:61edb416e860 229 PC.printf("\r");
redxeth 0:6432166d0781 230
redxeth 0:6432166d0781 231 // camera 2
codestar 1:4d7f3b5b7463 232 /* for(i=0;i<128;i++)
redxeth 0:6432166d0781 233 {
redxeth 0:6432166d0781 234 if(i==127)
codestar 4:61edb416e860 235 PC.printf("%X\r\n",TFC_LineScanImage1[i]);
redxeth 0:6432166d0781 236 else
codestar 4:61edb416e860 237 PC.printf("%X,",TFC_LineScanImage1[i]);
redxeth 0:6432166d0781 238
codestar 1:4d7f3b5b7463 239 } */
codestar 1:4d7f3b5b7463 240
codestar 1:4d7f3b5b7463 241 }
redxeth 0:6432166d0781 242 }
redxeth 0:6432166d0781 243
redxeth 0:6432166d0781 244 break;
redxeth 0:6432166d0781 245 }
redxeth 0:6432166d0781 246 }
redxeth 0:6432166d0781 247
redxeth 0:6432166d0781 248
redxeth 0:6432166d0781 249 }
codestar 1:4d7f3b5b7463 250 }
codestar 2:729063e39fb3 251 }
codestar 2:729063e39fb3 252
codestar 2:729063e39fb3 253 // finds and sets by reference the values of the left and right bounds
codestar 2:729063e39fb3 254
codestar 2:729063e39fb3 255 void bounds(int &a,int &b,int black)
codestar 2:729063e39fb3 256 {
codestar 2:729063e39fb3 257
codestar 2:729063e39fb3 258 for(int i=0; i<128; i++)
codestar 2:729063e39fb3 259 {
codestar 4:61edb416e860 260 if(i==0)
codestar 2:729063e39fb3 261 {
codestar 2:729063e39fb3 262 a = i;
codestar 2:729063e39fb3 263 }
codestar 2:729063e39fb3 264
codestar 4:61edb416e860 265 else if(TFC_LineScanImage0[i] <= black)
codestar 2:729063e39fb3 266 {
codestar 2:729063e39fb3 267 //if there are to black values next to eachother it sets a to newer inside value.
codestar 4:61edb416e860 268 if ( a == 0 && i!=0)
codestar 4:61edb416e860 269 a = i;
codestar 4:61edb416e860 270
codestar 4:61edb416e860 271 else if( (i-a) == 1 )
codestar 2:729063e39fb3 272 {
codestar 2:729063e39fb3 273 a = i;
codestar 2:729063e39fb3 274 }
codestar 2:729063e39fb3 275
codestar 2:729063e39fb3 276 // if there is a space between black values the next black value is the right bound.
codestar 2:729063e39fb3 277 else
codestar 2:729063e39fb3 278 {
codestar 2:729063e39fb3 279 // sets the right inside bound and ends the loop.
codestar 2:729063e39fb3 280
codestar 2:729063e39fb3 281 b = i;
codestar 2:729063e39fb3 282 break;
codestar 2:729063e39fb3 283 }
codestar 2:729063e39fb3 284 }
codestar 2:729063e39fb3 285 }
codestar 2:729063e39fb3 286 }
codestar 2:729063e39fb3 287
codestar 2:729063e39fb3 288
codestar 2:729063e39fb3 289
codestar 2:729063e39fb3 290
codestar 2:729063e39fb3 291 int FindBlack()
codestar 2:729063e39fb3 292 {
codestar 2:729063e39fb3 293 int low1=2000,low2=2000;
codestar 2:729063e39fb3 294 for(int i=2;i<64;i++)//first half of line scan
codestar 2:729063e39fb3 295 {
codestar 2:729063e39fb3 296 int ave=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
codestar 2:729063e39fb3 297 if(i==2)
codestar 2:729063e39fb3 298 {
codestar 2:729063e39fb3 299 low1=ave;//first loop sets lowest average
codestar 2:729063e39fb3 300 }
codestar 2:729063e39fb3 301 if(low1>ave)
codestar 2:729063e39fb3 302 {
codestar 2:729063e39fb3 303 low1=ave;
codestar 2:729063e39fb3 304 }
codestar 2:729063e39fb3 305 }
codestar 2:729063e39fb3 306 for(int i=66;i<128;i++)//second half of line scan
codestar 2:729063e39fb3 307 {
codestar 2:729063e39fb3 308 int ave2=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
codestar 2:729063e39fb3 309 if(i==66)
codestar 2:729063e39fb3 310 {
codestar 2:729063e39fb3 311 low2=ave2;
codestar 2:729063e39fb3 312 }
codestar 2:729063e39fb3 313 if(low2>ave2)
codestar 2:729063e39fb3 314 {
codestar 2:729063e39fb3 315 low2=ave2;
codestar 2:729063e39fb3 316 }
codestar 2:729063e39fb3 317 if(low1==low2)
codestar 2:729063e39fb3 318 {
codestar 2:729063e39fb3 319 return low1; //confirms bothe lines and breaks loop returns black
codestar 2:729063e39fb3 320 }
codestar 2:729063e39fb3 321 }
codestar 2:729063e39fb3 322 if(low1>low2)
codestar 2:729063e39fb3 323 {
codestar 2:729063e39fb3 324 return low1;
codestar 2:729063e39fb3 325 }
codestar 2:729063e39fb3 326 else
codestar 2:729063e39fb3 327 {
codestar 2:729063e39fb3 328 return low2;
codestar 2:729063e39fb3 329 }
anthonylamme 3:04f91137660a 330 }
anthonylamme 3:04f91137660a 331 int distance(int &a, int &b, int black)
anthonylamme 3:04f91137660a 332 {
anthonylamme 3:04f91137660a 333 bounds(a,b,black);
codestar 4:61edb416e860 334 return (b-a);
anthonylamme 3:04f91137660a 335 }
codestar 4:61edb416e860 336
codestar 4:61edb416e860 337 int turnDir( int &a, int &b, int black, int d)
codestar 4:61edb416e860 338 {
codestar 4:61edb416e860 339 int l = 200, r = 200;
codestar 4:61edb416e860 340
codestar 4:61edb416e860 341 bounds(a,b,black); // resets bounds for start of program
codestar 4:61edb416e860 342
codestar 4:61edb416e860 343 while(d != distance(l,r,black)) // keeps checking rate of change while approaching turn
codestar 4:61edb416e860 344 {
codestar 4:61edb416e860 345 if( abs(a-l) > abs(b-r) ) // if the change in the left direction is greater than the right
codestar 4:61edb416e860 346 {
codestar 4:61edb416e860 347 return 0; // this returns left
codestar 4:61edb416e860 348 }
codestar 4:61edb416e860 349
codestar 4:61edb416e860 350 if( abs(a-l) < abs(b-r)) // if the change in the right direction is greater than the left
codestar 4:61edb416e860 351 {
codestar 4:61edb416e860 352 return 1; // this returns right
codestar 4:61edb416e860 353 }
codestar 4:61edb416e860 354
codestar 4:61edb416e860 355 }
codestar 4:61edb416e860 356 }
codestar 4:61edb416e860 357
codestar 4:61edb416e860 358 /*void turnLeft(int &a, int &b, black, d)
codestar 4:61edb416e860 359 {
codestar 4:61edb416e860 360 // waits for the amount of time it takes to reach the turn
codestar 4:61edb416e860 361 wait(.146);
codestar 4:61edb416e860 362
codestar 4:61edb416e860 363
codestar 4:61edb416e860 364 // turns the wheels to the angle they need to be
codestar 4:61edb416e860 365 // while the distance between the lines
codestar 4:61edb416e860 366 // indicate the car is in a turn
codestar 4:61edb416e860 367
codestar 4:61edb416e860 368 while(d > distance(a, b, black) )
codestar 4:61edb416e860 369 {
codestar 4:61edb416e860 370 setservo( turn value);
codestar 4:61edb416e860 371 }
codestar 4:61edb416e860 372
codestar 4:61edb416e860 373 // returns the wheels back to straight
codestar 4:61edb416e860 374
codestar 4:61edb416e860 375 setservo(0,0.0);
codestar 4:61edb416e860 376 }
codestar 4:61edb416e860 377
codestar 4:61edb416e860 378 */
anthonylamme 3:04f91137660a 379