Seeker of Truth ,
QITH FLAGS
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
TF_conops_BAE1_3
by 
Team Fox
Diff: ACS.cpp
- Revision:
- 0:246d1b5f11ae
- Child:
- 1:7185136654ce
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ACS.cpp Tue Jun 30 05:55:48 2015 +0000
@@ -0,0 +1,502 @@
+#include "ACS.h"
+#include "pin_config.h"
+
+Serial pc1(USBTX, USBRX);
+//.....................................flags...................................................//
+extern uint32_t BAE_STATUS;
+extern uint32_t BAE_ENABLE;
+
+//....................................pwmgen...................................................//
+DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
+DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
+DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
+
+PwmOut PWM1(PIN93); //Functions used to generate PWM signal
+PwmOut PWM2(PIN94);
+PwmOut PWM3(PIN95); //PWM output comes from pins p6
+
+int g_err_flag_TR_x=0; // setting x-flag to zero
+int g_err_flag_TR_y=0; // setting y-flag to zero
+int g_err_flag_TR_z=0; // setting z-flag to zero
+
+//....................................ATS......................................................//
+
+DigitalOut g_enb1(PIN90);//switch for sensor 1
+DigitalOut g_enb2(PIN70);//switch for sensor 2
+Serial mnm(USBTX,USBRX); //for usb communication
+I2C i2c (PIN85,PIN84); //PTC2-sda,PTC1-scl
+
+Timeout g_to; //Timeout variable to
+int g_toflag;
+int ATS_reset_count=0;
+char g_cmd[2];
+float g_gyro_error[3]= {0,0,0}, g_mag_error[3]= {0,0,0};
+
+/*------------------------------------------------------------------------------------------------------------------------------------------------------
+------------------------------------------- ATS data acquisition------------------------------------------------------------------------------------------*/
+void FCTN_T_OUT()
+{
+ g_toflag=0; //as T_OUT function gets called the while loop gets terminated
+}
+
+void FCTN_ACS_INIT()
+{
+ BAE_STATUS |= 0x00000001; //set ACS_INIT_STATUS flag to 1
+ FCTN_ATS_SWITCH(1);
+ char store;
+ g_cmd[0]=RESETREQ;
+ g_cmd[1]=BIT_RESREQ;
+ i2c.write(SLAVE_ADDR,g_cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
+ wait_ms(2000); //waiting for loading configuration file stored in EEPROM
+ g_cmd[0]=SENTRALSTATUS;
+ i2c.write(SLAVE_ADDR,g_cmd,1);
+ i2c.read(SLAVE_ADDR_READ,&store,1);
+ wait_ms(100);
+ //to check whether EEPROM is uploaded
+ switch((int)store) {
+ case(3): {
+ break;
+ }
+ case(11): {
+ break;
+ }
+ default: {
+ g_cmd[0]=RESETREQ;
+ g_cmd[1]=BIT_RESREQ;
+ i2c.write(SLAVE_ADDR,g_cmd,2);
+ wait_ms(2000);
+ }
+ }
+ //pc.printf("Sentral Status is %x\n",(int)store);
+ g_cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
+ g_cmd[1]=BIT_RUN_ENB;
+ i2c.write(SLAVE_ADDR,g_cmd,2);
+ wait_ms(100);
+ g_cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
+ g_cmd[1]=BIT_MAGODR;
+ i2c.write(SLAVE_ADDR,g_cmd,2);
+ wait_ms(100);
+ g_cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
+ g_cmd[1]=BIT_GYROODR;
+ i2c.write(SLAVE_ADDR,g_cmd,2);
+ wait_ms(100);
+ g_cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
+ g_cmd[1]=0x00;
+ i2c.write(SLAVE_ADDR,g_cmd,2);
+ wait_ms(100);
+ g_cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
+ g_cmd[1]=BIT_EVT_ENB;
+ i2c.write(SLAVE_ADDR,g_cmd,2);
+ wait_ms(100);
+ PWM1 = 0; // clear pwm pins
+ PWM2 = 0;
+ PWM3 = 0;
+ BAE_STATUS &= 0xFFFFFFFE; //clear ACS_INIT_STATUS flag
+ printf("\n\r ACS_STATUS flag reads %x",&BAE_STATUS);
+}
+
+void FCTN_ACS_DATA_ACQ(float g_gyro_data[3],float g_mag_data[3])
+{
+ BAE_STATUS |= 0x00000020; //set ACS_DATA_ACQ_STATUS flag to 1
+ if(BAE_ENABLE & 0x00000010 == 0x00000010) // check ACS_ATS_ENABLE = 1?
+ {
+ char status;
+ g_toflag=1; //toFlag is set to 1 so that it enters while loop
+ g_to.attach(&FCTN_T_OUT,2); //after 2 seconds the while loop gets terminated
+ g_cmd[0]=EVT_STATUS;
+ i2c.write(SLAVE_ADDR,g_cmd,1);
+ i2c.read(SLAVE_ADDR_READ,&status,1);
+ wait_ms(100);
+ //pc.printf("\nEvent Status is %x\n",(int)status);
+ //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
+ if(((int)status&40)==40)
+ {
+ FCTN_GET_DATA(g_gyro_data,g_mag_data);
+ printf("\n\r data received \n");
+ for(int i=0; i<3; i++)
+ {
+ printf("%f\t",g_gyro_data[i]);
+ }
+ for(int i=0; i<3; i++)
+ {
+ printf("%f\t",g_mag_data[i]);
+ }
+ }
+ //checking for the error
+ else if (((int)status&2)==2)
+ {
+ if(ATS_reset_count!=2)
+ {
+ FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
+ ATS_reset_count++;
+ }
+ else
+ {
+ FCTN_ATS_SWITCH(0);
+ }
+ }
+ BAE_STATUS |= 0x00000100; //set ACS_ATS_STATUS = ACS_ATS_OPERATIONAL
+ }
+ else
+ {
+ BAE_STATUS |= 0x000000C0; //set ACS_DATA_ACQ_ATS = ACS_ATS_DISABLED // ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
+ }
+
+ BAE_STATUS &= 0xFFFFFFDF; //clear ACS_DATA_ACQ_STATUS flag to 1
+}
+
+void FCTN_GET_DATA(float g_gyro_data[3],float g_mag_data[3])
+{
+ char raw_gyro[6];
+ char raw_mag[6];
+ int16_t bit_data;
+
+ float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
+ float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
+ g_cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
+ i2c.write(SLAVE_ADDR,g_cmd,1);
+ i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
+ g_cmd[0]=MAG_XOUT_H; //LSB of x
+ i2c.write(SLAVE_ADDR,g_cmd,1);
+ i2c.read(SLAVE_ADDR_READ,raw_mag,6);
+ //pc.printf("\nGyro Values:\n");
+ for(int i=0; i<3; i++) {
+ //concatenating gyro LSB and MSB to get 16 bit signed data values
+ bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
+ g_gyro_data[i]=(float)bit_data;
+ g_gyro_data[i]=g_gyro_data[i]/senstivity_gyro;
+ g_gyro_data[i]+=g_gyro_error[i];
+ //pc.printf("%f\t",gyro_data[i]);
+ }
+ for(int i=0; i<3; i++) {
+ //concatenating mag LSB and MSB to get 16 bit signed data values
+ bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
+ g_mag_data[i]=(float)bit_data;
+ g_mag_data[i]=g_mag_data[i]/senstivity_mag;
+ g_mag_data[i]+=g_mag_error[i];
+ //pc.printf("%f\t",mag_data[i]);
+ }
+
+}
+
+void FCTN_ATS_SWITCH(bool c)
+{
+ if(c==0){
+ g_enb1 = 1;//enabling it high switches OFF the sensor 1
+ g_enb2 = 0;//swtiches ON the sensor 2
+ }
+ else{
+ g_enb1 = 0;//enabling it low switches the sensor ON
+ g_enb2 = 1;//switches OFF the sensor 2
+ }
+}
+
+/*------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
+
+int ctrl_count = 0;
+float bcopy[3];
+
+void FCTN_ACS_CNTRLALGO(float b[3],float omega[3],float moment[3])
+{
+ float db[3];
+ float bb[3]={0,0,0};
+ float d[3]={0,0,0};
+ float Jm[3][3]={{0.2730,0,0},{0,0.3018,0},{0,0,0.3031}};
+ float den=0,den2;
+ int i,j; //temporary variables
+ float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0}; //outputs
+ float invJm[3][3];
+ float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
+
+ //................. calculating db values...........................
+ if(ctrl_count!=0)
+ {
+ for(i=0;i<3;i++)
+ db[i]= (b[i]-bcopy[i])/10;
+ }
+ else
+ {
+ for(i=0;i<3;i++)
+ db[i]= 0;
+ }
+ ctrl_count++;
+ //..................................................................
+ printf("\n\r Entered cntrl algo\n\r");
+ for(int i=0; i<3; i++)
+ {
+ printf("%f\t",omega[i]);
+ }
+ for(int i=0; i<3; i++)
+ {
+ printf("%f\t",b[i]);
+ }
+
+ //.........................algo......................................
+ den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
+ den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
+ for(i=0;i<3;i++)
+ {
+ db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3));
+ //db[i]/=den*den*den;
+ }
+ for(i=0;i<3;i++)
+ {
+ b[i]/=den;
+ }
+ // select kz, kmu, gamma
+ if(b[0]>0.9||b[0]<-0.9)
+ {
+ kz=kz2;
+ kmu=kmu2;
+ gamma=gamma2;
+ }
+ // calculate Mu, v, dv, z, u
+ for(i=0;i<2;i++)
+ {
+ Mu[i]=b[i+1];
+ v[i]=-kmu*Mu[i];
+ dv[i]=-kmu*db[i+1];
+ z[i]=db[i+1]-v[i];
+ u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
+ }
+ inverse(Jm,invJm);
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
+ }
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ d[i]+=bb[j]*invJm[i][j];
+ }
+ bb[1]=u[0]+(d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2])+(omega[2]*db[0]);
+ bb[2]=u[1]-(d[0]*b[1])+(d[1]*b[0])+(omega[0]*db[1])-(omega[1]*db[0]);
+ bb[0]=0;
+ for(i=0;i<3;i++)
+ {
+ d[i]=invJm[1][i];
+ invJm[1][i]=b[2]*invJm[0][i]-b[0]*invJm[2][i];
+ invJm[2][i]=-b[1]*invJm[0][i]+b[0]*d[i];
+ invJm[0][i]=b[i];
+ }
+ inverse(invJm,Jm);
+ printf("\n \r calculating tauc");
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ tauc[i]+=Jm[i][j]*bb[j]; // calculating torque values
+ printf(" %f \t",tauc[i]);
+ }
+ //..........................tauc to moment conversion..........................
+ printf("\n \r calculating moment");
+ for(i=0;i<3;i++)
+ bcopy[i]=b[i]*den;
+ for(i=0;i<3;i++)
+ {
+ moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
+ moment[i]/=den;
+ printf(" %f \t",moment[i]);
+ }
+ printf("\n\r exited control algo\n");
+}
+//..........................function to find inverse..................
+void inverse(float mat[3][3],float inv[3][3])
+{
+ int i,j;
+ float det=0;
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ inv[j][i]=(mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3])-(mat[(i+2)%3][(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
+ }
+ det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ inv[i][j]/=det;
+ }
+}
+
+/*------------------------------------------------------------------------------------------------------------------------------------------------------
+---------------------------------------------------PWM GENERATION-----------------------------------------------------------------------------------------*/
+
+void FCTN_ACS_GENPWM_MAIN(float Moment[3])
+{
+ printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
+
+ float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
+ float l_current_x=0; //Current sent in x TR's
+ float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
+ float l_current_y=0; //Current sent in y TR's
+ float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
+ float l_current_z=0; //Current sent in z TR's
+
+
+ for(int i = 0 ; i<3;i++)
+ {
+ printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
+ }
+
+ //----------------------------- x-direction TR --------------------------------------------//
+
+
+ float l_moment_x = Moment[0]; //Moment in x direction
+
+ phase_TR_x = 1; // setting the default current direction
+ if (l_moment_x <0)
+ {
+ phase_TR_x = 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_x = abs(l_moment_x);
+ }
+
+ l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
+
+ if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
+ {
+ l_duty_cycle_x = 6*1000000*pow(l_current_x,4) - 377291*pow(l_current_x,3) + 4689.6*pow(l_current_x,2) + 149.19*l_current_x - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
+ PWM1.period(TIME_PERIOD);
+ PWM1 = l_duty_cycle_x/100 ;
+ }
+ else if( l_current_x >= 0.006 && l_current_x < 0.0116)
+ {
+ l_duty_cycle_x = 1*100000000*pow(l_current_x,4) - 5*1000000*pow(l_current_x,3) + 62603*pow(l_current_x,2) - 199.29*l_current_x + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
+ PWM1.period(TIME_PERIOD);
+ PWM1 = l_duty_cycle_x/100 ;
+ }
+ else if (l_current_x >= 0.0116 && l_current_x < 0.0624)
+ {
+ l_duty_cycle_x = 212444*pow(l_current_x,4) - 33244*pow(l_current_x,3) + 1778.4*pow(l_current_x,2) + 120.91*l_current_x + 0.3878; // calculating upto 10% dutycycle by polynomial interpolation
+ PWM1.period(TIME_PERIOD);
+ PWM1 = l_duty_cycle_x/100 ;
+ }
+ else if(l_current_x >= 0.0624 && l_current_x < 0.555)
+ {
+ l_duty_cycle_x = 331.15*pow(l_current_x,4) - 368.09*pow(l_current_x,3) + 140.43*pow(l_current_x,2) + 158.59*l_current_x + 0.0338; // calculating upto 100% dutycycle by polynomial interpolation
+ PWM1.period(TIME_PERIOD);
+ PWM1 = l_duty_cycle_x/100 ;
+ }
+ else if(l_current_x==0)
+ {
+ printf("\n \r l_current_x====0");
+ l_duty_cycle_x = 0; // default value of duty cycle
+ PWM1.period(TIME_PERIOD);
+ PWM1 = l_duty_cycle_x/100 ;
+ }
+ else //not necessary
+ {
+ g_err_flag_TR_x = 1;
+ }
+
+ //------------------------------------- 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
+ l_moment_y = abs(l_moment_y);
+ }
+
+
+ l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
+
+ if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
+ {
+ l_duty_cycle_y = 6*1000000*pow(l_current_y,4) - 377291*pow(l_current_y,3) + 4689.6*pow(l_current_y,2) + 149.19*l_current_y - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
+ PWM2.period(TIME_PERIOD);
+ PWM2 = l_duty_cycle_y/100 ;
+ }
+ else if( l_current_y >= 0.006 && l_current_y < 0.0116)
+ {
+ l_duty_cycle_y = 1*100000000*pow(l_current_y,4) - 5*1000000*pow(l_current_y,3) + 62603*pow(l_current_y,2) - 199.29*l_current_y + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
+ PWM2.period(TIME_PERIOD);
+ PWM2 = l_duty_cycle_y/100 ;
+ }
+ else if (l_current_y >= 0.0116&& l_current_y < 0.0624)
+ {
+ l_duty_cycle_y = 212444*pow(l_current_y,4) - 33244*pow(l_current_y,3) + 1778.4*pow(l_current_y,2) + 120.91*l_current_y + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
+ PWM2.period(TIME_PERIOD);
+ PWM2 = l_duty_cycle_y/100 ;
+ }
+ else if(l_current_y >= 0.0624 && l_current_y < 0.555)
+ {
+ l_duty_cycle_y = 331.15*pow(l_current_y,4) - 368.09*pow(l_current_y,3) + 140.43*pow(l_current_y,2) + 158.59*l_current_y + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
+ 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
+ PWM2.period(TIME_PERIOD);
+ PWM2 = l_duty_cycle_y/100 ;
+ }
+ else // not necessary
+ {
+ g_err_flag_TR_y = 1;
+ }
+
+ //----------------------------------------------- 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
+ l_moment_z = abs(l_moment_z);
+ }
+
+
+ l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
+
+ if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
+ {
+ l_duty_cycle_z = 6*1000000*pow(l_current_z,4) - 377291*pow(l_current_z,3) + 4689.6*pow(l_current_z,2) + 149.19*l_current_z - 0.0008;// calculating upto 0.1% dutycycle by polynomial interpolation
+ PWM3.period(TIME_PERIOD);
+ PWM3 = l_duty_cycle_z/100 ;
+ }
+ else if( l_current_z >= 0.006 && l_current_z < 0.0116)
+ {
+ l_duty_cycle_z = 1*100000000*pow(l_current_z,4) - 5*1000000*pow(l_current_z,3) + 62603*pow(l_current_z,2) - 199.29*l_current_z + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
+ PWM3.period(TIME_PERIOD);
+ PWM3 = l_duty_cycle_z/100 ;
+ }
+ else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
+ {
+ l_duty_cycle_z = 212444*pow(l_current_z,4) - 33244*pow(l_current_z,3) + 1778.4*pow(l_current_z,2) + 120.91*l_current_z + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
+ PWM3.period(TIME_PERIOD);
+ PWM3 = l_duty_cycle_z/100 ;
+ }
+ else if(l_current_z >= 0.0624 && l_current_z < 0.555)
+ {
+ l_duty_cycle_z = 331.15*pow(l_current_z,4) - 368.09*pow(l_current_z,3) + 140.43*pow(l_current_z,2) + 158.59*l_current_z + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
+ 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
+ PWM3.period(TIME_PERIOD);
+ PWM3 = l_duty_cycle_z/100 ;
+ }
+ else // not necessary
+ {
+ g_err_flag_TR_z = 1;
+ }
+
+ //-----------------------------------------exiting the function-----------------------------------//
+
+ printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
+
+}
+
+
+
+