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@2:729063e39fb3, 2015-04-04 (annotated)

Committer:
codestar
Date:
Sat Apr 04 21:55:09 2015 +0000
Revision:
2:729063e39fb3
Parent:
1:4d7f3b5b7463
Child:
3:04f91137660a
added FindBlack() and bounds() functions

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);
redxeth 0:6432166d0781 38
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
codestar 2:729063e39fb3 44 int a = 200, b = 200;
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:
redxeth 0:6432166d0781 67 case 0 :
codestar 1:4d7f3b5b7463 68
codestar 1:4d7f3b5b7463 69 TFC_HBRIDGE_ENABLE;
codestar 1:4d7f3b5b7463 70 TFC_SetMotorPWM(.6,.6);
redxeth 0:6432166d0781 71
redxeth 0:6432166d0781 72
redxeth 0:6432166d0781 73 case 1:
redxeth 0:6432166d0781 74
redxeth 0:6432166d0781 75 //Demo mode 1 will just move the servos with the on-board potentiometers
redxeth 0:6432166d0781 76 if(TFC_Ticker[0]>=20)
redxeth 0:6432166d0781 77 {
redxeth 0:6432166d0781 78 TFC_Ticker[0] = 0; //reset the Ticker
redxeth 0:6432166d0781 79 //Every 20 mSeconds, update the Servos
redxeth 0:6432166d0781 80 TFC_SetServo(0,TFC_ReadPot(0));
redxeth 0:6432166d0781 81 TFC_SetServo(1,TFC_ReadPot(1));
codestar 1:4d7f3b5b7463 82
codestar 1:4d7f3b5b7463 83
redxeth 0:6432166d0781 84 }
codestar 1:4d7f3b5b7463 85
codestar 1:4d7f3b5b7463 86 if(TFC_PUSH_BUTTON_1_PRESSED)
codestar 1:4d7f3b5b7463 87 {
codestar 1:4d7f3b5b7463 88
codestar 1:4d7f3b5b7463 89 TERMINAL_PRINTF("%f", TFC_ReadPot(0));
codestar 1:4d7f3b5b7463 90 }
codestar 1:4d7f3b5b7463 91
codestar 1:4d7f3b5b7463 92
redxeth 0:6432166d0781 93 //Let's put a pattern on the LEDs
redxeth 0:6432166d0781 94 if(TFC_Ticker[1] >= 125)
redxeth 0:6432166d0781 95 {
redxeth 0:6432166d0781 96 TFC_Ticker[1] = 0;
redxeth 0:6432166d0781 97 t++;
redxeth 0:6432166d0781 98 if(t>4)
redxeth 0:6432166d0781 99 {
redxeth 0:6432166d0781 100 t=0;
redxeth 0:6432166d0781 101 }
redxeth 0:6432166d0781 102 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 103 }
redxeth 0:6432166d0781 104
codestar 1:4d7f3b5b7463 105 // TFC_SetMotorPWM(0,0); //Make sure motors are off
codestar 1:4d7f3b5b7463 106 // TFC_HBRIDGE_DISABLE;
redxeth 0:6432166d0781 107
redxeth 0:6432166d0781 108
redxeth 0:6432166d0781 109 break;
redxeth 0:6432166d0781 110
redxeth 0:6432166d0781 111 case 2 :
codestar 1:4d7f3b5b7463 112
codestar 1:4d7f3b5b7463 113
codestar 1:4d7f3b5b7463 114 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
codestar 1:4d7f3b5b7463 115 //Labview Application
redxeth 0:6432166d0781 116
redxeth 0:6432166d0781 117
codestar 1:4d7f3b5b7463 118 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
codestar 1:4d7f3b5b7463 119 {
codestar 1:4d7f3b5b7463 120 TFC_Ticker[0] = 0;
codestar 1:4d7f3b5b7463 121 TFC_LineScanImageReady=0;
codestar 1:4d7f3b5b7463 122 // TERMINAL_PRINTF("\r\n");
codestar 1:4d7f3b5b7463 123 // TERMINAL_PRINTF("L:");
codestar 1:4d7f3b5b7463 124
codestar 1:4d7f3b5b7463 125 if(t==0)
codestar 1:4d7f3b5b7463 126 t=4;
codestar 1:4d7f3b5b7463 127 else
codestar 1:4d7f3b5b7463 128 t--;
codestar 1:4d7f3b5b7463 129
codestar 1:4d7f3b5b7463 130 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 131
codestar 1:4d7f3b5b7463 132 // camera 1
codestar 1:4d7f3b5b7463 133
codestar 1:4d7f3b5b7463 134 if(TFC_PUSH_BUTTON_0_PRESSED)
codestar 1:4d7f3b5b7463 135 {
codestar 2:729063e39fb3 136
codestar 1:4d7f3b5b7463 137
codestar 2:729063e39fb3 138 black = FindBlack();
codestar 1:4d7f3b5b7463 139
codestar 1:4d7f3b5b7463 140 TERMINAL_PRINTF("%i", black);
codestar 1:4d7f3b5b7463 141
codestar 1:4d7f3b5b7463 142
codestar 1:4d7f3b5b7463 143
redxeth 0:6432166d0781 144 break;
redxeth 0:6432166d0781 145
redxeth 0:6432166d0781 146 case 3 :
redxeth 0:6432166d0781 147
redxeth 0:6432166d0781 148
redxeth 0:6432166d0781 149 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
redxeth 0:6432166d0781 150 //Labview Application
redxeth 0:6432166d0781 151
redxeth 0:6432166d0781 152
redxeth 0:6432166d0781 153 if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
redxeth 0:6432166d0781 154 {
redxeth 0:6432166d0781 155 TFC_Ticker[0] = 0;
redxeth 0:6432166d0781 156 TFC_LineScanImageReady=0;
codestar 1:4d7f3b5b7463 157 // TERMINAL_PRINTF("\r\n");
codestar 1:4d7f3b5b7463 158 // TERMINAL_PRINTF("L:");
redxeth 0:6432166d0781 159
redxeth 0:6432166d0781 160 if(t==0)
redxeth 0:6432166d0781 161 t=4;
redxeth 0:6432166d0781 162 else
redxeth 0:6432166d0781 163 t--;
redxeth 0:6432166d0781 164
redxeth 0:6432166d0781 165 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 166
redxeth 0:6432166d0781 167 // camera 1
codestar 1:4d7f3b5b7463 168
codestar 1:4d7f3b5b7463 169 if(TFC_PUSH_BUTTON_0_PRESSED)
codestar 1:4d7f3b5b7463 170 {
codestar 1:4d7f3b5b7463 171 //Demo Mode 3 will be in Freescale Garage Mode. It will beam data from the Camera to the
codestar 1:4d7f3b5b7463 172 //Labview Application
codestar 1:4d7f3b5b7463 173
codestar 1:4d7f3b5b7463 174
codestar 1:4d7f3b5b7463 175 // camera 1
redxeth 0:6432166d0781 176 for(i=0;i<128;i++)
redxeth 0:6432166d0781 177 {
codestar 1:4d7f3b5b7463 178 if(TFC_LineScanImage0[i]<=black)
codestar 1:4d7f3b5b7463 179 {
codestar 2:729063e39fb3 180 // zero is black
codestar 2:729063e39fb3 181
codestar 1:4d7f3b5b7463 182 data[i]=0;
codestar 1:4d7f3b5b7463 183 }
codestar 1:4d7f3b5b7463 184 else
codestar 1:4d7f3b5b7463 185 {
codestar 2:729063e39fb3 186 // one is white
codestar 1:4d7f3b5b7463 187 data[i]=1;
codestar 1:4d7f3b5b7463 188 }
codestar 1:4d7f3b5b7463 189 TERMINAL_PRINTF("%d", data[i]);
redxeth 0:6432166d0781 190 }
codestar 1:4d7f3b5b7463 191 TERMINAL_PRINTF("\r");
redxeth 0:6432166d0781 192
redxeth 0:6432166d0781 193 // camera 2
codestar 1:4d7f3b5b7463 194 /* for(i=0;i<128;i++)
redxeth 0:6432166d0781 195 {
redxeth 0:6432166d0781 196 if(i==127)
redxeth 0:6432166d0781 197 TERMINAL_PRINTF("%X\r\n",TFC_LineScanImage1[i]);
redxeth 0:6432166d0781 198 else
redxeth 0:6432166d0781 199 TERMINAL_PRINTF("%X,",TFC_LineScanImage1[i]);
redxeth 0:6432166d0781 200
codestar 1:4d7f3b5b7463 201 } */
codestar 1:4d7f3b5b7463 202
codestar 1:4d7f3b5b7463 203 }
redxeth 0:6432166d0781 204 }
redxeth 0:6432166d0781 205
redxeth 0:6432166d0781 206 break;
redxeth 0:6432166d0781 207 }
redxeth 0:6432166d0781 208 }
redxeth 0:6432166d0781 209
redxeth 0:6432166d0781 210
redxeth 0:6432166d0781 211 }
codestar 1:4d7f3b5b7463 212 }
codestar 2:729063e39fb3 213 }
codestar 2:729063e39fb3 214
codestar 2:729063e39fb3 215 // finds and sets by reference the values of the left and right bounds
codestar 2:729063e39fb3 216
codestar 2:729063e39fb3 217 void bounds(int &a,int &b,int black)
codestar 2:729063e39fb3 218 {
codestar 2:729063e39fb3 219
codestar 2:729063e39fb3 220 for(int i=0; i<128; i++)
codestar 2:729063e39fb3 221 {
codestar 2:729063e39fb3 222 if(TFC_LineScanImage0[i]==black&& i==0)
codestar 2:729063e39fb3 223 {
codestar 2:729063e39fb3 224 a = i;
codestar 2:729063e39fb3 225 }
codestar 2:729063e39fb3 226
codestar 2:729063e39fb3 227 else if(TFC_LineScanImage0[i]==black)
codestar 2:729063e39fb3 228 {
codestar 2:729063e39fb3 229 //if there are to black values next to eachother it sets a to newer inside value.
codestar 2:729063e39fb3 230
codestar 2:729063e39fb3 231 if( (i-a) == 1 )
codestar 2:729063e39fb3 232 {
codestar 2:729063e39fb3 233 a = i;
codestar 2:729063e39fb3 234 }
codestar 2:729063e39fb3 235
codestar 2:729063e39fb3 236 // if there is a space between black values the next black value is the right bound.
codestar 2:729063e39fb3 237 else
codestar 2:729063e39fb3 238 {
codestar 2:729063e39fb3 239 // sets the right inside bound and ends the loop.
codestar 2:729063e39fb3 240
codestar 2:729063e39fb3 241 b = i;
codestar 2:729063e39fb3 242 break;
codestar 2:729063e39fb3 243 }
codestar 2:729063e39fb3 244 }
codestar 2:729063e39fb3 245 }
codestar 2:729063e39fb3 246 }
codestar 2:729063e39fb3 247
codestar 2:729063e39fb3 248
codestar 2:729063e39fb3 249
codestar 2:729063e39fb3 250
codestar 2:729063e39fb3 251 int FindBlack()
codestar 2:729063e39fb3 252 {
codestar 2:729063e39fb3 253 int low1=2000,low2=2000;
codestar 2:729063e39fb3 254 for(int i=2;i<64;i++)//first half of line scan
codestar 2:729063e39fb3 255 {
codestar 2:729063e39fb3 256 int ave=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
codestar 2:729063e39fb3 257 if(i==2)
codestar 2:729063e39fb3 258 {
codestar 2:729063e39fb3 259 low1=ave;//first loop sets lowest average
codestar 2:729063e39fb3 260 }
codestar 2:729063e39fb3 261 if(low1>ave)
codestar 2:729063e39fb3 262 {
codestar 2:729063e39fb3 263 low1=ave;
codestar 2:729063e39fb3 264 }
codestar 2:729063e39fb3 265 }
codestar 2:729063e39fb3 266 for(int i=66;i<128;i++)//second half of line scan
codestar 2:729063e39fb3 267 {
codestar 2:729063e39fb3 268 int ave2=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
codestar 2:729063e39fb3 269 if(i==66)
codestar 2:729063e39fb3 270 {
codestar 2:729063e39fb3 271 low2=ave2;
codestar 2:729063e39fb3 272 }
codestar 2:729063e39fb3 273 if(low2>ave2)
codestar 2:729063e39fb3 274 {
codestar 2:729063e39fb3 275 low2=ave2;
codestar 2:729063e39fb3 276 }
codestar 2:729063e39fb3 277 if(low1==low2)
codestar 2:729063e39fb3 278 {
codestar 2:729063e39fb3 279 return low1; //confirms bothe lines and breaks loop returns black
codestar 2:729063e39fb3 280 }
codestar 2:729063e39fb3 281 }
codestar 2:729063e39fb3 282 if(low1>low2)
codestar 2:729063e39fb3 283 {
codestar 2:729063e39fb3 284 return low1;
codestar 2:729063e39fb3 285 }
codestar 2:729063e39fb3 286 else
codestar 2:729063e39fb3 287 {
codestar 2:729063e39fb3 288 return low2;
codestar 2:729063e39fb3 289 }
codestar 2:729063e39fb3 290 }