Seeker of Truth , / Mbed 2 deprecated TF_conops_BAEFLAGIMAN

QITH FLAGS

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of TF_conops_BAE1_3 by Team Fox

ACS.cpp@1:7185136654ce, 2015-07-06 (annotated)

Committer:
sakthipriya
Date:
Mon Jul 06 07:34:00 2015 +0000
Revision:
1:7185136654ce
Parent:
0:246d1b5f11ae
Child:
2:3c6c33509772
varun - to modify acs part of this code

Who changed what in which revision?

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