Team Fox
/
BAE_QM_MAR9
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
workinQM_5thJan_azad
by 
Team Fox
Diff: ACS.cpp
- Revision:
- 52:daa685b0e390
- Parent:
- 49:61c9f28332ba
- Child:
- 53:459b71b1861c
--- a/ACS.cpp Fri Jul 22 17:32:41 2016 +0000
+++ b/ACS.cpp Fri Jul 22 22:21:05 2016 +0000
@@ -805,7 +805,7 @@
/// acs_pc.printf("Sensor 1 not working.Powering off.\n \r"); //Sensor 1 INIT failure and power off
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
}
@@ -1238,7 +1238,7 @@
//other sensor gyro working, this sensor not working , off
//other sensor mag working, this sensor not working,off
- ATS1_SW_ENABLE = 1; //switch off sensor 1
+ ATS1_SW_ENABLE = 01; //switch off sensor 1
wait_ms(5);
if(acq == 1)
{
@@ -1262,7 +1262,7 @@
acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
ATS2_SW_ENABLE = 1;
wait_ms(5);
- ATS1_SW_ENABLE = 0;
+ ATS1_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
if(acq == 1)
{
@@ -1289,7 +1289,7 @@
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
- ATS1_SW_ENABLE = 0; //Sensor 2 gyro only,sensor 1 mag only
+ ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
return 3;
@@ -1311,7 +1311,7 @@
acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
ATS2_SW_ENABLE = 1;
wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
- ATS1_SW_ENABLE = 0;
+ ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
return acq;
@@ -1339,7 +1339,7 @@
else if(acq == 0)
{
acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
wait_ms(5); //Switch ON sensor 2
ATS2_SW_ENABLE = 0;
wait_ms(5);
@@ -1418,14 +1418,14 @@
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
}
- ATS1_SW_ENABLE = 0; //switch on sensor 1
+ ATS1_SW_ENABLE = 1; //switch on sensor 1
wait_ms(5);
init = SENSOR_INIT(); //sensor 2 init
if( init == 0)
{
acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
wait_ms(5);
ATS2_SW_ENABLE = 0;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
@@ -1454,7 +1454,7 @@
{
if(acq==2)
{
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
wait_ms(5);
ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
@@ -1476,7 +1476,7 @@
else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
{
acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
ATS2_SW_ENABLE = 0;
wait_ms(5);
@@ -1507,7 +1507,7 @@
acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
ATS2_SW_ENABLE = 1;
wait_ms(5); //Switch ON sensor 1
- ATS1_SW_ENABLE = 0;
+ ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
@@ -1542,7 +1542,7 @@
else if(acq2 == 0)
{
acs_pc.printf(" Sensor 1 data acq failure..\n \r");
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
return 0;
}
@@ -1577,7 +1577,7 @@
//other sensor gyro working, this sensor not working , off
//other sensor mag working, this sensor not working,off
- ATS1_SW_ENABLE = 1; //switch off sensor 1
+ ATS1_SW_ENABLE = 0; //switch off sensor 1
wait_ms(5);
if(acq == 1)
{
@@ -1597,7 +1597,7 @@
acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
ATS2_SW_ENABLE = 1;
wait_ms(5);
- ATS1_SW_ENABLE = 0;
+ ATS1_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
if(acq == 1)
{
@@ -1624,7 +1624,7 @@
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
- ATS1_SW_ENABLE = 0; //Sensor 2 gyro only,sensor 1 mag only
+ ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
return 3;
@@ -1646,7 +1646,7 @@
acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
ATS2_SW_ENABLE = 1;
wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
- ATS1_SW_ENABLE = 0;
+ ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
return acq;
@@ -1674,7 +1674,7 @@
else if(acq == 0)
{
acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
wait_ms(5); //Switch ON sensor 2
ATS2_SW_ENABLE = 0;
wait_ms(5);
@@ -1755,14 +1755,14 @@
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
}
- ATS1_SW_ENABLE = 0; //switch on sensor 1
+ ATS1_SW_ENABLE = 1; //switch on sensor 1
wait_ms(5);
init = SENSOR_INIT(); //sensor 2 init
if( init == 0)
{
acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
wait_ms(5);
ATS2_SW_ENABLE = 0;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
@@ -1791,7 +1791,7 @@
{
if(acq==2)
{
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
wait_ms(5);
ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
@@ -1813,7 +1813,7 @@
else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
{
acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
ATS2_SW_ENABLE = 0;
wait_ms(5);
@@ -1844,7 +1844,7 @@
acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
ATS2_SW_ENABLE = 1;
wait_ms(5); //Switch ON sensor 1
- ATS1_SW_ENABLE = 0;
+ ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
@@ -1879,7 +1879,7 @@
else if(acq2 == 0)
{
acs_pc.printf(" Sensor 1 data acq failure..\n \r");
- ATS1_SW_ENABLE = 1;
+ ATS1_SW_ENABLE = 0;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
return 0;
}
@@ -1959,19 +1959,19 @@
}
acs_pc.printf("DC for trx is %f \r \n",l_duty_cycle_x);
- //------------------------------------- y-direction TR--------------------------------------//
-
+ //----------------------------- y-direction TR --------------------------------------------//
+
+
float l_moment_y = Moment[1]; //Moment in y direction
phase_TR_y = 1; // setting the default current direction
if (l_moment_y <0)
{
- phase_TR_y = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
+ phase_TR_y = 0; // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
l_moment_y = abs(l_moment_y);
}
-
l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
//// printf("current in try is %f \r \n",l_current_y);
if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
@@ -1994,34 +1994,34 @@
//// printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
- }
+ }
else if(l_current_y==0)
{
//// printf("\n \r l_current_y====0");
- l_duty_cycle_y = 0; // default value of duty cycle
+ l_duty_cycle_y = 0; // default value of duty cycle
//// printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
- else // not necessary
+ else //not necessary
{
- g_err_flag_TR_y = 1;
- }
- acs_pc.printf("DC for try is %f \r \n",l_duty_cycle_y);
+ g_err_flag_TR_y = 1;
+ }
+ acs_pc.printf("DC for try is %f \r \n",l_duty_cycle_y);
//----------------------------------------------- z-direction TR -------------------------//
+
-
+
float l_moment_z = Moment[2]; //Moment in z direction
phase_TR_z = 1; // setting the default current direction
if (l_moment_z <0)
{
- phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
+ phase_TR_z = 0; // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
l_moment_z = abs(l_moment_z);
}
-
l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
//// printf("current in trz is %f \r \n",l_current_z);
if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
@@ -2044,20 +2044,20 @@
//// printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
- }
+ }
else if(l_current_z==0)
{
//// printf("\n \r l_current_z====0");
- l_duty_cycle_z = 0; // default value of duty cycle
+ l_duty_cycle_z = 0; // default value of duty cycle
//// printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
}
- else // not necessary
+ else //not necessary
{
- g_err_flag_TR_z = 1;
- }
- acs_pc.printf("DC for trz is %f \r \n",l_duty_cycle_z);
+ g_err_flag_TR_z = 1;
+ }
+ acs_pc.printf("DC for trz is %f \r \n",l_duty_cycle_z);
//changed
if(phase_TR_x)