Anthony Lamme / TFC-TEST_TXSTATE_TEAM6v2

added stuff

Dependencies:   FRDM-TFC

Fork of TFC-TEST_TXSTATE_TEAM6 by Cody Blakeney

main.cpp@5:0c142433cc51, 2015-04-06 (annotated)

Committer:
anthonylamme
Date:
Mon Apr 06 23:22:53 2015 +0000
Revision:
5:0c142433cc51
Parent:
4:97014a4611c2 
fixed errors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
anthonylamme 4:97014a4611c2 1 //made case 2 which is has our main
redxeth 0:6432166d0781 2 #include "mbed.h"
redxeth 0:6432166d0781 3 #include "TFC.h"
anthonylamme 4:97014a4611c2 4 #include <cmath>
redxeth 0:6432166d0781 5
redxeth 0:6432166d0781 6
redxeth 0:6432166d0781 7 //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 8 Serial PC(USBTX,USBRX);
redxeth 0:6432166d0781 9
redxeth 0:6432166d0781 10
redxeth 0:6432166d0781 11
redxeth 0:6432166d0781 12 //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 13
redxeth 0:6432166d0781 14 #define NUM_TFC_TICKERS 4
redxeth 0:6432166d0781 15
redxeth 0:6432166d0781 16 Ticker TFC_TickerObj;
redxeth 0:6432166d0781 17
redxeth 0:6432166d0781 18 volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
redxeth 0:6432166d0781 19
redxeth 0:6432166d0781 20 void TFC_TickerUpdate()
redxeth 0:6432166d0781 21 {
redxeth 0:6432166d0781 22 int i;
redxeth 0:6432166d0781 23
redxeth 0:6432166d0781 24 for(i=0; i<NUM_TFC_TICKERS; i++)
redxeth 0:6432166d0781 25 {
redxeth 0:6432166d0781 26 if(TFC_Ticker[i]<0xFFFFFFFF)
redxeth 0:6432166d0781 27 {
redxeth 0:6432166d0781 28 TFC_Ticker[i]++;
redxeth 0:6432166d0781 29 }
redxeth 0:6432166d0781 30 }
redxeth 0:6432166d0781 31 }
redxeth 0:6432166d0781 32
codestar 2:729063e39fb3 33 //finds light intensity values of lines.
redxeth 0:6432166d0781 34
codestar 2:729063e39fb3 35 int FindBlack();
codestar 2:729063e39fb3 36
codestar 2:729063e39fb3 37 // finds and sets by reference the values of the left and right bounds
codestar 2:729063e39fb3 38 void Bounds(int &, int &, int);
anthonylamme 4:97014a4611c2 39
anthonylamme 4:97014a4611c2 40 // finds distance between bounds
anthonylamme 3:04f91137660a 41 int distance(int &, int &, int);
anthonylamme 4:97014a4611c2 42
anthonylamme 4:97014a4611c2 43 // returns interger for turn direction 0 for left 1 for right
anthonylamme 4:97014a4611c2 44
anthonylamme 4:97014a4611c2 45 int turnDir( int &, int &, int, int);
anthonylamme 4:97014a4611c2 46 int startstop(int);
anthonylamme 4:97014a4611c2 47
anthonylamme 5:0c142433cc51 48 void turnLeft(int &, int &, int, int); //needs to be finished
anthonylamme 5:0c142433cc51 49 void turnRight(int &, int &, int, int); //needs to be finished
redxeth 0:6432166d0781 50
redxeth 0:6432166d0781 51 int main()
redxeth 0:6432166d0781 52 {
codestar 1:4d7f3b5b7463 53 uint32_t i,t,time = 0;
codestar 1:4d7f3b5b7463 54
anthonylamme 4:97014a4611c2 55 int a = 200, b = 200, d = 200;
codestar 2:729063e39fb3 56
codestar 1:4d7f3b5b7463 57 int data[128];
codestar 1:4d7f3b5b7463 58
codestar 1:4d7f3b5b7463 59 int black = 0;
redxeth 0:6432166d0781 60
codestar 1:4d7f3b5b7463 61 PC.baud(9600);
redxeth 0:6432166d0781 62 TFC_TickerObj.attach_us(&TFC_TickerUpdate,2000);
redxeth 0:6432166d0781 63
redxeth 0:6432166d0781 64 TFC_Init();
redxeth 0:6432166d0781 65
anthonylamme 4:97014a4611c2 66 bool didInit = false ; // flag for track calibration
codestar 1:4d7f3b5b7463 67
redxeth 0:6432166d0781 68 for(;;)
redxeth 0:6432166d0781 69 {
redxeth 0:6432166d0781 70 //TFC_Task must be called in your main loop. This keeps certain processing happy (I.E. Serial port queue check)
redxeth 0:6432166d0781 71 // TFC_Task();
redxeth 0:6432166d0781 72
redxeth 0:6432166d0781 73 //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
redxeth 0:6432166d0781 74 //Let's look at the middle 2 switches
redxeth 0:6432166d0781 75 switch((TFC_GetDIP_Switch()>>1)&0x03)
redxeth 0:6432166d0781 76 {
redxeth 0:6432166d0781 77 default:
anthonylamme 3:04f91137660a 78 case 0 ://initilazation case
anthonylamme 3:04f91137660a 79 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
anthonylamme 3:04f91137660a 80 {
anthonylamme 3:04f91137660a 81 TFC_Ticker[0] = 0;
anthonylamme 3:04f91137660a 82 TFC_LineScanImageReady=0;
anthonylamme 4:97014a4611c2 83 if(TFC_PUSH_BUTTON_0_PRESSED&& didInit == false)
anthonylamme 3:04f91137660a 84 {
anthonylamme 3:04f91137660a 85 black = FindBlack();
anthonylamme 3:04f91137660a 86 d=distance(a,b,black);
anthonylamme 4:97014a4611c2 87
anthonylamme 4:97014a4611c2 88 PC.printf("%d \n" , a );
anthonylamme 4:97014a4611c2 89 PC.printf("%d \n", b );
anthonylamme 4:97014a4611c2 90 PC.printf("%d \n", black);
anthonylamme 4:97014a4611c2 91 PC.printf("%d \n" , d );
anthonylamme 4:97014a4611c2 92
anthonylamme 4:97014a4611c2 93 didInit = true; // sets initialzation to true
anthonylamme 3:04f91137660a 94 }
anthonylamme 3:04f91137660a 95 }
anthonylamme 3:04f91137660a 96 TFC_SetServo(0,0.0);
anthonylamme 4:97014a4611c2 97
anthonylamme 4:97014a4611c2 98 // this is test code to have the car print if a turn case happened.
anthonylamme 4:97014a4611c2 99
anthonylamme 4:97014a4611c2 100 if(didInit) // check to see if initalized
anthonylamme 4:97014a4611c2 101 {
anthonylamme 4:97014a4611c2 102 if(d < distance(a,b,black))
anthonylamme 4:97014a4611c2 103 {
anthonylamme 4:97014a4611c2 104 int whichTurn = 2; // impossible for function to return
anthonylamme 4:97014a4611c2 105
anthonylamme 4:97014a4611c2 106 whichTurn = turnDir(a,b,black,d); // sets which turn to 0 for left and 1 for right
anthonylamme 4:97014a4611c2 107
anthonylamme 4:97014a4611c2 108 PC.printf("%d \n", whichTurn);
anthonylamme 4:97014a4611c2 109 }
anthonylamme 4:97014a4611c2 110 }
anthonylamme 3:04f91137660a 111 break;
redxeth 0:6432166d0781 112
anthonylamme 4:97014a4611c2 113 case 1: // moves servo and prints pot value.
redxeth 0:6432166d0781 114 break;
redxeth 0:6432166d0781 115
anthonylamme 4:97014a4611c2 116 case 2 ://case 2 will run program after case 0
anthonylamme 4:97014a4611c2 117 TFC_HBRIDGE_ENABLE;
anthonylamme 4:97014a4611c2 118 if(TFC_PUSH_BUTTON_0_PRESSED)
anthonylamme 4:97014a4611c2 119 {
anthonylamme 4:97014a4611c2 120 while(true)
codestar 1:4d7f3b5b7463 121 {
anthonylamme 5:0c142433cc51 122 //dark=FindBlack();//determines if its dark
anthonylamme 5:0c142433cc51 123 while((d < (distance(a,b,black)*1.02))&&(d > (distance(a,b,black)*.98)))//if straight line it just goes forward
anthonylamme 4:97014a4611c2 124 {
anthonylamme 4:97014a4611c2 125 TFC_SetMotorPWM(.6,.6);
anthonylamme 4:97014a4611c2 126 }
anthonylamme 4:97014a4611c2 127 if(turnDir(a,b,black,d)==1)//straight line is broken determine direction if 1 go right
anthonylamme 4:97014a4611c2 128 {
anthonylamme 5:0c142433cc51 129 turnRight(a,b,black,d);
anthonylamme 4:97014a4611c2 130 }
anthonylamme 4:97014a4611c2 131 if(turnDir(a,b,black,d)==0)//straight line is broken determine direction if 0 go right
anthonylamme 4:97014a4611c2 132 {
anthonylamme 5:0c142433cc51 133 turnLeft(a,b,black,d);
anthonylamme 4:97014a4611c2 134 }
anthonylamme 5:0c142433cc51 135 /*if(FindBlack()>black)//if white line then find black will return starting value because no black line // change to findblack()>black
anthonylamme 4:97014a4611c2 136 {
anthonylamme 4:97014a4611c2 137 TFC_SetMotorPWM(.6,.6);//go straight for given time
anthonylamme 4:97014a4611c2 138 wait(0.146);
anthonylamme 4:97014a4611c2 139 continue;// start loop over and see where you are
anthonylamme 4:97014a4611c2 140 }
anthonylamme 4:97014a4611c2 141 if(dark>black)//determine if hill then slow down until hill is over
anthonylamme 4:97014a4611c2 142 {
anthonylamme 4:97014a4611c2 143 TFC_SetMotorPWM(0,0);
anthonylamme 4:97014a4611c2 144 wait(0.146);
anthonylamme 4:97014a4611c2 145 continue;
anthonylamme 4:97014a4611c2 146 }
anthonylamme 4:97014a4611c2 147 if(dark<black)//determine if dark then go foreword
anthonylamme 4:97014a4611c2 148 {
anthonylamme 4:97014a4611c2 149 TFC_SetMotorPWM(.6,.6);
anthonylamme 4:97014a4611c2 150 wait(0.143);
anthonylamme 4:97014a4611c2 151 continue;
anthonylamme 4:97014a4611c2 152 }
anthonylamme 5:0c142433cc51 153 if(startstop()==1)// if start line is seen stop
anthonylamme 4:97014a4611c2 154 {
anthonylamme 4:97014a4611c2 155 TFC_SetMotorPWM(0,0);
anthonylamme 4:97014a4611c2 156 TFC_HBRIDGE_DISABLE;
anthonylamme 4:97014a4611c2 157 break;
anthonylamme 4:97014a4611c2 158 }
anthonylamme 5:0c142433cc51 159 */
anthonylamme 4:97014a4611c2 160 }
anthonylamme 4:97014a4611c2 161 }
redxeth 0:6432166d0781 162 break;
redxeth 0:6432166d0781 163
redxeth 0:6432166d0781 164 case 3 :
redxeth 0:6432166d0781 165 break;
redxeth 0:6432166d0781 166 }
redxeth 0:6432166d0781 167 }
redxeth 0:6432166d0781 168
redxeth 0:6432166d0781 169
redxeth 0:6432166d0781 170 }
anthonylamme 4:97014a4611c2 171
codestar 2:729063e39fb3 172
codestar 2:729063e39fb3 173 // finds and sets by reference the values of the left and right bounds
codestar 2:729063e39fb3 174
codestar 2:729063e39fb3 175 void bounds(int &a,int &b,int black)
codestar 2:729063e39fb3 176 {
codestar 2:729063e39fb3 177 for(int i=0; i<128; i++)
codestar 2:729063e39fb3 178 {
anthonylamme 4:97014a4611c2 179 if(i==0)
codestar 2:729063e39fb3 180 {
codestar 2:729063e39fb3 181 a = i;
codestar 2:729063e39fb3 182 }
codestar 2:729063e39fb3 183
anthonylamme 4:97014a4611c2 184 else if(TFC_LineScanImage0[i] <= black)
codestar 2:729063e39fb3 185 {
codestar 2:729063e39fb3 186 //if there are to black values next to eachother it sets a to newer inside value.
anthonylamme 4:97014a4611c2 187 if ( a == 0 && i!=0)
anthonylamme 4:97014a4611c2 188 a = i;
anthonylamme 4:97014a4611c2 189
anthonylamme 4:97014a4611c2 190 else if( (i-a) == 1 )
codestar 2:729063e39fb3 191 {
codestar 2:729063e39fb3 192 a = i;
codestar 2:729063e39fb3 193 }
codestar 2:729063e39fb3 194
codestar 2:729063e39fb3 195 // if there is a space between black values the next black value is the right bound.
codestar 2:729063e39fb3 196 else
codestar 2:729063e39fb3 197 {
codestar 2:729063e39fb3 198 // sets the right inside bound and ends the loop.
anthonylamme 4:97014a4611c2 199
codestar 2:729063e39fb3 200 b = i;
codestar 2:729063e39fb3 201 break;
codestar 2:729063e39fb3 202 }
codestar 2:729063e39fb3 203 }
codestar 2:729063e39fb3 204 }
codestar 2:729063e39fb3 205 }
codestar 2:729063e39fb3 206 int FindBlack()
codestar 2:729063e39fb3 207 {
codestar 2:729063e39fb3 208 int low1=2000,low2=2000;
codestar 2:729063e39fb3 209 for(int i=2;i<64;i++)//first half of line scan
codestar 2:729063e39fb3 210 {
codestar 2:729063e39fb3 211 int ave=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
codestar 2:729063e39fb3 212 if(i==2)
codestar 2:729063e39fb3 213 {
codestar 2:729063e39fb3 214 low1=ave;//first loop sets lowest average
codestar 2:729063e39fb3 215 }
codestar 2:729063e39fb3 216 if(low1>ave)
codestar 2:729063e39fb3 217 {
codestar 2:729063e39fb3 218 low1=ave;
codestar 2:729063e39fb3 219 }
codestar 2:729063e39fb3 220 }
codestar 2:729063e39fb3 221 for(int i=66;i<128;i++)//second half of line scan
codestar 2:729063e39fb3 222 {
codestar 2:729063e39fb3 223 int ave2=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
codestar 2:729063e39fb3 224 if(i==66)
codestar 2:729063e39fb3 225 {
codestar 2:729063e39fb3 226 low2=ave2;
codestar 2:729063e39fb3 227 }
codestar 2:729063e39fb3 228 if(low2>ave2)
codestar 2:729063e39fb3 229 {
codestar 2:729063e39fb3 230 low2=ave2;
codestar 2:729063e39fb3 231 }
codestar 2:729063e39fb3 232 if(low1==low2)
codestar 2:729063e39fb3 233 {
codestar 2:729063e39fb3 234 return low1; //confirms bothe lines and breaks loop returns black
codestar 2:729063e39fb3 235 }
codestar 2:729063e39fb3 236 }
codestar 2:729063e39fb3 237 if(low1>low2)
codestar 2:729063e39fb3 238 {
codestar 2:729063e39fb3 239 return low1;
codestar 2:729063e39fb3 240 }
codestar 2:729063e39fb3 241 else
codestar 2:729063e39fb3 242 {
codestar 2:729063e39fb3 243 return low2;
codestar 2:729063e39fb3 244 }
anthonylamme 3:04f91137660a 245 }
anthonylamme 3:04f91137660a 246 int distance(int &a, int &b, int black)
anthonylamme 3:04f91137660a 247 {
anthonylamme 3:04f91137660a 248 bounds(a,b,black);
anthonylamme 4:97014a4611c2 249 return (b-a);
anthonylamme 3:04f91137660a 250 }
anthonylamme 4:97014a4611c2 251
anthonylamme 4:97014a4611c2 252 int turnDir( int &a, int &b, int black, int d)
anthonylamme 4:97014a4611c2 253 {
anthonylamme 4:97014a4611c2 254 int l = 200, r = 200;
anthonylamme 4:97014a4611c2 255
anthonylamme 4:97014a4611c2 256 bounds(a,b,black); // resets bounds for start of program
anthonylamme 4:97014a4611c2 257
anthonylamme 4:97014a4611c2 258 while(d != distance(l,r,black)) // keeps checking rate of change while approaching turn
anthonylamme 4:97014a4611c2 259 {
anthonylamme 4:97014a4611c2 260 if( abs(a-l) > abs(b-r) ) // if the change in the left direction is greater than the right
anthonylamme 4:97014a4611c2 261 {
anthonylamme 4:97014a4611c2 262 return 0; // this returns left
anthonylamme 4:97014a4611c2 263 }
anthonylamme 4:97014a4611c2 264
anthonylamme 4:97014a4611c2 265 if( abs(a-l) < abs(b-r)) // if the change in the right direction is greater than the left
anthonylamme 4:97014a4611c2 266 {
anthonylamme 4:97014a4611c2 267 return 1; // this returns right
anthonylamme 4:97014a4611c2 268 }
anthonylamme 4:97014a4611c2 269
anthonylamme 4:97014a4611c2 270 }
anthonylamme 5:0c142433cc51 271 }
anthonylamme 5:0c142433cc51 272 void turnLeft(int &a, int &b,int black,int d)
anthonylamme 4:97014a4611c2 273 {
anthonylamme 4:97014a4611c2 274 // waits for the amount of time it takes to reach the turn
anthonylamme 4:97014a4611c2 275 wait(.146);
anthonylamme 4:97014a4611c2 276
anthonylamme 4:97014a4611c2 277
anthonylamme 4:97014a4611c2 278 // turns the wheels to the angle they need to be
anthonylamme 4:97014a4611c2 279 // while the distance between the lines
anthonylamme 4:97014a4611c2 280 // indicate the car is in a turn
anthonylamme 4:97014a4611c2 281
anthonylamme 5:0c142433cc51 282 while(((d < (distance(a, b, black)*1.27))&&(d!=distance(a,b,black)))||(b==128))
anthonylamme 5:0c142433cc51 283 {
anthonylamme 5:0c142433cc51 284 TFC_SetServo(0,-.214652);
anthonylamme 5:0c142433cc51 285 }
anthonylamme 5:0c142433cc51 286 // returns the wheels back to straight
anthonylamme 4:97014a4611c2 287
anthonylamme 5:0c142433cc51 288 TFC_SetServo(0,0.0);
anthonylamme 5:0c142433cc51 289 }
anthonylamme 5:0c142433cc51 290 void turnRight(int &a, int &b,int black,int d)
anthonylamme 4:97014a4611c2 291 {
anthonylamme 4:97014a4611c2 292 // waits for the amount of time it takes to reach the turn
anthonylamme 4:97014a4611c2 293 wait(.146);
anthonylamme 5:0c142433cc51 294 while(((d < (distance(a, b, black)*1.27))&&(d!=distance(a,b,black)))||(b==128))
anthonylamme 4:97014a4611c2 295 {
anthonylamme 5:0c142433cc51 296 TFC_SetServo(0,.2688640);
anthonylamme 5:0c142433cc51 297 }
anthonylamme 5:0c142433cc51 298 TFC_SetServo(0,0.0);
anthonylamme 5:0c142433cc51 299 }
anthonylamme 4:97014a4611c2 300 int startstop(int black)
anthonylamme 4:97014a4611c2 301 {
anthonylamme 5:0c142433cc51 302 int data[128];
anthonylamme 4:97014a4611c2 303 int counter=0;
anthonylamme 4:97014a4611c2 304
anthonylamme 5:0c142433cc51 305 for(int i=0;i<128;i++)
anthonylamme 4:97014a4611c2 306 {
anthonylamme 4:97014a4611c2 307 if(TFC_LineScanImage0[i]<=black)
anthonylamme 5:0c142433cc51 308 { // zero is black
anthonylamme 4:97014a4611c2 309 data[i]=0;
anthonylamme 4:97014a4611c2 310 counter++;
anthonylamme 4:97014a4611c2 311 }
anthonylamme 4:97014a4611c2 312 else
anthonylamme 5:0c142433cc51 313 { // one is white
anthonylamme 4:97014a4611c2 314 data[i]=1;
anthonylamme 4:97014a4611c2 315 }
anthonylamme 4:97014a4611c2 316 }
anthonylamme 5:0c142433cc51 317 if (counter>6)
anthonylamme 4:97014a4611c2 318 {
anthonylamme 4:97014a4611c2 319 return 1;
anthonylamme 4:97014a4611c2 320 }
anthonylamme 4:97014a4611c2 321 else
anthonylamme 4:97014a4611c2 322 {
anthonylamme 4:97014a4611c2 323 return 0;
anthonylamme 4:97014a4611c2 324 }
anthonylamme 4:97014a4611c2 325 }
anthonylamme 4:97014a4611c2 326
anthonylamme 5:0c142433cc51 327