Team Fox / Mbed 2 deprecated BAE_QM_MAR9

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 workinQM_5thJan_azad by Team Fox

ACS.cpp@19:79e69017c855, 2016-06-04 (annotated)

Committer:
lakshya
Date:
Sat Jun 04 11:29:13 2016 +0000
Revision:
19:79e69017c855
Parent:
18:3662058a7c10
Child:
20:949d13045431
before going home

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
lakshya 19:79e69017c855 11 /*timer for determining the execution time*/
lakshya 19:79e69017c855 12 //Timer time;
lakshya 19:79e69017c855 13
sakthipriya 0:7b4c00e3912f 14 //********************************flags******************************************//
sakthipriya 0:7b4c00e3912f 15 extern uint32_t BAE_STATUS;
sakthipriya 0:7b4c00e3912f 16 extern uint32_t BAE_ENABLE;
Bragadeesh153 13:fb7facaf308b 17 extern uint8_t ACS_INIT_STATUS;
Bragadeesh153 13:fb7facaf308b 18 extern uint8_t ACS_DATA_ACQ_STATUS;
Bragadeesh153 13:fb7facaf308b 19 extern uint8_t ACS_ATS_STATUS;
Bragadeesh153 13:fb7facaf308b 20 extern uint8_t ACS_MAIN_STATUS;
Bragadeesh153 13:fb7facaf308b 21 extern uint8_t ACS_STATUS;
sakthipriya 0:7b4c00e3912f 22
Bragadeesh153 13:fb7facaf308b 23 extern uint8_t ACS_ATS_ENABLE;
Bragadeesh153 13:fb7facaf308b 24 extern uint8_t ACS_DATA_ACQ_ENABLE;
Bragadeesh153 13:fb7facaf308b 25 extern uint8_t ACS_STATE;
sakthipriya 0:7b4c00e3912f 26
sakthipriya 0:7b4c00e3912f 27 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
sakthipriya 0:7b4c00e3912f 28 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
sakthipriya 0:7b4c00e3912f 29 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
sakthipriya 0:7b4c00e3912f 30
sakthipriya 0:7b4c00e3912f 31 extern PwmOut PWM1; //x //Functions used to generate PWM signal
sakthipriya 0:7b4c00e3912f 32 extern PwmOut PWM2; //y
sakthipriya 0:7b4c00e3912f 33 extern PwmOut PWM3; //z //PWM output comes from pins p6
sakthipriya 0:7b4c00e3912f 34
sakthipriya 0:7b4c00e3912f 35 int g_err_flag_TR_x=0; // setting x-flag to zero
sakthipriya 0:7b4c00e3912f 36 int g_err_flag_TR_y=0; // setting y-flag to zero
sakthipriya 0:7b4c00e3912f 37 int g_err_flag_TR_z=0; // setting z-flag to zero
sakthipriya 0:7b4c00e3912f 38
sakthipriya 0:7b4c00e3912f 39 extern float data[6];
sakthipriya 6:036d08b62785 40 extern BAE_HK_actual actual_data;
sakthipriya 0:7b4c00e3912f 41
sakthipriya 0:7b4c00e3912f 42
lakshya 10:f93407b97750 43 //DigitalOut gpo1(PTC0); // enable of att sens2 switch
lakshya 10:f93407b97750 44 //DigitalOut gpo2(PTC16); // enable of att sens switch
sakthipriya 0:7b4c00e3912f 45
sakthipriya 0:7b4c00e3912f 46
sakthipriya 0:7b4c00e3912f 47 Serial pc_acs(USBTX,USBRX); //for usb communication
lakshya 10:f93407b97750 48 //CONTROL_ALGO
lakshya 10:f93407b97750 49
lakshya 10:f93407b97750 50 float moment[3]; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 51 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla
lakshya 10:f93407b97750 52 int flag_firsttime=1, controlmode, alarmmode=0;
lakshya 10:f93407b97750 53
lakshya 10:f93407b97750 54
lakshya 10:f93407b97750 55
lakshya 10:f93407b97750 56 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 57 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax);
lakshya 10:f93407b97750 58 float max_array(float arr[3]);
sakthipriya 0:7b4c00e3912f 59 void inverse(float mat[3][3],float inv[3][3]);
lakshya 10:f93407b97750 60
lakshya 10:f93407b97750 61 //CONTROLALGO PARAMETERS
lakshya 10:f93407b97750 62
lakshya 10:f93407b97750 63
sakthipriya 0:7b4c00e3912f 64 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
sakthipriya 0:7b4c00e3912f 65 {
lakshya 10:f93407b97750 66 float b1[3];
lakshya 10:f93407b97750 67 float omega1[3];
lakshya 10:f93407b97750 68 b1[0] = b[0]/1000000.0;
lakshya 10:f93407b97750 69 b1[1] = b[1]/1000000.0;
lakshya 10:f93407b97750 70 b1[2] = b[2]/1000000.0;
lakshya 10:f93407b97750 71
lakshya 10:f93407b97750 72 omega1[0] = omega[0]*3.14159/180;
lakshya 10:f93407b97750 73 omega1[1] = omega[1]*3.14159/180;
lakshya 10:f93407b97750 74 omega1[2] = omega[2]*3.14159/180;
lakshya 10:f93407b97750 75 controller (moment, b1, omega1, b_old, alarmmode, flag_firsttime, controlmode);
sakthipriya 0:7b4c00e3912f 76
lakshya 10:f93407b97750 77 }
lakshya 10:f93407b97750 78 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 79 {
lakshya 10:f93407b97750 80 float db1[3]; // Unit: Tesla/Second
lakshya 10:f93407b97750 81 float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds
lakshya 10:f93407b97750 82 float MmntMax=1.1; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 83 float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second
lakshya 10:f93407b97750 84 float normalising_fact;
lakshya 10:f93407b97750 85 float b1_copy[3], omega1_copy[3], db1_copy[3];
lakshya 10:f93407b97750 86 int i;
lakshya 10:f93407b97750 87 if(flag_firsttime==1)
lakshya 10:f93407b97750 88 {
lakshya 10:f93407b97750 89 for(i=0;i<3;i++)
lakshya 10:f93407b97750 90 {
lakshya 10:f93407b97750 91 db1[i]=0; // Unit: Tesla/Second
lakshya 10:f93407b97750 92 }
lakshya 10:f93407b97750 93 flag_firsttime=0;
lakshya 10:f93407b97750 94 }
sakthipriya 0:7b4c00e3912f 95 else
sakthipriya 0:7b4c00e3912f 96 {
sakthipriya 0:7b4c00e3912f 97 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 98 {
lakshya 10:f93407b97750 99 db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
sakthipriya 0:7b4c00e3912f 100 }
lakshya 10:f93407b97750 101 }
lakshya 10:f93407b97750 102
lakshya 10:f93407b97750 103 if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
sakthipriya 0:7b4c00e3912f 104 {
lakshya 10:f93407b97750 105 alarmmode=0;
sakthipriya 0:7b4c00e3912f 106 }
lakshya 10:f93407b97750 107 else if(max_array(omega1)>OmegaMax && alarmmode==0)
sakthipriya 0:7b4c00e3912f 108 {
lakshya 10:f93407b97750 109 alarmmode=1;
sakthipriya 0:7b4c00e3912f 110 }
lakshya 10:f93407b97750 111
lakshya 10:f93407b97750 112 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 113 {
lakshya 10:f93407b97750 114 b1_copy[i]=b1[i];
lakshya 10:f93407b97750 115 db1_copy[i]=db1[i];
lakshya 10:f93407b97750 116 omega1_copy[i]=omega1[i];
sakthipriya 0:7b4c00e3912f 117 }
lakshya 10:f93407b97750 118
lakshya 10:f93407b97750 119 if(alarmmode==0)
lakshya 10:f93407b97750 120 {
lakshya 10:f93407b97750 121 controlmode=0;
lakshya 10:f93407b97750 122 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
lakshya 10:f93407b97750 123 for (i=0;i<3;i++)
lakshya 10:f93407b97750 124 {
lakshya 10:f93407b97750 125 b1[i]=b1_copy[i];
lakshya 10:f93407b97750 126 db1[i]=db1_copy[i];
lakshya 10:f93407b97750 127 omega1[i]=omega1_copy[i];
lakshya 10:f93407b97750 128 }
lakshya 10:f93407b97750 129 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 130 {
lakshya 10:f93407b97750 131 controlmode=1;
lakshya 10:f93407b97750 132 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
lakshya 10:f93407b97750 133 for (i=0;i<3;i++)
lakshya 10:f93407b97750 134 {
lakshya 10:f93407b97750 135 b1[i]=b1_copy[i];
lakshya 10:f93407b97750 136 db1[i]=db1_copy[i];
lakshya 10:f93407b97750 137 omega1[i]=omega1_copy[i];
lakshya 10:f93407b97750 138 }
lakshya 10:f93407b97750 139 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 140 {
lakshya 10:f93407b97750 141 normalising_fact=max_array(moment)/MmntMax;
lakshya 10:f93407b97750 142 for(i=0;i<3;i++)
lakshya 10:f93407b97750 143 {
lakshya 10:f93407b97750 144 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 145 }
lakshya 10:f93407b97750 146 }
lakshya 10:f93407b97750 147 }
lakshya 10:f93407b97750 148 }
lakshya 10:f93407b97750 149 else
lakshya 10:f93407b97750 150 {
lakshya 10:f93407b97750 151 controlmode=1;
lakshya 10:f93407b97750 152 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
lakshya 10:f93407b97750 153 for (i=0;i<3;i++)
lakshya 10:f93407b97750 154 {
lakshya 10:f93407b97750 155 b1[i]=b1_copy[i];
lakshya 10:f93407b97750 156 db1[i]=db1_copy[i];
lakshya 10:f93407b97750 157 omega1[i]=omega1_copy[i];
lakshya 10:f93407b97750 158 }
lakshya 10:f93407b97750 159 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 160 {
lakshya 10:f93407b97750 161 normalising_fact=max_array(moment)/MmntMax;
lakshya 10:f93407b97750 162 for(i=0;i<3;i++)
lakshya 10:f93407b97750 163 {
lakshya 10:f93407b97750 164 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 165 }
lakshya 10:f93407b97750 166 }
lakshya 10:f93407b97750 167
lakshya 10:f93407b97750 168 }
lakshya 10:f93407b97750 169 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 170 {
lakshya 10:f93407b97750 171 b_old[i]=b1[i];
sakthipriya 0:7b4c00e3912f 172 }
sakthipriya 0:7b4c00e3912f 173 }
lakshya 10:f93407b97750 174
sakthipriya 0:7b4c00e3912f 175 void inverse(float mat[3][3],float inv[3][3])
sakthipriya 0:7b4c00e3912f 176 {
sakthipriya 0:7b4c00e3912f 177 int i,j;
sakthipriya 0:7b4c00e3912f 178 float det=0;
sakthipriya 0:7b4c00e3912f 179 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 180 {
sakthipriya 0:7b4c00e3912f 181 for(j=0;j<3;j++)
lakshya 10:f93407b97750 182 {
sakthipriya 0:7b4c00e3912f 183 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 184 }
sakthipriya 0:7b4c00e3912f 185 }
sakthipriya 0:7b4c00e3912f 186 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
sakthipriya 0:7b4c00e3912f 187 for(i=0;i<3;i++)
lakshya 10:f93407b97750 188 {
sakthipriya 0:7b4c00e3912f 189 for(j=0;j<3;j++)
lakshya 10:f93407b97750 190 {
sakthipriya 0:7b4c00e3912f 191 inv[i][j]/=det;
lakshya 10:f93407b97750 192 }
sakthipriya 0:7b4c00e3912f 193 }
sakthipriya 0:7b4c00e3912f 194 }
sakthipriya 0:7b4c00e3912f 195
lakshya 10:f93407b97750 196 float max_array(float arr[3])
lakshya 10:f93407b97750 197 {
lakshya 10:f93407b97750 198 int i;
lakshya 10:f93407b97750 199 float temp_max=fabs(arr[0]);
lakshya 10:f93407b97750 200 for(i=1;i<3;i++)
lakshya 10:f93407b97750 201 {
lakshya 10:f93407b97750 202 if(fabs(arr[i])>temp_max)
lakshya 10:f93407b97750 203 {
lakshya 10:f93407b97750 204 temp_max=fabs(arr[i]);
lakshya 10:f93407b97750 205 }
lakshya 10:f93407b97750 206 }
lakshya 10:f93407b97750 207 return temp_max;
lakshya 10:f93407b97750 208 }
lakshya 10:f93407b97750 209
lakshya 10:f93407b97750 210
lakshya 10:f93407b97750 211 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax)
lakshya 10:f93407b97750 212 {
lakshya 10:f93407b97750 213 float bb[3]={0,0,0};
lakshya 10:f93407b97750 214 float d[3]={0,0,0};
lakshya 10:f93407b97750 215 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 216 float den=0,den2;
lakshya 10:f93407b97750 217 float bcopy[3];
lakshya 10:f93407b97750 218 int i, j;//temporary variables
lakshya 10:f93407b97750 219 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
lakshya 10:f93407b97750 220 float invJm[3][3];
lakshya 10:f93407b97750 221 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 222 int infflag; // Flag variable to check if the moment value is infinity or NaN
lakshya 10:f93407b97750 223
lakshya 10:f93407b97750 224 if(controlmode1==0)
lakshya 10:f93407b97750 225 {
lakshya 10:f93407b97750 226 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
lakshya 10:f93407b97750 227 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
lakshya 10:f93407b97750 228 for(i=0;i<3;i++)
lakshya 10:f93407b97750 229 {
lakshya 10:f93407b97750 230 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
lakshya 10:f93407b97750 231 }
lakshya 10:f93407b97750 232 for(i=0;i<3;i++)
lakshya 10:f93407b97750 233 {
lakshya 10:f93407b97750 234 b[i]/=den; // Mormalized b. Hence no unit.
lakshya 10:f93407b97750 235 }
lakshya 10:f93407b97750 236 if(b[2]>0.9 || b[2]<-0.9)
lakshya 10:f93407b97750 237 {
lakshya 10:f93407b97750 238 kz=kz2;
lakshya 10:f93407b97750 239 kmu=kmu2;
lakshya 10:f93407b97750 240 gamma=gamma2;
lakshya 10:f93407b97750 241 }
lakshya 10:f93407b97750 242 for(i=0;i<2;i++)
lakshya 10:f93407b97750 243 {
lakshya 10:f93407b97750 244 Mu[i]=b[i];
lakshya 10:f93407b97750 245 v[i]=-kmu*Mu[i];
lakshya 10:f93407b97750 246 dv[i]=-kmu*db[i];
lakshya 10:f93407b97750 247 z[i]=db[i]-v[i];
lakshya 10:f93407b97750 248 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
lakshya 10:f93407b97750 249 }
lakshya 10:f93407b97750 250 inverse(Jm,invJm);
lakshya 10:f93407b97750 251 for(i=0;i<3;i++)
lakshya 10:f93407b97750 252 {
lakshya 10:f93407b97750 253 for(j=0;j<3;j++)
lakshya 10:f93407b97750 254 {
lakshya 10:f93407b97750 255 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 256 }
lakshya 10:f93407b97750 257 }
lakshya 10:f93407b97750 258 for(i=0;i<3;i++)
lakshya 10:f93407b97750 259 {
lakshya 10:f93407b97750 260 for(j=0;j<3;j++)
lakshya 10:f93407b97750 261 {
lakshya 10:f93407b97750 262 d[i]+=bb[j]*invJm[i][j];
lakshya 10:f93407b97750 263 }
lakshya 10:f93407b97750 264 }
lakshya 10:f93407b97750 265 bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]);
lakshya 10:f93407b97750 266 bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]);
lakshya 10:f93407b97750 267 bb[0]=0;
lakshya 10:f93407b97750 268 for(i=0;i<3;i++)
lakshya 10:f93407b97750 269 {
lakshya 10:f93407b97750 270 d[i]=invJm[2][i];
lakshya 10:f93407b97750 271 invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
lakshya 10:f93407b97750 272 invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
lakshya 10:f93407b97750 273 invJm[0][i]=b[i];
lakshya 10:f93407b97750 274 }
lakshya 10:f93407b97750 275 inverse(invJm,Jm);
lakshya 10:f93407b97750 276 for(i=0;i<3;i++)
lakshya 10:f93407b97750 277 {
lakshya 10:f93407b97750 278 for(j=0;j<3;j++)
lakshya 10:f93407b97750 279 {
lakshya 10:f93407b97750 280 tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
lakshya 10:f93407b97750 281 }
lakshya 10:f93407b97750 282 }
lakshya 10:f93407b97750 283 for(i=0;i<3;i++)
lakshya 10:f93407b97750 284 {
lakshya 10:f93407b97750 285 bcopy[i]=b[i]*den;
lakshya 10:f93407b97750 286 }
lakshya 10:f93407b97750 287 for(i=0;i<3;i++)
lakshya 10:f93407b97750 288 {
lakshya 10:f93407b97750 289 Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
lakshya 10:f93407b97750 290 Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 291 }
lakshya 10:f93407b97750 292 infflag=0;
lakshya 10:f93407b97750 293 for (i=0; i<3 && infflag==0; i++)
lakshya 10:f93407b97750 294 {
lakshya 10:f93407b97750 295 if (isinf(Mmnt[i])==1 || isnan(Mmnt[i])==1)
lakshya 10:f93407b97750 296 infflag=1;
lakshya 10:f93407b97750 297 }
lakshya 10:f93407b97750 298 if (infflag==1)
lakshya 10:f93407b97750 299 {
lakshya 10:f93407b97750 300 for (i=0; i<3; i++)
lakshya 10:f93407b97750 301 Mmnt[i]=2*MmntMax;
lakshya 10:f93407b97750 302 }
lakshya 10:f93407b97750 303
lakshya 10:f93407b97750 304 }
lakshya 10:f93407b97750 305 else if(controlmode1==1)
lakshya 10:f93407b97750 306 {
lakshya 10:f93407b97750 307 for(i=0;i<3;i++)
lakshya 10:f93407b97750 308 {
lakshya 10:f93407b97750 309 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 310 }
lakshya 10:f93407b97750 311 }
lakshya 10:f93407b97750 312 for(i=0;i<3;i++)
lakshya 10:f93407b97750 313 {
lakshya 10:f93407b97750 314 moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 315 }
lakshya 10:f93407b97750 316 }
sakthipriya 0:7b4c00e3912f 317
sakthipriya 0:7b4c00e3912f 318 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
sakthipriya 0:7b4c00e3912f 319
sakthipriya 0:7b4c00e3912f 320 void FCTN_ACS_INIT(void); //initialization of registers happens
sakthipriya 0:7b4c00e3912f 321 void FCTN_ATS_DATA_ACQ(); //data is obtained
sakthipriya 0:7b4c00e3912f 322 void T_OUT(); //timeout function to stop infinite loop
sakthipriya 0:7b4c00e3912f 323 Timeout to; //Timeout variable to
sakthipriya 0:7b4c00e3912f 324 int toFlag;
sakthipriya 0:7b4c00e3912f 325
sakthipriya 0:7b4c00e3912f 326 int count =0; // Time for which the BAE uC is running (in seconds)
sakthipriya 0:7b4c00e3912f 327 void T_OUT()
sakthipriya 0:7b4c00e3912f 328 {
sakthipriya 0:7b4c00e3912f 329 toFlag=0; //as T_OUT function gets called the while loop gets terminated
sakthipriya 0:7b4c00e3912f 330 }
sakthipriya 0:7b4c00e3912f 331
sakthipriya 0:7b4c00e3912f 332
sakthipriya 0:7b4c00e3912f 333 //DEFINING VARIABLES
sakthipriya 0:7b4c00e3912f 334 char cmd[2];
sakthipriya 0:7b4c00e3912f 335 char raw_gyro[6];
sakthipriya 0:7b4c00e3912f 336 char raw_mag[6];
sakthipriya 0:7b4c00e3912f 337 char store,status;
sakthipriya 0:7b4c00e3912f 338 int16_t bit_data;
sakthipriya 0:7b4c00e3912f 339 float gyro_data[3], mag_data[3],combined_values[6];
sakthipriya 0:7b4c00e3912f 340 float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
sakthipriya 0:7b4c00e3912f 341 float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
sakthipriya 0:7b4c00e3912f 342 float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
sakthipriya 0:7b4c00e3912f 343
sakthipriya 0:7b4c00e3912f 344 void FCTN_ACS_INIT()
sakthipriya 0:7b4c00e3912f 345 {
lakshya 10:f93407b97750 346 ACS_INIT_STATUS = 's'; //set ACS_INIT_STATUS flag
sakthipriya 3:07e15677a75c 347 //FLAG();
sakthipriya 0:7b4c00e3912f 348 pc_acs.printf("Attitude sensor init called \n \r");
sakthipriya 0:7b4c00e3912f 349 //FLAG();
lakshya 16:f9e484dc505e 350 /* as of now no reset
lakshya 16:f9e484dc505e 351 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 352 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 353 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
sakthipriya 0:7b4c00e3912f 354 wait_ms(2000); //waiting for loading configuration file stored in EEPROM
lakshya 16:f9e484dc505e 355 // */
lakshya 16:f9e484dc505e 356 //wait_ms(3000);
sakthipriya 0:7b4c00e3912f 357 cmd[0]=SENTRALSTATUS;
sakthipriya 0:7b4c00e3912f 358 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 359 i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 16:f9e484dc505e 360 wait_ms(20); // initially 100
sakthipriya 0:7b4c00e3912f 361 //to check whether EEPROM is uploaded
sakthipriya 0:7b4c00e3912f 362 switch((int)store) {
lakshya 16:f9e484dc505e 363 case(3): { //actually this state correct
sakthipriya 0:7b4c00e3912f 364 break;
sakthipriya 0:7b4c00e3912f 365 }
sakthipriya 0:7b4c00e3912f 366 case(11): {
sakthipriya 0:7b4c00e3912f 367 break;
sakthipriya 0:7b4c00e3912f 368 }
sakthipriya 0:7b4c00e3912f 369 default: {
sakthipriya 0:7b4c00e3912f 370 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 371 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 372 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 16:f9e484dc505e 373 wait_ms(2000);//see if it can be changed
sakthipriya 0:7b4c00e3912f 374 }
sakthipriya 0:7b4c00e3912f 375 }
lakshya 16:f9e484dc505e 376 pc_acs.printf("\n\n\rwait is 1 \n\r");
sakthipriya 0:7b4c00e3912f 377 pc_acs.printf("Sentral Status is %x\n \r",(int)store);
lakshya 17:fc782f7548c6 378
lakshya 17:fc782f7548c6 379 /*ways to make the sensors work even if one sensor cups first making data rate 0x00
lakshya 17:fc782f7548c6 380 1>make data rate 0x00 change the enable events register value to 0x0A or 0x22 as suitable
lakshya 17:fc782f7548c6 381 2>change the condition for getting the values from the sensors i.e in data_acq function
lakshya 17:fc782f7548c6 382 3>check the other register values
lakshya 17:fc782f7548c6 383
lakshya 17:fc782f7548c6 384 other method is pass through working in how it works / decoding SENtral algorithms
lakshya 17:fc782f7548c6 385 */
lakshya 17:fc782f7548c6 386
sakthipriya 0:7b4c00e3912f 387 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
sakthipriya 0:7b4c00e3912f 388 cmd[1]=BIT_RUN_ENB;
sakthipriya 0:7b4c00e3912f 389 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 17:fc782f7548c6 390 wait_ms(100);
sakthipriya 0:7b4c00e3912f 391 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
sakthipriya 0:7b4c00e3912f 392 cmd[1]=BIT_MAGODR;
sakthipriya 0:7b4c00e3912f 393 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 17:fc782f7548c6 394 wait_ms(100);
lakshya 17:fc782f7548c6 395 //___________________________________________________________________________________________________
lakshya 17:fc782f7548c6 396
lakshya 17:fc782f7548c6 397 /*just leave it then see what happens?? the gyro data without inputing anything*/
lakshya 17:fc782f7548c6 398
lakshya 17:fc782f7548c6 399 //___________________________________________________________________________________________________
sakthipriya 0:7b4c00e3912f 400 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
sakthipriya 0:7b4c00e3912f 401 cmd[1]=BIT_GYROODR;
sakthipriya 0:7b4c00e3912f 402 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 16:f9e484dc505e 403 wait_ms(1);
sakthipriya 0:7b4c00e3912f 404 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
lakshya 17:fc782f7548c6 405 cmd[1]=0x00;//actually 0x00
sakthipriya 0:7b4c00e3912f 406 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 16:f9e484dc505e 407 wait_ms(1);
sakthipriya 0:7b4c00e3912f 408 cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
lakshya 17:fc782f7548c6 409 cmd[1]=BIT_EVT_ENB;//__________________________________________________________________________________
lakshya 17:fc782f7548c6 410 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 17:fc782f7548c6 411 wait_ms(1);
lakshya 17:fc782f7548c6 412 //_______________________________________________________________________________//
lakshya 17:fc782f7548c6 413
lakshya 18:3662058a7c10 414 /*start as of now this approach i.e pass through state*/
lakshya 18:3662058a7c10 415 /*
lakshya 18:3662058a7c10 416 cmd[0]=0x35;//event status
lakshya 18:3662058a7c10 417 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 18:3662058a7c10 418 i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 18:3662058a7c10 419 wait_ms(1);
lakshya 18:3662058a7c10 420 pc_acs.printf("\n\rEvent Status at start is %x\n \r",(int)status);
lakshya 18:3662058a7c10 421
lakshya 17:fc782f7548c6 422 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
lakshya 17:fc782f7548c6 423 cmd[1]=0x01;//Places sentral in standby state
sakthipriya 0:7b4c00e3912f 424 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 16:f9e484dc505e 425 wait_ms(1);
lakshya 17:fc782f7548c6 426
lakshya 17:fc782f7548c6 427
lakshya 17:fc782f7548c6 428 cmd[0]=0xA0; //Pass through control register
lakshya 17:fc782f7548c6 429 cmd[1]=0x01;//places SENtral in pass through state
lakshya 17:fc782f7548c6 430 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 17:fc782f7548c6 431 wait_ms(1);
lakshya 17:fc782f7548c6 432
lakshya 17:fc782f7548c6 433
lakshya 17:fc782f7548c6 434 cmd[0]=0x9E;//Pass through status reg
lakshya 17:fc782f7548c6 435 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 17:fc782f7548c6 436 i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 17:fc782f7548c6 437 wait_ms(1);
lakshya 17:fc782f7548c6 438 printf("\n\r the value of the pass through status register is = %x",(int)store);
lakshya 17:fc782f7548c6 439
lakshya 18:3662058a7c10 440 cmd[0]=0x35;//event status
lakshya 18:3662058a7c10 441 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 18:3662058a7c10 442 i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 18:3662058a7c10 443 wait_ms(1);
lakshya 18:3662058a7c10 444 pc_acs.printf("\n\rEvent Status at end initialization is %x\n \r",(int)status);
lakshya 17:fc782f7548c6 445 */
lakshya 18:3662058a7c10 446
lakshya 17:fc782f7548c6 447 //_______________________________________________________________________________//
lakshya 17:fc782f7548c6 448
lakshya 10:f93407b97750 449 ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
sakthipriya 0:7b4c00e3912f 450 }
sakthipriya 0:7b4c00e3912f 451
sakthipriya 0:7b4c00e3912f 452 void FCTN_ATS_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 453 {
lakshya 19:79e69017c855 454 //time.start();
lakshya 19:79e69017c855 455
Bragadeesh153 13:fb7facaf308b 456 ACS_DATA_ACQ_STATUS = 1; //set ACS_DATA_ACQ_STATUS flag for att sens 2
Bragadeesh153 13:fb7facaf308b 457 if( ACS_ATS_ENABLE == 1)
sakthipriya 0:7b4c00e3912f 458 {
sakthipriya 0:7b4c00e3912f 459 FLAG();
sakthipriya 0:7b4c00e3912f 460 pc_acs.printf("attitude sensor execution called \n \r");
sakthipriya 0:7b4c00e3912f 461 toFlag=1; //toFlag is set to 1 so that it enters while loop
sakthipriya 0:7b4c00e3912f 462 to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated
sakthipriya 0:7b4c00e3912f 463 while(toFlag) {
sakthipriya 0:7b4c00e3912f 464 cmd[0]=EVT_STATUS;
sakthipriya 0:7b4c00e3912f 465 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 466 i2c.read(SLAVE_ADDR_READ,&status,1);
sakthipriya 0:7b4c00e3912f 467 wait_ms(100);
sakthipriya 0:7b4c00e3912f 468 pc_acs.printf("Event Status is %x\n \r",(int)status);
sakthipriya 0:7b4c00e3912f 469 //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
lakshya 17:fc782f7548c6 470 if(((int)status&40)==40){ //when both work
lakshya 19:79e69017c855 471 ///// if((int)status==8){ //for just mag
lakshya 19:79e69017c855 472 ///// if((int)status == 36){ //for just gyro as status 24(in binary) = 36 in decimal
lakshya 19:79e69017c855 473 ///// if((int)status==___yet to be decided___){ //for pass through state see how it tworks
lakshya 17:fc782f7548c6 474
lakshya 17:fc782f7548c6 475 /*don't ask for info if the gyro cupped as interrupt makes it go haywire that is enables the pins for gyro*/
lakshya 17:fc782f7548c6 476 cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
lakshya 17:fc782f7548c6 477 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 478 i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
lakshya 19:79e69017c855 479
lakshya 17:fc782f7548c6 480 /*editing the data*/
lakshya 17:fc782f7548c6 481
sakthipriya 0:7b4c00e3912f 482 cmd[0]=MAG_XOUT_H; //LSB of x
sakthipriya 0:7b4c00e3912f 483 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 484 i2c.read(SLAVE_ADDR_READ,raw_mag,6);
sakthipriya 0:7b4c00e3912f 485 // pc_acs.printf("\nGyro Values:\n");
sakthipriya 0:7b4c00e3912f 486 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 487 //concatenating gyro LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 488 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
sakthipriya 0:7b4c00e3912f 489 gyro_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 490 gyro_data[i]=gyro_data[i]/senstivity_gyro;
sakthipriya 0:7b4c00e3912f 491 gyro_data[i]+=gyro_error[i];
lakshya 15:e09aaaccf134 492 // pc_acs.printf("%f\t",gyro_data[i]);
sakthipriya 0:7b4c00e3912f 493 }
lakshya 17:fc782f7548c6 494
lakshya 17:fc782f7548c6 495 /*read the status values to determine the actual condition/registers values for the gyro/mag off case */
lakshya 17:fc782f7548c6 496 //starts here
lakshya 17:fc782f7548c6 497
lakshya 17:fc782f7548c6 498 cmd[0]=0x35;//event status
lakshya 17:fc782f7548c6 499 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 17:fc782f7548c6 500 i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 17:fc782f7548c6 501 wait_ms(1);
lakshya 18:3662058a7c10 502 pc_acs.printf("\n\rEvent Status at the end is %x",(int)status);
lakshya 17:fc782f7548c6 503
lakshya 17:fc782f7548c6 504
lakshya 17:fc782f7548c6 505 cmd[0]=0x33;//enable events
lakshya 17:fc782f7548c6 506 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 17:fc782f7548c6 507 i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 17:fc782f7548c6 508 wait_ms(1);
lakshya 18:3662058a7c10 509 pc_acs.printf("\n\rEnable events is %x",(int)status);
lakshya 17:fc782f7548c6 510
lakshya 17:fc782f7548c6 511
lakshya 17:fc782f7548c6 512 cmd[0]=0x57;//gyro rate
lakshya 17:fc782f7548c6 513 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 17:fc782f7548c6 514 i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 17:fc782f7548c6 515 wait_ms(1);
lakshya 18:3662058a7c10 516 pc_acs.printf("\n\rgyro rate is %x",(int)status);
lakshya 17:fc782f7548c6 517
lakshya 17:fc782f7548c6 518
lakshya 17:fc782f7548c6 519 cmd[0]=0x55;//mag rate 0x00 indicate value lost
lakshya 17:fc782f7548c6 520 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 17:fc782f7548c6 521 i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 17:fc782f7548c6 522 wait_ms(1);
lakshya 18:3662058a7c10 523 pc_acs.printf("\n\rmag rate is %x",(int)status);
lakshya 17:fc782f7548c6 524
lakshya 17:fc782f7548c6 525
lakshya 17:fc782f7548c6 526 cmd[0]=0x36;//sensorstatus
lakshya 17:fc782f7548c6 527 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 17:fc782f7548c6 528 i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 17:fc782f7548c6 529 wait_ms(1);
lakshya 18:3662058a7c10 530 pc_acs.printf("\n\rsensor Status is %x",(int)status);
lakshya 17:fc782f7548c6 531
lakshya 17:fc782f7548c6 532
lakshya 17:fc782f7548c6 533 cmd[0]=0x50;//error register
lakshya 17:fc782f7548c6 534 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 17:fc782f7548c6 535 i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 17:fc782f7548c6 536 wait_ms(1);
lakshya 17:fc782f7548c6 537 pc_acs.printf("\n\rerror register value is %x\n \r",(int)status);
lakshya 17:fc782f7548c6 538
lakshya 17:fc782f7548c6 539 //end here
lakshya 17:fc782f7548c6 540
sakthipriya 0:7b4c00e3912f 541 // pc_acs.printf("\nMag Values:\n");
sakthipriya 0:7b4c00e3912f 542 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 543 //concatenating mag LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 544 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
sakthipriya 0:7b4c00e3912f 545 mag_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 546 mag_data[i]=mag_data[i]/senstivity_mag;
sakthipriya 0:7b4c00e3912f 547 mag_data[i]+=mag_error[i];
lakshya 15:e09aaaccf134 548 //pc_acs.printf("%f\t",mag_data[i]);
sakthipriya 0:7b4c00e3912f 549 }
sakthipriya 0:7b4c00e3912f 550 for(int i=0; i<3; i++) {
sakthipriya 6:036d08b62785 551 // data[i]=gyro_data[i];
sakthipriya 6:036d08b62785 552 actual_data.AngularSpeed_actual[i] = gyro_data[i];
sakthipriya 6:036d08b62785 553 actual_data.Bvalue_actual[i] = mag_data[i];
sakthipriya 6:036d08b62785 554 //data[i+3]=mag_data[i];
sakthipriya 0:7b4c00e3912f 555 }
sakthipriya 0:7b4c00e3912f 556 // return(combined_values); //returning poiter combined values
sakthipriya 0:7b4c00e3912f 557 }
sakthipriya 0:7b4c00e3912f 558 //checking for the error
sakthipriya 0:7b4c00e3912f 559 else if (((int)status&2)==2) {
sakthipriya 0:7b4c00e3912f 560 FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
sakthipriya 0:7b4c00e3912f 561 }
sakthipriya 0:7b4c00e3912f 562 }
sakthipriya 0:7b4c00e3912f 563 }
sakthipriya 0:7b4c00e3912f 564 else //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
sakthipriya 0:7b4c00e3912f 565 {
lakshya 10:f93407b97750 566 ACS_DATA_ACQ_STATUS = 'f';
sakthipriya 0:7b4c00e3912f 567 }
Bragadeesh153 13:fb7facaf308b 568 ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
sakthipriya 0:7b4c00e3912f 569 }
sakthipriya 0:7b4c00e3912f 570
sakthipriya 0:7b4c00e3912f 571 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 572 {
sakthipriya 0:7b4c00e3912f 573 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 574
sakthipriya 0:7b4c00e3912f 575 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 576 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 577 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 578 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 579 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 580 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 581
sakthipriya 0:7b4c00e3912f 582
sakthipriya 0:7b4c00e3912f 583 for(int i = 0 ; i<3;i++)
sakthipriya 0:7b4c00e3912f 584 {
sakthipriya 0:7b4c00e3912f 585 // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
sakthipriya 0:7b4c00e3912f 586 }
sakthipriya 0:7b4c00e3912f 587
sakthipriya 0:7b4c00e3912f 588 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 589
sakthipriya 0:7b4c00e3912f 590
sakthipriya 0:7b4c00e3912f 591 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 592
sakthipriya 0:7b4c00e3912f 593 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 594 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 595 {
sakthipriya 0:7b4c00e3912f 596 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 597 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 598 }
sakthipriya 0:7b4c00e3912f 599
sakthipriya 0:7b4c00e3912f 600 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 10:f93407b97750 601 printf("current in trx is %f \r \n",l_current_x);
lakshya 10:f93407b97750 602 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 603 {
lakshya 10:f93407b97750 604 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 605 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 606 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 607 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 608 }
lakshya 10:f93407b97750 609 else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
lakshya 10:f93407b97750 610 {
lakshya 10:f93407b97750 611 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 612 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 613 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 614 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 615 }
lakshya 10:f93407b97750 616 else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
lakshya 10:f93407b97750 617 {
lakshya 10:f93407b97750 618 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 619 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 620 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 621 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 622 }
lakshya 10:f93407b97750 623 else if(l_current_x==0)
lakshya 10:f93407b97750 624 {
lakshya 10:f93407b97750 625 printf("\n \r l_current_x====0");
lakshya 10:f93407b97750 626 l_duty_cycle_x = 0; // default value of duty cycle
lakshya 10:f93407b97750 627 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 628 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 629 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 630 }
lakshya 10:f93407b97750 631 else //not necessary
lakshya 10:f93407b97750 632 {
lakshya 10:f93407b97750 633 g_err_flag_TR_x = 1;
lakshya 10:f93407b97750 634 }
lakshya 10:f93407b97750 635
lakshya 10:f93407b97750 636 //------------------------------------- y-direction TR--------------------------------------//
lakshya 10:f93407b97750 637
lakshya 10:f93407b97750 638
lakshya 10:f93407b97750 639 float l_moment_y = Moment[1]; //Moment in y direction
lakshya 10:f93407b97750 640
lakshya 10:f93407b97750 641 phase_TR_y = 1; // setting the default current direction
lakshya 10:f93407b97750 642 if (l_moment_y <0)
lakshya 10:f93407b97750 643 {
lakshya 10:f93407b97750 644 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 645 l_moment_y = abs(l_moment_y);
lakshya 10:f93407b97750 646 }
lakshya 10:f93407b97750 647
lakshya 10:f93407b97750 648
lakshya 10:f93407b97750 649 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 10:f93407b97750 650 printf("current in try is %f \r \n",l_current_y);
lakshya 10:f93407b97750 651 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 652 {
lakshya 10:f93407b97750 653 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 654 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 655 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 656 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 657 }
lakshya 10:f93407b97750 658 else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
lakshya 10:f93407b97750 659 {
lakshya 10:f93407b97750 660 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 661 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 662 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 663 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 664 }
lakshya 10:f93407b97750 665 else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
lakshya 10:f93407b97750 666 {
lakshya 10:f93407b97750 667 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 668 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 669 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 670 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 671 }
lakshya 10:f93407b97750 672 else if(l_current_y==0)
lakshya 10:f93407b97750 673 {
lakshya 10:f93407b97750 674 printf("\n \r l_current_y====0");
lakshya 10:f93407b97750 675 l_duty_cycle_y = 0; // default value of duty cycle
lakshya 10:f93407b97750 676 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 677 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 678 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 679 }
lakshya 10:f93407b97750 680 else // not necessary
lakshya 10:f93407b97750 681 {
lakshya 10:f93407b97750 682 g_err_flag_TR_y = 1;
lakshya 10:f93407b97750 683 }
lakshya 10:f93407b97750 684
lakshya 10:f93407b97750 685 //----------------------------------------------- z-direction TR -------------------------//
lakshya 10:f93407b97750 686
lakshya 10:f93407b97750 687
lakshya 10:f93407b97750 688 float l_moment_z = Moment[2]; //Moment in z direction
lakshya 10:f93407b97750 689
lakshya 10:f93407b97750 690 phase_TR_z = 1; // setting the default current direction
lakshya 10:f93407b97750 691 if (l_moment_z <0)
lakshya 10:f93407b97750 692 {
lakshya 10:f93407b97750 693 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 694 l_moment_z = abs(l_moment_z);
lakshya 10:f93407b97750 695 }
lakshya 10:f93407b97750 696
lakshya 10:f93407b97750 697
lakshya 10:f93407b97750 698 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 10:f93407b97750 699 printf("current in trz is %f \r \n",l_current_z);
lakshya 10:f93407b97750 700 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 701 {
lakshya 10:f93407b97750 702 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 703 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 704 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 705 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 706 }
lakshya 10:f93407b97750 707 else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
lakshya 10:f93407b97750 708 {
lakshya 10:f93407b97750 709 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 710 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 711 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 712 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 713 }
lakshya 10:f93407b97750 714 else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
lakshya 10:f93407b97750 715 {
lakshya 10:f93407b97750 716 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 717 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 718 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 719 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 720 }
lakshya 10:f93407b97750 721 else if(l_current_z==0)
lakshya 10:f93407b97750 722 {
lakshya 10:f93407b97750 723 printf("\n \r l_current_z====0");
lakshya 10:f93407b97750 724 l_duty_cycle_z = 0; // default value of duty cycle
lakshya 10:f93407b97750 725 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 726 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 727 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 728 }
lakshya 10:f93407b97750 729 else // not necessary
lakshya 10:f93407b97750 730 {
lakshya 10:f93407b97750 731 g_err_flag_TR_z = 1;
lakshya 10:f93407b97750 732 }
lakshya 10:f93407b97750 733
lakshya 10:f93407b97750 734 //-----------------------------------------exiting the function-----------------------------------//
lakshya 10:f93407b97750 735
lakshya 10:f93407b97750 736 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
lakshya 10:f93407b97750 737
lakshya 10:f93407b97750 738 }
lakshya 10:f93407b97750 739
lakshya 10:f93407b97750 740
lakshya 10:f93407b97750 741 /*void FCTN_ACS_GENPWM_MAIN(float Moment[3])
lakshya 10:f93407b97750 742 {
lakshya 10:f93407b97750 743 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
lakshya 10:f93407b97750 744
lakshya 10:f93407b97750 745 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
lakshya 10:f93407b97750 746 float l_current_x=0; //Current sent in x TR's
lakshya 10:f93407b97750 747 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
lakshya 10:f93407b97750 748 float l_current_y=0; //Current sent in y TR's
lakshya 10:f93407b97750 749 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
lakshya 10:f93407b97750 750 float l_current_z=0; //Current sent in z TR's
lakshya 10:f93407b97750 751
lakshya 10:f93407b97750 752
lakshya 10:f93407b97750 753 for(int i = 0 ; i<3;i++)
lakshya 10:f93407b97750 754 {
lakshya 10:f93407b97750 755 printf("pwm %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
lakshya 10:f93407b97750 756 }
lakshya 10:f93407b97750 757
lakshya 10:f93407b97750 758 //----------------------------- x-direction TR --------------------------------------------//
lakshya 10:f93407b97750 759
lakshya 10:f93407b97750 760
lakshya 10:f93407b97750 761 float l_moment_x = Moment[0]; //Moment in x direction
lakshya 10:f93407b97750 762
lakshya 10:f93407b97750 763 phase_TR_x = 1; // setting the default current direction
lakshya 10:f93407b97750 764 if (l_moment_x <0)
lakshya 10:f93407b97750 765 {
lakshya 10:f93407b97750 766 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 767 l_moment_x = abs(l_moment_x);
lakshya 10:f93407b97750 768 }
lakshya 10:f93407b97750 769
lakshya 10:f93407b97750 770 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 771 pc_acs.printf("current in trx is %f \r \n",l_current_x);
sakthipriya 0:7b4c00e3912f 772 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 773 {
sakthipriya 0:7b4c00e3912f 774 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 775 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 776 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 777 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 778 }
sakthipriya 0:7b4c00e3912f 779 else if( l_current_x >= 0.006 && l_current_x < 0.0116)
sakthipriya 0:7b4c00e3912f 780 {
sakthipriya 0:7b4c00e3912f 781 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 782 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 783 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 784 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 785 }
sakthipriya 0:7b4c00e3912f 786 else if (l_current_x >= 0.0116 && l_current_x < 0.0624)
sakthipriya 0:7b4c00e3912f 787 {
sakthipriya 0:7b4c00e3912f 788 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 789 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 790 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 791 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 792 }
sakthipriya 0:7b4c00e3912f 793 else if(l_current_x >= 0.0624 && l_current_x < 0.555)
sakthipriya 0:7b4c00e3912f 794 {
sakthipriya 0:7b4c00e3912f 795 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 796 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 797 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 798 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 799 }
sakthipriya 0:7b4c00e3912f 800 else if(l_current_x==0)
sakthipriya 0:7b4c00e3912f 801 {
sakthipriya 0:7b4c00e3912f 802 printf("\n \r l_current_x====0");
sakthipriya 0:7b4c00e3912f 803 l_duty_cycle_x = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 804 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 805 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 806 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 807 }
sakthipriya 0:7b4c00e3912f 808 else //not necessary
sakthipriya 0:7b4c00e3912f 809 {
sakthipriya 0:7b4c00e3912f 810 g_err_flag_TR_x = 1;
sakthipriya 0:7b4c00e3912f 811 }
sakthipriya 0:7b4c00e3912f 812
sakthipriya 0:7b4c00e3912f 813 //------------------------------------- y-direction TR--------------------------------------//
sakthipriya 0:7b4c00e3912f 814
sakthipriya 0:7b4c00e3912f 815
sakthipriya 0:7b4c00e3912f 816 float l_moment_y = Moment[1]; //Moment in y direction
sakthipriya 0:7b4c00e3912f 817
sakthipriya 0:7b4c00e3912f 818 phase_TR_y = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 819 if (l_moment_y <0)
sakthipriya 0:7b4c00e3912f 820 {
sakthipriya 0:7b4c00e3912f 821 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 822 l_moment_y = abs(l_moment_y);
sakthipriya 0:7b4c00e3912f 823 }
sakthipriya 0:7b4c00e3912f 824
sakthipriya 0:7b4c00e3912f 825
sakthipriya 0:7b4c00e3912f 826 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 827 pc_acs.printf("current in try is %f \r \n",l_current_y);
sakthipriya 0:7b4c00e3912f 828 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 829 {
sakthipriya 0:7b4c00e3912f 830 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 831 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 832 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 833 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 834 }
sakthipriya 0:7b4c00e3912f 835 else if( l_current_y >= 0.006 && l_current_y < 0.0116)
sakthipriya 0:7b4c00e3912f 836 {
sakthipriya 0:7b4c00e3912f 837 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 838 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 839 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 840 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 841 }
sakthipriya 0:7b4c00e3912f 842 else if (l_current_y >= 0.0116&& l_current_y < 0.0624)
sakthipriya 0:7b4c00e3912f 843 {
sakthipriya 0:7b4c00e3912f 844 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 845 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 846 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 847 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 848 }
sakthipriya 0:7b4c00e3912f 849 else if(l_current_y >= 0.0624 && l_current_y < 0.555)
sakthipriya 0:7b4c00e3912f 850 {
sakthipriya 0:7b4c00e3912f 851 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 852 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 853 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 854 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 855 }
sakthipriya 0:7b4c00e3912f 856 else if(l_current_y==0)
sakthipriya 0:7b4c00e3912f 857 {
sakthipriya 0:7b4c00e3912f 858 printf("\n \r l_current_y====0");
sakthipriya 0:7b4c00e3912f 859 l_duty_cycle_y = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 860 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 861 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 862 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 863 }
sakthipriya 0:7b4c00e3912f 864 else // not necessary
sakthipriya 0:7b4c00e3912f 865 {
sakthipriya 0:7b4c00e3912f 866 g_err_flag_TR_y = 1;
sakthipriya 0:7b4c00e3912f 867 }
sakthipriya 0:7b4c00e3912f 868
sakthipriya 0:7b4c00e3912f 869 //----------------------------------------------- z-direction TR -------------------------//
sakthipriya 0:7b4c00e3912f 870
sakthipriya 0:7b4c00e3912f 871
sakthipriya 0:7b4c00e3912f 872 float l_moment_z = Moment[2]; //Moment in z direction
sakthipriya 0:7b4c00e3912f 873
sakthipriya 0:7b4c00e3912f 874 phase_TR_z = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 875 if (l_moment_z <0)
sakthipriya 0:7b4c00e3912f 876 {
sakthipriya 0:7b4c00e3912f 877 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 878 l_moment_z = abs(l_moment_z);
sakthipriya 0:7b4c00e3912f 879 }
sakthipriya 0:7b4c00e3912f 880
sakthipriya 0:7b4c00e3912f 881
sakthipriya 0:7b4c00e3912f 882 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 883 pc_acs.printf("current in trz is %f \r \n",l_current_z);
sakthipriya 0:7b4c00e3912f 884 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 885 {
sakthipriya 0:7b4c00e3912f 886 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 887 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 888 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 889 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 890 }
sakthipriya 0:7b4c00e3912f 891 else if( l_current_z >= 0.006 && l_current_z < 0.0116)
sakthipriya 0:7b4c00e3912f 892 {
sakthipriya 0:7b4c00e3912f 893 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 894 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 895 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 896 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 897 }
sakthipriya 0:7b4c00e3912f 898 else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
sakthipriya 0:7b4c00e3912f 899 {
sakthipriya 0:7b4c00e3912f 900 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 901 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 902 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 903 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 904 }
sakthipriya 0:7b4c00e3912f 905 else if(l_current_z >= 0.0624 && l_current_z < 0.555)
sakthipriya 0:7b4c00e3912f 906 {
sakthipriya 0:7b4c00e3912f 907 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 908 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 909 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 910 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 911 }
sakthipriya 0:7b4c00e3912f 912 else if(l_current_z==0)
sakthipriya 0:7b4c00e3912f 913 {
sakthipriya 0:7b4c00e3912f 914 printf("\n \r l_current_z====0");
sakthipriya 0:7b4c00e3912f 915 l_duty_cycle_z = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 916 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 917 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 918 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 919 }
sakthipriya 0:7b4c00e3912f 920 else // not necessary
sakthipriya 0:7b4c00e3912f 921 {
sakthipriya 0:7b4c00e3912f 922 g_err_flag_TR_z = 1;
sakthipriya 0:7b4c00e3912f 923 }
sakthipriya 0:7b4c00e3912f 924
sakthipriya 0:7b4c00e3912f 925 //-----------------------------------------exiting the function-----------------------------------//
sakthipriya 0:7b4c00e3912f 926
sakthipriya 0:7b4c00e3912f 927 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
sakthipriya 0:7b4c00e3912f 928
lakshya 10:f93407b97750 929 }*/
sakthipriya 0:7b4c00e3912f 930
sakthipriya 0:7b4c00e3912f 931
sakthipriya 0:7b4c00e3912f 932