Diff: main.cpp

Revision:

4:61edb416e860

Parent:

3:04f91137660a

--- 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