Javier Dardón
An example program that uses FRDM-TFC library written by Eli Hughes.
Fork of
TFC-TEST
by 
Daniel Hadad
Revision 1:1dd8d5ffdd02, committed 2014-10-08
- Comitter:
- SaverioF15
- Date:
- Wed Oct 08 22:50:52 2014 +0000
- Parent:
- 0:6432166d0781
- Commit message:
- Some tests for the freescale cup
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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;*/
}
-
-
}