Team Fox / Mbed 2 deprecated TFR_BAE_vr1_1working_finally_Commissioning_PWM_update

PWM and ALGO updated.. PWM getting generated

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of TFR_BAE_vr1_1working_finally_PWM_CTRLALGO_update by Team Fox

ACS.cpp@12:f82fbe93fab3, 2016-04-01 (annotated)

Committer:
Bragadeesh153
Date:
Fri Apr 01 18:58:43 2016 +0000
Revision:
12:f82fbe93fab3
Parent:
11:0f71a96987bd
Child:
13:75aa0104f5e6
UPDATED PWM and CONTROL ALGO BOTH WORKING

Who changed what in which revision?

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