Team Fox
/
TR_EXCITATION_TEST
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.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
BAE_CODE_MARCH_2017
by 
Team Fox
Revision 101:cb5086eb48b4, committed 2017-04-18
- Comitter:
- Bragadeesh153
- Date:
- Tue Apr 18 17:05:21 2017 +0000
- Parent:
- 100:54514f9d3de2
- Commit message:
- Use this code for testing MM excitation by Torquerod.This code updates value of PWM every 5 seconds , waits 1 second and measures Magnetic field.
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Mar 06 19:18:56 2017 +0000
+++ b/main.cpp Tue Apr 18 17:05:21 2017 +0000
@@ -536,7 +536,7 @@
extern uint8_t float_to_uint8(float min,float max,float val);
//#define print 0
-
+float set_pwm = 0;
void F_ACS()
{
ACS_MAIN_COUNTER++;
@@ -546,11 +546,17 @@
ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag
//FLAG();
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
+ set_pwm = set_pwm + 0.1;
+ if(set_pwm >= 1)
+ {set_pwm = 0;
+ }
- wait_ms(ACS_DEMAG_TIME_DELAY);
+ PWM1 = set_pwm; //clear pwm pins
+ PWM2 = set_pwm; //clear pwm pins
+ PWM3 = set_pwm; //clear pwm pins
+ printf("Set x,y,x PWM values as %f \r\n ",set_pwm*100);
+ wait(1);
+ printf("..Wait for 1 second..\r\n");
ACS_DATA_ACQ_STATUS = (uint8_t) FCTN_ATS_DATA_ACQ();
//#if print
@@ -574,220 +580,7 @@
gyro_data[i] = actual_data.AngularSpeed_actual[i]*3.14159/180;
}
- float b_inclination = mag_data[2]/sqrt(mag_data[0]*mag_data[0]+mag_data[1]*mag_data[1]+mag_data[2]*mag_data[2]);
-
- if(b_inclination <0)
- {
- b_inclination = (-1)*b_inclination;
- }
- B_SCZ_ANGLE = (uint8_t)(b_inclination*16);
- if( b_inclination >= 16)
- {
- B_SCZ_ANGLE = 0x0F;
- }
- if(b_inclination <=0)
- {
- B_SCZ_ANGLE = 0x00;
- }
- //printf("POWER MODE is : %d\r\n",actual_data.power_mode);
-
- if(ACS_STATE == 0) // check ACS_STATE = ACS_CONTROL_OFF?
- {
- #if print
- pc.printf("\n\r acs control off\n");
- #endif
- ACS_STATUS = 0; // set ACS_STATUS = ACS_CONTROL_OFF
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if((actual_data.power_mode<=2)||( (( ACS_STATE)&0x08) == 0x08))
- {
- //#if print
- pc.printf("\n\r \n\r");
- //#endif
- DRV_Z_EN = 0;
- DRV_XY_EN = 0;
- ACS_TR_Z_ENABLE = 0;
- ACS_TR_XY_ENABLE = 0;
- ACS_STATUS = 1; // set ACS_STATUS = ACS_LOW_POWER
- ACS_MAIN_STATUS = 0;
- return;
-
- }
- else if(ACS_TR_Z_SW_STATUS != 1)
- {
- pc.printf("\n\r Z disabled \n\r");
- DRV_Z_EN = 0;
- DRV_XY_EN = 0;
- ACS_TR_Z_ENABLE = 0;
- ACS_TR_XY_ENABLE = 0;
- ACS_STATUS = 2; // set ACS_STAUS = ACS_TRZ_DISABLED
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if(ACS_TR_XY_SW_STATUS != 1)
- {
- pc.printf("\n\r Z only \n\r");
- DRV_Z_EN = 1;
- DRV_XY_EN = 0;
- ACS_TR_Z_ENABLE = 1;
- ACS_TR_XY_ENABLE = 0;
- ACS_STATUS = 3; // set ACS_STAUS = ACS_TRXY_DISABLED , Z axis only
- moment[0] = 0;
- moment[1] = 0;
- moment[2] = ACS_Z_FIXED_MOMENT; // is a dummy value
-
- //timer_FCTN_ACS_GENPWM_MAIN.start();
- FCTN_ACS_GENPWM_MAIN(moment) ;
- //timer_FCTN_ACS_GENPWM_MAIN.stop();
- //pc.printf("\n\r the timer_FCTN_ACS_GENPWM_MAIN is %f",timer_FCTN_ACS_GENPWM_MAIN.read());
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if((ACS_DATA_ACQ_STATUS == 0)||(ACS_DATA_ACQ_STATUS == 1))
- {
-
- pc.printf("\n\r Z only no data \n\r");
- DRV_Z_EN = 1;
- DRV_XY_EN = 0;
- ACS_TR_Z_ENABLE = 1;
- ACS_TR_XY_ENABLE = 0;
- ACS_STATUS = 3; // set Set ACS_STATUS = ACS_DATA_ACQN_FAILURE , Z axis only
-
- moment[0] = 0;
- moment[1] = 0;
- moment[2] = ACS_Z_FIXED_MOMENT; // is a dummy value
-
- //timer_FCTN_ACS_GENPWM_MAIN.start();
- FCTN_ACS_GENPWM_MAIN(moment) ;
- //timer_FCTN_ACS_GENPWM_MAIN.stop();
- //pc.printf("\n\r the timer_FCTN_ACS_GENPWM_MAIN is %f",timer_FCTN_ACS_GENPWM_MAIN.read());
-
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if((ACS_STATE == 1)||(ACS_STATE == 9))
- {
- pc.printf("\n\r Z only by state \n\r");
- DRV_Z_EN = 1;
- DRV_XY_EN = 0;
- ACS_TR_Z_ENABLE = 1;
- ACS_TR_XY_ENABLE = 0;
- ACS_STATUS = 3; // set ACS_STAUS = ACS_TRXY_DISABLED by ACS_STATE i.e Z axis only
-
- moment[0] = 0;
- moment[1] = 0;
- moment[2] = ACS_Z_FIXED_MOMENT; // 1.3 is a dummy value
- FCTN_ACS_GENPWM_MAIN(moment) ;
-
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if(ACS_DATA_ACQ_STATUS == 2) // MM only is available
- {
- pc.printf("\n\r MM only BDOT \n\r");
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
- ACS_TR_Z_ENABLE = 1;
- ACS_TR_XY_ENABLE = 1;
-
- ACS_STATUS = 4; // set Set ACS_STATUS = ACS_BDOT_CONTROL
- ACS_DETUMBLING_ALGO_TYPE = 0x01;
- FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
- controlmode_mms=0x00;
- #if print
- pc.printf("\n\r Moment values returned by control algo \n");
- #endif
- for(int i=0; i<3; i++)
- {
- pc.printf("%f\t",moment[i]);
- }
- FCTN_ACS_GENPWM_MAIN(moment) ;
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if((ACS_STATE == 2)||(ACS_STATE == 10)) // Nominal mode
- {
- #if print
- pc.printf("\n\r Nominal mode \n");
- #endif
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
- ACS_TR_Z_ENABLE = 1;
- ACS_TR_XY_ENABLE = 1;
-
- //timer_FCTN_ACS_CNTRLALGO.start();
- FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE);
- //timer_FCTN_ACS_CNTRLALGO.stop();
- //pc.printf("\n\r the timer_FCTN_ACS_GENPWM_MAIN is %f",timer_FCTN_ACS_CNTRLALGO.read());
-
- controlmode_mms = 0x01;
- #if print
- pc.printf("\n\r Moment values returned by control algo \n");
- #endif
- for(int i=0; i<3; i++)
- {
- pc.printf("%f\t",moment[i]);
- }
-
- //timer_FCTN_ACS_GENPWM_MAIN.start();
- FCTN_ACS_GENPWM_MAIN(moment) ;
- //timer_FCTN_ACS_GENPWM_MAIN.stop();
- //pc.printf("\n\r the timer_FCTN_ACS_CNTRLALGO is %f",timer_FCTN_ACS_GENPWM_MAIN.read());
-
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if((ACS_STATE == 3)||(ACS_STATE == 11)) // Auto Control
- {
- #if print
- pc.printf("\n\r Auto control mode \n");
- #endif
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
- ACS_TR_Z_ENABLE = 1;
- ACS_TR_XY_ENABLE = 1;
-
- timer_FCTN_ACS_CNTRLALGO.start();
- FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE);
- timer_FCTN_ACS_CNTRLALGO.stop();
- //pc.printf("\n\r the timer_FCTN_ACS_CNTRLALGO is %f",timer_FCTN_ACS_CNTRLALGO.read());
-
- controlmode_mms = 0x00;
- #if print
- pc.printf("\n\r Moment values returned by control algo \n");
- for(int i=0; i<3; i++)
- {
- pc.printf("%f\t",moment[i]);
- }
- #endif
- pc.printf("\r\n");
- timer_FCTN_ACS_GENPWM_MAIN.start();
- FCTN_ACS_GENPWM_MAIN(moment) ;// set ACS_STATUS in function
- timer_FCTN_ACS_GENPWM_MAIN.stop();
- //pc.printf("\n\r the timer_FCTN_ACS_GENPWM_MAIN is %f",timer_FCTN_ACS_GENPWM_MAIN.read());
-
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if((ACS_STATE == 4)||(ACS_STATE == 12)) // Detumbling
- {
- pc.printf("\n\r Detumbling \n\r");
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
- ACS_TR_Z_ENABLE = 1;
- ACS_TR_XY_ENABLE = 1;
- FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
- controlmode_mms = 0x00;
- FCTN_ACS_GENPWM_MAIN(moment) ;
- ACS_MAIN_STATUS = 0;
- return;
- }
- ACS_STATUS = 7; //INVALID_STATE
- DRV_Z_EN = 0;
- ACS_TR_Z_ENABLE = 0;
- ACS_TR_XY_ENABLE = 0;
- DRV_XY_EN = 0;
+
ACS_MAIN_STATUS = 0; //clear ACS_MAIN_STATUS flag
}
@@ -1252,7 +1045,7 @@
//time_wdog = 1;
timer_F_ESP.reset();
timer_F_ESP.start();
- F_EPS();
+ //F_EPS();
timer_F_ESP.stop();
int zz;
pc.printf("\n\r");
@@ -1283,7 +1076,7 @@
// time_wdog = 0;
timer_F_BCN.reset();
timer_F_BCN.start();
- F_BCN();
+ //F_BCN();
timer_F_BCN.stop();
/*pc.printf("\n\r timer_F_BCN is %f",timer_F_BCN.read());
pc.printf("\n\r timer_Init_BEACON_HW is %f",timer_Init_BEACON_HW.read());