Seeker of Truth , / Mbed 2 deprecated TV_BAE_conops1_1_1

bae wrking isr no bcn

Dependencies:   mbed-rtos mbed

Fork of TV_BAE_conops1_1_1 by Team Fox

ACS.cpp@0:913c9e982740, 2015-11-03 (annotated)

Committer:
sakthipriya
Date:
Tue Nov 03 14:46:51 2015 +0000
Revision:
0:913c9e982740
i2c in rtos working

Who changed what in which revision?

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