Team Fox
/
ACS_Flowchart_BAE
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
QM_BAE_review_1
by 
Team Fox
Embed:
(wiki syntax)
Show/hide line numbers
ACS.cpp
00001 /*------------------------------------------------------------------------------------------------------------------------------------------------------ 00002 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/ 00003 #include <mbed.h> 00004 #include <math.h> 00005 00006 #include "pni.h" //pni header file 00007 #include "pin_config.h" 00008 #include "ACS.h" 00009 #include "EPS.h" 00010 00011 //********************************flags******************************************// 00012 extern uint32_t BAE_STATUS; 00013 extern uint32_t BAE_ENABLE; 00014 extern uint8_t ACS_INIT_STATUS; 00015 extern uint8_t ACS_DATA_ACQ_STATUS; 00016 extern uint8_t ACS_ATS_STATUS; 00017 extern uint8_t ACS_MAIN_STATUS; 00018 extern uint8_t ACS_STATUS; 00019 00020 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch 00021 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch 00022 00023 extern uint8_t ACS_ATS_ENABLE; 00024 extern uint8_t ACS_DATA_ACQ_ENABLE; 00025 extern uint8_t ACS_STATE; 00026 00027 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod 00028 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod 00029 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod 00030 00031 extern PwmOut PWM1; //x //Functions used to generate PWM signal 00032 extern PwmOut PWM2; //y 00033 extern PwmOut PWM3; //z //PWM output comes from pins p6 00034 00035 int g_err_flag_TR_x=0; // setting x-flag to zero 00036 int g_err_flag_TR_y=0; // setting y-flag to zero 00037 int g_err_flag_TR_z=0; // setting z-flag to zero 00038 00039 extern float data[6]; 00040 extern BAE_HK_actual actual_data; 00041 00042 00043 //DigitalOut gpo1(PTC0); // enable of att sens2 switch 00044 //DigitalOut gpo2(PTC16); // enable of att sens switch 00045 00046 00047 Serial pc_acs(USBTX,USBRX); //for usb communication 00048 //CONTROL_ALGO 00049 00050 float moment[3]; // Unit: Ampere*Meter^2 00051 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla 00052 int flag_firsttime=1, controlmode, alarmmode=0; 00053 00054 00055 00056 void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode); 00057 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax); 00058 float max_array(float arr[3]); 00059 void inverse(float mat[3][3],float inv[3][3]); 00060 00061 //CONTROLALGO PARAMETERS 00062 00063 00064 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3]) 00065 { 00066 00067 float b1[3]; 00068 float omega1[3]; 00069 b1[0] = b[0]/1000000.0; 00070 b1[1] = b[1]/1000000.0; 00071 b1[2] = b[2]/1000000.0; 00072 00073 omega1[0] = omega[0]*3.14159/180; 00074 omega1[1] = omega[1]*3.14159/180; 00075 omega1[2] = omega[2]*3.14159/180; 00076 controller (moment, b1, omega1, b_old, alarmmode, flag_firsttime, controlmode); 00077 00078 } 00079 void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode) 00080 { 00081 float db1[3]; // Unit: Tesla/Second 00082 float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds 00083 float MmntMax=1.1; // Unit: Ampere*Meter^2 00084 float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second 00085 float normalising_fact; 00086 float b1_copy[3], omega1_copy[3], db1_copy[3]; 00087 int i; 00088 if(flag_firsttime==1) 00089 { 00090 for(i=0;i<3;i++) 00091 { 00092 db1[i]=0; // Unit: Tesla/Second 00093 } 00094 flag_firsttime=0; 00095 } 00096 else 00097 { 00098 for(i=0;i<3;i++) 00099 { 00100 db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second 00101 } 00102 } 00103 00104 if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1) 00105 { 00106 alarmmode=0; 00107 } 00108 else if(max_array(omega1)>OmegaMax && alarmmode==0) 00109 { 00110 alarmmode=1; 00111 } 00112 00113 for (i=0;i<3;i++) 00114 { 00115 b1_copy[i]=b1[i]; 00116 db1_copy[i]=db1[i]; 00117 omega1_copy[i]=omega1[i]; 00118 } 00119 00120 if(alarmmode==0) 00121 { 00122 controlmode=0; 00123 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax); 00124 for (i=0;i<3;i++) 00125 { 00126 b1[i]=b1_copy[i]; 00127 db1[i]=db1_copy[i]; 00128 omega1[i]=omega1_copy[i]; 00129 } 00130 if(max_array(moment)>MmntMax) 00131 { 00132 controlmode=1; 00133 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax); 00134 for (i=0;i<3;i++) 00135 { 00136 b1[i]=b1_copy[i]; 00137 db1[i]=db1_copy[i]; 00138 omega1[i]=omega1_copy[i]; 00139 } 00140 if(max_array(moment)>MmntMax) 00141 { 00142 normalising_fact=max_array(moment)/MmntMax; 00143 for(i=0;i<3;i++) 00144 { 00145 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 00146 } 00147 } 00148 } 00149 } 00150 else 00151 { 00152 controlmode=1; 00153 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax); 00154 for (i=0;i<3;i++) 00155 { 00156 b1[i]=b1_copy[i]; 00157 db1[i]=db1_copy[i]; 00158 omega1[i]=omega1_copy[i]; 00159 } 00160 if(max_array(moment)>MmntMax) 00161 { 00162 normalising_fact=max_array(moment)/MmntMax; 00163 for(i=0;i<3;i++) 00164 { 00165 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 00166 } 00167 } 00168 00169 } 00170 for (i=0;i<3;i++) 00171 { 00172 b_old[i]=b1[i]; 00173 } 00174 } 00175 00176 void inverse(float mat[3][3],float inv[3][3]) 00177 { 00178 int i,j; 00179 float det=0; 00180 for(i=0;i<3;i++) 00181 { 00182 for(j=0;j<3;j++) 00183 { 00184 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]); 00185 } 00186 } 00187 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]); 00188 for(i=0;i<3;i++) 00189 { 00190 for(j=0;j<3;j++) 00191 { 00192 inv[i][j]/=det; 00193 } 00194 } 00195 } 00196 00197 float max_array(float arr[3]) 00198 { 00199 int i; 00200 float temp_max=fabs(arr[0]); 00201 for(i=1;i<3;i++) 00202 { 00203 if(fabs(arr[i])>temp_max) 00204 { 00205 temp_max=fabs(arr[i]); 00206 } 00207 } 00208 return temp_max; 00209 } 00210 00211 00212 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax) 00213 { 00214 float bb[3]={0,0,0}; 00215 float d[3]={0,0,0}; 00216 float Jm[3][3]={{0.2271,0.0014,-0.0026},{0.0014,0.2167,-0.004},{-0.0026,-0.004,0.2406}}; // Unit: Kilogram*Meter^2. Jm may change depending on the final satellite structure 00217 float den=0,den2; 00218 float bcopy[3]; 00219 int i, j;//temporary variables 00220 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs 00221 float invJm[3][3]; 00222 float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4,kdetumble=2000000; 00223 int infflag; // Flag variable to check if the moment value is infinity or NaN 00224 00225 if(controlmode1==0) 00226 { 00227 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2])); 00228 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]); 00229 for(i=0;i<3;i++) 00230 { 00231 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1) 00232 } 00233 for(i=0;i<3;i++) 00234 { 00235 b[i]/=den; // Mormalized b. Hence no unit. 00236 } 00237 if(b[2]>0.9 || b[2]<-0.9) 00238 { 00239 kz=kz2; 00240 kmu=kmu2; 00241 gamma=gamma2; 00242 } 00243 for(i=0;i<2;i++) 00244 { 00245 Mu[i]=b[i]; 00246 v[i]=-kmu*Mu[i]; 00247 dv[i]=-kmu*db[i]; 00248 z[i]=db[i]-v[i]; 00249 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma); 00250 } 00251 inverse(Jm,invJm); 00252 for(i=0;i<3;i++) 00253 { 00254 for(j=0;j<3;j++) 00255 { 00256 bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]); 00257 } 00258 } 00259 for(i=0;i<3;i++) 00260 { 00261 for(j=0;j<3;j++) 00262 { 00263 d[i]+=bb[j]*invJm[i][j]; 00264 } 00265 } 00266 bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]); 00267 bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]); 00268 bb[0]=0; 00269 for(i=0;i<3;i++) 00270 { 00271 d[i]=invJm[2][i]; 00272 invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i]; 00273 invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i]; 00274 invJm[0][i]=b[i]; 00275 } 00276 inverse(invJm,Jm); 00277 for(i=0;i<3;i++) 00278 { 00279 for(j=0;j<3;j++) 00280 { 00281 tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2 00282 } 00283 } 00284 for(i=0;i<3;i++) 00285 { 00286 bcopy[i]=b[i]*den; 00287 } 00288 for(i=0;i<3;i++) 00289 { 00290 Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3]; 00291 Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2 00292 } 00293 infflag=0; 00294 for (i=0; i<3 && infflag==0; i++) 00295 { 00296 if (isinf(Mmnt[i])==1 || isnan(Mmnt[i])==1) 00297 infflag=1; 00298 } 00299 if (infflag==1) 00300 { 00301 for (i=0; i<3; i++) 00302 Mmnt[i]=2*MmntMax; 00303 } 00304 00305 } 00306 else if(controlmode1==1) 00307 { 00308 for(i=0;i<3;i++) 00309 { 00310 Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2 00311 } 00312 } 00313 for(i=0;i<3;i++) 00314 { 00315 moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2 00316 } 00317 } 00318 00319 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge 00320 00321 int FCTN_ACS_INIT(); //initialization of registers happens 00322 int SENSOR_INIT(); 00323 int FCTN_ATS_DATA_ACQ(); //data is obtained 00324 int SENSOR_DATA_ACQ(); 00325 void T_OUT(); //timeout function to stop infinite loop 00326 00327 void CONFIG_UPLOAD(); 00328 Timeout to; //Timeout variable to 00329 int toFlag; 00330 00331 int count =0; // Time for which the BAE uC is running (in seconds) 00332 void T_OUT() 00333 { 00334 toFlag=0; //as T_OUT function gets called the while loop gets terminated 00335 } 00336 00337 00338 //DEFINING VARIABLES 00339 char cmd[2]; 00340 char raw_gyro[6]; 00341 char raw_mag[6]; 00342 char store,status; 00343 int16_t bit_data; 00344 float gyro_data[3], mag_data[3],combined_values[6]; 00345 float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps 00346 float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla 00347 float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0}; 00348 00349 void CONFIG_UPLOAD() 00350 { 00351 cmd[0]=RESETREQ; 00352 cmd[1]=BIT_RESREQ; 00353 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up 00354 wait_ms(600); 00355 00356 //Verify magic number 00357 00358 cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload 00359 cmd[1]=BIT_HOST_UPLD_ENB; 00360 i2c.write(SLAVE_ADDR,cmd,2); 00361 wait_ms(100); 00362 00363 cmd[0]=UPLOAD_ADDR; //0x02 is written in HOST CONTROL register to enable upload 00364 cmd[1]=0x0000; 00365 i2c.write(SLAVE_ADDR,cmd,3); 00366 wait_ms(100); 00367 00368 00369 00370 00371 cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload 00372 cmd[1]=0x00; 00373 i2c.write(SLAVE_ADDR,cmd,2); 00374 wait_ms(100); 00375 00376 00377 00378 } 00379 00380 int SENSOR_INIT() 00381 { 00382 00383 pc_acs.printf("Entered sensor init\n \r"); 00384 cmd[0]=RESETREQ; 00385 cmd[1]=BIT_RESREQ; 00386 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up 00387 wait_ms(800); //waiting for loading configuration file stored in EEPROM 00388 cmd[0]=SENTRALSTATUS; 00389 i2c.write(SLAVE_ADDR,cmd,1); 00390 i2c.read(SLAVE_ADDR_READ,&store,1); 00391 wait_ms(100); 00392 pc_acs.printf("Sentral Status is %x\n \r",(int)store); 00393 00394 //to check whether EEPROM is uploaded 00395 switch((int)store) { 00396 case(3): { 00397 cmd[0]=RESETREQ; 00398 cmd[1]=BIT_RESREQ; 00399 i2c.write(SLAVE_ADDR,cmd,2); 00400 wait_ms(600); 00401 break; 00402 } 00403 case(11): { 00404 break; 00405 } 00406 default: { 00407 cmd[0]=RESETREQ; 00408 cmd[1]=BIT_RESREQ; 00409 i2c.write(SLAVE_ADDR,cmd,2); 00410 wait_ms(600); 00411 00412 } 00413 } 00414 cmd[0]=SENTRALSTATUS; 00415 i2c.write(SLAVE_ADDR,cmd,1); 00416 i2c.read(SLAVE_ADDR_READ,&store,1); 00417 wait_ms(100); 00418 pc_acs.printf("Sentral Status is %x\n \r",(int)store); 00419 00420 int manual=0; 00421 if((int)store != 11) 00422 { 00423 00424 cmd[0]=RESETREQ; 00425 cmd[1]=BIT_RESREQ; 00426 i2c.write(SLAVE_ADDR,cmd,2); 00427 wait_ms(600); 00428 00429 //manually upload 00430 00431 if(manual == 0) 00432 { 00433 return 0; 00434 } 00435 00436 } 00437 00438 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors 00439 cmd[1]=BIT_RUN_ENB; 00440 i2c.write(SLAVE_ADDR,cmd,2); 00441 wait_ms(100); 00442 00443 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer 00444 cmd[1]=BIT_MAGODR; 00445 i2c.write(SLAVE_ADDR,cmd,2); 00446 wait_ms(100); 00447 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope 00448 cmd[1]=BIT_GYROODR; 00449 i2c.write(SLAVE_ADDR,cmd,2); 00450 wait_ms(100); 00451 cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer 00452 cmd[1]=0x00; 00453 wait_ms(100); 00454 00455 i2c.write(SLAVE_ADDR,cmd,2); 00456 wait_ms(100); 00457 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values 00458 cmd[1]=0x00; 00459 i2c.write(SLAVE_ADDR,cmd,2); 00460 wait_ms(100); 00461 cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values 00462 cmd[1]=BIT_EVT_ENB; 00463 i2c.write(SLAVE_ADDR,cmd,2); 00464 wait_ms(100); 00465 pc_acs.printf("Exited sensor init successfully\n \r"); 00466 return 1; 00467 00468 00469 } 00470 int FCTN_ACS_INIT() 00471 { 00472 ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag 00473 if( (ATS1_SW_ENABLE!= 0 )&&(ATS2_SW_ENABLE!= 0 ) ) 00474 { 00475 ATS2_SW_ENABLE = 1; 00476 ATS1_SW_ENABLE = 0; 00477 wait_ms(5); 00478 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 00479 } 00480 00481 int working=0; 00482 00483 pc_acs.printf("Attitude sensor init called \n \r"); 00484 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 00485 pc_acs.printf("ATS Status & 0xC0 is %x\n\n \r",(int)(ACS_ATS_STATUS&0xC0)); 00486 pc_acs.printf("Sensor 1 condition is %d \n\n \r",(int)(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80))); 00487 00488 00489 if(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) 00490 { 00491 00492 pc_acs.printf("Sensor 1 marked working \n \r"); 00493 working = SENSOR_INIT(); 00494 if(working ==1) 00495 { 00496 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 00497 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 00498 pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r"); 00499 ACS_INIT_STATUS = 0; 00500 return 1; 00501 } 00502 00503 00504 00505 pc_acs.printf("Sensor 1 not working.Powering off.\n \r"); 00506 ATS1_SW_ENABLE = 1; 00507 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0; 00508 00509 } 00510 00511 pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r"); 00512 00513 if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) 00514 { 00515 00516 00517 ATS2_SW_ENABLE = 0; 00518 wait_ms(5); 00519 working = SENSOR_INIT(); 00520 if(working ==1) 00521 { 00522 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 00523 pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); 00524 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 00525 ACS_INIT_STATUS = 0; 00526 return 2; 00527 } 00528 00529 00530 } 00531 00532 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 00533 pc_acs.printf("Sensor 2 also not working.Exit init.\n \r"); 00534 00535 00536 00537 ATS2_SW_ENABLE = 1; 00538 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E; 00539 ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag 00540 return 0; 00541 } 00542 00543 00544 int SENSOR_DATA_ACQ() 00545 { 00546 int mag_only=0; 00547 pc_acs.printf("Entering Sensor data acq.\n \r"); 00548 char reg; 00549 00550 //int sentral; 00551 int event; 00552 int sensor; 00553 int error; 00554 00555 int init; 00556 cmd[0]=EVT_STATUS; 00557 i2c.write(SLAVE_ADDR,cmd,1); 00558 i2c.read(SLAVE_ADDR_READ,&status,1); 00559 wait_ms(100); 00560 pc_acs.printf("Event Status is %x\n \r",(int)status); 00561 event = (int)status; 00562 00563 cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x 00564 i2c.write(SLAVE_ADDR,cmd,1); 00565 i2c.read(SLAVE_ADDR_READ,raw_gyro,6); 00566 cmd[0]=MAG_XOUT_H; //LSB of x 00567 i2c.write(SLAVE_ADDR,cmd,1); 00568 i2c.read(SLAVE_ADDR_READ,raw_mag,6); 00569 // pc_acs.printf("\nGyro Values:\n"); 00570 for(int i=0; i<3; i++) { 00571 //concatenating gyro LSB and MSB to get 16 bit signed data values 00572 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; 00573 gyro_data[i]=(float)bit_data; 00574 gyro_data[i]=gyro_data[i]/senstivity_gyro; 00575 gyro_data[i]+=gyro_error[i]; 00576 // pc_acs.printf("%f\t",gyro_data[i]); 00577 } 00578 // pc_acs.printf("\nMag Values:\n"); 00579 for(int i=0; i<3; i++) { 00580 //concatenating mag LSB and MSB to get 16 bit signed data values 00581 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i]; 00582 mag_data[i]=(float)bit_data; 00583 mag_data[i]=mag_data[i]/senstivity_mag; 00584 mag_data[i]+=mag_error[i]; 00585 //pc_acs.printf("%f\t",mag_data[i]); 00586 } 00587 for(int i=0; i<3; i++) { 00588 // data[i]=gyro_data[i]; 00589 actual_data.AngularSpeed_actual[i] = gyro_data[i]; 00590 actual_data.Bvalue_actual[i] = mag_data[i]; 00591 //data[i+3]=mag_data[i]; 00592 } 00593 00594 00595 00596 //(event & 40 != 40 ) || (event & 08 != 08 ) || (event & 01 == 01 )|| (event & 02 == 02 ) 00597 00598 if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )) //check for any error 00599 { 00600 cmd[0]=RESETREQ; 00601 cmd[1]=BIT_RESREQ; 00602 i2c.write(SLAVE_ADDR,cmd,2); 00603 wait_ms(600); 00604 00605 00606 00607 cmd[0]=EVT_STATUS; 00608 i2c.write(SLAVE_ADDR,cmd,1); 00609 i2c.read(SLAVE_ADDR_READ,&status,1); 00610 wait_ms(100); 00611 pc_acs.printf("Event Status after resetting is %x\n \r",(int)status); 00612 00613 if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )) 00614 { 00615 00616 00617 00618 // cmd[0]=SENTRALSTATUS; 00619 // i2c.write(SLAVE_ADDR,cmd,1); 00620 /// i2c.read(SLAVE_ADDR_READ,®,1); 00621 // wait_ms(100); 00622 // sentral = (int)reg; 00623 00624 cmd[0]=SENSORSTATUS; 00625 i2c.write(SLAVE_ADDR,cmd,1); 00626 i2c.read(SLAVE_ADDR_READ,®,1); 00627 wait_ms(100); 00628 00629 sensor = (int)reg; 00630 00631 cmd[0]=ERROR; 00632 i2c.write(SLAVE_ADDR,cmd,1); 00633 i2c.read(SLAVE_ADDR_READ,®,1); 00634 wait_ms(100); 00635 00636 error = (int)reg; 00637 00638 if( error&128 == 128) 00639 { 00640 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer 00641 cmd[1]=BIT_MAGODR; 00642 i2c.write(SLAVE_ADDR,cmd,2); 00643 wait_ms(100); 00644 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope 00645 cmd[1]=BIT_GYROODR; 00646 i2c.write(SLAVE_ADDR,cmd,2); 00647 wait_ms(100); 00648 cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer 00649 cmd[1]=0x00; 00650 wait_ms(100); 00651 cmd[0]=ERROR; 00652 i2c.write(SLAVE_ADDR,cmd,1); 00653 i2c.read(SLAVE_ADDR_READ,®,1); 00654 wait_ms(100); 00655 00656 error = (int)reg; 00657 00658 if( error&128 == 128) 00659 { 00660 pc_acs.printf("Rate error.Exiting.\n \r"); 00661 return 1; 00662 } 00663 00664 00665 } 00666 00667 00668 if((error&16 == 16) || (error&32 == 32) || (sensor!=0)) 00669 { 00670 if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) ) 00671 { 00672 00673 if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) ) 00674 { 00675 pc_acs.printf("error in both sensors.Exiting.\n \r"); 00676 return 1; 00677 } 00678 pc_acs.printf("error in gyro alone.Exiting.\n \r"); 00679 return 2; 00680 } 00681 pc_acs.printf("error in something else.Exiting.\n \r"); 00682 return 1; 00683 } 00684 00685 if(((int)status & 1 == 1 )) 00686 { 00687 pc_acs.printf("error in CPU Reset.calling init.\n \r"); 00688 init = SENSOR_INIT(); 00689 if(init == 0) 00690 return 1; 00691 00692 cmd[0]=EVT_STATUS; 00693 i2c.write(SLAVE_ADDR,cmd,1); 00694 i2c.read(SLAVE_ADDR_READ,&status,1); 00695 wait_ms(100); 00696 if(((int)status & 1 == 1 )) 00697 { 00698 pc_acs.printf("Still error in CPU Reset.Exiting.\n \r"); 00699 return 1; 00700 } 00701 pc_acs.printf("error in CPU Reset cleared.\n \r"); 00702 00703 } 00704 00705 if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 ))) 00706 { 00707 pc_acs.printf("Data not ready waiting...\n \r"); 00708 //POLL 00709 wait_ms(1500); 00710 cmd[0]=EVT_STATUS; 00711 i2c.write(SLAVE_ADDR,cmd,1); 00712 i2c.read(SLAVE_ADDR_READ,&status,1); 00713 wait_ms(100); 00714 if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 ))) 00715 { 00716 00717 if((int)status & 32 != 32 ) 00718 { 00719 if((int)status & 8 != 8 ) 00720 { 00721 pc_acs.printf("Both data still not ready.Exiting..\n \r"); 00722 return 1; 00723 } 00724 pc_acs.printf("Mag data only ready.Read..\n \r"); 00725 mag_only = 1; 00726 //return 2; 00727 00728 } 00729 00730 00731 } 00732 00733 00734 } 00735 00736 00737 } 00738 00739 00740 00741 00742 00743 } 00744 00745 cmd[0]=EVT_STATUS; 00746 i2c.write(SLAVE_ADDR,cmd,1); 00747 i2c.read(SLAVE_ADDR_READ,&status,1); 00748 wait_ms(100); 00749 pc_acs.printf("Event Status is %x\n \r",(int)status); 00750 00751 //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take 00752 00753 cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x 00754 i2c.write(SLAVE_ADDR,cmd,1); 00755 i2c.read(SLAVE_ADDR_READ,raw_gyro,6); 00756 cmd[0]=MAG_XOUT_H; //LSB of x 00757 i2c.write(SLAVE_ADDR,cmd,1); 00758 i2c.read(SLAVE_ADDR_READ,raw_mag,6); 00759 // pc_acs.printf("\nGyro Values:\n"); 00760 for(int i=0; i<3; i++) { 00761 //concatenating gyro LSB and MSB to get 16 bit signed data values 00762 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; 00763 gyro_data[i]=(float)bit_data; 00764 gyro_data[i]=gyro_data[i]/senstivity_gyro; 00765 gyro_data[i]+=gyro_error[i]; 00766 // pc_acs.printf("%f\t",gyro_data[i]); 00767 } 00768 // pc_acs.printf("\nMag Values:\n"); 00769 for(int i=0; i<3; i++) { 00770 //concatenating mag LSB and MSB to get 16 bit signed data values 00771 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i]; 00772 mag_data[i]=(float)bit_data; 00773 mag_data[i]=mag_data[i]/senstivity_mag; 00774 mag_data[i]+=mag_error[i]; 00775 //pc_acs.printf("%f\t",mag_data[i]); 00776 } 00777 for(int i=0; i<3; i++) { 00778 // data[i]=gyro_data[i]; 00779 actual_data.AngularSpeed_actual[i] = gyro_data[i]; 00780 actual_data.Bvalue_actual[i] = mag_data[i]; 00781 //data[i+3]=mag_data[i]; 00782 } 00783 00784 if(mag_only == 0) 00785 { 00786 00787 pc_acs.printf("Reading data successful.\n \r"); 00788 return 0; 00789 } 00790 else if(mag_only == 1) 00791 { 00792 pc_acs.printf("Reading data partial success.\n \r"); 00793 return 2; 00794 } 00795 00796 pc_acs.printf("Reading data success.\n \r"); 00797 return 0; 00798 00799 } 00800 00801 00802 00803 int FCTN_ATS_DATA_ACQ() 00804 { 00805 00806 int acq; 00807 00808 pc_acs.printf("DATA_ACQ called \n \r"); 00809 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 00810 00811 // 0 success //1 full failure //2 partial failure 00812 00813 00814 00815 if(( (ACS_ATS_STATUS & 0xC0) == 0x40)) 00816 { 00817 00818 acq = SENSOR_DATA_ACQ(); 00819 if(acq == 0) 00820 { 00821 00822 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 00823 00824 ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 00825 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 00826 pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r"); 00827 return 0; 00828 } 00829 else if(acq == 2) 00830 { 00831 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40; 00832 pc_acs.printf(" Sensor 1 data partial success.Exiting.\n \r"); 00833 return 2; 00834 00835 /*if((ACS_ATS_STATUS & 0x0F == 0x00)) 00836 { 00837 pc_acs.printf(" Sensor 1 data acq partial success.Trying Sensor 2\n \r"); 00838 ATS1_SW_ENABLE = 1; 00839 ATS2_SW_ENABLE = 0; 00840 wait_ms(5); 00841 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20; 00842 00843 int acq; 00844 acq = SENSOR_DATA_ACQ(); 00845 00846 if(acq == 0) 00847 { 00848 ACS_DATA_ACQ_STATUS = 0; 00849 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); 00850 return 0; 00851 00852 } 00853 else if(acq == 2) 00854 { 00855 ACS_DATA_ACQ_STATUS = 2; 00856 pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r"); 00857 return 2; 00858 } 00859 00860 else if(acq == 1) 00861 { 00862 00863 int acq; 00864 pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r"); 00865 ATS2_SW_ENABLE = 1; 00866 ATS1_SW_ENABLE = 0; 00867 wait_ms(5); 00868 acq = SENSOR_DATA_ACQ(); 00869 if(acq == 0) 00870 { 00871 pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); 00872 ACS_DATA_ACQ_STATUS = 0; 00873 return 0; 00874 } 00875 else if(acq == 2) 00876 { 00877 pc_acs.printf(" Sensor 1 data acq partial success.Exiting.\n \r"); 00878 ACS_DATA_ACQ_STATUS = 2; 00879 return 2; 00880 } 00881 else 00882 { 00883 pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r"); 00884 ATS1_SW_ENABLE = 0; 00885 ACS_DATA_ACQ_STATUS = 1; 00886 return 1; 00887 } 00888 00889 pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r"); 00890 ATS1_SW_ENABLE = 0; 00891 ACS_DATA_ACQ_STATUS = 1; 00892 return 1; 00893 00894 } 00895 00896 } 00897 00898 else 00899 { 00900 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40; 00901 pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r"); 00902 return 2; 00903 00904 00905 }*/ 00906 00907 } 00908 00909 else if(acq == 1) 00910 { 00911 pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); 00912 ATS1_SW_ENABLE = 1; 00913 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0; 00914 } 00915 00916 00917 00918 } 00919 00920 00921 00922 00923 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0; 00924 00925 00926 if(( (ACS_ATS_STATUS & 0x0C) == 0x04)) 00927 { 00928 ATS2_SW_ENABLE = 0; 00929 wait_ms(5); 00930 00931 acq = SENSOR_DATA_ACQ(); 00932 if(acq == 0) 00933 { 00934 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); 00935 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 00936 ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 00937 return 0; 00938 } 00939 else if(acq == 2) 00940 { 00941 00942 pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r"); 00943 00944 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x04; 00945 ACS_DATA_ACQ_STATUS = 2; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 00946 return 2; 00947 00948 } 00949 00950 else if(acq == 1) 00951 { 00952 pc_acs.printf(" Sensor 2 data acq failure.Exiting.\n \r"); 00953 ATS2_SW_ENABLE = 1; 00954 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E; 00955 //Sensor 2 also not working 00956 } 00957 00958 00959 00960 00961 00962 } 00963 00964 pc_acs.printf(" Reading value from sensor 1 before exiting\n \r"); 00965 ATS1_SW_ENABLE = 0; 00966 wait_ms(5); 00967 SENSOR_DATA_ACQ(); 00968 ATS1_SW_ENABLE = 1; 00969 wait_ms(5); 00970 00971 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E; 00972 00973 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 00974 pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r"); 00975 00976 ACS_DATA_ACQ_STATUS = 1; //set ACS_DATA_ACQ_STATUS flag for att sens 2 00977 return 1; 00978 } 00979 00980 void FCTN_ACS_GENPWM_MAIN(float Moment[3]) 00981 { 00982 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function 00983 00984 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction 00985 float l_current_x=0; //Current sent in x TR's 00986 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction 00987 float l_current_y=0; //Current sent in y TR's 00988 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction 00989 float l_current_z=0; //Current sent in z TR's 00990 00991 00992 for(int i = 0 ; i<3;i++) 00993 { 00994 // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs 00995 } 00996 00997 //----------------------------- x-direction TR --------------------------------------------// 00998 00999 01000 float l_moment_x = Moment[0]; //Moment in x direction 01001 01002 phase_TR_x = 1; // setting the default current direction 01003 if (l_moment_x <0) 01004 { 01005 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 01006 l_moment_x = abs(l_moment_x); 01007 } 01008 01009 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship 01010 printf("current in trx is %f \r \n",l_current_x); 01011 if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% 01012 { 01013 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 01014 printf("DC for trx is %f \r \n",l_duty_cycle_x); 01015 PWM1.period(TIME_PERIOD); 01016 PWM1 = l_duty_cycle_x/100 ; 01017 } 01018 else if (l_current_x >= 0.0016 && l_current_x < 0.0171) 01019 { 01020 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 01021 printf("DC for trx is %f \r \n",l_duty_cycle_x); 01022 PWM1.period(TIME_PERIOD); 01023 PWM1 = l_duty_cycle_x/100 ; 01024 } 01025 else if(l_current_x >= 0.0171 && l_current_x < 0.1678) 01026 { 01027 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 01028 printf("DC for trx is %f \r \n",l_duty_cycle_x); 01029 PWM1.period(TIME_PERIOD); 01030 PWM1 = l_duty_cycle_x/100 ; 01031 } 01032 else if(l_current_x==0) 01033 { 01034 printf("\n \r l_current_x====0"); 01035 l_duty_cycle_x = 0; // default value of duty cycle 01036 printf("DC for trx is %f \r \n",l_duty_cycle_x); 01037 PWM1.period(TIME_PERIOD); 01038 PWM1 = l_duty_cycle_x/100 ; 01039 } 01040 else //not necessary 01041 { 01042 g_err_flag_TR_x = 1; 01043 } 01044 01045 //------------------------------------- y-direction TR--------------------------------------// 01046 01047 01048 float l_moment_y = Moment[1]; //Moment in y direction 01049 01050 phase_TR_y = 1; // setting the default current direction 01051 if (l_moment_y <0) 01052 { 01053 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 01054 l_moment_y = abs(l_moment_y); 01055 } 01056 01057 01058 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship 01059 printf("current in try is %f \r \n",l_current_y); 01060 if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% 01061 { 01062 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 01063 printf("DC for try is %f \r \n",l_duty_cycle_y); 01064 PWM2.period(TIME_PERIOD); 01065 PWM2 = l_duty_cycle_y/100 ; 01066 } 01067 else if (l_current_y >= 0.0016 && l_current_y < 0.0171) 01068 { 01069 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 01070 printf("DC for try is %f \r \n",l_duty_cycle_y); 01071 PWM2.period(TIME_PERIOD); 01072 PWM2 = l_duty_cycle_y/100 ; 01073 } 01074 else if(l_current_y >= 0.0171 && l_current_y < 0.1678) 01075 { 01076 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 01077 printf("DC for try is %f \r \n",l_duty_cycle_y); 01078 PWM2.period(TIME_PERIOD); 01079 PWM2 = l_duty_cycle_y/100 ; 01080 } 01081 else if(l_current_y==0) 01082 { 01083 printf("\n \r l_current_y====0"); 01084 l_duty_cycle_y = 0; // default value of duty cycle 01085 printf("DC for try is %f \r \n",l_duty_cycle_y); 01086 PWM2.period(TIME_PERIOD); 01087 PWM2 = l_duty_cycle_y/100 ; 01088 } 01089 else // not necessary 01090 { 01091 g_err_flag_TR_y = 1; 01092 } 01093 01094 //----------------------------------------------- z-direction TR -------------------------// 01095 01096 01097 float l_moment_z = Moment[2]; //Moment in z direction 01098 01099 phase_TR_z = 1; // setting the default current direction 01100 if (l_moment_z <0) 01101 { 01102 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 01103 l_moment_z = abs(l_moment_z); 01104 } 01105 01106 01107 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship 01108 printf("current in trz is %f \r \n",l_current_z); 01109 if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% 01110 { 01111 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 01112 printf("DC for trz is %f \r \n",l_duty_cycle_z); 01113 PWM3.period(TIME_PERIOD); 01114 PWM3 = l_duty_cycle_z/100 ; 01115 } 01116 else if (l_current_z >= 0.0016 && l_current_z < 0.0171) 01117 { 01118 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 01119 printf("DC for trz is %f \r \n",l_duty_cycle_z); 01120 PWM3.period(TIME_PERIOD); 01121 PWM3 = l_duty_cycle_z/100 ; 01122 } 01123 else if(l_current_z >= 0.0171 && l_current_z < 0.1678) 01124 { 01125 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 01126 printf("DC for trz is %f \r \n",l_duty_cycle_z); 01127 PWM3.period(TIME_PERIOD); 01128 PWM3 = l_duty_cycle_z/100 ; 01129 } 01130 else if(l_current_z==0) 01131 { 01132 printf("\n \r l_current_z====0"); 01133 l_duty_cycle_z = 0; // default value of duty cycle 01134 printf("DC for trz is %f \r \n",l_duty_cycle_z); 01135 PWM3.period(TIME_PERIOD); 01136 PWM3 = l_duty_cycle_z/100 ; 01137 } 01138 else // not necessary 01139 { 01140 g_err_flag_TR_z = 1; 01141 } 01142 01143 //-----------------------------------------exiting the function-----------------------------------// 01144 01145 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function 01146 01147 } 01148 01149 01150 /*void FCTN_ACS_GENPWM_MAIN(float Moment[3]) 01151 { 01152 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function 01153 01154 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction 01155 float l_current_x=0; //Current sent in x TR's 01156 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction 01157 float l_current_y=0; //Current sent in y TR's 01158 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction 01159 float l_current_z=0; //Current sent in z TR's 01160 01161 01162 for(int i = 0 ; i<3;i++) 01163 { 01164 printf("pwm %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs 01165 } 01166 01167 //----------------------------- x-direction TR --------------------------------------------// 01168 01169 01170 float l_moment_x = Moment[0]; //Moment in x direction 01171 01172 phase_TR_x = 1; // setting the default current direction 01173 if (l_moment_x <0) 01174 { 01175 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 01176 l_moment_x = abs(l_moment_x); 01177 } 01178 01179 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship 01180 pc_acs.printf("current in trx is %f \r \n",l_current_x); 01181 if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100% 01182 { 01183 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 01184 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); 01185 PWM1.period(TIME_PERIOD); 01186 PWM1 = l_duty_cycle_x/100 ; 01187 } 01188 else if( l_current_x >= 0.006 && l_current_x < 0.0116) 01189 { 01190 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 01191 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); 01192 PWM1.period(TIME_PERIOD); 01193 PWM1 = l_duty_cycle_x/100 ; 01194 } 01195 else if (l_current_x >= 0.0116 && l_current_x < 0.0624) 01196 { 01197 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 01198 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); 01199 PWM1.period(TIME_PERIOD); 01200 PWM1 = l_duty_cycle_x/100 ; 01201 } 01202 else if(l_current_x >= 0.0624 && l_current_x < 0.555) 01203 { 01204 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 01205 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); 01206 PWM1.period(TIME_PERIOD); 01207 PWM1 = l_duty_cycle_x/100 ; 01208 } 01209 else if(l_current_x==0) 01210 { 01211 printf("\n \r l_current_x====0"); 01212 l_duty_cycle_x = 0; // default value of duty cycle 01213 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); 01214 PWM1.period(TIME_PERIOD); 01215 PWM1 = l_duty_cycle_x/100 ; 01216 } 01217 else //not necessary 01218 { 01219 g_err_flag_TR_x = 1; 01220 } 01221 01222 //------------------------------------- y-direction TR--------------------------------------// 01223 01224 01225 float l_moment_y = Moment[1]; //Moment in y direction 01226 01227 phase_TR_y = 1; // setting the default current direction 01228 if (l_moment_y <0) 01229 { 01230 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 01231 l_moment_y = abs(l_moment_y); 01232 } 01233 01234 01235 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship 01236 pc_acs.printf("current in try is %f \r \n",l_current_y); 01237 if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100% 01238 { 01239 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 01240 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); 01241 PWM2.period(TIME_PERIOD); 01242 PWM2 = l_duty_cycle_y/100 ; 01243 } 01244 else if( l_current_y >= 0.006 && l_current_y < 0.0116) 01245 { 01246 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 01247 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); 01248 PWM2.period(TIME_PERIOD); 01249 PWM2 = l_duty_cycle_y/100 ; 01250 } 01251 else if (l_current_y >= 0.0116&& l_current_y < 0.0624) 01252 { 01253 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 01254 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); 01255 PWM2.period(TIME_PERIOD); 01256 PWM2 = l_duty_cycle_y/100 ; 01257 } 01258 else if(l_current_y >= 0.0624 && l_current_y < 0.555) 01259 { 01260 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 01261 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); 01262 PWM2.period(TIME_PERIOD); 01263 PWM2 = l_duty_cycle_y/100 ; 01264 } 01265 else if(l_current_y==0) 01266 { 01267 printf("\n \r l_current_y====0"); 01268 l_duty_cycle_y = 0; // default value of duty cycle 01269 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); 01270 PWM2.period(TIME_PERIOD); 01271 PWM2 = l_duty_cycle_y/100 ; 01272 } 01273 else // not necessary 01274 { 01275 g_err_flag_TR_y = 1; 01276 } 01277 01278 //----------------------------------------------- z-direction TR -------------------------// 01279 01280 01281 float l_moment_z = Moment[2]; //Moment in z direction 01282 01283 phase_TR_z = 1; // setting the default current direction 01284 if (l_moment_z <0) 01285 { 01286 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 01287 l_moment_z = abs(l_moment_z); 01288 } 01289 01290 01291 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship 01292 pc_acs.printf("current in trz is %f \r \n",l_current_z); 01293 if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100% 01294 { 01295 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 01296 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); 01297 PWM3.period(TIME_PERIOD); 01298 PWM3 = l_duty_cycle_z/100 ; 01299 } 01300 else if( l_current_z >= 0.006 && l_current_z < 0.0116) 01301 { 01302 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 01303 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); 01304 PWM3.period(TIME_PERIOD); 01305 PWM3 = l_duty_cycle_z/100 ; 01306 } 01307 else if (l_current_z >= 0.0116 && l_current_z < 0.0624) 01308 { 01309 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 01310 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); 01311 PWM3.period(TIME_PERIOD); 01312 PWM3 = l_duty_cycle_z/100 ; 01313 } 01314 else if(l_current_z >= 0.0624 && l_current_z < 0.555) 01315 { 01316 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 01317 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); 01318 PWM3.period(TIME_PERIOD); 01319 PWM3 = l_duty_cycle_z/100 ; 01320 } 01321 else if(l_current_z==0) 01322 { 01323 printf("\n \r l_current_z====0"); 01324 l_duty_cycle_z = 0; // default value of duty cycle 01325 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); 01326 PWM3.period(TIME_PERIOD); 01327 PWM3 = l_duty_cycle_z/100 ; 01328 } 01329 else // not necessary 01330 { 01331 g_err_flag_TR_z = 1; 01332 } 01333 01334 //-----------------------------------------exiting the function-----------------------------------// 01335 01336 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function 01337 01338 }*/ 01339 01340 01341
Generated on Tue Jul 12 2022 17:24:01 by
1.7.2