Team Fox / Mbed 2 deprecated TFR_BAE_vr1_1working_finally_PWM_update

Updated PWM working; Entering nominal mode

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of TFR_BAE_vr1_1working_finally_USE_FOR_TESTING by Team Fox

ACS.cpp@11:0f71a96987bd, 2016-03-31 (annotated)

Committer:
Bragadeesh153
Date:
Thu Mar 31 11:44:20 2016 +0000
Revision:
11:0f71a96987bd
Parent:
10:54598e22a857 
Updated, working

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 6:036d08b62785 9 #include "EPS.h"
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 6:036d08b62785 37 extern BAE_HK_actual actual_data;
sakthipriya 0:7b4c00e3912f 38
sakthipriya 0:7b4c00e3912f 39
sakthipriya 0:7b4c00e3912f 40 //DigitalOut gpo1(PTC0); // enable of att sens2 switch
sakthipriya 0:7b4c00e3912f 41 //DigitalOut gpo2(PTC16); // enable of att sens switch
sakthipriya 0:7b4c00e3912f 42
sakthipriya 0:7b4c00e3912f 43
sakthipriya 0:7b4c00e3912f 44 Serial pc_acs(USBTX,USBRX); //for usb communication
sakthipriya 0:7b4c00e3912f 45 void inverse(float mat[3][3],float inv[3][3]);
sakthipriya 0:7b4c00e3912f 46
sakthipriya 0:7b4c00e3912f 47 int ctrl_count = 0;
sakthipriya 0:7b4c00e3912f 48 float bcopy[3];
sakthipriya 0:7b4c00e3912f 49 float moment[3];
sakthipriya 0:7b4c00e3912f 50 ///////algo working well
sakthipriya 0:7b4c00e3912f 51 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
sakthipriya 0:7b4c00e3912f 52 {
sakthipriya 0:7b4c00e3912f 53 float db[3];
sakthipriya 0:7b4c00e3912f 54 float bb[3]={0,0,0};
sakthipriya 0:7b4c00e3912f 55 float d[3]={0,0,0};
sakthipriya 0:7b4c00e3912f 56 float Jm[3][3]={{0.2730,0,0},{0,0.3018,0},{0,0,0.3031}};
sakthipriya 0:7b4c00e3912f 57 float den=0,den2;
sakthipriya 0:7b4c00e3912f 58 int i,j; //temporary variables
sakthipriya 0:7b4c00e3912f 59 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0}; //outputs
sakthipriya 0:7b4c00e3912f 60 float invJm[3][3];
sakthipriya 0:7b4c00e3912f 61 float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
sakthipriya 0:7b4c00e3912f 62
sakthipriya 0:7b4c00e3912f 63 //................. calculating db values...........................
sakthipriya 0:7b4c00e3912f 64 if(ctrl_count!=0)
sakthipriya 0:7b4c00e3912f 65 {
sakthipriya 0:7b4c00e3912f 66 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 67 db[i]= (b[i]-bcopy[i])/10;
sakthipriya 0:7b4c00e3912f 68 }
sakthipriya 0:7b4c00e3912f 69 else
sakthipriya 0:7b4c00e3912f 70 {
sakthipriya 0:7b4c00e3912f 71 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 72 db[i]= 0;
sakthipriya 0:7b4c00e3912f 73 }
sakthipriya 0:7b4c00e3912f 74 ctrl_count++;
sakthipriya 0:7b4c00e3912f 75 //..................................................................
sakthipriya 0:7b4c00e3912f 76 printf("\n\r Entered cntrl algo\n\r");
sakthipriya 0:7b4c00e3912f 77 for(int i=0; i<3; i++)
sakthipriya 0:7b4c00e3912f 78 {
sakthipriya 0:7b4c00e3912f 79 printf("%f\t",omega[i]);
sakthipriya 0:7b4c00e3912f 80 }
sakthipriya 0:7b4c00e3912f 81 for(int i=0; i<3; i++)
sakthipriya 0:7b4c00e3912f 82 {
sakthipriya 0:7b4c00e3912f 83 printf("%f\t",b[i]);
sakthipriya 0:7b4c00e3912f 84 }
sakthipriya 0:7b4c00e3912f 85
sakthipriya 0:7b4c00e3912f 86 //.........................algo......................................
sakthipriya 0:7b4c00e3912f 87 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
sakthipriya 0:7b4c00e3912f 88 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
sakthipriya 0:7b4c00e3912f 89 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 90 {
sakthipriya 0:7b4c00e3912f 91 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3));
sakthipriya 0:7b4c00e3912f 92 //db[i]/=den*den*den;
sakthipriya 0:7b4c00e3912f 93 }
sakthipriya 0:7b4c00e3912f 94 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 95 {
sakthipriya 0:7b4c00e3912f 96 b[i]/=den;
sakthipriya 0:7b4c00e3912f 97 }
sakthipriya 0:7b4c00e3912f 98 // select kz, kmu, gamma
sakthipriya 0:7b4c00e3912f 99 if(b[0]>0.9||b[0]<-0.9)
sakthipriya 0:7b4c00e3912f 100 {
sakthipriya 0:7b4c00e3912f 101 kz=kz2;
sakthipriya 0:7b4c00e3912f 102 kmu=kmu2;
sakthipriya 0:7b4c00e3912f 103 gamma=gamma2;
sakthipriya 0:7b4c00e3912f 104 }
sakthipriya 0:7b4c00e3912f 105 // calculate Mu, v, dv, z, u
sakthipriya 0:7b4c00e3912f 106 for(i=0;i<2;i++)
sakthipriya 0:7b4c00e3912f 107 {
sakthipriya 0:7b4c00e3912f 108 Mu[i]=b[i+1];
sakthipriya 0:7b4c00e3912f 109 v[i]=-kmu*Mu[i];
sakthipriya 0:7b4c00e3912f 110 dv[i]=-kmu*db[i+1];
sakthipriya 0:7b4c00e3912f 111 z[i]=db[i+1]-v[i];
sakthipriya 0:7b4c00e3912f 112 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
sakthipriya 0:7b4c00e3912f 113 }
sakthipriya 0:7b4c00e3912f 114 inverse(Jm,invJm);
sakthipriya 0:7b4c00e3912f 115 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 116 {
sakthipriya 0:7b4c00e3912f 117 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 118 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 119 }
sakthipriya 0:7b4c00e3912f 120 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 121 {
sakthipriya 0:7b4c00e3912f 122 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 123 d[i]+=bb[j]*invJm[i][j];
sakthipriya 0:7b4c00e3912f 124 }
sakthipriya 0:7b4c00e3912f 125 bb[1]=u[0]+(d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2])+(omega[2]*db[0]);
sakthipriya 0:7b4c00e3912f 126 bb[2]=u[1]-(d[0]*b[1])+(d[1]*b[0])+(omega[0]*db[1])-(omega[1]*db[0]);
sakthipriya 0:7b4c00e3912f 127 bb[0]=0;
sakthipriya 0:7b4c00e3912f 128 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 129 {
sakthipriya 0:7b4c00e3912f 130 d[i]=invJm[1][i];
sakthipriya 0:7b4c00e3912f 131 invJm[1][i]=b[2]*invJm[0][i]-b[0]*invJm[2][i];
sakthipriya 0:7b4c00e3912f 132 invJm[2][i]=-b[1]*invJm[0][i]+b[0]*d[i];
sakthipriya 0:7b4c00e3912f 133 invJm[0][i]=b[i];
sakthipriya 0:7b4c00e3912f 134 }
sakthipriya 0:7b4c00e3912f 135 inverse(invJm,Jm);
sakthipriya 0:7b4c00e3912f 136 printf("\n \r calculating tauc");
sakthipriya 0:7b4c00e3912f 137 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 138 {
sakthipriya 0:7b4c00e3912f 139 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 140 tauc[i]+=Jm[i][j]*bb[j]; // calculating torque values
sakthipriya 0:7b4c00e3912f 141 printf(" %f \t",tauc[i]);
sakthipriya 0:7b4c00e3912f 142 }
sakthipriya 0:7b4c00e3912f 143 //..........................tauc to moment conversion..........................
sakthipriya 0:7b4c00e3912f 144 printf("\n \r calculating moment");
sakthipriya 0:7b4c00e3912f 145 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 146 bcopy[i]=b[i]*den;
sakthipriya 0:7b4c00e3912f 147 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 148 {
sakthipriya 0:7b4c00e3912f 149 moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
sakthipriya 0:7b4c00e3912f 150 moment[i]/=den;
sakthipriya 0:7b4c00e3912f 151 printf(" %f \t",moment[i]);
sakthipriya 0:7b4c00e3912f 152 }
sakthipriya 0:7b4c00e3912f 153 printf("\n\r exited control algo\n");
sakthipriya 0:7b4c00e3912f 154 }
sakthipriya 0:7b4c00e3912f 155 //..........................function to find inverse..................
sakthipriya 0:7b4c00e3912f 156 void inverse(float mat[3][3],float inv[3][3])
sakthipriya 0:7b4c00e3912f 157 {
sakthipriya 0:7b4c00e3912f 158 int i,j;
sakthipriya 0:7b4c00e3912f 159 float det=0;
sakthipriya 0:7b4c00e3912f 160 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 161 {
sakthipriya 0:7b4c00e3912f 162 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 163 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 164 }
sakthipriya 0:7b4c00e3912f 165 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
sakthipriya 0:7b4c00e3912f 166 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 167 {
sakthipriya 0:7b4c00e3912f 168 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 169 inv[i][j]/=det;
sakthipriya 0:7b4c00e3912f 170 }
sakthipriya 0:7b4c00e3912f 171 }
sakthipriya 0:7b4c00e3912f 172
sakthipriya 0:7b4c00e3912f 173
sakthipriya 0:7b4c00e3912f 174 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
sakthipriya 0:7b4c00e3912f 175
sakthipriya 0:7b4c00e3912f 176 void FCTN_ACS_INIT(void); //initialization of registers happens
sakthipriya 0:7b4c00e3912f 177 void FCTN_ATS_DATA_ACQ(); //data is obtained
sakthipriya 0:7b4c00e3912f 178 void T_OUT(); //timeout function to stop infinite loop
sakthipriya 0:7b4c00e3912f 179 Timeout to; //Timeout variable to
sakthipriya 0:7b4c00e3912f 180 int toFlag;
sakthipriya 0:7b4c00e3912f 181
sakthipriya 0:7b4c00e3912f 182 int count =0; // Time for which the BAE uC is running (in seconds)
sakthipriya 0:7b4c00e3912f 183 void T_OUT()
sakthipriya 0:7b4c00e3912f 184 {
sakthipriya 0:7b4c00e3912f 185 toFlag=0; //as T_OUT function gets called the while loop gets terminated
sakthipriya 0:7b4c00e3912f 186 }
sakthipriya 0:7b4c00e3912f 187
sakthipriya 0:7b4c00e3912f 188
sakthipriya 0:7b4c00e3912f 189 //DEFINING VARIABLES
sakthipriya 0:7b4c00e3912f 190 char cmd[2];
sakthipriya 0:7b4c00e3912f 191 char raw_gyro[6];
sakthipriya 0:7b4c00e3912f 192 char raw_mag[6];
sakthipriya 0:7b4c00e3912f 193 char store,status;
sakthipriya 0:7b4c00e3912f 194 int16_t bit_data;
sakthipriya 0:7b4c00e3912f 195 float gyro_data[3], mag_data[3],combined_values[6];
sakthipriya 0:7b4c00e3912f 196 float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
sakthipriya 0:7b4c00e3912f 197 float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
sakthipriya 0:7b4c00e3912f 198 float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
sakthipriya 0:7b4c00e3912f 199
sakthipriya 0:7b4c00e3912f 200 void FCTN_ACS_INIT()
sakthipriya 0:7b4c00e3912f 201 {
sakthipriya 0:7b4c00e3912f 202 ACS_INIT_STATUS = 's'; //set ACS_INIT_STATUS flag
sakthipriya 3:07e15677a75c 203 //FLAG();
sakthipriya 0:7b4c00e3912f 204 pc_acs.printf("Attitude sensor init called \n \r");
sakthipriya 0:7b4c00e3912f 205 //FLAG();
sakthipriya 0:7b4c00e3912f 206 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 207 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 208 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
sakthipriya 0:7b4c00e3912f 209 wait_ms(2000); //waiting for loading configuration file stored in EEPROM
sakthipriya 0:7b4c00e3912f 210 cmd[0]=SENTRALSTATUS;
sakthipriya 0:7b4c00e3912f 211 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 212 i2c.read(SLAVE_ADDR_READ,&store,1);
sakthipriya 0:7b4c00e3912f 213 wait_ms(100);
sakthipriya 0:7b4c00e3912f 214 //to check whether EEPROM is uploaded
sakthipriya 0:7b4c00e3912f 215 switch((int)store) {
sakthipriya 0:7b4c00e3912f 216 case(3): {
sakthipriya 0:7b4c00e3912f 217 break;
sakthipriya 0:7b4c00e3912f 218 }
sakthipriya 0:7b4c00e3912f 219 case(11): {
sakthipriya 0:7b4c00e3912f 220 break;
sakthipriya 0:7b4c00e3912f 221 }
sakthipriya 0:7b4c00e3912f 222 default: {
sakthipriya 0:7b4c00e3912f 223 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 224 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 225 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 226 wait_ms(2000);
sakthipriya 0:7b4c00e3912f 227 }
sakthipriya 0:7b4c00e3912f 228 }
sakthipriya 0:7b4c00e3912f 229 pc_acs.printf("Sentral Status is %x\n \r",(int)store);
sakthipriya 0:7b4c00e3912f 230 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
sakthipriya 0:7b4c00e3912f 231 cmd[1]=BIT_RUN_ENB;
sakthipriya 0:7b4c00e3912f 232 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 233 wait_ms(100);
sakthipriya 0:7b4c00e3912f 234 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
sakthipriya 0:7b4c00e3912f 235 cmd[1]=BIT_MAGODR;
sakthipriya 0:7b4c00e3912f 236 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 237 wait_ms(100);
sakthipriya 0:7b4c00e3912f 238 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
sakthipriya 0:7b4c00e3912f 239 cmd[1]=BIT_GYROODR;
sakthipriya 0:7b4c00e3912f 240 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 241 wait_ms(100);
sakthipriya 0:7b4c00e3912f 242 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
sakthipriya 0:7b4c00e3912f 243 cmd[1]=0x00;
sakthipriya 0:7b4c00e3912f 244 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 245 wait_ms(100);
sakthipriya 0:7b4c00e3912f 246 cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
sakthipriya 0:7b4c00e3912f 247 cmd[1]=BIT_EVT_ENB;
sakthipriya 0:7b4c00e3912f 248 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 249 wait_ms(100);
sakthipriya 0:7b4c00e3912f 250 ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
sakthipriya 0:7b4c00e3912f 251 }
sakthipriya 0:7b4c00e3912f 252
sakthipriya 0:7b4c00e3912f 253 void FCTN_ATS_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 254 {
sakthipriya 0:7b4c00e3912f 255 ACS_DATA_ACQ_STATUS = 's'; //set ACS_DATA_ACQ_STATUS flag for att sens 2
sakthipriya 0:7b4c00e3912f 256 if( ACS_ATS_ENABLE == 'e')
sakthipriya 0:7b4c00e3912f 257 {
sakthipriya 0:7b4c00e3912f 258 FLAG();
sakthipriya 0:7b4c00e3912f 259 pc_acs.printf("attitude sensor execution called \n \r");
sakthipriya 0:7b4c00e3912f 260 toFlag=1; //toFlag is set to 1 so that it enters while loop
sakthipriya 0:7b4c00e3912f 261 to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated
sakthipriya 0:7b4c00e3912f 262 while(toFlag) {
sakthipriya 0:7b4c00e3912f 263 cmd[0]=EVT_STATUS;
sakthipriya 0:7b4c00e3912f 264 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 265 i2c.read(SLAVE_ADDR_READ,&status,1);
sakthipriya 0:7b4c00e3912f 266 wait_ms(100);
sakthipriya 0:7b4c00e3912f 267 pc_acs.printf("Event Status is %x\n \r",(int)status);
sakthipriya 0:7b4c00e3912f 268 //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
sakthipriya 0:7b4c00e3912f 269 if(((int)status&40)==40) {
sakthipriya 0:7b4c00e3912f 270 cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
sakthipriya 0:7b4c00e3912f 271 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 272 i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
sakthipriya 0:7b4c00e3912f 273 cmd[0]=MAG_XOUT_H; //LSB of x
sakthipriya 0:7b4c00e3912f 274 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 275 i2c.read(SLAVE_ADDR_READ,raw_mag,6);
sakthipriya 0:7b4c00e3912f 276 // pc_acs.printf("\nGyro Values:\n");
sakthipriya 0:7b4c00e3912f 277 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 278 //concatenating gyro LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 279 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
sakthipriya 0:7b4c00e3912f 280 gyro_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 281 gyro_data[i]=gyro_data[i]/senstivity_gyro;
sakthipriya 0:7b4c00e3912f 282 gyro_data[i]+=gyro_error[i];
sakthipriya 0:7b4c00e3912f 283 // pc_acs.printf("%f\t",gyro_data[i]);
sakthipriya 0:7b4c00e3912f 284 }
sakthipriya 0:7b4c00e3912f 285 // pc_acs.printf("\nMag Values:\n");
sakthipriya 0:7b4c00e3912f 286 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 287 //concatenating mag LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 288 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
sakthipriya 0:7b4c00e3912f 289 mag_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 290 mag_data[i]=mag_data[i]/senstivity_mag;
sakthipriya 0:7b4c00e3912f 291 mag_data[i]+=mag_error[i];
sakthipriya 0:7b4c00e3912f 292 // pc_acs.printf("%f\t",mag_data[i]);
sakthipriya 0:7b4c00e3912f 293 }
sakthipriya 0:7b4c00e3912f 294 for(int i=0; i<3; i++) {
sakthipriya 6:036d08b62785 295 // data[i]=gyro_data[i];
sakthipriya 6:036d08b62785 296 actual_data.AngularSpeed_actual[i] = gyro_data[i];
sakthipriya 6:036d08b62785 297 actual_data.Bvalue_actual[i] = mag_data[i];
sakthipriya 6:036d08b62785 298 //data[i+3]=mag_data[i];
sakthipriya 0:7b4c00e3912f 299 }
sakthipriya 0:7b4c00e3912f 300 // return(combined_values); //returning poiter combined values
sakthipriya 0:7b4c00e3912f 301 }
sakthipriya 0:7b4c00e3912f 302 //checking for the error
sakthipriya 0:7b4c00e3912f 303 else if (((int)status&2)==2) {
sakthipriya 0:7b4c00e3912f 304 FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
sakthipriya 0:7b4c00e3912f 305 }
sakthipriya 0:7b4c00e3912f 306 }
sakthipriya 0:7b4c00e3912f 307 }
sakthipriya 0:7b4c00e3912f 308 else //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
sakthipriya 0:7b4c00e3912f 309 {
sakthipriya 0:7b4c00e3912f 310 ACS_DATA_ACQ_STATUS = 'f';
sakthipriya 0:7b4c00e3912f 311 }
sakthipriya 0:7b4c00e3912f 312 ACS_DATA_ACQ_STATUS = 'c'; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
sakthipriya 0:7b4c00e3912f 313 }
sakthipriya 0:7b4c00e3912f 314
sakthipriya 0:7b4c00e3912f 315 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 316 {
sakthipriya 0:7b4c00e3912f 317 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 318
sakthipriya 0:7b4c00e3912f 319 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 320 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 321 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 322 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 323 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 324 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 325
sakthipriya 0:7b4c00e3912f 326
sakthipriya 0:7b4c00e3912f 327 for(int i = 0 ; i<3;i++)
sakthipriya 0:7b4c00e3912f 328 {
sakthipriya 0:7b4c00e3912f 329 // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
sakthipriya 0:7b4c00e3912f 330 }
sakthipriya 0:7b4c00e3912f 331
sakthipriya 0:7b4c00e3912f 332 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 333
sakthipriya 0:7b4c00e3912f 334
sakthipriya 0:7b4c00e3912f 335 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 336
sakthipriya 0:7b4c00e3912f 337 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 338 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 339 {
sakthipriya 0:7b4c00e3912f 340 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 341 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 342 }
sakthipriya 0:7b4c00e3912f 343
sakthipriya 0:7b4c00e3912f 344 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
Bragadeesh153 10:54598e22a857 345 printf("current in trx is %f \r \n",l_current_x);
Bragadeesh153 10:54598e22a857 346 if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 347 {
Bragadeesh153 10:54598e22a857 348 l_duty_cycle_x = 3*10000000*pow(l_current_x,3)- 90216*pow(l_current_x,2) + 697.78*l_current_x - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
Bragadeesh153 10:54598e22a857 349 printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 350 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 351 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 352 }
Bragadeesh153 10:54598e22a857 353 else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
sakthipriya 0:7b4c00e3912f 354 {
Bragadeesh153 10:54598e22a857 355 l_duty_cycle_x = - 76880*pow(l_current_x,3) + 1280.8*pow(l_current_x,2) + 583.78*l_current_x + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
Bragadeesh153 10:54598e22a857 356 printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 357 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 358 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 359 }
Bragadeesh153 10:54598e22a857 360 else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
sakthipriya 0:7b4c00e3912f 361 {
Bragadeesh153 10:54598e22a857 362 l_duty_cycle_x = 275.92*pow(l_current_x,2) + 546.13*l_current_x + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
Bragadeesh153 10:54598e22a857 363 printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 364 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 365 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 366 }
sakthipriya 0:7b4c00e3912f 367 else if(l_current_x==0)
sakthipriya 0:7b4c00e3912f 368 {
sakthipriya 0:7b4c00e3912f 369 printf("\n \r l_current_x====0");
sakthipriya 0:7b4c00e3912f 370 l_duty_cycle_x = 0; // default value of duty cycle
Bragadeesh153 10:54598e22a857 371 printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 372 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 373 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 374 }
sakthipriya 0:7b4c00e3912f 375 else //not necessary
sakthipriya 0:7b4c00e3912f 376 {
sakthipriya 0:7b4c00e3912f 377 g_err_flag_TR_x = 1;
sakthipriya 0:7b4c00e3912f 378 }
sakthipriya 0:7b4c00e3912f 379
sakthipriya 0:7b4c00e3912f 380 //------------------------------------- y-direction TR--------------------------------------//
sakthipriya 0:7b4c00e3912f 381
sakthipriya 0:7b4c00e3912f 382
sakthipriya 0:7b4c00e3912f 383 float l_moment_y = Moment[1]; //Moment in y direction
sakthipriya 0:7b4c00e3912f 384
sakthipriya 0:7b4c00e3912f 385 phase_TR_y = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 386 if (l_moment_y <0)
sakthipriya 0:7b4c00e3912f 387 {
sakthipriya 0:7b4c00e3912f 388 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 389 l_moment_y = abs(l_moment_y);
sakthipriya 0:7b4c00e3912f 390 }
sakthipriya 0:7b4c00e3912f 391
sakthipriya 0:7b4c00e3912f 392
sakthipriya 0:7b4c00e3912f 393 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
Bragadeesh153 10:54598e22a857 394 printf("current in try is %f \r \n",l_current_y);
Bragadeesh153 10:54598e22a857 395 if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 396 {
Bragadeesh153 10:54598e22a857 397 l_duty_cycle_y = 3*10000000*pow(l_current_y,3)- 90216*pow(l_current_y,2) + 697.78*l_current_y - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
Bragadeesh153 10:54598e22a857 398 printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 399 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 400 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 401 }
Bragadeesh153 10:54598e22a857 402 else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
sakthipriya 0:7b4c00e3912f 403 {
Bragadeesh153 10:54598e22a857 404 l_duty_cycle_y = - 76880*pow(l_current_y,3) + 1280.8*pow(l_current_y,2) + 583.78*l_current_y + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
Bragadeesh153 10:54598e22a857 405 printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 406 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 407 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 408 }
Bragadeesh153 10:54598e22a857 409 else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
sakthipriya 0:7b4c00e3912f 410 {
Bragadeesh153 10:54598e22a857 411 l_duty_cycle_y = 275.92*pow(l_current_y,2) + 546.13*l_current_y + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
Bragadeesh153 10:54598e22a857 412 printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 413 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 414 PWM2 = l_duty_cycle_y/100 ;
Bragadeesh153 10:54598e22a857 415 }
sakthipriya 0:7b4c00e3912f 416 else if(l_current_y==0)
sakthipriya 0:7b4c00e3912f 417 {
sakthipriya 0:7b4c00e3912f 418 printf("\n \r l_current_y====0");
sakthipriya 0:7b4c00e3912f 419 l_duty_cycle_y = 0; // default value of duty cycle
Bragadeesh153 10:54598e22a857 420 printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 421 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 422 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 423 }
sakthipriya 0:7b4c00e3912f 424 else // not necessary
sakthipriya 0:7b4c00e3912f 425 {
sakthipriya 0:7b4c00e3912f 426 g_err_flag_TR_y = 1;
sakthipriya 0:7b4c00e3912f 427 }
sakthipriya 0:7b4c00e3912f 428
sakthipriya 0:7b4c00e3912f 429 //----------------------------------------------- z-direction TR -------------------------//
sakthipriya 0:7b4c00e3912f 430
sakthipriya 0:7b4c00e3912f 431
sakthipriya 0:7b4c00e3912f 432 float l_moment_z = Moment[2]; //Moment in z direction
sakthipriya 0:7b4c00e3912f 433
sakthipriya 0:7b4c00e3912f 434 phase_TR_z = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 435 if (l_moment_z <0)
sakthipriya 0:7b4c00e3912f 436 {
sakthipriya 0:7b4c00e3912f 437 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 438 l_moment_z = abs(l_moment_z);
sakthipriya 0:7b4c00e3912f 439 }
sakthipriya 0:7b4c00e3912f 440
sakthipriya 0:7b4c00e3912f 441
sakthipriya 0:7b4c00e3912f 442 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
Bragadeesh153 10:54598e22a857 443 printf("current in trz is %f \r \n",l_current_z);
Bragadeesh153 10:54598e22a857 444 if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 445 {
Bragadeesh153 10:54598e22a857 446 l_duty_cycle_z = 3*10000000*pow(l_current_z,3)- 90216*pow(l_current_z,2) + 697.78*l_current_z - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
Bragadeesh153 10:54598e22a857 447 printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 448 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 449 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 450 }
Bragadeesh153 10:54598e22a857 451 else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
sakthipriya 0:7b4c00e3912f 452 {
Bragadeesh153 10:54598e22a857 453 l_duty_cycle_z = - 76880*pow(l_current_z,3) + 1280.8*pow(l_current_z,2) + 583.78*l_current_z + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
Bragadeesh153 10:54598e22a857 454 printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 455 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 456 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 457 }
Bragadeesh153 10:54598e22a857 458 else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
sakthipriya 0:7b4c00e3912f 459 {
Bragadeesh153 10:54598e22a857 460 l_duty_cycle_z = 275.92*pow(l_current_z,2) + 546.13*l_current_z + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
Bragadeesh153 10:54598e22a857 461 printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 462 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 463 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 464 }
sakthipriya 0:7b4c00e3912f 465 else if(l_current_z==0)
sakthipriya 0:7b4c00e3912f 466 {
sakthipriya 0:7b4c00e3912f 467 printf("\n \r l_current_z====0");
sakthipriya 0:7b4c00e3912f 468 l_duty_cycle_z = 0; // default value of duty cycle
Bragadeesh153 10:54598e22a857 469 printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 470 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 471 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 472 }
sakthipriya 0:7b4c00e3912f 473 else // not necessary
sakthipriya 0:7b4c00e3912f 474 {
sakthipriya 0:7b4c00e3912f 475 g_err_flag_TR_z = 1;
sakthipriya 0:7b4c00e3912f 476 }
sakthipriya 0:7b4c00e3912f 477
sakthipriya 0:7b4c00e3912f 478 //-----------------------------------------exiting the function-----------------------------------//
sakthipriya 0:7b4c00e3912f 479
sakthipriya 0:7b4c00e3912f 480 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
sakthipriya 0:7b4c00e3912f 481
sakthipriya 0:7b4c00e3912f 482 }
sakthipriya 0:7b4c00e3912f 483
sakthipriya 0:7b4c00e3912f 484
Bragadeesh153 11:0f71a96987bd 485 /*void FCTN_ACS_GENPWM_MAIN(float Moment[3])
Bragadeesh153 11:0f71a96987bd 486 {
Bragadeesh153 11:0f71a96987bd 487 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
Bragadeesh153 11:0f71a96987bd 488
Bragadeesh153 11:0f71a96987bd 489 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
Bragadeesh153 11:0f71a96987bd 490 float l_current_x=0; //Current sent in x TR's
Bragadeesh153 11:0f71a96987bd 491 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
Bragadeesh153 11:0f71a96987bd 492 float l_current_y=0; //Current sent in y TR's
Bragadeesh153 11:0f71a96987bd 493 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
Bragadeesh153 11:0f71a96987bd 494 float l_current_z=0; //Current sent in z TR's
Bragadeesh153 11:0f71a96987bd 495
Bragadeesh153 11:0f71a96987bd 496
Bragadeesh153 11:0f71a96987bd 497 for(int i = 0 ; i<3;i++)
Bragadeesh153 11:0f71a96987bd 498 {
Bragadeesh153 11:0f71a96987bd 499 printf("pwm %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
Bragadeesh153 11:0f71a96987bd 500 }
Bragadeesh153 11:0f71a96987bd 501
Bragadeesh153 11:0f71a96987bd 502 //----------------------------- x-direction TR --------------------------------------------//
Bragadeesh153 11:0f71a96987bd 503
Bragadeesh153 11:0f71a96987bd 504
Bragadeesh153 11:0f71a96987bd 505 float l_moment_x = Moment[0]; //Moment in x direction
Bragadeesh153 11:0f71a96987bd 506
Bragadeesh153 11:0f71a96987bd 507 phase_TR_x = 1; // setting the default current direction
Bragadeesh153 11:0f71a96987bd 508 if (l_moment_x <0)
Bragadeesh153 11:0f71a96987bd 509 {
Bragadeesh153 11:0f71a96987bd 510 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
Bragadeesh153 11:0f71a96987bd 511 l_moment_x = abs(l_moment_x);
Bragadeesh153 11:0f71a96987bd 512 }
Bragadeesh153 11:0f71a96987bd 513
Bragadeesh153 11:0f71a96987bd 514 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
Bragadeesh153 11:0f71a96987bd 515 pc_acs.printf("current in trx is %f \r \n",l_current_x);
Bragadeesh153 11:0f71a96987bd 516 if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
Bragadeesh153 11:0f71a96987bd 517 {
Bragadeesh153 11:0f71a96987bd 518 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
Bragadeesh153 11:0f71a96987bd 519 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
Bragadeesh153 11:0f71a96987bd 520 PWM1.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 521 PWM1 = l_duty_cycle_x/100 ;
Bragadeesh153 11:0f71a96987bd 522 }
Bragadeesh153 11:0f71a96987bd 523 else if( l_current_x >= 0.006 && l_current_x < 0.0116)
Bragadeesh153 11:0f71a96987bd 524 {
Bragadeesh153 11:0f71a96987bd 525 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
Bragadeesh153 11:0f71a96987bd 526 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
Bragadeesh153 11:0f71a96987bd 527 PWM1.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 528 PWM1 = l_duty_cycle_x/100 ;
Bragadeesh153 11:0f71a96987bd 529 }
Bragadeesh153 11:0f71a96987bd 530 else if (l_current_x >= 0.0116 && l_current_x < 0.0624)
Bragadeesh153 11:0f71a96987bd 531 {
Bragadeesh153 11:0f71a96987bd 532 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
Bragadeesh153 11:0f71a96987bd 533 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
Bragadeesh153 11:0f71a96987bd 534 PWM1.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 535 PWM1 = l_duty_cycle_x/100 ;
Bragadeesh153 11:0f71a96987bd 536 }
Bragadeesh153 11:0f71a96987bd 537 else if(l_current_x >= 0.0624 && l_current_x < 0.555)
Bragadeesh153 11:0f71a96987bd 538 {
Bragadeesh153 11:0f71a96987bd 539 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
Bragadeesh153 11:0f71a96987bd 540 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
Bragadeesh153 11:0f71a96987bd 541 PWM1.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 542 PWM1 = l_duty_cycle_x/100 ;
Bragadeesh153 11:0f71a96987bd 543 }
Bragadeesh153 11:0f71a96987bd 544 else if(l_current_x==0)
Bragadeesh153 11:0f71a96987bd 545 {
Bragadeesh153 11:0f71a96987bd 546 printf("\n \r l_current_x====0");
Bragadeesh153 11:0f71a96987bd 547 l_duty_cycle_x = 0; // default value of duty cycle
Bragadeesh153 11:0f71a96987bd 548 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
Bragadeesh153 11:0f71a96987bd 549 PWM1.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 550 PWM1 = l_duty_cycle_x/100 ;
Bragadeesh153 11:0f71a96987bd 551 }
Bragadeesh153 11:0f71a96987bd 552 else //not necessary
Bragadeesh153 11:0f71a96987bd 553 {
Bragadeesh153 11:0f71a96987bd 554 g_err_flag_TR_x = 1;
Bragadeesh153 11:0f71a96987bd 555 }
Bragadeesh153 11:0f71a96987bd 556
Bragadeesh153 11:0f71a96987bd 557 //------------------------------------- y-direction TR--------------------------------------//
Bragadeesh153 11:0f71a96987bd 558
Bragadeesh153 11:0f71a96987bd 559
Bragadeesh153 11:0f71a96987bd 560 float l_moment_y = Moment[1]; //Moment in y direction
Bragadeesh153 11:0f71a96987bd 561
Bragadeesh153 11:0f71a96987bd 562 phase_TR_y = 1; // setting the default current direction
Bragadeesh153 11:0f71a96987bd 563 if (l_moment_y <0)
Bragadeesh153 11:0f71a96987bd 564 {
Bragadeesh153 11:0f71a96987bd 565 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
Bragadeesh153 11:0f71a96987bd 566 l_moment_y = abs(l_moment_y);
Bragadeesh153 11:0f71a96987bd 567 }
Bragadeesh153 11:0f71a96987bd 568
Bragadeesh153 11:0f71a96987bd 569
Bragadeesh153 11:0f71a96987bd 570 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
Bragadeesh153 11:0f71a96987bd 571 pc_acs.printf("current in try is %f \r \n",l_current_y);
Bragadeesh153 11:0f71a96987bd 572 if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
Bragadeesh153 11:0f71a96987bd 573 {
Bragadeesh153 11:0f71a96987bd 574 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
Bragadeesh153 11:0f71a96987bd 575 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
Bragadeesh153 11:0f71a96987bd 576 PWM2.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 577 PWM2 = l_duty_cycle_y/100 ;
Bragadeesh153 11:0f71a96987bd 578 }
Bragadeesh153 11:0f71a96987bd 579 else if( l_current_y >= 0.006 && l_current_y < 0.0116)
Bragadeesh153 11:0f71a96987bd 580 {
Bragadeesh153 11:0f71a96987bd 581 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
Bragadeesh153 11:0f71a96987bd 582 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
Bragadeesh153 11:0f71a96987bd 583 PWM2.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 584 PWM2 = l_duty_cycle_y/100 ;
Bragadeesh153 11:0f71a96987bd 585 }
Bragadeesh153 11:0f71a96987bd 586 else if (l_current_y >= 0.0116&& l_current_y < 0.0624)
Bragadeesh153 11:0f71a96987bd 587 {
Bragadeesh153 11:0f71a96987bd 588 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
Bragadeesh153 11:0f71a96987bd 589 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
Bragadeesh153 11:0f71a96987bd 590 PWM2.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 591 PWM2 = l_duty_cycle_y/100 ;
Bragadeesh153 11:0f71a96987bd 592 }
Bragadeesh153 11:0f71a96987bd 593 else if(l_current_y >= 0.0624 && l_current_y < 0.555)
Bragadeesh153 11:0f71a96987bd 594 {
Bragadeesh153 11:0f71a96987bd 595 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
Bragadeesh153 11:0f71a96987bd 596 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
Bragadeesh153 11:0f71a96987bd 597 PWM2.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 598 PWM2 = l_duty_cycle_y/100 ;
Bragadeesh153 11:0f71a96987bd 599 }
Bragadeesh153 11:0f71a96987bd 600 else if(l_current_y==0)
Bragadeesh153 11:0f71a96987bd 601 {
Bragadeesh153 11:0f71a96987bd 602 printf("\n \r l_current_y====0");
Bragadeesh153 11:0f71a96987bd 603 l_duty_cycle_y = 0; // default value of duty cycle
Bragadeesh153 11:0f71a96987bd 604 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
Bragadeesh153 11:0f71a96987bd 605 PWM2.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 606 PWM2 = l_duty_cycle_y/100 ;
Bragadeesh153 11:0f71a96987bd 607 }
Bragadeesh153 11:0f71a96987bd 608 else // not necessary
Bragadeesh153 11:0f71a96987bd 609 {
Bragadeesh153 11:0f71a96987bd 610 g_err_flag_TR_y = 1;
Bragadeesh153 11:0f71a96987bd 611 }
Bragadeesh153 11:0f71a96987bd 612
Bragadeesh153 11:0f71a96987bd 613 //----------------------------------------------- z-direction TR -------------------------//
Bragadeesh153 11:0f71a96987bd 614
Bragadeesh153 11:0f71a96987bd 615
Bragadeesh153 11:0f71a96987bd 616 float l_moment_z = Moment[2]; //Moment in z direction
Bragadeesh153 11:0f71a96987bd 617
Bragadeesh153 11:0f71a96987bd 618 phase_TR_z = 1; // setting the default current direction
Bragadeesh153 11:0f71a96987bd 619 if (l_moment_z <0)
Bragadeesh153 11:0f71a96987bd 620 {
Bragadeesh153 11:0f71a96987bd 621 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
Bragadeesh153 11:0f71a96987bd 622 l_moment_z = abs(l_moment_z);
Bragadeesh153 11:0f71a96987bd 623 }
Bragadeesh153 11:0f71a96987bd 624
Bragadeesh153 11:0f71a96987bd 625
Bragadeesh153 11:0f71a96987bd 626 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
Bragadeesh153 11:0f71a96987bd 627 pc_acs.printf("current in trz is %f \r \n",l_current_z);
Bragadeesh153 11:0f71a96987bd 628 if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
Bragadeesh153 11:0f71a96987bd 629 {
Bragadeesh153 11:0f71a96987bd 630 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
Bragadeesh153 11:0f71a96987bd 631 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
Bragadeesh153 11:0f71a96987bd 632 PWM3.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 633 PWM3 = l_duty_cycle_z/100 ;
Bragadeesh153 11:0f71a96987bd 634 }
Bragadeesh153 11:0f71a96987bd 635 else if( l_current_z >= 0.006 && l_current_z < 0.0116)
Bragadeesh153 11:0f71a96987bd 636 {
Bragadeesh153 11:0f71a96987bd 637 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
Bragadeesh153 11:0f71a96987bd 638 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
Bragadeesh153 11:0f71a96987bd 639 PWM3.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 640 PWM3 = l_duty_cycle_z/100 ;
Bragadeesh153 11:0f71a96987bd 641 }
Bragadeesh153 11:0f71a96987bd 642 else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
Bragadeesh153 11:0f71a96987bd 643 {
Bragadeesh153 11:0f71a96987bd 644 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
Bragadeesh153 11:0f71a96987bd 645 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
Bragadeesh153 11:0f71a96987bd 646 PWM3.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 647 PWM3 = l_duty_cycle_z/100 ;
Bragadeesh153 11:0f71a96987bd 648 }
Bragadeesh153 11:0f71a96987bd 649 else if(l_current_z >= 0.0624 && l_current_z < 0.555)
Bragadeesh153 11:0f71a96987bd 650 {
Bragadeesh153 11:0f71a96987bd 651 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
Bragadeesh153 11:0f71a96987bd 652 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
Bragadeesh153 11:0f71a96987bd 653 PWM3.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 654 PWM3 = l_duty_cycle_z/100 ;
Bragadeesh153 11:0f71a96987bd 655 }
Bragadeesh153 11:0f71a96987bd 656 else if(l_current_z==0)
Bragadeesh153 11:0f71a96987bd 657 {
Bragadeesh153 11:0f71a96987bd 658 printf("\n \r l_current_z====0");
Bragadeesh153 11:0f71a96987bd 659 l_duty_cycle_z = 0; // default value of duty cycle
Bragadeesh153 11:0f71a96987bd 660 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
Bragadeesh153 11:0f71a96987bd 661 PWM3.period(TIME_PERIOD);
Bragadeesh153 11:0f71a96987bd 662 PWM3 = l_duty_cycle_z/100 ;
Bragadeesh153 11:0f71a96987bd 663 }
Bragadeesh153 11:0f71a96987bd 664 else // not necessary
Bragadeesh153 11:0f71a96987bd 665 {
Bragadeesh153 11:0f71a96987bd 666 g_err_flag_TR_z = 1;
Bragadeesh153 11:0f71a96987bd 667 }
Bragadeesh153 11:0f71a96987bd 668
Bragadeesh153 11:0f71a96987bd 669 //-----------------------------------------exiting the function-----------------------------------//
Bragadeesh153 11:0f71a96987bd 670
Bragadeesh153 11:0f71a96987bd 671 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
Bragadeesh153 11:0f71a96987bd 672
Bragadeesh153 11:0f71a96987bd 673 }*/
Bragadeesh153 11:0f71a96987bd 674
Bragadeesh153 11:0f71a96987bd 675
sakthipriya 0:7b4c00e3912f 676