Cody Blakeney
added distance function, added turnDir function, started function that completes left turn
Fork of
TFC-TEST_TXSTATE_TEAM6v2
by 
Anthony Lamme
Revision 4:61edb416e860, committed 2015-04-05
- Comitter:
- codestar
- Date:
- Sun Apr 05 01:57:47 2015 +0000
- Parent:
- 3:04f91137660a
- Commit message:
- 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
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Apr 04 22:21:19 2015 +0000
+++ b/main.cpp Sun Apr 05 01:57:47 2015 +0000
@@ -1,11 +1,11 @@
#include "mbed.h"
#include "TFC.h"
+#include <cmath>
//This macro is to maintain compatibility with Codewarrior version of the sample. This version uses the MBED libraries for serial port access
Serial PC(USBTX,USBRX);
-#define TERMINAL_PRINTF PC.printf
//This ticker code is used to maintain compability with the Codewarrior version of the sample. This code uses an MBED Ticker for background timing.
@@ -35,13 +35,22 @@
// finds and sets by reference the values of the left and right bounds
void Bounds(int &, int &, int);
+
+// finds distance between bounds
int distance(int &, int &, int);
+
+// returns interger for turn direction 0 for left 1 for right
+
+int turnDir( int &, int &, int, int);
+
+//void turnLeft(int &, int &, int, int); needs to be finished
+
int main()
{
uint32_t i,t,time = 0;
- int a = 200, b = 200, d;
+ int a = 200, b = 200, d = 200;
int data[128];
@@ -52,7 +61,7 @@
TFC_Init();
- bool timeTrue = true;
+ bool didInit = false ; // flag for track calibration
for(;;)
{
@@ -69,18 +78,39 @@
{
TFC_Ticker[0] = 0;
TFC_LineScanImageReady=0;
- if(TFC_PUSH_BUTTON_0_PRESSED)
+ if(TFC_PUSH_BUTTON_0_PRESSED&& didInit == false)
{
black = FindBlack();
d=distance(a,b,black);
+
+ PC.printf("%d \n" , a );
+ PC.printf("%d \n", b );
+ PC.printf("%d \n", black);
+ PC.printf("%d \n" , d );
+
+ didInit = true; // sets initialzation to true
}
}
TFC_SetServo(0,0.0);
+
+ // this is test code to have the car print if a turn case happened.
+
+ if(didInit) // check to see if initalized
+ {
+ if(d < distance(a,b,black))
+ {
+ int whichTurn = 2; // impossible for function to return
+
+ whichTurn = turnDir(a,b,black,d); // sets which turn to 0 for left and 1 for right
+
+ PC.printf("%d \n", whichTurn);
+ }
+ }
break;
- case 1:
+ case 1: // moves servo and prints pot value.
- //Demo mode 1 will just move the servos with the on-board potentiometers
+
if(TFC_Ticker[0]>=20)
{
TFC_Ticker[0] = 0; //reset the Ticker
@@ -94,7 +124,7 @@
if(TFC_PUSH_BUTTON_1_PRESSED)
{
- TERMINAL_PRINTF("%f", TFC_ReadPot(0));
+ PC.printf("%f", TFC_ReadPot(0));
}
@@ -127,8 +157,8 @@
{
TFC_Ticker[0] = 0;
TFC_LineScanImageReady=0;
- // TERMINAL_PRINTF("\r\n");
- // TERMINAL_PRINTF("L:");
+ // PC.printf("\r\n");
+ // PC.printf("L:");
if(t==0)
t=4;
@@ -145,7 +175,7 @@
black = FindBlack();
- TERMINAL_PRINTF("%i", black);
+ PC.printf("%i", black);
@@ -162,8 +192,8 @@
{
TFC_Ticker[0] = 0;
TFC_LineScanImageReady=0;
- // TERMINAL_PRINTF("\r\n");
- // TERMINAL_PRINTF("L:");
+ // PC.printf("\r\n");
+ // PC.printf("L:");
if(t==0)
t=4;
@@ -194,17 +224,17 @@
// one is white
data[i]=1;
}
- TERMINAL_PRINTF("%d", data[i]);
+ PC.printf("%d", data[i]);
}
- TERMINAL_PRINTF("\r");
+ PC.printf("\r");
// camera 2
/* for(i=0;i<128;i++)
{
if(i==127)
- TERMINAL_PRINTF("%X\r\n",TFC_LineScanImage1[i]);
+ PC.printf("%X\r\n",TFC_LineScanImage1[i]);
else
- TERMINAL_PRINTF("%X,",TFC_LineScanImage1[i]);
+ PC.printf("%X,",TFC_LineScanImage1[i]);
} */
@@ -227,16 +257,18 @@
for(int i=0; i<128; i++)
{
- if(TFC_LineScanImage0[i]==black&& i==0)
+ if(i==0)
{
a = i;
}
- else if(TFC_LineScanImage0[i]==black)
+ else if(TFC_LineScanImage0[i] <= black)
{
//if there are to black values next to eachother it sets a to newer inside value.
-
- if( (i-a) == 1 )
+ if ( a == 0 && i!=0)
+ a = i;
+
+ else if( (i-a) == 1 )
{
a = i;
}
@@ -299,6 +331,49 @@
int distance(int &a, int &b, int black)
{
bounds(a,b,black);
- return (a-b);
+ return (b-a);
}
+
+int turnDir( int &a, int &b, int black, int d)
+ {
+ int l = 200, r = 200;
+
+ bounds(a,b,black); // resets bounds for start of program
+
+ while(d != distance(l,r,black)) // keeps checking rate of change while approaching turn
+ {
+ if( abs(a-l) > abs(b-r) ) // if the change in the left direction is greater than the right
+ {
+ return 0; // this returns left
+ }
+
+ if( abs(a-l) < abs(b-r)) // if the change in the right direction is greater than the left
+ {
+ return 1; // this returns right
+ }
+
+ }
+ }
+
+/*void turnLeft(int &a, int &b, black, d)
+ {
+ // waits for the amount of time it takes to reach the turn
+ wait(.146);
+
+
+ // turns the wheels to the angle they need to be
+ // while the distance between the lines
+ // indicate the car is in a turn
+
+ while(d > distance(a, b, black) )
+ {
+ setservo( turn value);
+ }
+
+ // returns the wheels back to straight
+
+ setservo(0,0.0);
+ }
+
+ */
\ No newline at end of file