Team Fox / Mbed 2 deprecated BAE_Updated_Commissioning

Changes to be made for ATS_Fault logic and ACS_State

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of Japan_BAE_sensorworking_interrupr_reoccuring_copy by Team Fox

ACS.cpp@0:7b4c00e3912f, 2015-12-24 (annotated)

Committer:
sakthipriya
Date:
Thu Dec 24 19:15:43 2015 +0000
Revision:
0:7b4c00e3912f
Child:
3:07e15677a75c
EPS CONOPS BTRY GAUGE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakthipriya 0:7b4c00e3912f 1 /*------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:7b4c00e3912f 2 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
sakthipriya 0:7b4c00e3912f 3 #include <mbed.h>
sakthipriya 0:7b4c00e3912f 4 #include <math.h>
sakthipriya 0:7b4c00e3912f 5
sakthipriya 0:7b4c00e3912f 6 #include "pni.h" //pni header file
sakthipriya 0:7b4c00e3912f 7 #include "pin_config.h"
sakthipriya 0:7b4c00e3912f 8 #include "ACS.h"
sakthipriya 0:7b4c00e3912f 9
sakthipriya 0:7b4c00e3912f 10
sakthipriya 0:7b4c00e3912f 11 //********************************flags******************************************//
sakthipriya 0:7b4c00e3912f 12 extern uint32_t BAE_STATUS;
sakthipriya 0:7b4c00e3912f 13 extern uint32_t BAE_ENABLE;
sakthipriya 0:7b4c00e3912f 14 extern char ACS_INIT_STATUS;
sakthipriya 0:7b4c00e3912f 15 extern char ACS_DATA_ACQ_STATUS;
sakthipriya 0:7b4c00e3912f 16 extern char ACS_ATS_STATUS;
sakthipriya 0:7b4c00e3912f 17 extern char ACS_MAIN_STATUS;
sakthipriya 0:7b4c00e3912f 18 extern char ACS_STATUS;
sakthipriya 0:7b4c00e3912f 19
sakthipriya 0:7b4c00e3912f 20 extern char ACS_ATS_ENABLE;
sakthipriya 0:7b4c00e3912f 21 extern char ACS_DATA_ACQ_ENABLE;
sakthipriya 0:7b4c00e3912f 22 extern char ACS_STATE;
sakthipriya 0:7b4c00e3912f 23
sakthipriya 0:7b4c00e3912f 24 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
sakthipriya 0:7b4c00e3912f 25 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
sakthipriya 0:7b4c00e3912f 26 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
sakthipriya 0:7b4c00e3912f 27
sakthipriya 0:7b4c00e3912f 28 extern PwmOut PWM1; //x //Functions used to generate PWM signal
sakthipriya 0:7b4c00e3912f 29 extern PwmOut PWM2; //y
sakthipriya 0:7b4c00e3912f 30 extern PwmOut PWM3; //z //PWM output comes from pins p6
sakthipriya 0:7b4c00e3912f 31
sakthipriya 0:7b4c00e3912f 32 int g_err_flag_TR_x=0; // setting x-flag to zero
sakthipriya 0:7b4c00e3912f 33 int g_err_flag_TR_y=0; // setting y-flag to zero
sakthipriya 0:7b4c00e3912f 34 int g_err_flag_TR_z=0; // setting z-flag to zero
sakthipriya 0:7b4c00e3912f 35
sakthipriya 0:7b4c00e3912f 36 extern float data[6];
sakthipriya 0:7b4c00e3912f 37
sakthipriya 0:7b4c00e3912f 38
sakthipriya 0:7b4c00e3912f 39 //DigitalOut gpo1(PTC0); // enable of att sens2 switch
sakthipriya 0:7b4c00e3912f 40 //DigitalOut gpo2(PTC16); // enable of att sens switch
sakthipriya 0:7b4c00e3912f 41
sakthipriya 0:7b4c00e3912f 42
sakthipriya 0:7b4c00e3912f 43 Serial pc_acs(USBTX,USBRX); //for usb communication
sakthipriya 0:7b4c00e3912f 44 void inverse(float mat[3][3],float inv[3][3]);
sakthipriya 0:7b4c00e3912f 45
sakthipriya 0:7b4c00e3912f 46 int ctrl_count = 0;
sakthipriya 0:7b4c00e3912f 47 float bcopy[3];
sakthipriya 0:7b4c00e3912f 48 float moment[3];
sakthipriya 0:7b4c00e3912f 49 ///////algo working well
sakthipriya 0:7b4c00e3912f 50 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
sakthipriya 0:7b4c00e3912f 51 {
sakthipriya 0:7b4c00e3912f 52 float db[3];
sakthipriya 0:7b4c00e3912f 53 float bb[3]={0,0,0};
sakthipriya 0:7b4c00e3912f 54 float d[3]={0,0,0};
sakthipriya 0:7b4c00e3912f 55 float Jm[3][3]={{0.2730,0,0},{0,0.3018,0},{0,0,0.3031}};
sakthipriya 0:7b4c00e3912f 56 float den=0,den2;
sakthipriya 0:7b4c00e3912f 57 int i,j; //temporary variables
sakthipriya 0:7b4c00e3912f 58 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0}; //outputs
sakthipriya 0:7b4c00e3912f 59 float invJm[3][3];
sakthipriya 0:7b4c00e3912f 60 float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
sakthipriya 0:7b4c00e3912f 61
sakthipriya 0:7b4c00e3912f 62 //................. calculating db values...........................
sakthipriya 0:7b4c00e3912f 63 if(ctrl_count!=0)
sakthipriya 0:7b4c00e3912f 64 {
sakthipriya 0:7b4c00e3912f 65 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 66 db[i]= (b[i]-bcopy[i])/10;
sakthipriya 0:7b4c00e3912f 67 }
sakthipriya 0:7b4c00e3912f 68 else
sakthipriya 0:7b4c00e3912f 69 {
sakthipriya 0:7b4c00e3912f 70 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 71 db[i]= 0;
sakthipriya 0:7b4c00e3912f 72 }
sakthipriya 0:7b4c00e3912f 73 ctrl_count++;
sakthipriya 0:7b4c00e3912f 74 //..................................................................
sakthipriya 0:7b4c00e3912f 75 printf("\n\r Entered cntrl algo\n\r");
sakthipriya 0:7b4c00e3912f 76 for(int i=0; i<3; i++)
sakthipriya 0:7b4c00e3912f 77 {
sakthipriya 0:7b4c00e3912f 78 printf("%f\t",omega[i]);
sakthipriya 0:7b4c00e3912f 79 }
sakthipriya 0:7b4c00e3912f 80 for(int i=0; i<3; i++)
sakthipriya 0:7b4c00e3912f 81 {
sakthipriya 0:7b4c00e3912f 82 printf("%f\t",b[i]);
sakthipriya 0:7b4c00e3912f 83 }
sakthipriya 0:7b4c00e3912f 84
sakthipriya 0:7b4c00e3912f 85 //.........................algo......................................
sakthipriya 0:7b4c00e3912f 86 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
sakthipriya 0:7b4c00e3912f 87 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
sakthipriya 0:7b4c00e3912f 88 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 89 {
sakthipriya 0:7b4c00e3912f 90 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3));
sakthipriya 0:7b4c00e3912f 91 //db[i]/=den*den*den;
sakthipriya 0:7b4c00e3912f 92 }
sakthipriya 0:7b4c00e3912f 93 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 94 {
sakthipriya 0:7b4c00e3912f 95 b[i]/=den;
sakthipriya 0:7b4c00e3912f 96 }
sakthipriya 0:7b4c00e3912f 97 // select kz, kmu, gamma
sakthipriya 0:7b4c00e3912f 98 if(b[0]>0.9||b[0]<-0.9)
sakthipriya 0:7b4c00e3912f 99 {
sakthipriya 0:7b4c00e3912f 100 kz=kz2;
sakthipriya 0:7b4c00e3912f 101 kmu=kmu2;
sakthipriya 0:7b4c00e3912f 102 gamma=gamma2;
sakthipriya 0:7b4c00e3912f 103 }
sakthipriya 0:7b4c00e3912f 104 // calculate Mu, v, dv, z, u
sakthipriya 0:7b4c00e3912f 105 for(i=0;i<2;i++)
sakthipriya 0:7b4c00e3912f 106 {
sakthipriya 0:7b4c00e3912f 107 Mu[i]=b[i+1];
sakthipriya 0:7b4c00e3912f 108 v[i]=-kmu*Mu[i];
sakthipriya 0:7b4c00e3912f 109 dv[i]=-kmu*db[i+1];
sakthipriya 0:7b4c00e3912f 110 z[i]=db[i+1]-v[i];
sakthipriya 0:7b4c00e3912f 111 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
sakthipriya 0:7b4c00e3912f 112 }
sakthipriya 0:7b4c00e3912f 113 inverse(Jm,invJm);
sakthipriya 0:7b4c00e3912f 114 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 115 {
sakthipriya 0:7b4c00e3912f 116 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 117 bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
sakthipriya 0:7b4c00e3912f 118 }
sakthipriya 0:7b4c00e3912f 119 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 120 {
sakthipriya 0:7b4c00e3912f 121 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 122 d[i]+=bb[j]*invJm[i][j];
sakthipriya 0:7b4c00e3912f 123 }
sakthipriya 0:7b4c00e3912f 124 bb[1]=u[0]+(d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2])+(omega[2]*db[0]);
sakthipriya 0:7b4c00e3912f 125 bb[2]=u[1]-(d[0]*b[1])+(d[1]*b[0])+(omega[0]*db[1])-(omega[1]*db[0]);
sakthipriya 0:7b4c00e3912f 126 bb[0]=0;
sakthipriya 0:7b4c00e3912f 127 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 128 {
sakthipriya 0:7b4c00e3912f 129 d[i]=invJm[1][i];
sakthipriya 0:7b4c00e3912f 130 invJm[1][i]=b[2]*invJm[0][i]-b[0]*invJm[2][i];
sakthipriya 0:7b4c00e3912f 131 invJm[2][i]=-b[1]*invJm[0][i]+b[0]*d[i];
sakthipriya 0:7b4c00e3912f 132 invJm[0][i]=b[i];
sakthipriya 0:7b4c00e3912f 133 }
sakthipriya 0:7b4c00e3912f 134 inverse(invJm,Jm);
sakthipriya 0:7b4c00e3912f 135 printf("\n \r calculating tauc");
sakthipriya 0:7b4c00e3912f 136 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 137 {
sakthipriya 0:7b4c00e3912f 138 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 139 tauc[i]+=Jm[i][j]*bb[j]; // calculating torque values
sakthipriya 0:7b4c00e3912f 140 printf(" %f \t",tauc[i]);
sakthipriya 0:7b4c00e3912f 141 }
sakthipriya 0:7b4c00e3912f 142 //..........................tauc to moment conversion..........................
sakthipriya 0:7b4c00e3912f 143 printf("\n \r calculating moment");
sakthipriya 0:7b4c00e3912f 144 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 145 bcopy[i]=b[i]*den;
sakthipriya 0:7b4c00e3912f 146 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 147 {
sakthipriya 0:7b4c00e3912f 148 moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
sakthipriya 0:7b4c00e3912f 149 moment[i]/=den;
sakthipriya 0:7b4c00e3912f 150 printf(" %f \t",moment[i]);
sakthipriya 0:7b4c00e3912f 151 }
sakthipriya 0:7b4c00e3912f 152 printf("\n\r exited control algo\n");
sakthipriya 0:7b4c00e3912f 153 }
sakthipriya 0:7b4c00e3912f 154 //..........................function to find inverse..................
sakthipriya 0:7b4c00e3912f 155 void inverse(float mat[3][3],float inv[3][3])
sakthipriya 0:7b4c00e3912f 156 {
sakthipriya 0:7b4c00e3912f 157 int i,j;
sakthipriya 0:7b4c00e3912f 158 float det=0;
sakthipriya 0:7b4c00e3912f 159 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 160 {
sakthipriya 0:7b4c00e3912f 161 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 162 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]);
sakthipriya 0:7b4c00e3912f 163 }
sakthipriya 0:7b4c00e3912f 164 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
sakthipriya 0:7b4c00e3912f 165 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 166 {
sakthipriya 0:7b4c00e3912f 167 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 168 inv[i][j]/=det;
sakthipriya 0:7b4c00e3912f 169 }
sakthipriya 0:7b4c00e3912f 170 }
sakthipriya 0:7b4c00e3912f 171
sakthipriya 0:7b4c00e3912f 172
sakthipriya 0:7b4c00e3912f 173 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
sakthipriya 0:7b4c00e3912f 174
sakthipriya 0:7b4c00e3912f 175 void FCTN_ACS_INIT(void); //initialization of registers happens
sakthipriya 0:7b4c00e3912f 176 void FCTN_ATS_DATA_ACQ(); //data is obtained
sakthipriya 0:7b4c00e3912f 177 void T_OUT(); //timeout function to stop infinite loop
sakthipriya 0:7b4c00e3912f 178 Timeout to; //Timeout variable to
sakthipriya 0:7b4c00e3912f 179 int toFlag;
sakthipriya 0:7b4c00e3912f 180
sakthipriya 0:7b4c00e3912f 181 int count =0; // Time for which the BAE uC is running (in seconds)
sakthipriya 0:7b4c00e3912f 182 void T_OUT()
sakthipriya 0:7b4c00e3912f 183 {
sakthipriya 0:7b4c00e3912f 184 toFlag=0; //as T_OUT function gets called the while loop gets terminated
sakthipriya 0:7b4c00e3912f 185 }
sakthipriya 0:7b4c00e3912f 186
sakthipriya 0:7b4c00e3912f 187
sakthipriya 0:7b4c00e3912f 188 //DEFINING VARIABLES
sakthipriya 0:7b4c00e3912f 189 char cmd[2];
sakthipriya 0:7b4c00e3912f 190 char raw_gyro[6];
sakthipriya 0:7b4c00e3912f 191 char raw_mag[6];
sakthipriya 0:7b4c00e3912f 192 char store,status;
sakthipriya 0:7b4c00e3912f 193 int16_t bit_data;
sakthipriya 0:7b4c00e3912f 194 float gyro_data[3], mag_data[3],combined_values[6];
sakthipriya 0:7b4c00e3912f 195 float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
sakthipriya 0:7b4c00e3912f 196 float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
sakthipriya 0:7b4c00e3912f 197 float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
sakthipriya 0:7b4c00e3912f 198
sakthipriya 0:7b4c00e3912f 199 void FCTN_ACS_INIT()
sakthipriya 0:7b4c00e3912f 200 {
sakthipriya 0:7b4c00e3912f 201 ACS_INIT_STATUS = 's'; //set ACS_INIT_STATUS flag
sakthipriya 0:7b4c00e3912f 202 FLAG();
sakthipriya 0:7b4c00e3912f 203 pc_acs.printf("Attitude sensor init called \n \r");
sakthipriya 0:7b4c00e3912f 204 //FLAG();
sakthipriya 0:7b4c00e3912f 205 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 206 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 207 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
sakthipriya 0:7b4c00e3912f 208 wait_ms(2000); //waiting for loading configuration file stored in EEPROM
sakthipriya 0:7b4c00e3912f 209 cmd[0]=SENTRALSTATUS;
sakthipriya 0:7b4c00e3912f 210 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 211 i2c.read(SLAVE_ADDR_READ,&store,1);
sakthipriya 0:7b4c00e3912f 212 wait_ms(100);
sakthipriya 0:7b4c00e3912f 213 //to check whether EEPROM is uploaded
sakthipriya 0:7b4c00e3912f 214 switch((int)store) {
sakthipriya 0:7b4c00e3912f 215 case(3): {
sakthipriya 0:7b4c00e3912f 216 break;
sakthipriya 0:7b4c00e3912f 217 }
sakthipriya 0:7b4c00e3912f 218 case(11): {
sakthipriya 0:7b4c00e3912f 219 break;
sakthipriya 0:7b4c00e3912f 220 }
sakthipriya 0:7b4c00e3912f 221 default: {
sakthipriya 0:7b4c00e3912f 222 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 223 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 224 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 225 wait_ms(2000);
sakthipriya 0:7b4c00e3912f 226 }
sakthipriya 0:7b4c00e3912f 227 }
sakthipriya 0:7b4c00e3912f 228 pc_acs.printf("Sentral Status is %x\n \r",(int)store);
sakthipriya 0:7b4c00e3912f 229 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
sakthipriya 0:7b4c00e3912f 230 cmd[1]=BIT_RUN_ENB;
sakthipriya 0:7b4c00e3912f 231 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 232 wait_ms(100);
sakthipriya 0:7b4c00e3912f 233 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
sakthipriya 0:7b4c00e3912f 234 cmd[1]=BIT_MAGODR;
sakthipriya 0:7b4c00e3912f 235 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 236 wait_ms(100);
sakthipriya 0:7b4c00e3912f 237 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
sakthipriya 0:7b4c00e3912f 238 cmd[1]=BIT_GYROODR;
sakthipriya 0:7b4c00e3912f 239 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 240 wait_ms(100);
sakthipriya 0:7b4c00e3912f 241 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
sakthipriya 0:7b4c00e3912f 242 cmd[1]=0x00;
sakthipriya 0:7b4c00e3912f 243 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 244 wait_ms(100);
sakthipriya 0:7b4c00e3912f 245 cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
sakthipriya 0:7b4c00e3912f 246 cmd[1]=BIT_EVT_ENB;
sakthipriya 0:7b4c00e3912f 247 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 248 wait_ms(100);
sakthipriya 0:7b4c00e3912f 249 ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
sakthipriya 0:7b4c00e3912f 250 }
sakthipriya 0:7b4c00e3912f 251
sakthipriya 0:7b4c00e3912f 252 void FCTN_ATS_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 253 {
sakthipriya 0:7b4c00e3912f 254 ACS_DATA_ACQ_STATUS = 's'; //set ACS_DATA_ACQ_STATUS flag for att sens 2
sakthipriya 0:7b4c00e3912f 255 if( ACS_ATS_ENABLE == 'e')
sakthipriya 0:7b4c00e3912f 256 {
sakthipriya 0:7b4c00e3912f 257 FLAG();
sakthipriya 0:7b4c00e3912f 258 pc_acs.printf("attitude sensor execution called \n \r");
sakthipriya 0:7b4c00e3912f 259 toFlag=1; //toFlag is set to 1 so that it enters while loop
sakthipriya 0:7b4c00e3912f 260 to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated
sakthipriya 0:7b4c00e3912f 261 while(toFlag) {
sakthipriya 0:7b4c00e3912f 262 cmd[0]=EVT_STATUS;
sakthipriya 0:7b4c00e3912f 263 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 264 i2c.read(SLAVE_ADDR_READ,&status,1);
sakthipriya 0:7b4c00e3912f 265 wait_ms(100);
sakthipriya 0:7b4c00e3912f 266 pc_acs.printf("Event Status is %x\n \r",(int)status);
sakthipriya 0:7b4c00e3912f 267 //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
sakthipriya 0:7b4c00e3912f 268 if(((int)status&40)==40) {
sakthipriya 0:7b4c00e3912f 269 cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
sakthipriya 0:7b4c00e3912f 270 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 271 i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
sakthipriya 0:7b4c00e3912f 272 cmd[0]=MAG_XOUT_H; //LSB of x
sakthipriya 0:7b4c00e3912f 273 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 274 i2c.read(SLAVE_ADDR_READ,raw_mag,6);
sakthipriya 0:7b4c00e3912f 275 // pc_acs.printf("\nGyro Values:\n");
sakthipriya 0:7b4c00e3912f 276 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 277 //concatenating gyro LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 278 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
sakthipriya 0:7b4c00e3912f 279 gyro_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 280 gyro_data[i]=gyro_data[i]/senstivity_gyro;
sakthipriya 0:7b4c00e3912f 281 gyro_data[i]+=gyro_error[i];
sakthipriya 0:7b4c00e3912f 282 // pc_acs.printf("%f\t",gyro_data[i]);
sakthipriya 0:7b4c00e3912f 283 }
sakthipriya 0:7b4c00e3912f 284 // pc_acs.printf("\nMag Values:\n");
sakthipriya 0:7b4c00e3912f 285 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 286 //concatenating mag LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 287 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
sakthipriya 0:7b4c00e3912f 288 mag_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 289 mag_data[i]=mag_data[i]/senstivity_mag;
sakthipriya 0:7b4c00e3912f 290 mag_data[i]+=mag_error[i];
sakthipriya 0:7b4c00e3912f 291 // pc_acs.printf("%f\t",mag_data[i]);
sakthipriya 0:7b4c00e3912f 292 }
sakthipriya 0:7b4c00e3912f 293 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 294 data[i]=gyro_data[i];
sakthipriya 0:7b4c00e3912f 295 data[i+3]=mag_data[i];
sakthipriya 0:7b4c00e3912f 296 }
sakthipriya 0:7b4c00e3912f 297 // return(combined_values); //returning poiter combined values
sakthipriya 0:7b4c00e3912f 298 }
sakthipriya 0:7b4c00e3912f 299 //checking for the error
sakthipriya 0:7b4c00e3912f 300 else if (((int)status&2)==2) {
sakthipriya 0:7b4c00e3912f 301 FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
sakthipriya 0:7b4c00e3912f 302 }
sakthipriya 0:7b4c00e3912f 303 }
sakthipriya 0:7b4c00e3912f 304 }
sakthipriya 0:7b4c00e3912f 305 else //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
sakthipriya 0:7b4c00e3912f 306 {
sakthipriya 0:7b4c00e3912f 307 ACS_DATA_ACQ_STATUS = 'f';
sakthipriya 0:7b4c00e3912f 308 }
sakthipriya 0:7b4c00e3912f 309 ACS_DATA_ACQ_STATUS = 'c'; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
sakthipriya 0:7b4c00e3912f 310 }
sakthipriya 0:7b4c00e3912f 311
sakthipriya 0:7b4c00e3912f 312 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 313 {
sakthipriya 0:7b4c00e3912f 314 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 315
sakthipriya 0:7b4c00e3912f 316 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 317 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 318 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 319 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 320 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 321 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 322
sakthipriya 0:7b4c00e3912f 323
sakthipriya 0:7b4c00e3912f 324 for(int i = 0 ; i<3;i++)
sakthipriya 0:7b4c00e3912f 325 {
sakthipriya 0:7b4c00e3912f 326 // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
sakthipriya 0:7b4c00e3912f 327 }
sakthipriya 0:7b4c00e3912f 328
sakthipriya 0:7b4c00e3912f 329 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 330
sakthipriya 0:7b4c00e3912f 331
sakthipriya 0:7b4c00e3912f 332 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 333
sakthipriya 0:7b4c00e3912f 334 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 335 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 336 {
sakthipriya 0:7b4c00e3912f 337 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
sakthipriya 0:7b4c00e3912f 338 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 339 }
sakthipriya 0:7b4c00e3912f 340
sakthipriya 0:7b4c00e3912f 341 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 342 pc_acs.printf("current in trx is %f \r \n",l_current_x);
sakthipriya 0:7b4c00e3912f 343 if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 344 {
sakthipriya 0:7b4c00e3912f 345 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
sakthipriya 0:7b4c00e3912f 346 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 347 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 348 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 349 }
sakthipriya 0:7b4c00e3912f 350 else if( l_current_x >= 0.006 && l_current_x < 0.0116)
sakthipriya 0:7b4c00e3912f 351 {
sakthipriya 0:7b4c00e3912f 352 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
sakthipriya 0:7b4c00e3912f 353 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 354 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 355 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 356 }
sakthipriya 0:7b4c00e3912f 357 else if (l_current_x >= 0.0116 && l_current_x < 0.0624)
sakthipriya 0:7b4c00e3912f 358 {
sakthipriya 0:7b4c00e3912f 359 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
sakthipriya 0:7b4c00e3912f 360 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 361 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 362 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 363 }
sakthipriya 0:7b4c00e3912f 364 else if(l_current_x >= 0.0624 && l_current_x < 0.555)
sakthipriya 0:7b4c00e3912f 365 {
sakthipriya 0:7b4c00e3912f 366 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
sakthipriya 0:7b4c00e3912f 367 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 368 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 369 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 370 }
sakthipriya 0:7b4c00e3912f 371 else if(l_current_x==0)
sakthipriya 0:7b4c00e3912f 372 {
sakthipriya 0:7b4c00e3912f 373 printf("\n \r l_current_x====0");
sakthipriya 0:7b4c00e3912f 374 l_duty_cycle_x = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 375 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 376 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 377 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 378 }
sakthipriya 0:7b4c00e3912f 379 else //not necessary
sakthipriya 0:7b4c00e3912f 380 {
sakthipriya 0:7b4c00e3912f 381 g_err_flag_TR_x = 1;
sakthipriya 0:7b4c00e3912f 382 }
sakthipriya 0:7b4c00e3912f 383
sakthipriya 0:7b4c00e3912f 384 //------------------------------------- y-direction TR--------------------------------------//
sakthipriya 0:7b4c00e3912f 385
sakthipriya 0:7b4c00e3912f 386
sakthipriya 0:7b4c00e3912f 387 float l_moment_y = Moment[1]; //Moment in y direction
sakthipriya 0:7b4c00e3912f 388
sakthipriya 0:7b4c00e3912f 389 phase_TR_y = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 390 if (l_moment_y <0)
sakthipriya 0:7b4c00e3912f 391 {
sakthipriya 0:7b4c00e3912f 392 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
sakthipriya 0:7b4c00e3912f 393 l_moment_y = abs(l_moment_y);
sakthipriya 0:7b4c00e3912f 394 }
sakthipriya 0:7b4c00e3912f 395
sakthipriya 0:7b4c00e3912f 396
sakthipriya 0:7b4c00e3912f 397 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 398 pc_acs.printf("current in try is %f \r \n",l_current_y);
sakthipriya 0:7b4c00e3912f 399 if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 400 {
sakthipriya 0:7b4c00e3912f 401 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
sakthipriya 0:7b4c00e3912f 402 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 403 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 404 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 405 }
sakthipriya 0:7b4c00e3912f 406 else if( l_current_y >= 0.006 && l_current_y < 0.0116)
sakthipriya 0:7b4c00e3912f 407 {
sakthipriya 0:7b4c00e3912f 408 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
sakthipriya 0:7b4c00e3912f 409 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 410 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 411 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 412 }
sakthipriya 0:7b4c00e3912f 413 else if (l_current_y >= 0.0116&& l_current_y < 0.0624)
sakthipriya 0:7b4c00e3912f 414 {
sakthipriya 0:7b4c00e3912f 415 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
sakthipriya 0:7b4c00e3912f 416 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 417 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 418 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 419 }
sakthipriya 0:7b4c00e3912f 420 else if(l_current_y >= 0.0624 && l_current_y < 0.555)
sakthipriya 0:7b4c00e3912f 421 {
sakthipriya 0:7b4c00e3912f 422 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
sakthipriya 0:7b4c00e3912f 423 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 424 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 425 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 426 }
sakthipriya 0:7b4c00e3912f 427 else if(l_current_y==0)
sakthipriya 0:7b4c00e3912f 428 {
sakthipriya 0:7b4c00e3912f 429 printf("\n \r l_current_y====0");
sakthipriya 0:7b4c00e3912f 430 l_duty_cycle_y = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 431 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 432 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 433 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 434 }
sakthipriya 0:7b4c00e3912f 435 else // not necessary
sakthipriya 0:7b4c00e3912f 436 {
sakthipriya 0:7b4c00e3912f 437 g_err_flag_TR_y = 1;
sakthipriya 0:7b4c00e3912f 438 }
sakthipriya 0:7b4c00e3912f 439
sakthipriya 0:7b4c00e3912f 440 //----------------------------------------------- z-direction TR -------------------------//
sakthipriya 0:7b4c00e3912f 441
sakthipriya 0:7b4c00e3912f 442
sakthipriya 0:7b4c00e3912f 443 float l_moment_z = Moment[2]; //Moment in z direction
sakthipriya 0:7b4c00e3912f 444
sakthipriya 0:7b4c00e3912f 445 phase_TR_z = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 446 if (l_moment_z <0)
sakthipriya 0:7b4c00e3912f 447 {
sakthipriya 0:7b4c00e3912f 448 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
sakthipriya 0:7b4c00e3912f 449 l_moment_z = abs(l_moment_z);
sakthipriya 0:7b4c00e3912f 450 }
sakthipriya 0:7b4c00e3912f 451
sakthipriya 0:7b4c00e3912f 452
sakthipriya 0:7b4c00e3912f 453 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 454 pc_acs.printf("current in trz is %f \r \n",l_current_z);
sakthipriya 0:7b4c00e3912f 455 if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 456 {
sakthipriya 0:7b4c00e3912f 457 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
sakthipriya 0:7b4c00e3912f 458 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 459 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 460 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 461 }
sakthipriya 0:7b4c00e3912f 462 else if( l_current_z >= 0.006 && l_current_z < 0.0116)
sakthipriya 0:7b4c00e3912f 463 {
sakthipriya 0:7b4c00e3912f 464 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
sakthipriya 0:7b4c00e3912f 465 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 466 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 467 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 468 }
sakthipriya 0:7b4c00e3912f 469 else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
sakthipriya 0:7b4c00e3912f 470 {
sakthipriya 0:7b4c00e3912f 471 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
sakthipriya 0:7b4c00e3912f 472 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 473 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 474 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 475 }
sakthipriya 0:7b4c00e3912f 476 else if(l_current_z >= 0.0624 && l_current_z < 0.555)
sakthipriya 0:7b4c00e3912f 477 {
sakthipriya 0:7b4c00e3912f 478 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
sakthipriya 0:7b4c00e3912f 479 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 480 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 481 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 482 }
sakthipriya 0:7b4c00e3912f 483 else if(l_current_z==0)
sakthipriya 0:7b4c00e3912f 484 {
sakthipriya 0:7b4c00e3912f 485 printf("\n \r l_current_z====0");
sakthipriya 0:7b4c00e3912f 486 l_duty_cycle_z = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 487 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 488 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 489 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 490 }
sakthipriya 0:7b4c00e3912f 491 else // not necessary
sakthipriya 0:7b4c00e3912f 492 {
sakthipriya 0:7b4c00e3912f 493 g_err_flag_TR_z = 1;
sakthipriya 0:7b4c00e3912f 494 }
sakthipriya 0:7b4c00e3912f 495
sakthipriya 0:7b4c00e3912f 496 //-----------------------------------------exiting the function-----------------------------------//
sakthipriya 0:7b4c00e3912f 497
sakthipriya 0:7b4c00e3912f 498 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
sakthipriya 0:7b4c00e3912f 499
sakthipriya 0:7b4c00e3912f 500 }
sakthipriya 0:7b4c00e3912f 501
sakthipriya 0:7b4c00e3912f 502
sakthipriya 0:7b4c00e3912f 503