Javier Dardón
/
test_galileo
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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 |
diff -r 6432166d0781 -r 1dd8d5ffdd02 main.cpp
--- 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;*/
}
-
-
}