Diff: main.cpp

Revision:

1:1dd8d5ffdd02

Parent:

0:6432166d0781

--- a/main.cpp	Tue Apr 15 02:38:04 2014 +0000
+++ b/main.cpp	Wed Oct 08 22:50:52 2014 +0000
@@ -4,7 +4,7 @@
  
 //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 ref_val 5
 #define TERMINAL_PRINTF     PC.printf
 
  
@@ -15,7 +15,11 @@
 Ticker TFC_TickerObj;
  
 volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
- 
+ float servo_angle=0.0;
+ float vision[129];
+ unsigned int n_izq=0;
+ unsigned int n_der=0;
+ int error=0;
 void TFC_TickerUpdate()
 {
     int i;
@@ -48,125 +52,67 @@
 
             //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
             //Let's look at the middle 2 switches
-            switch((TFC_GetDIP_Switch()>>1)&0x03)
-            {
-            default:
-            case 0 :
-                //Demo mode 0 just tests the switches and LED's
-                if(TFC_PUSH_BUTTON_0_PRESSED)
-                    TFC_BAT_LED0_ON;
-                else
-                    TFC_BAT_LED0_OFF;
-                
-                if(TFC_PUSH_BUTTON_1_PRESSED)
-                    TFC_BAT_LED3_ON;
-                else
-                    TFC_BAT_LED3_OFF;
-                
-                
-                if(TFC_GetDIP_Switch()&0x01)
-                    TFC_BAT_LED1_ON;
-                else
-                    TFC_BAT_LED1_OFF;
-                
-                if(TFC_GetDIP_Switch()&0x08)
-                    TFC_BAT_LED2_ON;
-                else
-                    TFC_BAT_LED2_OFF;
-                
-                break;
-                    
-            case 1:
-                
-                //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
-                    //Every 20 mSeconds, update the Servos
-                    TFC_SetServo(0,TFC_ReadPot(0));
-                    TFC_SetServo(1,TFC_ReadPot(1));
-                }
-                //Let's put a pattern on the LEDs
-                if(TFC_Ticker[1] >= 125)
-                {
-                    TFC_Ticker[1] = 0;
-                    t++;
-                    if(t>4)
-                    {
-                        t=0;
-                    }           
-                    TFC_SetBatteryLED_Level(t);
-                }
-                
-                TFC_SetMotorPWM(0,0); //Make sure motors are off
-                TFC_HBRIDGE_DISABLE;
             
-
-                break;
-                
-            case 2 :
                 
                 //Demo Mode 2 will use the Pots to make the motors move
                 TFC_HBRIDGE_ENABLE;
                
-                TFC_SetMotorPWM(TFC_ReadPot(0),TFC_ReadPot(1));
+                 TFC_SetMotorPWM(TFC_ReadPot(1),TFC_ReadPot(1));
                 
-                        
-                //Let's put a pattern on the LEDs
-                if(TFC_Ticker[1] >= 125)
+                 if(TFC_Ticker[0]>=20)
+                {
+                    TFC_Ticker[0] = 0; //reset the Ticker
+                    //Every 20 mSeconds, update the Servos
+                    TFC_SetServo(0,servo_angle);
+                    n_der=0;
+                }     
+               
+                if(TFC_Ticker[1]>50 && TFC_LineScanImageReady>0)
                     {
-                        TFC_Ticker[1] = 0;
-                            t++;
-                            if(t>4)
-                            {
-                                t=0;
-                            }           
-                        TFC_SetBatteryLED_Level(t);
-                    }
-                break;
-            
-            case 3 :
-            
-         
-                //Demo Mode 3 will be in Freescale Garage Mode.  It will beam data from the Camera to the 
-                //Labview Application
-                
-                
-                if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
-                    {
-                     TFC_Ticker[0] = 0;
+                     TFC_Ticker[1] = 0;
                      TFC_LineScanImageReady=0;
-                     TERMINAL_PRINTF("\r\n");
-                     TERMINAL_PRINTF("L:");
-                     
                         if(t==0)
                             t=4;
                         else
                             t--;
-                        
                          TFC_SetBatteryLED_Level(t);
                         
                          // camera 1
                          for(i=0;i<128;i++)
-                         {
-                               TERMINAL_PRINTF("%X,",TFC_LineScanImage0[i]);
+                            
+                         {if(TFC_LineScanImage0[i]<=1250)
+                            vision[i]=0;
+                            else 
+                               vision[i]= 1;
                          }
                         
-                        // camera 2
-                         for(i=0;i<128;i++)
-                         {
-                                 if(i==127)
-                                     TERMINAL_PRINTF("%X\r\n",TFC_LineScanImage1[i]);
-                                 else
-                                     TERMINAL_PRINTF("%X,",TFC_LineScanImage1[i]);
-                           
-                         }
+                        
+                    }
+                    for(i=65;i<70;i++)
+                    {if(vision[i]==1)
+                        n_der++;
+                        else break;
                     }
-                 
-                break;
-            }
+                    //error= 5-n_der;
+                    //if(error>5) error=(error-5)*-1;
+                    //servo_angle=0.09*error;
+                   
+                    if(n_der<=7)
+                    servo_angle=0.30;
+                    else if(n_der<=12 && n_der>=8)servo_angle=0;
+                    else if (n_der>=13)servo_angle=-0.30;
+                    
+                    if(TFC_Ticker[1]>1000&&(vision[63]==0||vision[67]))
+                        servo_angle*=-1.0;
+                    /*if(n_der<3)
+                    servo_angle=0.25;
+                    else if(n_der<=7 && n_der>=3)servo_angle=0;
+                    else if (n_der>7)servo_angle=-0.25;*/
+                    /*if(n_der<4 && n_der>0)
+                    servo_angle=-0.3;
+                    //else if(n_der<=6 && n_der>=4)servo_angle=0;
+                    else if(n_der==5)servo_angle=0;
+                    else if (n_der>6 && n_der<10)servo_angle=0.3;*/
     }
-    
- 
 }