Team Fox / Mbed 2 deprecated QM_BAE_review_1_EPS_updated

Updated EPS code with flowchart v2.3 (CDMS and HW faults)

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of QM_BAE_review_1_EPS_faults by Mohamed Azad

ACS.cpp@17:bb0d64656ba1, 2016-06-23 (annotated)

Committer:
azaddevarm
Date:
Thu Jun 23 12:14:28 2016 +0000
Revision:
17:bb0d64656ba1
Parent:
15:e09aaaccf134 
Updated EPS code with flowchart v2.3

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;
Bragadeesh153 13:fb7facaf308b 14 extern uint8_t ACS_INIT_STATUS;
Bragadeesh153 13:fb7facaf308b 15 extern uint8_t ACS_DATA_ACQ_STATUS;
Bragadeesh153 13:fb7facaf308b 16 extern uint8_t ACS_ATS_STATUS;
Bragadeesh153 13:fb7facaf308b 17 extern uint8_t ACS_MAIN_STATUS;
Bragadeesh153 13:fb7facaf308b 18 extern uint8_t ACS_STATUS;
sakthipriya 0:7b4c00e3912f 19
Bragadeesh153 13:fb7facaf308b 20 extern uint8_t ACS_ATS_ENABLE;
Bragadeesh153 13:fb7facaf308b 21 extern uint8_t ACS_DATA_ACQ_ENABLE;
Bragadeesh153 13:fb7facaf308b 22 extern uint8_t 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
lakshya 10:f93407b97750 40 //DigitalOut gpo1(PTC0); // enable of att sens2 switch
lakshya 10:f93407b97750 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
lakshya 10:f93407b97750 45 //CONTROL_ALGO
lakshya 10:f93407b97750 46
lakshya 10:f93407b97750 47 float moment[3]; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 48 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla
lakshya 10:f93407b97750 49 int flag_firsttime=1, controlmode, alarmmode=0;
lakshya 10:f93407b97750 50
lakshya 10:f93407b97750 51
lakshya 10:f93407b97750 52
lakshya 10:f93407b97750 53 void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode);
lakshya 10:f93407b97750 54 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax);
lakshya 10:f93407b97750 55 float max_array(float arr[3]);
sakthipriya 0:7b4c00e3912f 56 void inverse(float mat[3][3],float inv[3][3]);
lakshya 10:f93407b97750 57
lakshya 10:f93407b97750 58 //CONTROLALGO PARAMETERS
lakshya 10:f93407b97750 59
lakshya 10:f93407b97750 60
sakthipriya 0:7b4c00e3912f 61 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
sakthipriya 0:7b4c00e3912f 62 {
lakshya 10:f93407b97750 63 float b1[3];
lakshya 10:f93407b97750 64 float omega1[3];
lakshya 10:f93407b97750 65 b1[0] = b[0]/1000000.0;
lakshya 10:f93407b97750 66 b1[1] = b[1]/1000000.0;
lakshya 10:f93407b97750 67 b1[2] = b[2]/1000000.0;
lakshya 10:f93407b97750 68
lakshya 10:f93407b97750 69 omega1[0] = omega[0]*3.14159/180;
lakshya 10:f93407b97750 70 omega1[1] = omega[1]*3.14159/180;
lakshya 10:f93407b97750 71 omega1[2] = omega[2]*3.14159/180;
lakshya 10:f93407b97750 72 controller (moment, b1, omega1, b_old, alarmmode, flag_firsttime, controlmode);
sakthipriya 0:7b4c00e3912f 73
lakshya 10:f93407b97750 74 }
lakshya 10:f93407b97750 75 void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode)
lakshya 10:f93407b97750 76 {
lakshya 10:f93407b97750 77 float db1[3]; // Unit: Tesla/Second
lakshya 10:f93407b97750 78 float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds
lakshya 10:f93407b97750 79 float MmntMax=1.1; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 80 float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second
lakshya 10:f93407b97750 81 float normalising_fact;
lakshya 10:f93407b97750 82 float b1_copy[3], omega1_copy[3], db1_copy[3];
lakshya 10:f93407b97750 83 int i;
lakshya 10:f93407b97750 84 if(flag_firsttime==1)
lakshya 10:f93407b97750 85 {
lakshya 10:f93407b97750 86 for(i=0;i<3;i++)
lakshya 10:f93407b97750 87 {
lakshya 10:f93407b97750 88 db1[i]=0; // Unit: Tesla/Second
lakshya 10:f93407b97750 89 }
lakshya 10:f93407b97750 90 flag_firsttime=0;
lakshya 10:f93407b97750 91 }
sakthipriya 0:7b4c00e3912f 92 else
sakthipriya 0:7b4c00e3912f 93 {
sakthipriya 0:7b4c00e3912f 94 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 95 {
lakshya 10:f93407b97750 96 db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
sakthipriya 0:7b4c00e3912f 97 }
lakshya 10:f93407b97750 98 }
lakshya 10:f93407b97750 99
lakshya 10:f93407b97750 100 if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
sakthipriya 0:7b4c00e3912f 101 {
lakshya 10:f93407b97750 102 alarmmode=0;
sakthipriya 0:7b4c00e3912f 103 }
lakshya 10:f93407b97750 104 else if(max_array(omega1)>OmegaMax && alarmmode==0)
sakthipriya 0:7b4c00e3912f 105 {
lakshya 10:f93407b97750 106 alarmmode=1;
sakthipriya 0:7b4c00e3912f 107 }
lakshya 10:f93407b97750 108
lakshya 10:f93407b97750 109 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 110 {
lakshya 10:f93407b97750 111 b1_copy[i]=b1[i];
lakshya 10:f93407b97750 112 db1_copy[i]=db1[i];
lakshya 10:f93407b97750 113 omega1_copy[i]=omega1[i];
sakthipriya 0:7b4c00e3912f 114 }
lakshya 10:f93407b97750 115
lakshya 10:f93407b97750 116 if(alarmmode==0)
lakshya 10:f93407b97750 117 {
lakshya 10:f93407b97750 118 controlmode=0;
lakshya 10:f93407b97750 119 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
lakshya 10:f93407b97750 120 for (i=0;i<3;i++)
lakshya 10:f93407b97750 121 {
lakshya 10:f93407b97750 122 b1[i]=b1_copy[i];
lakshya 10:f93407b97750 123 db1[i]=db1_copy[i];
lakshya 10:f93407b97750 124 omega1[i]=omega1_copy[i];
lakshya 10:f93407b97750 125 }
lakshya 10:f93407b97750 126 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 127 {
lakshya 10:f93407b97750 128 controlmode=1;
lakshya 10:f93407b97750 129 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
lakshya 10:f93407b97750 130 for (i=0;i<3;i++)
lakshya 10:f93407b97750 131 {
lakshya 10:f93407b97750 132 b1[i]=b1_copy[i];
lakshya 10:f93407b97750 133 db1[i]=db1_copy[i];
lakshya 10:f93407b97750 134 omega1[i]=omega1_copy[i];
lakshya 10:f93407b97750 135 }
lakshya 10:f93407b97750 136 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 137 {
lakshya 10:f93407b97750 138 normalising_fact=max_array(moment)/MmntMax;
lakshya 10:f93407b97750 139 for(i=0;i<3;i++)
lakshya 10:f93407b97750 140 {
lakshya 10:f93407b97750 141 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 142 }
lakshya 10:f93407b97750 143 }
lakshya 10:f93407b97750 144 }
lakshya 10:f93407b97750 145 }
lakshya 10:f93407b97750 146 else
lakshya 10:f93407b97750 147 {
lakshya 10:f93407b97750 148 controlmode=1;
lakshya 10:f93407b97750 149 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
lakshya 10:f93407b97750 150 for (i=0;i<3;i++)
lakshya 10:f93407b97750 151 {
lakshya 10:f93407b97750 152 b1[i]=b1_copy[i];
lakshya 10:f93407b97750 153 db1[i]=db1_copy[i];
lakshya 10:f93407b97750 154 omega1[i]=omega1_copy[i];
lakshya 10:f93407b97750 155 }
lakshya 10:f93407b97750 156 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 157 {
lakshya 10:f93407b97750 158 normalising_fact=max_array(moment)/MmntMax;
lakshya 10:f93407b97750 159 for(i=0;i<3;i++)
lakshya 10:f93407b97750 160 {
lakshya 10:f93407b97750 161 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 162 }
lakshya 10:f93407b97750 163 }
lakshya 10:f93407b97750 164
lakshya 10:f93407b97750 165 }
lakshya 10:f93407b97750 166 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 167 {
lakshya 10:f93407b97750 168 b_old[i]=b1[i];
sakthipriya 0:7b4c00e3912f 169 }
sakthipriya 0:7b4c00e3912f 170 }
lakshya 10:f93407b97750 171
sakthipriya 0:7b4c00e3912f 172 void inverse(float mat[3][3],float inv[3][3])
sakthipriya 0:7b4c00e3912f 173 {
sakthipriya 0:7b4c00e3912f 174 int i,j;
sakthipriya 0:7b4c00e3912f 175 float det=0;
sakthipriya 0:7b4c00e3912f 176 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 177 {
sakthipriya 0:7b4c00e3912f 178 for(j=0;j<3;j++)
lakshya 10:f93407b97750 179 {
sakthipriya 0:7b4c00e3912f 180 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]);
lakshya 10:f93407b97750 181 }
sakthipriya 0:7b4c00e3912f 182 }
sakthipriya 0:7b4c00e3912f 183 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
sakthipriya 0:7b4c00e3912f 184 for(i=0;i<3;i++)
lakshya 10:f93407b97750 185 {
sakthipriya 0:7b4c00e3912f 186 for(j=0;j<3;j++)
lakshya 10:f93407b97750 187 {
sakthipriya 0:7b4c00e3912f 188 inv[i][j]/=det;
lakshya 10:f93407b97750 189 }
sakthipriya 0:7b4c00e3912f 190 }
sakthipriya 0:7b4c00e3912f 191 }
sakthipriya 0:7b4c00e3912f 192
lakshya 10:f93407b97750 193 float max_array(float arr[3])
lakshya 10:f93407b97750 194 {
lakshya 10:f93407b97750 195 int i;
lakshya 10:f93407b97750 196 float temp_max=fabs(arr[0]);
lakshya 10:f93407b97750 197 for(i=1;i<3;i++)
lakshya 10:f93407b97750 198 {
lakshya 10:f93407b97750 199 if(fabs(arr[i])>temp_max)
lakshya 10:f93407b97750 200 {
lakshya 10:f93407b97750 201 temp_max=fabs(arr[i]);
lakshya 10:f93407b97750 202 }
lakshya 10:f93407b97750 203 }
lakshya 10:f93407b97750 204 return temp_max;
lakshya 10:f93407b97750 205 }
lakshya 10:f93407b97750 206
lakshya 10:f93407b97750 207
lakshya 10:f93407b97750 208 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax)
lakshya 10:f93407b97750 209 {
lakshya 10:f93407b97750 210 float bb[3]={0,0,0};
lakshya 10:f93407b97750 211 float d[3]={0,0,0};
lakshya 10:f93407b97750 212 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
lakshya 10:f93407b97750 213 float den=0,den2;
lakshya 10:f93407b97750 214 float bcopy[3];
lakshya 10:f93407b97750 215 int i, j;//temporary variables
lakshya 10:f93407b97750 216 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
lakshya 10:f93407b97750 217 float invJm[3][3];
lakshya 10:f93407b97750 218 float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4,kdetumble=2000000;
lakshya 10:f93407b97750 219 int infflag; // Flag variable to check if the moment value is infinity or NaN
lakshya 10:f93407b97750 220
lakshya 10:f93407b97750 221 if(controlmode1==0)
lakshya 10:f93407b97750 222 {
lakshya 10:f93407b97750 223 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
lakshya 10:f93407b97750 224 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
lakshya 10:f93407b97750 225 for(i=0;i<3;i++)
lakshya 10:f93407b97750 226 {
lakshya 10:f93407b97750 227 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
lakshya 10:f93407b97750 228 }
lakshya 10:f93407b97750 229 for(i=0;i<3;i++)
lakshya 10:f93407b97750 230 {
lakshya 10:f93407b97750 231 b[i]/=den; // Mormalized b. Hence no unit.
lakshya 10:f93407b97750 232 }
lakshya 10:f93407b97750 233 if(b[2]>0.9 || b[2]<-0.9)
lakshya 10:f93407b97750 234 {
lakshya 10:f93407b97750 235 kz=kz2;
lakshya 10:f93407b97750 236 kmu=kmu2;
lakshya 10:f93407b97750 237 gamma=gamma2;
lakshya 10:f93407b97750 238 }
lakshya 10:f93407b97750 239 for(i=0;i<2;i++)
lakshya 10:f93407b97750 240 {
lakshya 10:f93407b97750 241 Mu[i]=b[i];
lakshya 10:f93407b97750 242 v[i]=-kmu*Mu[i];
lakshya 10:f93407b97750 243 dv[i]=-kmu*db[i];
lakshya 10:f93407b97750 244 z[i]=db[i]-v[i];
lakshya 10:f93407b97750 245 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
lakshya 10:f93407b97750 246 }
lakshya 10:f93407b97750 247 inverse(Jm,invJm);
lakshya 10:f93407b97750 248 for(i=0;i<3;i++)
lakshya 10:f93407b97750 249 {
lakshya 10:f93407b97750 250 for(j=0;j<3;j++)
lakshya 10:f93407b97750 251 {
lakshya 10:f93407b97750 252 bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
lakshya 10:f93407b97750 253 }
lakshya 10:f93407b97750 254 }
lakshya 10:f93407b97750 255 for(i=0;i<3;i++)
lakshya 10:f93407b97750 256 {
lakshya 10:f93407b97750 257 for(j=0;j<3;j++)
lakshya 10:f93407b97750 258 {
lakshya 10:f93407b97750 259 d[i]+=bb[j]*invJm[i][j];
lakshya 10:f93407b97750 260 }
lakshya 10:f93407b97750 261 }
lakshya 10:f93407b97750 262 bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]);
lakshya 10:f93407b97750 263 bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]);
lakshya 10:f93407b97750 264 bb[0]=0;
lakshya 10:f93407b97750 265 for(i=0;i<3;i++)
lakshya 10:f93407b97750 266 {
lakshya 10:f93407b97750 267 d[i]=invJm[2][i];
lakshya 10:f93407b97750 268 invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
lakshya 10:f93407b97750 269 invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
lakshya 10:f93407b97750 270 invJm[0][i]=b[i];
lakshya 10:f93407b97750 271 }
lakshya 10:f93407b97750 272 inverse(invJm,Jm);
lakshya 10:f93407b97750 273 for(i=0;i<3;i++)
lakshya 10:f93407b97750 274 {
lakshya 10:f93407b97750 275 for(j=0;j<3;j++)
lakshya 10:f93407b97750 276 {
lakshya 10:f93407b97750 277 tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
lakshya 10:f93407b97750 278 }
lakshya 10:f93407b97750 279 }
lakshya 10:f93407b97750 280 for(i=0;i<3;i++)
lakshya 10:f93407b97750 281 {
lakshya 10:f93407b97750 282 bcopy[i]=b[i]*den;
lakshya 10:f93407b97750 283 }
lakshya 10:f93407b97750 284 for(i=0;i<3;i++)
lakshya 10:f93407b97750 285 {
lakshya 10:f93407b97750 286 Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
lakshya 10:f93407b97750 287 Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 288 }
lakshya 10:f93407b97750 289 infflag=0;
lakshya 10:f93407b97750 290 for (i=0; i<3 && infflag==0; i++)
lakshya 10:f93407b97750 291 {
lakshya 10:f93407b97750 292 if (isinf(Mmnt[i])==1 || isnan(Mmnt[i])==1)
lakshya 10:f93407b97750 293 infflag=1;
lakshya 10:f93407b97750 294 }
lakshya 10:f93407b97750 295 if (infflag==1)
lakshya 10:f93407b97750 296 {
lakshya 10:f93407b97750 297 for (i=0; i<3; i++)
lakshya 10:f93407b97750 298 Mmnt[i]=2*MmntMax;
lakshya 10:f93407b97750 299 }
lakshya 10:f93407b97750 300
lakshya 10:f93407b97750 301 }
lakshya 10:f93407b97750 302 else if(controlmode1==1)
lakshya 10:f93407b97750 303 {
lakshya 10:f93407b97750 304 for(i=0;i<3;i++)
lakshya 10:f93407b97750 305 {
lakshya 10:f93407b97750 306 Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 307 }
lakshya 10:f93407b97750 308 }
lakshya 10:f93407b97750 309 for(i=0;i<3;i++)
lakshya 10:f93407b97750 310 {
lakshya 10:f93407b97750 311 moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 312 }
lakshya 10:f93407b97750 313 }
sakthipriya 0:7b4c00e3912f 314
sakthipriya 0:7b4c00e3912f 315 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
sakthipriya 0:7b4c00e3912f 316
sakthipriya 0:7b4c00e3912f 317 void FCTN_ACS_INIT(void); //initialization of registers happens
sakthipriya 0:7b4c00e3912f 318 void FCTN_ATS_DATA_ACQ(); //data is obtained
sakthipriya 0:7b4c00e3912f 319 void T_OUT(); //timeout function to stop infinite loop
sakthipriya 0:7b4c00e3912f 320 Timeout to; //Timeout variable to
sakthipriya 0:7b4c00e3912f 321 int toFlag;
sakthipriya 0:7b4c00e3912f 322
sakthipriya 0:7b4c00e3912f 323 int count =0; // Time for which the BAE uC is running (in seconds)
sakthipriya 0:7b4c00e3912f 324 void T_OUT()
sakthipriya 0:7b4c00e3912f 325 {
sakthipriya 0:7b4c00e3912f 326 toFlag=0; //as T_OUT function gets called the while loop gets terminated
sakthipriya 0:7b4c00e3912f 327 }
sakthipriya 0:7b4c00e3912f 328
sakthipriya 0:7b4c00e3912f 329
sakthipriya 0:7b4c00e3912f 330 //DEFINING VARIABLES
sakthipriya 0:7b4c00e3912f 331 char cmd[2];
sakthipriya 0:7b4c00e3912f 332 char raw_gyro[6];
sakthipriya 0:7b4c00e3912f 333 char raw_mag[6];
sakthipriya 0:7b4c00e3912f 334 char store,status;
sakthipriya 0:7b4c00e3912f 335 int16_t bit_data;
sakthipriya 0:7b4c00e3912f 336 float gyro_data[3], mag_data[3],combined_values[6];
sakthipriya 0:7b4c00e3912f 337 float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
sakthipriya 0:7b4c00e3912f 338 float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
sakthipriya 0:7b4c00e3912f 339 float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
sakthipriya 0:7b4c00e3912f 340
sakthipriya 0:7b4c00e3912f 341 void FCTN_ACS_INIT()
sakthipriya 0:7b4c00e3912f 342 {
lakshya 10:f93407b97750 343 ACS_INIT_STATUS = 's'; //set ACS_INIT_STATUS flag
sakthipriya 3:07e15677a75c 344 //FLAG();
sakthipriya 0:7b4c00e3912f 345 pc_acs.printf("Attitude sensor init called \n \r");
sakthipriya 0:7b4c00e3912f 346 //FLAG();
sakthipriya 0:7b4c00e3912f 347 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 348 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 349 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
sakthipriya 0:7b4c00e3912f 350 wait_ms(2000); //waiting for loading configuration file stored in EEPROM
sakthipriya 0:7b4c00e3912f 351 cmd[0]=SENTRALSTATUS;
sakthipriya 0:7b4c00e3912f 352 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 353 i2c.read(SLAVE_ADDR_READ,&store,1);
sakthipriya 0:7b4c00e3912f 354 wait_ms(100);
sakthipriya 0:7b4c00e3912f 355 //to check whether EEPROM is uploaded
sakthipriya 0:7b4c00e3912f 356 switch((int)store) {
sakthipriya 0:7b4c00e3912f 357 case(3): {
sakthipriya 0:7b4c00e3912f 358 break;
sakthipriya 0:7b4c00e3912f 359 }
sakthipriya 0:7b4c00e3912f 360 case(11): {
sakthipriya 0:7b4c00e3912f 361 break;
sakthipriya 0:7b4c00e3912f 362 }
sakthipriya 0:7b4c00e3912f 363 default: {
sakthipriya 0:7b4c00e3912f 364 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 365 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 366 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 367 wait_ms(2000);
sakthipriya 0:7b4c00e3912f 368 }
sakthipriya 0:7b4c00e3912f 369 }
sakthipriya 0:7b4c00e3912f 370 pc_acs.printf("Sentral Status is %x\n \r",(int)store);
sakthipriya 0:7b4c00e3912f 371 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
sakthipriya 0:7b4c00e3912f 372 cmd[1]=BIT_RUN_ENB;
sakthipriya 0:7b4c00e3912f 373 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 374 wait_ms(100);
sakthipriya 0:7b4c00e3912f 375 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
sakthipriya 0:7b4c00e3912f 376 cmd[1]=BIT_MAGODR;
sakthipriya 0:7b4c00e3912f 377 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 378 wait_ms(100);
sakthipriya 0:7b4c00e3912f 379 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
sakthipriya 0:7b4c00e3912f 380 cmd[1]=BIT_GYROODR;
sakthipriya 0:7b4c00e3912f 381 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 382 wait_ms(100);
sakthipriya 0:7b4c00e3912f 383 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
sakthipriya 0:7b4c00e3912f 384 cmd[1]=0x00;
sakthipriya 0:7b4c00e3912f 385 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 386 wait_ms(100);
sakthipriya 0:7b4c00e3912f 387 cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
sakthipriya 0:7b4c00e3912f 388 cmd[1]=BIT_EVT_ENB;
sakthipriya 0:7b4c00e3912f 389 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 390 wait_ms(100);
lakshya 10:f93407b97750 391 ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
sakthipriya 0:7b4c00e3912f 392 }
sakthipriya 0:7b4c00e3912f 393
sakthipriya 0:7b4c00e3912f 394 void FCTN_ATS_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 395 {
Bragadeesh153 13:fb7facaf308b 396 ACS_DATA_ACQ_STATUS = 1; //set ACS_DATA_ACQ_STATUS flag for att sens 2
Bragadeesh153 13:fb7facaf308b 397 if( ACS_ATS_ENABLE == 1)
sakthipriya 0:7b4c00e3912f 398 {
sakthipriya 0:7b4c00e3912f 399 FLAG();
sakthipriya 0:7b4c00e3912f 400 pc_acs.printf("attitude sensor execution called \n \r");
sakthipriya 0:7b4c00e3912f 401 toFlag=1; //toFlag is set to 1 so that it enters while loop
sakthipriya 0:7b4c00e3912f 402 to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated
sakthipriya 0:7b4c00e3912f 403 while(toFlag) {
sakthipriya 0:7b4c00e3912f 404 cmd[0]=EVT_STATUS;
sakthipriya 0:7b4c00e3912f 405 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 406 i2c.read(SLAVE_ADDR_READ,&status,1);
sakthipriya 0:7b4c00e3912f 407 wait_ms(100);
sakthipriya 0:7b4c00e3912f 408 pc_acs.printf("Event Status is %x\n \r",(int)status);
sakthipriya 0:7b4c00e3912f 409 //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
sakthipriya 0:7b4c00e3912f 410 if(((int)status&40)==40) {
sakthipriya 0:7b4c00e3912f 411 cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
sakthipriya 0:7b4c00e3912f 412 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 413 i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
sakthipriya 0:7b4c00e3912f 414 cmd[0]=MAG_XOUT_H; //LSB of x
sakthipriya 0:7b4c00e3912f 415 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 416 i2c.read(SLAVE_ADDR_READ,raw_mag,6);
sakthipriya 0:7b4c00e3912f 417 // pc_acs.printf("\nGyro Values:\n");
sakthipriya 0:7b4c00e3912f 418 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 419 //concatenating gyro LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 420 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
sakthipriya 0:7b4c00e3912f 421 gyro_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 422 gyro_data[i]=gyro_data[i]/senstivity_gyro;
sakthipriya 0:7b4c00e3912f 423 gyro_data[i]+=gyro_error[i];
lakshya 15:e09aaaccf134 424 // pc_acs.printf("%f\t",gyro_data[i]);
sakthipriya 0:7b4c00e3912f 425 }
sakthipriya 0:7b4c00e3912f 426 // pc_acs.printf("\nMag Values:\n");
sakthipriya 0:7b4c00e3912f 427 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 428 //concatenating mag LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 429 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
sakthipriya 0:7b4c00e3912f 430 mag_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 431 mag_data[i]=mag_data[i]/senstivity_mag;
sakthipriya 0:7b4c00e3912f 432 mag_data[i]+=mag_error[i];
lakshya 15:e09aaaccf134 433 //pc_acs.printf("%f\t",mag_data[i]);
sakthipriya 0:7b4c00e3912f 434 }
sakthipriya 0:7b4c00e3912f 435 for(int i=0; i<3; i++) {
sakthipriya 6:036d08b62785 436 // data[i]=gyro_data[i];
sakthipriya 6:036d08b62785 437 actual_data.AngularSpeed_actual[i] = gyro_data[i];
sakthipriya 6:036d08b62785 438 actual_data.Bvalue_actual[i] = mag_data[i];
sakthipriya 6:036d08b62785 439 //data[i+3]=mag_data[i];
sakthipriya 0:7b4c00e3912f 440 }
sakthipriya 0:7b4c00e3912f 441 // return(combined_values); //returning poiter combined values
sakthipriya 0:7b4c00e3912f 442 }
sakthipriya 0:7b4c00e3912f 443 //checking for the error
sakthipriya 0:7b4c00e3912f 444 else if (((int)status&2)==2) {
sakthipriya 0:7b4c00e3912f 445 FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
sakthipriya 0:7b4c00e3912f 446 }
sakthipriya 0:7b4c00e3912f 447 }
sakthipriya 0:7b4c00e3912f 448 }
sakthipriya 0:7b4c00e3912f 449 else //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
sakthipriya 0:7b4c00e3912f 450 {
lakshya 10:f93407b97750 451 ACS_DATA_ACQ_STATUS = 'f';
sakthipriya 0:7b4c00e3912f 452 }
Bragadeesh153 13:fb7facaf308b 453 ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
sakthipriya 0:7b4c00e3912f 454 }
sakthipriya 0:7b4c00e3912f 455
sakthipriya 0:7b4c00e3912f 456 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 457 {
sakthipriya 0:7b4c00e3912f 458 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 459
sakthipriya 0:7b4c00e3912f 460 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 461 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 462 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 463 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 464 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 465 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 466
sakthipriya 0:7b4c00e3912f 467
sakthipriya 0:7b4c00e3912f 468 for(int i = 0 ; i<3;i++)
sakthipriya 0:7b4c00e3912f 469 {
sakthipriya 0:7b4c00e3912f 470 // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
sakthipriya 0:7b4c00e3912f 471 }
sakthipriya 0:7b4c00e3912f 472
sakthipriya 0:7b4c00e3912f 473 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 474
sakthipriya 0:7b4c00e3912f 475
sakthipriya 0:7b4c00e3912f 476 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 477
sakthipriya 0:7b4c00e3912f 478 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 479 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 480 {
sakthipriya 0:7b4c00e3912f 481 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 482 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 483 }
sakthipriya 0:7b4c00e3912f 484
sakthipriya 0:7b4c00e3912f 485 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 10:f93407b97750 486 printf("current in trx is %f \r \n",l_current_x);
lakshya 10:f93407b97750 487 if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
lakshya 10:f93407b97750 488 {
lakshya 10:f93407b97750 489 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
lakshya 10:f93407b97750 490 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 491 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 492 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 493 }
lakshya 10:f93407b97750 494 else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
lakshya 10:f93407b97750 495 {
lakshya 10:f93407b97750 496 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
lakshya 10:f93407b97750 497 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 498 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 499 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 500 }
lakshya 10:f93407b97750 501 else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
lakshya 10:f93407b97750 502 {
lakshya 10:f93407b97750 503 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
lakshya 10:f93407b97750 504 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 505 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 506 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 507 }
lakshya 10:f93407b97750 508 else if(l_current_x==0)
lakshya 10:f93407b97750 509 {
lakshya 10:f93407b97750 510 printf("\n \r l_current_x====0");
lakshya 10:f93407b97750 511 l_duty_cycle_x = 0; // default value of duty cycle
lakshya 10:f93407b97750 512 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 513 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 514 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 515 }
lakshya 10:f93407b97750 516 else //not necessary
lakshya 10:f93407b97750 517 {
lakshya 10:f93407b97750 518 g_err_flag_TR_x = 1;
lakshya 10:f93407b97750 519 }
lakshya 10:f93407b97750 520
lakshya 10:f93407b97750 521 //------------------------------------- y-direction TR--------------------------------------//
lakshya 10:f93407b97750 522
lakshya 10:f93407b97750 523
lakshya 10:f93407b97750 524 float l_moment_y = Moment[1]; //Moment in y direction
lakshya 10:f93407b97750 525
lakshya 10:f93407b97750 526 phase_TR_y = 1; // setting the default current direction
lakshya 10:f93407b97750 527 if (l_moment_y <0)
lakshya 10:f93407b97750 528 {
lakshya 10:f93407b97750 529 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
lakshya 10:f93407b97750 530 l_moment_y = abs(l_moment_y);
lakshya 10:f93407b97750 531 }
lakshya 10:f93407b97750 532
lakshya 10:f93407b97750 533
lakshya 10:f93407b97750 534 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 10:f93407b97750 535 printf("current in try is %f \r \n",l_current_y);
lakshya 10:f93407b97750 536 if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
lakshya 10:f93407b97750 537 {
lakshya 10:f93407b97750 538 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
lakshya 10:f93407b97750 539 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 540 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 541 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 542 }
lakshya 10:f93407b97750 543 else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
lakshya 10:f93407b97750 544 {
lakshya 10:f93407b97750 545 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
lakshya 10:f93407b97750 546 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 547 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 548 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 549 }
lakshya 10:f93407b97750 550 else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
lakshya 10:f93407b97750 551 {
lakshya 10:f93407b97750 552 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
lakshya 10:f93407b97750 553 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 554 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 555 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 556 }
lakshya 10:f93407b97750 557 else if(l_current_y==0)
lakshya 10:f93407b97750 558 {
lakshya 10:f93407b97750 559 printf("\n \r l_current_y====0");
lakshya 10:f93407b97750 560 l_duty_cycle_y = 0; // default value of duty cycle
lakshya 10:f93407b97750 561 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 562 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 563 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 564 }
lakshya 10:f93407b97750 565 else // not necessary
lakshya 10:f93407b97750 566 {
lakshya 10:f93407b97750 567 g_err_flag_TR_y = 1;
lakshya 10:f93407b97750 568 }
lakshya 10:f93407b97750 569
lakshya 10:f93407b97750 570 //----------------------------------------------- z-direction TR -------------------------//
lakshya 10:f93407b97750 571
lakshya 10:f93407b97750 572
lakshya 10:f93407b97750 573 float l_moment_z = Moment[2]; //Moment in z direction
lakshya 10:f93407b97750 574
lakshya 10:f93407b97750 575 phase_TR_z = 1; // setting the default current direction
lakshya 10:f93407b97750 576 if (l_moment_z <0)
lakshya 10:f93407b97750 577 {
lakshya 10:f93407b97750 578 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
lakshya 10:f93407b97750 579 l_moment_z = abs(l_moment_z);
lakshya 10:f93407b97750 580 }
lakshya 10:f93407b97750 581
lakshya 10:f93407b97750 582
lakshya 10:f93407b97750 583 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 10:f93407b97750 584 printf("current in trz is %f \r \n",l_current_z);
lakshya 10:f93407b97750 585 if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
lakshya 10:f93407b97750 586 {
lakshya 10:f93407b97750 587 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
lakshya 10:f93407b97750 588 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 589 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 590 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 591 }
lakshya 10:f93407b97750 592 else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
lakshya 10:f93407b97750 593 {
lakshya 10:f93407b97750 594 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
lakshya 10:f93407b97750 595 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 596 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 597 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 598 }
lakshya 10:f93407b97750 599 else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
lakshya 10:f93407b97750 600 {
lakshya 10:f93407b97750 601 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
lakshya 10:f93407b97750 602 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 603 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 604 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 605 }
lakshya 10:f93407b97750 606 else if(l_current_z==0)
lakshya 10:f93407b97750 607 {
lakshya 10:f93407b97750 608 printf("\n \r l_current_z====0");
lakshya 10:f93407b97750 609 l_duty_cycle_z = 0; // default value of duty cycle
lakshya 10:f93407b97750 610 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 611 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 612 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 613 }
lakshya 10:f93407b97750 614 else // not necessary
lakshya 10:f93407b97750 615 {
lakshya 10:f93407b97750 616 g_err_flag_TR_z = 1;
lakshya 10:f93407b97750 617 }
lakshya 10:f93407b97750 618
lakshya 10:f93407b97750 619 //-----------------------------------------exiting the function-----------------------------------//
lakshya 10:f93407b97750 620
lakshya 10:f93407b97750 621 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
lakshya 10:f93407b97750 622
lakshya 10:f93407b97750 623 }
lakshya 10:f93407b97750 624
lakshya 10:f93407b97750 625
lakshya 10:f93407b97750 626 /*void FCTN_ACS_GENPWM_MAIN(float Moment[3])
lakshya 10:f93407b97750 627 {
lakshya 10:f93407b97750 628 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
lakshya 10:f93407b97750 629
lakshya 10:f93407b97750 630 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
lakshya 10:f93407b97750 631 float l_current_x=0; //Current sent in x TR's
lakshya 10:f93407b97750 632 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
lakshya 10:f93407b97750 633 float l_current_y=0; //Current sent in y TR's
lakshya 10:f93407b97750 634 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
lakshya 10:f93407b97750 635 float l_current_z=0; //Current sent in z TR's
lakshya 10:f93407b97750 636
lakshya 10:f93407b97750 637
lakshya 10:f93407b97750 638 for(int i = 0 ; i<3;i++)
lakshya 10:f93407b97750 639 {
lakshya 10:f93407b97750 640 printf("pwm %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
lakshya 10:f93407b97750 641 }
lakshya 10:f93407b97750 642
lakshya 10:f93407b97750 643 //----------------------------- x-direction TR --------------------------------------------//
lakshya 10:f93407b97750 644
lakshya 10:f93407b97750 645
lakshya 10:f93407b97750 646 float l_moment_x = Moment[0]; //Moment in x direction
lakshya 10:f93407b97750 647
lakshya 10:f93407b97750 648 phase_TR_x = 1; // setting the default current direction
lakshya 10:f93407b97750 649 if (l_moment_x <0)
lakshya 10:f93407b97750 650 {
lakshya 10:f93407b97750 651 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
lakshya 10:f93407b97750 652 l_moment_x = abs(l_moment_x);
lakshya 10:f93407b97750 653 }
lakshya 10:f93407b97750 654
lakshya 10:f93407b97750 655 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 656 pc_acs.printf("current in trx is %f \r \n",l_current_x);
sakthipriya 0:7b4c00e3912f 657 if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 658 {
sakthipriya 0:7b4c00e3912f 659 l_duty_cycle_x = 6*1000000*pow(l_current_x,4) - 377291*pow(l_current_x,3) + 4689.6*pow(l_current_x,2) + 149.19*l_current_x - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 660 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 661 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 662 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 663 }
sakthipriya 0:7b4c00e3912f 664 else if( l_current_x >= 0.006 && l_current_x < 0.0116)
sakthipriya 0:7b4c00e3912f 665 {
sakthipriya 0:7b4c00e3912f 666 l_duty_cycle_x = 1*100000000*pow(l_current_x,4) - 5*1000000*pow(l_current_x,3) + 62603*pow(l_current_x,2) - 199.29*l_current_x + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 667 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 668 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 669 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 670 }
sakthipriya 0:7b4c00e3912f 671 else if (l_current_x >= 0.0116 && l_current_x < 0.0624)
sakthipriya 0:7b4c00e3912f 672 {
sakthipriya 0:7b4c00e3912f 673 l_duty_cycle_x = 212444*pow(l_current_x,4) - 33244*pow(l_current_x,3) + 1778.4*pow(l_current_x,2) + 120.91*l_current_x + 0.3878; // calculating upto 10% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 674 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 675 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 676 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 677 }
sakthipriya 0:7b4c00e3912f 678 else if(l_current_x >= 0.0624 && l_current_x < 0.555)
sakthipriya 0:7b4c00e3912f 679 {
sakthipriya 0:7b4c00e3912f 680 l_duty_cycle_x = 331.15*pow(l_current_x,4) - 368.09*pow(l_current_x,3) + 140.43*pow(l_current_x,2) + 158.59*l_current_x + 0.0338; // calculating upto 100% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 681 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 682 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 683 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 684 }
sakthipriya 0:7b4c00e3912f 685 else if(l_current_x==0)
sakthipriya 0:7b4c00e3912f 686 {
sakthipriya 0:7b4c00e3912f 687 printf("\n \r l_current_x====0");
sakthipriya 0:7b4c00e3912f 688 l_duty_cycle_x = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 689 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 690 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 691 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 692 }
sakthipriya 0:7b4c00e3912f 693 else //not necessary
sakthipriya 0:7b4c00e3912f 694 {
sakthipriya 0:7b4c00e3912f 695 g_err_flag_TR_x = 1;
sakthipriya 0:7b4c00e3912f 696 }
sakthipriya 0:7b4c00e3912f 697
sakthipriya 0:7b4c00e3912f 698 //------------------------------------- y-direction TR--------------------------------------//
sakthipriya 0:7b4c00e3912f 699
sakthipriya 0:7b4c00e3912f 700
sakthipriya 0:7b4c00e3912f 701 float l_moment_y = Moment[1]; //Moment in y direction
sakthipriya 0:7b4c00e3912f 702
sakthipriya 0:7b4c00e3912f 703 phase_TR_y = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 704 if (l_moment_y <0)
sakthipriya 0:7b4c00e3912f 705 {
sakthipriya 0:7b4c00e3912f 706 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 707 l_moment_y = abs(l_moment_y);
sakthipriya 0:7b4c00e3912f 708 }
sakthipriya 0:7b4c00e3912f 709
sakthipriya 0:7b4c00e3912f 710
sakthipriya 0:7b4c00e3912f 711 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 712 pc_acs.printf("current in try is %f \r \n",l_current_y);
sakthipriya 0:7b4c00e3912f 713 if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 714 {
sakthipriya 0:7b4c00e3912f 715 l_duty_cycle_y = 6*1000000*pow(l_current_y,4) - 377291*pow(l_current_y,3) + 4689.6*pow(l_current_y,2) + 149.19*l_current_y - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 716 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 717 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 718 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 719 }
sakthipriya 0:7b4c00e3912f 720 else if( l_current_y >= 0.006 && l_current_y < 0.0116)
sakthipriya 0:7b4c00e3912f 721 {
sakthipriya 0:7b4c00e3912f 722 l_duty_cycle_y = 1*100000000*pow(l_current_y,4) - 5*1000000*pow(l_current_y,3) + 62603*pow(l_current_y,2) - 199.29*l_current_y + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 723 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 724 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 725 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 726 }
sakthipriya 0:7b4c00e3912f 727 else if (l_current_y >= 0.0116&& l_current_y < 0.0624)
sakthipriya 0:7b4c00e3912f 728 {
sakthipriya 0:7b4c00e3912f 729 l_duty_cycle_y = 212444*pow(l_current_y,4) - 33244*pow(l_current_y,3) + 1778.4*pow(l_current_y,2) + 120.91*l_current_y + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 730 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 731 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 732 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 733 }
sakthipriya 0:7b4c00e3912f 734 else if(l_current_y >= 0.0624 && l_current_y < 0.555)
sakthipriya 0:7b4c00e3912f 735 {
sakthipriya 0:7b4c00e3912f 736 l_duty_cycle_y = 331.15*pow(l_current_y,4) - 368.09*pow(l_current_y,3) + 140.43*pow(l_current_y,2) + 158.59*l_current_y + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 737 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 738 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 739 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 740 }
sakthipriya 0:7b4c00e3912f 741 else if(l_current_y==0)
sakthipriya 0:7b4c00e3912f 742 {
sakthipriya 0:7b4c00e3912f 743 printf("\n \r l_current_y====0");
sakthipriya 0:7b4c00e3912f 744 l_duty_cycle_y = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 745 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 746 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 747 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 748 }
sakthipriya 0:7b4c00e3912f 749 else // not necessary
sakthipriya 0:7b4c00e3912f 750 {
sakthipriya 0:7b4c00e3912f 751 g_err_flag_TR_y = 1;
sakthipriya 0:7b4c00e3912f 752 }
sakthipriya 0:7b4c00e3912f 753
sakthipriya 0:7b4c00e3912f 754 //----------------------------------------------- z-direction TR -------------------------//
sakthipriya 0:7b4c00e3912f 755
sakthipriya 0:7b4c00e3912f 756
sakthipriya 0:7b4c00e3912f 757 float l_moment_z = Moment[2]; //Moment in z direction
sakthipriya 0:7b4c00e3912f 758
sakthipriya 0:7b4c00e3912f 759 phase_TR_z = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 760 if (l_moment_z <0)
sakthipriya 0:7b4c00e3912f 761 {
sakthipriya 0:7b4c00e3912f 762 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 763 l_moment_z = abs(l_moment_z);
sakthipriya 0:7b4c00e3912f 764 }
sakthipriya 0:7b4c00e3912f 765
sakthipriya 0:7b4c00e3912f 766
sakthipriya 0:7b4c00e3912f 767 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 768 pc_acs.printf("current in trz is %f \r \n",l_current_z);
sakthipriya 0:7b4c00e3912f 769 if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 770 {
sakthipriya 0:7b4c00e3912f 771 l_duty_cycle_z = 6*1000000*pow(l_current_z,4) - 377291*pow(l_current_z,3) + 4689.6*pow(l_current_z,2) + 149.19*l_current_z - 0.0008;// calculating upto 0.1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 772 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 773 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 774 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 775 }
sakthipriya 0:7b4c00e3912f 776 else if( l_current_z >= 0.006 && l_current_z < 0.0116)
sakthipriya 0:7b4c00e3912f 777 {
sakthipriya 0:7b4c00e3912f 778 l_duty_cycle_z = 1*100000000*pow(l_current_z,4) - 5*1000000*pow(l_current_z,3) + 62603*pow(l_current_z,2) - 199.29*l_current_z + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 779 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 780 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 781 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 782 }
sakthipriya 0:7b4c00e3912f 783 else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
sakthipriya 0:7b4c00e3912f 784 {
sakthipriya 0:7b4c00e3912f 785 l_duty_cycle_z = 212444*pow(l_current_z,4) - 33244*pow(l_current_z,3) + 1778.4*pow(l_current_z,2) + 120.91*l_current_z + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 786 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 787 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 788 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 789 }
sakthipriya 0:7b4c00e3912f 790 else if(l_current_z >= 0.0624 && l_current_z < 0.555)
sakthipriya 0:7b4c00e3912f 791 {
sakthipriya 0:7b4c00e3912f 792 l_duty_cycle_z = 331.15*pow(l_current_z,4) - 368.09*pow(l_current_z,3) + 140.43*pow(l_current_z,2) + 158.59*l_current_z + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 793 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 794 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 795 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 796 }
sakthipriya 0:7b4c00e3912f 797 else if(l_current_z==0)
sakthipriya 0:7b4c00e3912f 798 {
sakthipriya 0:7b4c00e3912f 799 printf("\n \r l_current_z====0");
sakthipriya 0:7b4c00e3912f 800 l_duty_cycle_z = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 801 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 802 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 803 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 804 }
sakthipriya 0:7b4c00e3912f 805 else // not necessary
sakthipriya 0:7b4c00e3912f 806 {
sakthipriya 0:7b4c00e3912f 807 g_err_flag_TR_z = 1;
sakthipriya 0:7b4c00e3912f 808 }
sakthipriya 0:7b4c00e3912f 809
sakthipriya 0:7b4c00e3912f 810 //-----------------------------------------exiting the function-----------------------------------//
sakthipriya 0:7b4c00e3912f 811
sakthipriya 0:7b4c00e3912f 812 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
sakthipriya 0:7b4c00e3912f 813
lakshya 10:f93407b97750 814 }*/
sakthipriya 0:7b4c00e3912f 815
sakthipriya 0:7b4c00e3912f 816
sakthipriya 0:7b4c00e3912f 817