FINAL ACS TO BE USED FOR TESTING. COMMISSIONING, ACS MAIN, DATA ACQ ALL DONE.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of ACS_FULL_Flowchart_BAE by
ACS.cpp@16:cc77770d787f, 2016-06-03 (annotated)
- Committer:
- Bragadeesh153
- Date:
- Fri Jun 03 13:53:55 2016 +0000
- Revision:
- 16:cc77770d787f
- Parent:
- 15:e09aaaccf134
- Child:
- 17:1e1955f3db75
ACS Data_Acq updated. Tested and working.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sakthipriya | 0:7b4c00e3912f | 1 | /*------------------------------------------------------------------------------------------------------------------------------------------------------ |
sakthipriya | 0:7b4c00e3912f | 2 | -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/ |
sakthipriya | 0:7b4c00e3912f | 3 | #include <mbed.h> |
sakthipriya | 0:7b4c00e3912f | 4 | #include <math.h> |
sakthipriya | 0:7b4c00e3912f | 5 | |
sakthipriya | 0:7b4c00e3912f | 6 | #include "pni.h" //pni header file |
sakthipriya | 0:7b4c00e3912f | 7 | #include "pin_config.h" |
sakthipriya | 0:7b4c00e3912f | 8 | #include "ACS.h" |
sakthipriya | 6:036d08b62785 | 9 | #include "EPS.h" |
sakthipriya | 0:7b4c00e3912f | 10 | |
sakthipriya | 0:7b4c00e3912f | 11 | //********************************flags******************************************// |
sakthipriya | 0:7b4c00e3912f | 12 | extern uint32_t BAE_STATUS; |
sakthipriya | 0:7b4c00e3912f | 13 | extern uint32_t BAE_ENABLE; |
Bragadeesh153 | 13:fb7facaf308b | 14 | extern uint8_t ACS_INIT_STATUS; |
Bragadeesh153 | 13:fb7facaf308b | 15 | extern uint8_t ACS_DATA_ACQ_STATUS; |
Bragadeesh153 | 13:fb7facaf308b | 16 | extern uint8_t ACS_ATS_STATUS; |
Bragadeesh153 | 13:fb7facaf308b | 17 | extern uint8_t ACS_MAIN_STATUS; |
Bragadeesh153 | 13:fb7facaf308b | 18 | extern uint8_t ACS_STATUS; |
sakthipriya | 0:7b4c00e3912f | 19 | |
Bragadeesh153 | 16:cc77770d787f | 20 | extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch |
Bragadeesh153 | 16:cc77770d787f | 21 | extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch |
Bragadeesh153 | 16:cc77770d787f | 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 | { |
Bragadeesh153 | 16:cc77770d787f | 66 | |
lakshya | 10:f93407b97750 | 67 | float b1[3]; |
lakshya | 10:f93407b97750 | 68 | float omega1[3]; |
lakshya | 10:f93407b97750 | 69 | b1[0] = b[0]/1000000.0; |
lakshya | 10:f93407b97750 | 70 | b1[1] = b[1]/1000000.0; |
Bragadeesh153 | 16:cc77770d787f | 71 | b1[2] = b[2]/1000000.0; |
lakshya | 10:f93407b97750 | 72 | |
lakshya | 10:f93407b97750 | 73 | omega1[0] = omega[0]*3.14159/180; |
lakshya | 10:f93407b97750 | 74 | omega1[1] = omega[1]*3.14159/180; |
lakshya | 10:f93407b97750 | 75 | omega1[2] = omega[2]*3.14159/180; |
lakshya | 10:f93407b97750 | 76 | controller (moment, b1, omega1, b_old, alarmmode, flag_firsttime, controlmode); |
sakthipriya | 0:7b4c00e3912f | 77 | |
lakshya | 10:f93407b97750 | 78 | } |
lakshya | 10:f93407b97750 | 79 | 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 | 80 | { |
lakshya | 10:f93407b97750 | 81 | float db1[3]; // Unit: Tesla/Second |
lakshya | 10:f93407b97750 | 82 | float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds |
lakshya | 10:f93407b97750 | 83 | float MmntMax=1.1; // Unit: Ampere*Meter^2 |
lakshya | 10:f93407b97750 | 84 | float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second |
lakshya | 10:f93407b97750 | 85 | float normalising_fact; |
lakshya | 10:f93407b97750 | 86 | float b1_copy[3], omega1_copy[3], db1_copy[3]; |
lakshya | 10:f93407b97750 | 87 | int i; |
lakshya | 10:f93407b97750 | 88 | if(flag_firsttime==1) |
lakshya | 10:f93407b97750 | 89 | { |
lakshya | 10:f93407b97750 | 90 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 91 | { |
lakshya | 10:f93407b97750 | 92 | db1[i]=0; // Unit: Tesla/Second |
lakshya | 10:f93407b97750 | 93 | } |
lakshya | 10:f93407b97750 | 94 | flag_firsttime=0; |
lakshya | 10:f93407b97750 | 95 | } |
sakthipriya | 0:7b4c00e3912f | 96 | else |
sakthipriya | 0:7b4c00e3912f | 97 | { |
sakthipriya | 0:7b4c00e3912f | 98 | for(i=0;i<3;i++) |
sakthipriya | 0:7b4c00e3912f | 99 | { |
lakshya | 10:f93407b97750 | 100 | db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second |
sakthipriya | 0:7b4c00e3912f | 101 | } |
lakshya | 10:f93407b97750 | 102 | } |
lakshya | 10:f93407b97750 | 103 | |
lakshya | 10:f93407b97750 | 104 | if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1) |
sakthipriya | 0:7b4c00e3912f | 105 | { |
lakshya | 10:f93407b97750 | 106 | alarmmode=0; |
sakthipriya | 0:7b4c00e3912f | 107 | } |
lakshya | 10:f93407b97750 | 108 | else if(max_array(omega1)>OmegaMax && alarmmode==0) |
sakthipriya | 0:7b4c00e3912f | 109 | { |
lakshya | 10:f93407b97750 | 110 | alarmmode=1; |
sakthipriya | 0:7b4c00e3912f | 111 | } |
lakshya | 10:f93407b97750 | 112 | |
lakshya | 10:f93407b97750 | 113 | for (i=0;i<3;i++) |
sakthipriya | 0:7b4c00e3912f | 114 | { |
lakshya | 10:f93407b97750 | 115 | b1_copy[i]=b1[i]; |
lakshya | 10:f93407b97750 | 116 | db1_copy[i]=db1[i]; |
lakshya | 10:f93407b97750 | 117 | omega1_copy[i]=omega1[i]; |
sakthipriya | 0:7b4c00e3912f | 118 | } |
lakshya | 10:f93407b97750 | 119 | |
lakshya | 10:f93407b97750 | 120 | if(alarmmode==0) |
lakshya | 10:f93407b97750 | 121 | { |
lakshya | 10:f93407b97750 | 122 | controlmode=0; |
lakshya | 10:f93407b97750 | 123 | controlmodes(moment,b1,db1,omega1,controlmode,MmntMax); |
lakshya | 10:f93407b97750 | 124 | for (i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 125 | { |
lakshya | 10:f93407b97750 | 126 | b1[i]=b1_copy[i]; |
lakshya | 10:f93407b97750 | 127 | db1[i]=db1_copy[i]; |
lakshya | 10:f93407b97750 | 128 | omega1[i]=omega1_copy[i]; |
lakshya | 10:f93407b97750 | 129 | } |
lakshya | 10:f93407b97750 | 130 | if(max_array(moment)>MmntMax) |
lakshya | 10:f93407b97750 | 131 | { |
lakshya | 10:f93407b97750 | 132 | controlmode=1; |
lakshya | 10:f93407b97750 | 133 | controlmodes(moment,b1,db1,omega1,controlmode,MmntMax); |
lakshya | 10:f93407b97750 | 134 | for (i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 135 | { |
lakshya | 10:f93407b97750 | 136 | b1[i]=b1_copy[i]; |
lakshya | 10:f93407b97750 | 137 | db1[i]=db1_copy[i]; |
lakshya | 10:f93407b97750 | 138 | omega1[i]=omega1_copy[i]; |
lakshya | 10:f93407b97750 | 139 | } |
lakshya | 10:f93407b97750 | 140 | if(max_array(moment)>MmntMax) |
lakshya | 10:f93407b97750 | 141 | { |
lakshya | 10:f93407b97750 | 142 | normalising_fact=max_array(moment)/MmntMax; |
lakshya | 10:f93407b97750 | 143 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 144 | { |
lakshya | 10:f93407b97750 | 145 | moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 |
lakshya | 10:f93407b97750 | 146 | } |
lakshya | 10:f93407b97750 | 147 | } |
lakshya | 10:f93407b97750 | 148 | } |
lakshya | 10:f93407b97750 | 149 | } |
lakshya | 10:f93407b97750 | 150 | else |
lakshya | 10:f93407b97750 | 151 | { |
lakshya | 10:f93407b97750 | 152 | controlmode=1; |
lakshya | 10:f93407b97750 | 153 | controlmodes(moment,b1,db1,omega1,controlmode,MmntMax); |
lakshya | 10:f93407b97750 | 154 | for (i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 155 | { |
lakshya | 10:f93407b97750 | 156 | b1[i]=b1_copy[i]; |
lakshya | 10:f93407b97750 | 157 | db1[i]=db1_copy[i]; |
lakshya | 10:f93407b97750 | 158 | omega1[i]=omega1_copy[i]; |
lakshya | 10:f93407b97750 | 159 | } |
lakshya | 10:f93407b97750 | 160 | if(max_array(moment)>MmntMax) |
lakshya | 10:f93407b97750 | 161 | { |
lakshya | 10:f93407b97750 | 162 | normalising_fact=max_array(moment)/MmntMax; |
lakshya | 10:f93407b97750 | 163 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 164 | { |
lakshya | 10:f93407b97750 | 165 | moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 |
lakshya | 10:f93407b97750 | 166 | } |
lakshya | 10:f93407b97750 | 167 | } |
lakshya | 10:f93407b97750 | 168 | |
lakshya | 10:f93407b97750 | 169 | } |
lakshya | 10:f93407b97750 | 170 | for (i=0;i<3;i++) |
sakthipriya | 0:7b4c00e3912f | 171 | { |
lakshya | 10:f93407b97750 | 172 | b_old[i]=b1[i]; |
sakthipriya | 0:7b4c00e3912f | 173 | } |
sakthipriya | 0:7b4c00e3912f | 174 | } |
lakshya | 10:f93407b97750 | 175 | |
sakthipriya | 0:7b4c00e3912f | 176 | void inverse(float mat[3][3],float inv[3][3]) |
sakthipriya | 0:7b4c00e3912f | 177 | { |
sakthipriya | 0:7b4c00e3912f | 178 | int i,j; |
sakthipriya | 0:7b4c00e3912f | 179 | float det=0; |
sakthipriya | 0:7b4c00e3912f | 180 | for(i=0;i<3;i++) |
sakthipriya | 0:7b4c00e3912f | 181 | { |
sakthipriya | 0:7b4c00e3912f | 182 | for(j=0;j<3;j++) |
lakshya | 10:f93407b97750 | 183 | { |
sakthipriya | 0:7b4c00e3912f | 184 | 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 | 185 | } |
sakthipriya | 0:7b4c00e3912f | 186 | } |
sakthipriya | 0:7b4c00e3912f | 187 | det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]); |
sakthipriya | 0:7b4c00e3912f | 188 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 189 | { |
sakthipriya | 0:7b4c00e3912f | 190 | for(j=0;j<3;j++) |
lakshya | 10:f93407b97750 | 191 | { |
sakthipriya | 0:7b4c00e3912f | 192 | inv[i][j]/=det; |
lakshya | 10:f93407b97750 | 193 | } |
sakthipriya | 0:7b4c00e3912f | 194 | } |
sakthipriya | 0:7b4c00e3912f | 195 | } |
sakthipriya | 0:7b4c00e3912f | 196 | |
lakshya | 10:f93407b97750 | 197 | float max_array(float arr[3]) |
lakshya | 10:f93407b97750 | 198 | { |
lakshya | 10:f93407b97750 | 199 | int i; |
lakshya | 10:f93407b97750 | 200 | float temp_max=fabs(arr[0]); |
lakshya | 10:f93407b97750 | 201 | for(i=1;i<3;i++) |
lakshya | 10:f93407b97750 | 202 | { |
lakshya | 10:f93407b97750 | 203 | if(fabs(arr[i])>temp_max) |
lakshya | 10:f93407b97750 | 204 | { |
lakshya | 10:f93407b97750 | 205 | temp_max=fabs(arr[i]); |
lakshya | 10:f93407b97750 | 206 | } |
lakshya | 10:f93407b97750 | 207 | } |
lakshya | 10:f93407b97750 | 208 | return temp_max; |
lakshya | 10:f93407b97750 | 209 | } |
lakshya | 10:f93407b97750 | 210 | |
lakshya | 10:f93407b97750 | 211 | |
lakshya | 10:f93407b97750 | 212 | void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax) |
lakshya | 10:f93407b97750 | 213 | { |
lakshya | 10:f93407b97750 | 214 | float bb[3]={0,0,0}; |
lakshya | 10:f93407b97750 | 215 | float d[3]={0,0,0}; |
lakshya | 10:f93407b97750 | 216 | 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 | 217 | float den=0,den2; |
lakshya | 10:f93407b97750 | 218 | float bcopy[3]; |
lakshya | 10:f93407b97750 | 219 | int i, j;//temporary variables |
lakshya | 10:f93407b97750 | 220 | float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs |
lakshya | 10:f93407b97750 | 221 | float invJm[3][3]; |
lakshya | 10:f93407b97750 | 222 | 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 | 223 | int infflag; // Flag variable to check if the moment value is infinity or NaN |
lakshya | 10:f93407b97750 | 224 | |
lakshya | 10:f93407b97750 | 225 | if(controlmode1==0) |
lakshya | 10:f93407b97750 | 226 | { |
lakshya | 10:f93407b97750 | 227 | den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2])); |
lakshya | 10:f93407b97750 | 228 | den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]); |
lakshya | 10:f93407b97750 | 229 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 230 | { |
lakshya | 10:f93407b97750 | 231 | db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1) |
lakshya | 10:f93407b97750 | 232 | } |
lakshya | 10:f93407b97750 | 233 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 234 | { |
lakshya | 10:f93407b97750 | 235 | b[i]/=den; // Mormalized b. Hence no unit. |
lakshya | 10:f93407b97750 | 236 | } |
lakshya | 10:f93407b97750 | 237 | if(b[2]>0.9 || b[2]<-0.9) |
lakshya | 10:f93407b97750 | 238 | { |
lakshya | 10:f93407b97750 | 239 | kz=kz2; |
lakshya | 10:f93407b97750 | 240 | kmu=kmu2; |
lakshya | 10:f93407b97750 | 241 | gamma=gamma2; |
lakshya | 10:f93407b97750 | 242 | } |
lakshya | 10:f93407b97750 | 243 | for(i=0;i<2;i++) |
lakshya | 10:f93407b97750 | 244 | { |
lakshya | 10:f93407b97750 | 245 | Mu[i]=b[i]; |
lakshya | 10:f93407b97750 | 246 | v[i]=-kmu*Mu[i]; |
lakshya | 10:f93407b97750 | 247 | dv[i]=-kmu*db[i]; |
lakshya | 10:f93407b97750 | 248 | z[i]=db[i]-v[i]; |
lakshya | 10:f93407b97750 | 249 | u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma); |
lakshya | 10:f93407b97750 | 250 | } |
lakshya | 10:f93407b97750 | 251 | inverse(Jm,invJm); |
lakshya | 10:f93407b97750 | 252 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 253 | { |
lakshya | 10:f93407b97750 | 254 | for(j=0;j<3;j++) |
lakshya | 10:f93407b97750 | 255 | { |
lakshya | 10:f93407b97750 | 256 | 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 | 257 | } |
lakshya | 10:f93407b97750 | 258 | } |
lakshya | 10:f93407b97750 | 259 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 260 | { |
lakshya | 10:f93407b97750 | 261 | for(j=0;j<3;j++) |
lakshya | 10:f93407b97750 | 262 | { |
lakshya | 10:f93407b97750 | 263 | d[i]+=bb[j]*invJm[i][j]; |
lakshya | 10:f93407b97750 | 264 | } |
lakshya | 10:f93407b97750 | 265 | } |
lakshya | 10:f93407b97750 | 266 | bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]); |
lakshya | 10:f93407b97750 | 267 | bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]); |
lakshya | 10:f93407b97750 | 268 | bb[0]=0; |
lakshya | 10:f93407b97750 | 269 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 270 | { |
lakshya | 10:f93407b97750 | 271 | d[i]=invJm[2][i]; |
lakshya | 10:f93407b97750 | 272 | invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i]; |
lakshya | 10:f93407b97750 | 273 | invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i]; |
lakshya | 10:f93407b97750 | 274 | invJm[0][i]=b[i]; |
lakshya | 10:f93407b97750 | 275 | } |
lakshya | 10:f93407b97750 | 276 | inverse(invJm,Jm); |
lakshya | 10:f93407b97750 | 277 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 278 | { |
lakshya | 10:f93407b97750 | 279 | for(j=0;j<3;j++) |
lakshya | 10:f93407b97750 | 280 | { |
lakshya | 10:f93407b97750 | 281 | tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2 |
lakshya | 10:f93407b97750 | 282 | } |
lakshya | 10:f93407b97750 | 283 | } |
lakshya | 10:f93407b97750 | 284 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 285 | { |
lakshya | 10:f93407b97750 | 286 | bcopy[i]=b[i]*den; |
lakshya | 10:f93407b97750 | 287 | } |
lakshya | 10:f93407b97750 | 288 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 289 | { |
lakshya | 10:f93407b97750 | 290 | Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3]; |
lakshya | 10:f93407b97750 | 291 | Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2 |
lakshya | 10:f93407b97750 | 292 | } |
lakshya | 10:f93407b97750 | 293 | infflag=0; |
lakshya | 10:f93407b97750 | 294 | for (i=0; i<3 && infflag==0; i++) |
lakshya | 10:f93407b97750 | 295 | { |
lakshya | 10:f93407b97750 | 296 | if (isinf(Mmnt[i])==1 || isnan(Mmnt[i])==1) |
lakshya | 10:f93407b97750 | 297 | infflag=1; |
lakshya | 10:f93407b97750 | 298 | } |
lakshya | 10:f93407b97750 | 299 | if (infflag==1) |
lakshya | 10:f93407b97750 | 300 | { |
lakshya | 10:f93407b97750 | 301 | for (i=0; i<3; i++) |
lakshya | 10:f93407b97750 | 302 | Mmnt[i]=2*MmntMax; |
lakshya | 10:f93407b97750 | 303 | } |
lakshya | 10:f93407b97750 | 304 | |
lakshya | 10:f93407b97750 | 305 | } |
lakshya | 10:f93407b97750 | 306 | else if(controlmode1==1) |
lakshya | 10:f93407b97750 | 307 | { |
lakshya | 10:f93407b97750 | 308 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 309 | { |
lakshya | 10:f93407b97750 | 310 | 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 | 311 | } |
lakshya | 10:f93407b97750 | 312 | } |
lakshya | 10:f93407b97750 | 313 | for(i=0;i<3;i++) |
lakshya | 10:f93407b97750 | 314 | { |
lakshya | 10:f93407b97750 | 315 | moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2 |
lakshya | 10:f93407b97750 | 316 | } |
lakshya | 10:f93407b97750 | 317 | } |
sakthipriya | 0:7b4c00e3912f | 318 | |
sakthipriya | 0:7b4c00e3912f | 319 | I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge |
sakthipriya | 0:7b4c00e3912f | 320 | |
Bragadeesh153 | 16:cc77770d787f | 321 | int FCTN_ACS_INIT(); //initialization of registers happens |
Bragadeesh153 | 16:cc77770d787f | 322 | int SENSOR_INIT(); |
Bragadeesh153 | 16:cc77770d787f | 323 | int FCTN_ATS_DATA_ACQ(); //data is obtained |
Bragadeesh153 | 16:cc77770d787f | 324 | int SENSOR_DATA_ACQ(); |
sakthipriya | 0:7b4c00e3912f | 325 | void T_OUT(); //timeout function to stop infinite loop |
Bragadeesh153 | 16:cc77770d787f | 326 | |
Bragadeesh153 | 16:cc77770d787f | 327 | void CONFIG_UPLOAD(); |
sakthipriya | 0:7b4c00e3912f | 328 | Timeout to; //Timeout variable to |
sakthipriya | 0:7b4c00e3912f | 329 | int toFlag; |
sakthipriya | 0:7b4c00e3912f | 330 | |
sakthipriya | 0:7b4c00e3912f | 331 | int count =0; // Time for which the BAE uC is running (in seconds) |
sakthipriya | 0:7b4c00e3912f | 332 | void T_OUT() |
sakthipriya | 0:7b4c00e3912f | 333 | { |
sakthipriya | 0:7b4c00e3912f | 334 | toFlag=0; //as T_OUT function gets called the while loop gets terminated |
sakthipriya | 0:7b4c00e3912f | 335 | } |
sakthipriya | 0:7b4c00e3912f | 336 | |
sakthipriya | 0:7b4c00e3912f | 337 | |
sakthipriya | 0:7b4c00e3912f | 338 | //DEFINING VARIABLES |
sakthipriya | 0:7b4c00e3912f | 339 | char cmd[2]; |
sakthipriya | 0:7b4c00e3912f | 340 | char raw_gyro[6]; |
sakthipriya | 0:7b4c00e3912f | 341 | char raw_mag[6]; |
sakthipriya | 0:7b4c00e3912f | 342 | char store,status; |
sakthipriya | 0:7b4c00e3912f | 343 | int16_t bit_data; |
sakthipriya | 0:7b4c00e3912f | 344 | float gyro_data[3], mag_data[3],combined_values[6]; |
sakthipriya | 0:7b4c00e3912f | 345 | float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps |
sakthipriya | 0:7b4c00e3912f | 346 | float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla |
sakthipriya | 0:7b4c00e3912f | 347 | float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0}; |
sakthipriya | 0:7b4c00e3912f | 348 | |
Bragadeesh153 | 16:cc77770d787f | 349 | void CONFIG_UPLOAD() |
sakthipriya | 0:7b4c00e3912f | 350 | { |
sakthipriya | 0:7b4c00e3912f | 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 |
Bragadeesh153 | 16:cc77770d787f | 354 | wait_ms(600); |
Bragadeesh153 | 16:cc77770d787f | 355 | |
Bragadeesh153 | 16:cc77770d787f | 356 | //Verify magic number |
Bragadeesh153 | 16:cc77770d787f | 357 | |
Bragadeesh153 | 16:cc77770d787f | 358 | cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload |
Bragadeesh153 | 16:cc77770d787f | 359 | cmd[1]=BIT_HOST_UPLD_ENB; |
Bragadeesh153 | 16:cc77770d787f | 360 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 361 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 362 | |
Bragadeesh153 | 16:cc77770d787f | 363 | cmd[0]=UPLOAD_ADDR; //0x02 is written in HOST CONTROL register to enable upload |
Bragadeesh153 | 16:cc77770d787f | 364 | cmd[1]=0x0000; |
Bragadeesh153 | 16:cc77770d787f | 365 | i2c.write(SLAVE_ADDR,cmd,3); |
Bragadeesh153 | 16:cc77770d787f | 366 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 367 | |
Bragadeesh153 | 16:cc77770d787f | 368 | |
Bragadeesh153 | 16:cc77770d787f | 369 | |
Bragadeesh153 | 16:cc77770d787f | 370 | |
Bragadeesh153 | 16:cc77770d787f | 371 | cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload |
Bragadeesh153 | 16:cc77770d787f | 372 | cmd[1]=0x00; |
Bragadeesh153 | 16:cc77770d787f | 373 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 374 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 375 | |
Bragadeesh153 | 16:cc77770d787f | 376 | |
Bragadeesh153 | 16:cc77770d787f | 377 | |
Bragadeesh153 | 16:cc77770d787f | 378 | } |
Bragadeesh153 | 16:cc77770d787f | 379 | |
Bragadeesh153 | 16:cc77770d787f | 380 | int SENSOR_INIT() |
Bragadeesh153 | 16:cc77770d787f | 381 | { |
Bragadeesh153 | 16:cc77770d787f | 382 | |
Bragadeesh153 | 16:cc77770d787f | 383 | pc_acs.printf("Entered sensor init\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 384 | cmd[0]=RESETREQ; |
Bragadeesh153 | 16:cc77770d787f | 385 | cmd[1]=BIT_RESREQ; |
Bragadeesh153 | 16:cc77770d787f | 386 | i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up |
Bragadeesh153 | 16:cc77770d787f | 387 | wait_ms(800); //waiting for loading configuration file stored in EEPROM |
sakthipriya | 0:7b4c00e3912f | 388 | cmd[0]=SENTRALSTATUS; |
sakthipriya | 0:7b4c00e3912f | 389 | i2c.write(SLAVE_ADDR,cmd,1); |
sakthipriya | 0:7b4c00e3912f | 390 | i2c.read(SLAVE_ADDR_READ,&store,1); |
sakthipriya | 0:7b4c00e3912f | 391 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 392 | pc_acs.printf("Sentral Status is %x\n \r",(int)store); |
Bragadeesh153 | 16:cc77770d787f | 393 | |
sakthipriya | 0:7b4c00e3912f | 394 | //to check whether EEPROM is uploaded |
sakthipriya | 0:7b4c00e3912f | 395 | switch((int)store) { |
sakthipriya | 0:7b4c00e3912f | 396 | case(3): { |
Bragadeesh153 | 16:cc77770d787f | 397 | cmd[0]=RESETREQ; |
Bragadeesh153 | 16:cc77770d787f | 398 | cmd[1]=BIT_RESREQ; |
Bragadeesh153 | 16:cc77770d787f | 399 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 400 | wait_ms(600); |
sakthipriya | 0:7b4c00e3912f | 401 | break; |
sakthipriya | 0:7b4c00e3912f | 402 | } |
sakthipriya | 0:7b4c00e3912f | 403 | case(11): { |
sakthipriya | 0:7b4c00e3912f | 404 | break; |
sakthipriya | 0:7b4c00e3912f | 405 | } |
sakthipriya | 0:7b4c00e3912f | 406 | default: { |
sakthipriya | 0:7b4c00e3912f | 407 | cmd[0]=RESETREQ; |
sakthipriya | 0:7b4c00e3912f | 408 | cmd[1]=BIT_RESREQ; |
sakthipriya | 0:7b4c00e3912f | 409 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 410 | wait_ms(600); |
Bragadeesh153 | 16:cc77770d787f | 411 | |
sakthipriya | 0:7b4c00e3912f | 412 | } |
sakthipriya | 0:7b4c00e3912f | 413 | } |
Bragadeesh153 | 16:cc77770d787f | 414 | cmd[0]=SENTRALSTATUS; |
Bragadeesh153 | 16:cc77770d787f | 415 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 416 | i2c.read(SLAVE_ADDR_READ,&store,1); |
sakthipriya | 0:7b4c00e3912f | 417 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 418 | pc_acs.printf("Sentral Status is %x\n \r",(int)store); |
Bragadeesh153 | 16:cc77770d787f | 419 | |
Bragadeesh153 | 16:cc77770d787f | 420 | int manual=0; |
Bragadeesh153 | 16:cc77770d787f | 421 | if((int)store != 11) |
Bragadeesh153 | 16:cc77770d787f | 422 | { |
Bragadeesh153 | 16:cc77770d787f | 423 | |
Bragadeesh153 | 16:cc77770d787f | 424 | cmd[0]=RESETREQ; |
Bragadeesh153 | 16:cc77770d787f | 425 | cmd[1]=BIT_RESREQ; |
Bragadeesh153 | 16:cc77770d787f | 426 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 427 | wait_ms(600); |
Bragadeesh153 | 16:cc77770d787f | 428 | |
Bragadeesh153 | 16:cc77770d787f | 429 | //manually upload |
Bragadeesh153 | 16:cc77770d787f | 430 | |
Bragadeesh153 | 16:cc77770d787f | 431 | if(manual == 0) |
Bragadeesh153 | 16:cc77770d787f | 432 | { |
Bragadeesh153 | 16:cc77770d787f | 433 | return 0; |
Bragadeesh153 | 16:cc77770d787f | 434 | } |
Bragadeesh153 | 16:cc77770d787f | 435 | |
Bragadeesh153 | 16:cc77770d787f | 436 | } |
Bragadeesh153 | 16:cc77770d787f | 437 | |
Bragadeesh153 | 16:cc77770d787f | 438 | cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors |
Bragadeesh153 | 16:cc77770d787f | 439 | cmd[1]=BIT_RUN_ENB; |
Bragadeesh153 | 16:cc77770d787f | 440 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 441 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 442 | |
Bragadeesh153 | 16:cc77770d787f | 443 | cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer |
Bragadeesh153 | 16:cc77770d787f | 444 | cmd[1]=BIT_MAGODR; |
Bragadeesh153 | 16:cc77770d787f | 445 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 446 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 447 | cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope |
Bragadeesh153 | 16:cc77770d787f | 448 | cmd[1]=BIT_GYROODR; |
Bragadeesh153 | 16:cc77770d787f | 449 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 450 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 451 | cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer |
Bragadeesh153 | 16:cc77770d787f | 452 | cmd[1]=0x00; |
Bragadeesh153 | 16:cc77770d787f | 453 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 454 | |
Bragadeesh153 | 16:cc77770d787f | 455 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 456 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 457 | cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values |
Bragadeesh153 | 16:cc77770d787f | 458 | cmd[1]=0x00; |
Bragadeesh153 | 16:cc77770d787f | 459 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 460 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 461 | cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values |
Bragadeesh153 | 16:cc77770d787f | 462 | cmd[1]=BIT_EVT_ENB; |
Bragadeesh153 | 16:cc77770d787f | 463 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 464 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 465 | pc_acs.printf("Exited sensor init successfully\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 466 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 467 | |
Bragadeesh153 | 16:cc77770d787f | 468 | |
Bragadeesh153 | 16:cc77770d787f | 469 | } |
Bragadeesh153 | 16:cc77770d787f | 470 | int FCTN_ACS_INIT() |
Bragadeesh153 | 16:cc77770d787f | 471 | { |
Bragadeesh153 | 16:cc77770d787f | 472 | ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag |
Bragadeesh153 | 16:cc77770d787f | 473 | if( (ATS1_SW_ENABLE!= 0 )&&(ATS2_SW_ENABLE!= 0 ) ) |
Bragadeesh153 | 16:cc77770d787f | 474 | { |
Bragadeesh153 | 16:cc77770d787f | 475 | ATS2_SW_ENABLE = 1; |
Bragadeesh153 | 16:cc77770d787f | 476 | ATS1_SW_ENABLE = 0; |
Bragadeesh153 | 16:cc77770d787f | 477 | wait_ms(5); |
Bragadeesh153 | 16:cc77770d787f | 478 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; |
Bragadeesh153 | 16:cc77770d787f | 479 | } |
Bragadeesh153 | 16:cc77770d787f | 480 | |
Bragadeesh153 | 16:cc77770d787f | 481 | int working=0; |
Bragadeesh153 | 16:cc77770d787f | 482 | |
Bragadeesh153 | 16:cc77770d787f | 483 | pc_acs.printf("Attitude sensor init called \n \r"); |
Bragadeesh153 | 16:cc77770d787f | 484 | pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); |
Bragadeesh153 | 16:cc77770d787f | 485 | pc_acs.printf("ATS Status & 0xC0 is %x\n\n \r",(int)(ACS_ATS_STATUS&0xC0)); |
Bragadeesh153 | 16:cc77770d787f | 486 | pc_acs.printf("Sensor 1 condition is %d \n\n \r",(int)(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80))); |
Bragadeesh153 | 16:cc77770d787f | 487 | |
Bragadeesh153 | 16:cc77770d787f | 488 | |
Bragadeesh153 | 16:cc77770d787f | 489 | if(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) |
Bragadeesh153 | 16:cc77770d787f | 490 | { |
Bragadeesh153 | 16:cc77770d787f | 491 | |
Bragadeesh153 | 16:cc77770d787f | 492 | pc_acs.printf("Sensor 1 marked working \n \r"); |
Bragadeesh153 | 16:cc77770d787f | 493 | working = SENSOR_INIT(); |
Bragadeesh153 | 16:cc77770d787f | 494 | if(working ==1) |
Bragadeesh153 | 16:cc77770d787f | 495 | { |
Bragadeesh153 | 16:cc77770d787f | 496 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; |
Bragadeesh153 | 16:cc77770d787f | 497 | pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); |
Bragadeesh153 | 16:cc77770d787f | 498 | pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 499 | ACS_INIT_STATUS = 0; |
Bragadeesh153 | 16:cc77770d787f | 500 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 501 | } |
Bragadeesh153 | 16:cc77770d787f | 502 | |
Bragadeesh153 | 16:cc77770d787f | 503 | |
Bragadeesh153 | 16:cc77770d787f | 504 | |
Bragadeesh153 | 16:cc77770d787f | 505 | pc_acs.printf("Sensor 1 not working.Powering off.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 506 | ATS1_SW_ENABLE = 1; |
Bragadeesh153 | 16:cc77770d787f | 507 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0; |
Bragadeesh153 | 16:cc77770d787f | 508 | |
Bragadeesh153 | 16:cc77770d787f | 509 | } |
Bragadeesh153 | 16:cc77770d787f | 510 | |
Bragadeesh153 | 16:cc77770d787f | 511 | pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 512 | |
Bragadeesh153 | 16:cc77770d787f | 513 | if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) |
Bragadeesh153 | 16:cc77770d787f | 514 | { |
Bragadeesh153 | 16:cc77770d787f | 515 | |
Bragadeesh153 | 16:cc77770d787f | 516 | |
Bragadeesh153 | 16:cc77770d787f | 517 | ATS2_SW_ENABLE = 0; |
Bragadeesh153 | 16:cc77770d787f | 518 | wait_ms(5); |
Bragadeesh153 | 16:cc77770d787f | 519 | working = SENSOR_INIT(); |
Bragadeesh153 | 16:cc77770d787f | 520 | if(working ==1) |
Bragadeesh153 | 16:cc77770d787f | 521 | { |
Bragadeesh153 | 16:cc77770d787f | 522 | pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); |
Bragadeesh153 | 16:cc77770d787f | 523 | pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 524 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; |
Bragadeesh153 | 16:cc77770d787f | 525 | ACS_INIT_STATUS = 0; |
Bragadeesh153 | 16:cc77770d787f | 526 | return 2; |
Bragadeesh153 | 16:cc77770d787f | 527 | } |
Bragadeesh153 | 16:cc77770d787f | 528 | |
Bragadeesh153 | 16:cc77770d787f | 529 | |
Bragadeesh153 | 16:cc77770d787f | 530 | } |
Bragadeesh153 | 16:cc77770d787f | 531 | |
Bragadeesh153 | 16:cc77770d787f | 532 | pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); |
Bragadeesh153 | 16:cc77770d787f | 533 | pc_acs.printf("Sensor 2 also not working.Exit init.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 534 | |
Bragadeesh153 | 16:cc77770d787f | 535 | |
Bragadeesh153 | 16:cc77770d787f | 536 | |
Bragadeesh153 | 16:cc77770d787f | 537 | ATS2_SW_ENABLE = 1; |
Bragadeesh153 | 16:cc77770d787f | 538 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E; |
Bragadeesh153 | 16:cc77770d787f | 539 | ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag |
Bragadeesh153 | 16:cc77770d787f | 540 | return 0; |
sakthipriya | 0:7b4c00e3912f | 541 | } |
sakthipriya | 0:7b4c00e3912f | 542 | |
Bragadeesh153 | 16:cc77770d787f | 543 | |
Bragadeesh153 | 16:cc77770d787f | 544 | int SENSOR_DATA_ACQ() |
sakthipriya | 0:7b4c00e3912f | 545 | { |
Bragadeesh153 | 16:cc77770d787f | 546 | int mag_only=0; |
Bragadeesh153 | 16:cc77770d787f | 547 | pc_acs.printf("Entering Sensor data acq.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 548 | char reg; |
Bragadeesh153 | 16:cc77770d787f | 549 | |
Bragadeesh153 | 16:cc77770d787f | 550 | //int sentral; |
Bragadeesh153 | 16:cc77770d787f | 551 | int event; |
Bragadeesh153 | 16:cc77770d787f | 552 | int sensor; |
Bragadeesh153 | 16:cc77770d787f | 553 | int error; |
Bragadeesh153 | 16:cc77770d787f | 554 | |
Bragadeesh153 | 16:cc77770d787f | 555 | int init; |
sakthipriya | 0:7b4c00e3912f | 556 | cmd[0]=EVT_STATUS; |
sakthipriya | 0:7b4c00e3912f | 557 | i2c.write(SLAVE_ADDR,cmd,1); |
sakthipriya | 0:7b4c00e3912f | 558 | i2c.read(SLAVE_ADDR_READ,&status,1); |
sakthipriya | 0:7b4c00e3912f | 559 | wait_ms(100); |
sakthipriya | 0:7b4c00e3912f | 560 | pc_acs.printf("Event Status is %x\n \r",(int)status); |
Bragadeesh153 | 16:cc77770d787f | 561 | event = (int)status; |
Bragadeesh153 | 16:cc77770d787f | 562 | |
Bragadeesh153 | 16:cc77770d787f | 563 | cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x |
Bragadeesh153 | 16:cc77770d787f | 564 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 565 | i2c.read(SLAVE_ADDR_READ,raw_gyro,6); |
Bragadeesh153 | 16:cc77770d787f | 566 | cmd[0]=MAG_XOUT_H; //LSB of x |
Bragadeesh153 | 16:cc77770d787f | 567 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 568 | i2c.read(SLAVE_ADDR_READ,raw_mag,6); |
Bragadeesh153 | 16:cc77770d787f | 569 | // pc_acs.printf("\nGyro Values:\n"); |
Bragadeesh153 | 16:cc77770d787f | 570 | for(int i=0; i<3; i++) { |
Bragadeesh153 | 16:cc77770d787f | 571 | //concatenating gyro LSB and MSB to get 16 bit signed data values |
Bragadeesh153 | 16:cc77770d787f | 572 | bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; |
Bragadeesh153 | 16:cc77770d787f | 573 | gyro_data[i]=(float)bit_data; |
Bragadeesh153 | 16:cc77770d787f | 574 | gyro_data[i]=gyro_data[i]/senstivity_gyro; |
Bragadeesh153 | 16:cc77770d787f | 575 | gyro_data[i]+=gyro_error[i]; |
Bragadeesh153 | 16:cc77770d787f | 576 | // pc_acs.printf("%f\t",gyro_data[i]); |
Bragadeesh153 | 16:cc77770d787f | 577 | } |
Bragadeesh153 | 16:cc77770d787f | 578 | // pc_acs.printf("\nMag Values:\n"); |
Bragadeesh153 | 16:cc77770d787f | 579 | for(int i=0; i<3; i++) { |
Bragadeesh153 | 16:cc77770d787f | 580 | //concatenating mag LSB and MSB to get 16 bit signed data values |
Bragadeesh153 | 16:cc77770d787f | 581 | bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i]; |
Bragadeesh153 | 16:cc77770d787f | 582 | mag_data[i]=(float)bit_data; |
Bragadeesh153 | 16:cc77770d787f | 583 | mag_data[i]=mag_data[i]/senstivity_mag; |
Bragadeesh153 | 16:cc77770d787f | 584 | mag_data[i]+=mag_error[i]; |
Bragadeesh153 | 16:cc77770d787f | 585 | //pc_acs.printf("%f\t",mag_data[i]); |
Bragadeesh153 | 16:cc77770d787f | 586 | } |
Bragadeesh153 | 16:cc77770d787f | 587 | for(int i=0; i<3; i++) { |
Bragadeesh153 | 16:cc77770d787f | 588 | // data[i]=gyro_data[i]; |
Bragadeesh153 | 16:cc77770d787f | 589 | actual_data.AngularSpeed_actual[i] = gyro_data[i]; |
Bragadeesh153 | 16:cc77770d787f | 590 | actual_data.Bvalue_actual[i] = mag_data[i]; |
Bragadeesh153 | 16:cc77770d787f | 591 | //data[i+3]=mag_data[i]; |
Bragadeesh153 | 16:cc77770d787f | 592 | } |
Bragadeesh153 | 16:cc77770d787f | 593 | |
Bragadeesh153 | 16:cc77770d787f | 594 | |
Bragadeesh153 | 16:cc77770d787f | 595 | |
Bragadeesh153 | 16:cc77770d787f | 596 | //(event & 40 != 40 ) || (event & 08 != 08 ) || (event & 01 == 01 )|| (event & 02 == 02 ) |
Bragadeesh153 | 16:cc77770d787f | 597 | |
Bragadeesh153 | 16:cc77770d787f | 598 | if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )) //check for any error |
Bragadeesh153 | 16:cc77770d787f | 599 | { |
Bragadeesh153 | 16:cc77770d787f | 600 | cmd[0]=RESETREQ; |
Bragadeesh153 | 16:cc77770d787f | 601 | cmd[1]=BIT_RESREQ; |
Bragadeesh153 | 16:cc77770d787f | 602 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 603 | wait_ms(600); |
Bragadeesh153 | 16:cc77770d787f | 604 | |
Bragadeesh153 | 16:cc77770d787f | 605 | |
Bragadeesh153 | 16:cc77770d787f | 606 | |
Bragadeesh153 | 16:cc77770d787f | 607 | cmd[0]=EVT_STATUS; |
Bragadeesh153 | 16:cc77770d787f | 608 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 609 | i2c.read(SLAVE_ADDR_READ,&status,1); |
Bragadeesh153 | 16:cc77770d787f | 610 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 611 | pc_acs.printf("Event Status after resetting is %x\n \r",(int)status); |
Bragadeesh153 | 16:cc77770d787f | 612 | |
Bragadeesh153 | 16:cc77770d787f | 613 | if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )) |
Bragadeesh153 | 16:cc77770d787f | 614 | { |
Bragadeesh153 | 16:cc77770d787f | 615 | |
Bragadeesh153 | 16:cc77770d787f | 616 | |
Bragadeesh153 | 16:cc77770d787f | 617 | |
Bragadeesh153 | 16:cc77770d787f | 618 | // cmd[0]=SENTRALSTATUS; |
Bragadeesh153 | 16:cc77770d787f | 619 | // i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 620 | /// i2c.read(SLAVE_ADDR_READ,®,1); |
Bragadeesh153 | 16:cc77770d787f | 621 | // wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 622 | // sentral = (int)reg; |
Bragadeesh153 | 16:cc77770d787f | 623 | |
Bragadeesh153 | 16:cc77770d787f | 624 | cmd[0]=SENSORSTATUS; |
Bragadeesh153 | 16:cc77770d787f | 625 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 626 | i2c.read(SLAVE_ADDR_READ,®,1); |
Bragadeesh153 | 16:cc77770d787f | 627 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 628 | |
Bragadeesh153 | 16:cc77770d787f | 629 | sensor = (int)reg; |
Bragadeesh153 | 16:cc77770d787f | 630 | |
Bragadeesh153 | 16:cc77770d787f | 631 | cmd[0]=ERROR; |
Bragadeesh153 | 16:cc77770d787f | 632 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 633 | i2c.read(SLAVE_ADDR_READ,®,1); |
Bragadeesh153 | 16:cc77770d787f | 634 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 635 | |
Bragadeesh153 | 16:cc77770d787f | 636 | error = (int)reg; |
Bragadeesh153 | 16:cc77770d787f | 637 | |
Bragadeesh153 | 16:cc77770d787f | 638 | if( error&128 == 128) |
Bragadeesh153 | 16:cc77770d787f | 639 | { |
Bragadeesh153 | 16:cc77770d787f | 640 | cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer |
Bragadeesh153 | 16:cc77770d787f | 641 | cmd[1]=BIT_MAGODR; |
Bragadeesh153 | 16:cc77770d787f | 642 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 643 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 644 | cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope |
Bragadeesh153 | 16:cc77770d787f | 645 | cmd[1]=BIT_GYROODR; |
Bragadeesh153 | 16:cc77770d787f | 646 | i2c.write(SLAVE_ADDR,cmd,2); |
Bragadeesh153 | 16:cc77770d787f | 647 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 648 | cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer |
Bragadeesh153 | 16:cc77770d787f | 649 | cmd[1]=0x00; |
Bragadeesh153 | 16:cc77770d787f | 650 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 651 | cmd[0]=ERROR; |
Bragadeesh153 | 16:cc77770d787f | 652 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 653 | i2c.read(SLAVE_ADDR_READ,®,1); |
Bragadeesh153 | 16:cc77770d787f | 654 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 655 | |
Bragadeesh153 | 16:cc77770d787f | 656 | error = (int)reg; |
Bragadeesh153 | 16:cc77770d787f | 657 | |
Bragadeesh153 | 16:cc77770d787f | 658 | if( error&128 == 128) |
Bragadeesh153 | 16:cc77770d787f | 659 | { |
Bragadeesh153 | 16:cc77770d787f | 660 | pc_acs.printf("Rate error.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 661 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 662 | } |
Bragadeesh153 | 16:cc77770d787f | 663 | |
Bragadeesh153 | 16:cc77770d787f | 664 | |
Bragadeesh153 | 16:cc77770d787f | 665 | } |
Bragadeesh153 | 16:cc77770d787f | 666 | |
Bragadeesh153 | 16:cc77770d787f | 667 | |
Bragadeesh153 | 16:cc77770d787f | 668 | if((error&16 == 16) || (error&32 == 32) || (sensor!=0)) |
Bragadeesh153 | 16:cc77770d787f | 669 | { |
Bragadeesh153 | 16:cc77770d787f | 670 | if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) ) |
Bragadeesh153 | 16:cc77770d787f | 671 | { |
Bragadeesh153 | 16:cc77770d787f | 672 | |
Bragadeesh153 | 16:cc77770d787f | 673 | if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) ) |
Bragadeesh153 | 16:cc77770d787f | 674 | { |
Bragadeesh153 | 16:cc77770d787f | 675 | pc_acs.printf("error in both sensors.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 676 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 677 | } |
Bragadeesh153 | 16:cc77770d787f | 678 | pc_acs.printf("error in gyro alone.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 679 | return 2; |
Bragadeesh153 | 16:cc77770d787f | 680 | } |
Bragadeesh153 | 16:cc77770d787f | 681 | pc_acs.printf("error in something else.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 682 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 683 | } |
Bragadeesh153 | 16:cc77770d787f | 684 | |
Bragadeesh153 | 16:cc77770d787f | 685 | if(((int)status & 1 == 1 )) |
Bragadeesh153 | 16:cc77770d787f | 686 | { |
Bragadeesh153 | 16:cc77770d787f | 687 | pc_acs.printf("error in CPU Reset.calling init.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 688 | init = SENSOR_INIT(); |
Bragadeesh153 | 16:cc77770d787f | 689 | if(init == 0) |
Bragadeesh153 | 16:cc77770d787f | 690 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 691 | |
Bragadeesh153 | 16:cc77770d787f | 692 | cmd[0]=EVT_STATUS; |
Bragadeesh153 | 16:cc77770d787f | 693 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 694 | i2c.read(SLAVE_ADDR_READ,&status,1); |
Bragadeesh153 | 16:cc77770d787f | 695 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 696 | if(((int)status & 1 == 1 )) |
Bragadeesh153 | 16:cc77770d787f | 697 | { |
Bragadeesh153 | 16:cc77770d787f | 698 | pc_acs.printf("Still error in CPU Reset.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 699 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 700 | } |
Bragadeesh153 | 16:cc77770d787f | 701 | pc_acs.printf("error in CPU Reset cleared.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 702 | |
Bragadeesh153 | 16:cc77770d787f | 703 | } |
Bragadeesh153 | 16:cc77770d787f | 704 | |
Bragadeesh153 | 16:cc77770d787f | 705 | if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 ))) |
Bragadeesh153 | 16:cc77770d787f | 706 | { |
Bragadeesh153 | 16:cc77770d787f | 707 | pc_acs.printf("Data not ready waiting...\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 708 | //POLL |
Bragadeesh153 | 16:cc77770d787f | 709 | wait_ms(1500); |
Bragadeesh153 | 16:cc77770d787f | 710 | cmd[0]=EVT_STATUS; |
Bragadeesh153 | 16:cc77770d787f | 711 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 712 | i2c.read(SLAVE_ADDR_READ,&status,1); |
Bragadeesh153 | 16:cc77770d787f | 713 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 714 | if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 ))) |
Bragadeesh153 | 16:cc77770d787f | 715 | { |
Bragadeesh153 | 16:cc77770d787f | 716 | |
Bragadeesh153 | 16:cc77770d787f | 717 | if((int)status & 32 != 32 ) |
Bragadeesh153 | 16:cc77770d787f | 718 | { |
Bragadeesh153 | 16:cc77770d787f | 719 | if((int)status & 8 != 8 ) |
Bragadeesh153 | 16:cc77770d787f | 720 | { |
Bragadeesh153 | 16:cc77770d787f | 721 | pc_acs.printf("Both data still not ready.Exiting..\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 722 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 723 | } |
Bragadeesh153 | 16:cc77770d787f | 724 | pc_acs.printf("Mag data only ready.Read..\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 725 | mag_only = 1; |
Bragadeesh153 | 16:cc77770d787f | 726 | //return 2; |
Bragadeesh153 | 16:cc77770d787f | 727 | |
Bragadeesh153 | 16:cc77770d787f | 728 | } |
Bragadeesh153 | 16:cc77770d787f | 729 | |
Bragadeesh153 | 16:cc77770d787f | 730 | |
Bragadeesh153 | 16:cc77770d787f | 731 | } |
Bragadeesh153 | 16:cc77770d787f | 732 | |
Bragadeesh153 | 16:cc77770d787f | 733 | |
Bragadeesh153 | 16:cc77770d787f | 734 | } |
Bragadeesh153 | 16:cc77770d787f | 735 | |
Bragadeesh153 | 16:cc77770d787f | 736 | |
Bragadeesh153 | 16:cc77770d787f | 737 | } |
Bragadeesh153 | 16:cc77770d787f | 738 | |
Bragadeesh153 | 16:cc77770d787f | 739 | |
Bragadeesh153 | 16:cc77770d787f | 740 | |
Bragadeesh153 | 16:cc77770d787f | 741 | |
Bragadeesh153 | 16:cc77770d787f | 742 | |
Bragadeesh153 | 16:cc77770d787f | 743 | } |
Bragadeesh153 | 16:cc77770d787f | 744 | |
Bragadeesh153 | 16:cc77770d787f | 745 | cmd[0]=EVT_STATUS; |
Bragadeesh153 | 16:cc77770d787f | 746 | i2c.write(SLAVE_ADDR,cmd,1); |
Bragadeesh153 | 16:cc77770d787f | 747 | i2c.read(SLAVE_ADDR_READ,&status,1); |
Bragadeesh153 | 16:cc77770d787f | 748 | wait_ms(100); |
Bragadeesh153 | 16:cc77770d787f | 749 | pc_acs.printf("Event Status is %x\n \r",(int)status); |
Bragadeesh153 | 16:cc77770d787f | 750 | |
sakthipriya | 0:7b4c00e3912f | 751 | //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take |
Bragadeesh153 | 16:cc77770d787f | 752 | |
sakthipriya | 0:7b4c00e3912f | 753 | cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x |
sakthipriya | 0:7b4c00e3912f | 754 | i2c.write(SLAVE_ADDR,cmd,1); |
sakthipriya | 0:7b4c00e3912f | 755 | i2c.read(SLAVE_ADDR_READ,raw_gyro,6); |
sakthipriya | 0:7b4c00e3912f | 756 | cmd[0]=MAG_XOUT_H; //LSB of x |
sakthipriya | 0:7b4c00e3912f | 757 | i2c.write(SLAVE_ADDR,cmd,1); |
sakthipriya | 0:7b4c00e3912f | 758 | i2c.read(SLAVE_ADDR_READ,raw_mag,6); |
sakthipriya | 0:7b4c00e3912f | 759 | // pc_acs.printf("\nGyro Values:\n"); |
sakthipriya | 0:7b4c00e3912f | 760 | for(int i=0; i<3; i++) { |
sakthipriya | 0:7b4c00e3912f | 761 | //concatenating gyro LSB and MSB to get 16 bit signed data values |
sakthipriya | 0:7b4c00e3912f | 762 | bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; |
sakthipriya | 0:7b4c00e3912f | 763 | gyro_data[i]=(float)bit_data; |
sakthipriya | 0:7b4c00e3912f | 764 | gyro_data[i]=gyro_data[i]/senstivity_gyro; |
sakthipriya | 0:7b4c00e3912f | 765 | gyro_data[i]+=gyro_error[i]; |
lakshya | 15:e09aaaccf134 | 766 | // pc_acs.printf("%f\t",gyro_data[i]); |
sakthipriya | 0:7b4c00e3912f | 767 | } |
sakthipriya | 0:7b4c00e3912f | 768 | // pc_acs.printf("\nMag Values:\n"); |
sakthipriya | 0:7b4c00e3912f | 769 | for(int i=0; i<3; i++) { |
sakthipriya | 0:7b4c00e3912f | 770 | //concatenating mag LSB and MSB to get 16 bit signed data values |
sakthipriya | 0:7b4c00e3912f | 771 | bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i]; |
sakthipriya | 0:7b4c00e3912f | 772 | mag_data[i]=(float)bit_data; |
sakthipriya | 0:7b4c00e3912f | 773 | mag_data[i]=mag_data[i]/senstivity_mag; |
sakthipriya | 0:7b4c00e3912f | 774 | mag_data[i]+=mag_error[i]; |
lakshya | 15:e09aaaccf134 | 775 | //pc_acs.printf("%f\t",mag_data[i]); |
sakthipriya | 0:7b4c00e3912f | 776 | } |
sakthipriya | 0:7b4c00e3912f | 777 | for(int i=0; i<3; i++) { |
sakthipriya | 6:036d08b62785 | 778 | // data[i]=gyro_data[i]; |
sakthipriya | 6:036d08b62785 | 779 | actual_data.AngularSpeed_actual[i] = gyro_data[i]; |
sakthipriya | 6:036d08b62785 | 780 | actual_data.Bvalue_actual[i] = mag_data[i]; |
sakthipriya | 6:036d08b62785 | 781 | //data[i+3]=mag_data[i]; |
sakthipriya | 0:7b4c00e3912f | 782 | } |
Bragadeesh153 | 16:cc77770d787f | 783 | |
Bragadeesh153 | 16:cc77770d787f | 784 | if(mag_only == 0) |
Bragadeesh153 | 16:cc77770d787f | 785 | { |
Bragadeesh153 | 16:cc77770d787f | 786 | |
Bragadeesh153 | 16:cc77770d787f | 787 | pc_acs.printf("Reading data successful.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 788 | return 0; |
Bragadeesh153 | 16:cc77770d787f | 789 | } |
Bragadeesh153 | 16:cc77770d787f | 790 | else if(mag_only == 1) |
Bragadeesh153 | 16:cc77770d787f | 791 | { |
Bragadeesh153 | 16:cc77770d787f | 792 | pc_acs.printf("Reading data partial success.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 793 | return 2; |
sakthipriya | 0:7b4c00e3912f | 794 | } |
Bragadeesh153 | 16:cc77770d787f | 795 | |
Bragadeesh153 | 16:cc77770d787f | 796 | pc_acs.printf("Reading data success.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 797 | return 0; |
Bragadeesh153 | 16:cc77770d787f | 798 | |
Bragadeesh153 | 16:cc77770d787f | 799 | } |
Bragadeesh153 | 16:cc77770d787f | 800 | |
Bragadeesh153 | 16:cc77770d787f | 801 | |
Bragadeesh153 | 16:cc77770d787f | 802 | |
Bragadeesh153 | 16:cc77770d787f | 803 | int FCTN_ATS_DATA_ACQ() |
Bragadeesh153 | 16:cc77770d787f | 804 | { |
Bragadeesh153 | 16:cc77770d787f | 805 | |
Bragadeesh153 | 16:cc77770d787f | 806 | int acq; |
Bragadeesh153 | 16:cc77770d787f | 807 | |
Bragadeesh153 | 16:cc77770d787f | 808 | pc_acs.printf("DATA_ACQ called \n \r"); |
Bragadeesh153 | 16:cc77770d787f | 809 | pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); |
Bragadeesh153 | 16:cc77770d787f | 810 | |
Bragadeesh153 | 16:cc77770d787f | 811 | // 0 success //1 full failure //2 partial failure |
Bragadeesh153 | 16:cc77770d787f | 812 | |
Bragadeesh153 | 16:cc77770d787f | 813 | |
Bragadeesh153 | 16:cc77770d787f | 814 | |
Bragadeesh153 | 16:cc77770d787f | 815 | if(( (ACS_ATS_STATUS & 0xC0) == 0x40)) |
Bragadeesh153 | 16:cc77770d787f | 816 | { |
Bragadeesh153 | 16:cc77770d787f | 817 | |
Bragadeesh153 | 16:cc77770d787f | 818 | acq = SENSOR_DATA_ACQ(); |
Bragadeesh153 | 16:cc77770d787f | 819 | if(acq == 0) |
Bragadeesh153 | 16:cc77770d787f | 820 | { |
Bragadeesh153 | 16:cc77770d787f | 821 | |
Bragadeesh153 | 16:cc77770d787f | 822 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; |
Bragadeesh153 | 16:cc77770d787f | 823 | |
Bragadeesh153 | 16:cc77770d787f | 824 | ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 |
Bragadeesh153 | 16:cc77770d787f | 825 | pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); |
Bragadeesh153 | 16:cc77770d787f | 826 | pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 827 | return 0; |
Bragadeesh153 | 16:cc77770d787f | 828 | } |
Bragadeesh153 | 16:cc77770d787f | 829 | else if(acq == 2) |
Bragadeesh153 | 16:cc77770d787f | 830 | { |
Bragadeesh153 | 16:cc77770d787f | 831 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40; |
Bragadeesh153 | 16:cc77770d787f | 832 | pc_acs.printf(" Sensor 1 data partial success.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 833 | return 2; |
Bragadeesh153 | 16:cc77770d787f | 834 | |
Bragadeesh153 | 16:cc77770d787f | 835 | /*if((ACS_ATS_STATUS & 0x0F == 0x00)) |
Bragadeesh153 | 16:cc77770d787f | 836 | { |
Bragadeesh153 | 16:cc77770d787f | 837 | pc_acs.printf(" Sensor 1 data acq partial success.Trying Sensor 2\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 838 | ATS1_SW_ENABLE = 1; |
Bragadeesh153 | 16:cc77770d787f | 839 | ATS2_SW_ENABLE = 0; |
Bragadeesh153 | 16:cc77770d787f | 840 | wait_ms(5); |
Bragadeesh153 | 16:cc77770d787f | 841 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20; |
Bragadeesh153 | 16:cc77770d787f | 842 | |
Bragadeesh153 | 16:cc77770d787f | 843 | int acq; |
Bragadeesh153 | 16:cc77770d787f | 844 | acq = SENSOR_DATA_ACQ(); |
Bragadeesh153 | 16:cc77770d787f | 845 | |
Bragadeesh153 | 16:cc77770d787f | 846 | if(acq == 0) |
Bragadeesh153 | 16:cc77770d787f | 847 | { |
Bragadeesh153 | 16:cc77770d787f | 848 | ACS_DATA_ACQ_STATUS = 0; |
Bragadeesh153 | 16:cc77770d787f | 849 | pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 850 | return 0; |
Bragadeesh153 | 16:cc77770d787f | 851 | |
Bragadeesh153 | 16:cc77770d787f | 852 | } |
Bragadeesh153 | 16:cc77770d787f | 853 | else if(acq == 2) |
Bragadeesh153 | 16:cc77770d787f | 854 | { |
Bragadeesh153 | 16:cc77770d787f | 855 | ACS_DATA_ACQ_STATUS = 2; |
Bragadeesh153 | 16:cc77770d787f | 856 | pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 857 | return 2; |
Bragadeesh153 | 16:cc77770d787f | 858 | } |
Bragadeesh153 | 16:cc77770d787f | 859 | |
Bragadeesh153 | 16:cc77770d787f | 860 | else if(acq == 1) |
Bragadeesh153 | 16:cc77770d787f | 861 | { |
Bragadeesh153 | 16:cc77770d787f | 862 | |
Bragadeesh153 | 16:cc77770d787f | 863 | int acq; |
Bragadeesh153 | 16:cc77770d787f | 864 | pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 865 | ATS2_SW_ENABLE = 1; |
Bragadeesh153 | 16:cc77770d787f | 866 | ATS1_SW_ENABLE = 0; |
Bragadeesh153 | 16:cc77770d787f | 867 | wait_ms(5); |
Bragadeesh153 | 16:cc77770d787f | 868 | acq = SENSOR_DATA_ACQ(); |
Bragadeesh153 | 16:cc77770d787f | 869 | if(acq == 0) |
Bragadeesh153 | 16:cc77770d787f | 870 | { |
Bragadeesh153 | 16:cc77770d787f | 871 | pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 872 | ACS_DATA_ACQ_STATUS = 0; |
Bragadeesh153 | 16:cc77770d787f | 873 | return 0; |
Bragadeesh153 | 16:cc77770d787f | 874 | } |
Bragadeesh153 | 16:cc77770d787f | 875 | else if(acq == 2) |
Bragadeesh153 | 16:cc77770d787f | 876 | { |
Bragadeesh153 | 16:cc77770d787f | 877 | pc_acs.printf(" Sensor 1 data acq partial success.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 878 | ACS_DATA_ACQ_STATUS = 2; |
Bragadeesh153 | 16:cc77770d787f | 879 | return 2; |
Bragadeesh153 | 16:cc77770d787f | 880 | } |
Bragadeesh153 | 16:cc77770d787f | 881 | else |
Bragadeesh153 | 16:cc77770d787f | 882 | { |
Bragadeesh153 | 16:cc77770d787f | 883 | pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 884 | ATS1_SW_ENABLE = 0; |
Bragadeesh153 | 16:cc77770d787f | 885 | ACS_DATA_ACQ_STATUS = 1; |
Bragadeesh153 | 16:cc77770d787f | 886 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 887 | } |
Bragadeesh153 | 16:cc77770d787f | 888 | |
Bragadeesh153 | 16:cc77770d787f | 889 | pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 890 | ATS1_SW_ENABLE = 0; |
Bragadeesh153 | 16:cc77770d787f | 891 | ACS_DATA_ACQ_STATUS = 1; |
Bragadeesh153 | 16:cc77770d787f | 892 | return 1; |
Bragadeesh153 | 16:cc77770d787f | 893 | |
Bragadeesh153 | 16:cc77770d787f | 894 | } |
Bragadeesh153 | 16:cc77770d787f | 895 | |
Bragadeesh153 | 16:cc77770d787f | 896 | } |
Bragadeesh153 | 16:cc77770d787f | 897 | |
Bragadeesh153 | 16:cc77770d787f | 898 | else |
Bragadeesh153 | 16:cc77770d787f | 899 | { |
Bragadeesh153 | 16:cc77770d787f | 900 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40; |
Bragadeesh153 | 16:cc77770d787f | 901 | pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 902 | return 2; |
Bragadeesh153 | 16:cc77770d787f | 903 | |
Bragadeesh153 | 16:cc77770d787f | 904 | |
Bragadeesh153 | 16:cc77770d787f | 905 | }*/ |
Bragadeesh153 | 16:cc77770d787f | 906 | |
Bragadeesh153 | 16:cc77770d787f | 907 | } |
Bragadeesh153 | 16:cc77770d787f | 908 | |
Bragadeesh153 | 16:cc77770d787f | 909 | else if(acq == 1) |
Bragadeesh153 | 16:cc77770d787f | 910 | { |
Bragadeesh153 | 16:cc77770d787f | 911 | pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 912 | ATS1_SW_ENABLE = 1; |
Bragadeesh153 | 16:cc77770d787f | 913 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0; |
Bragadeesh153 | 16:cc77770d787f | 914 | } |
Bragadeesh153 | 16:cc77770d787f | 915 | |
Bragadeesh153 | 16:cc77770d787f | 916 | |
Bragadeesh153 | 16:cc77770d787f | 917 | |
sakthipriya | 0:7b4c00e3912f | 918 | } |
Bragadeesh153 | 16:cc77770d787f | 919 | |
Bragadeesh153 | 16:cc77770d787f | 920 | |
Bragadeesh153 | 16:cc77770d787f | 921 | |
Bragadeesh153 | 16:cc77770d787f | 922 | |
Bragadeesh153 | 16:cc77770d787f | 923 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0; |
Bragadeesh153 | 16:cc77770d787f | 924 | |
Bragadeesh153 | 16:cc77770d787f | 925 | |
Bragadeesh153 | 16:cc77770d787f | 926 | if(( (ACS_ATS_STATUS & 0x0C) == 0x04)) |
sakthipriya | 0:7b4c00e3912f | 927 | { |
Bragadeesh153 | 16:cc77770d787f | 928 | ATS2_SW_ENABLE = 0; |
Bragadeesh153 | 16:cc77770d787f | 929 | wait_ms(5); |
Bragadeesh153 | 16:cc77770d787f | 930 | |
Bragadeesh153 | 16:cc77770d787f | 931 | acq = SENSOR_DATA_ACQ(); |
Bragadeesh153 | 16:cc77770d787f | 932 | if(acq == 0) |
Bragadeesh153 | 16:cc77770d787f | 933 | { |
Bragadeesh153 | 16:cc77770d787f | 934 | pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 935 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; |
Bragadeesh153 | 16:cc77770d787f | 936 | ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 |
Bragadeesh153 | 16:cc77770d787f | 937 | return 0; |
Bragadeesh153 | 16:cc77770d787f | 938 | } |
Bragadeesh153 | 16:cc77770d787f | 939 | else if(acq == 2) |
Bragadeesh153 | 16:cc77770d787f | 940 | { |
Bragadeesh153 | 16:cc77770d787f | 941 | |
Bragadeesh153 | 16:cc77770d787f | 942 | pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 943 | |
Bragadeesh153 | 16:cc77770d787f | 944 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x04; |
Bragadeesh153 | 16:cc77770d787f | 945 | ACS_DATA_ACQ_STATUS = 2; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 |
Bragadeesh153 | 16:cc77770d787f | 946 | return 2; |
Bragadeesh153 | 16:cc77770d787f | 947 | |
Bragadeesh153 | 16:cc77770d787f | 948 | } |
Bragadeesh153 | 16:cc77770d787f | 949 | |
Bragadeesh153 | 16:cc77770d787f | 950 | else if(acq == 1) |
Bragadeesh153 | 16:cc77770d787f | 951 | { |
Bragadeesh153 | 16:cc77770d787f | 952 | pc_acs.printf(" Sensor 2 data acq failure.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 953 | ATS2_SW_ENABLE = 1; |
Bragadeesh153 | 16:cc77770d787f | 954 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E; |
Bragadeesh153 | 16:cc77770d787f | 955 | //Sensor 2 also not working |
Bragadeesh153 | 16:cc77770d787f | 956 | } |
Bragadeesh153 | 16:cc77770d787f | 957 | |
Bragadeesh153 | 16:cc77770d787f | 958 | |
Bragadeesh153 | 16:cc77770d787f | 959 | |
Bragadeesh153 | 16:cc77770d787f | 960 | |
Bragadeesh153 | 16:cc77770d787f | 961 | |
sakthipriya | 0:7b4c00e3912f | 962 | } |
Bragadeesh153 | 16:cc77770d787f | 963 | |
Bragadeesh153 | 16:cc77770d787f | 964 | pc_acs.printf(" Reading value from sensor 1 before exiting\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 965 | ATS1_SW_ENABLE = 0; |
Bragadeesh153 | 16:cc77770d787f | 966 | wait_ms(5); |
Bragadeesh153 | 16:cc77770d787f | 967 | SENSOR_DATA_ACQ(); |
Bragadeesh153 | 16:cc77770d787f | 968 | ATS1_SW_ENABLE = 1; |
Bragadeesh153 | 16:cc77770d787f | 969 | wait_ms(5); |
Bragadeesh153 | 16:cc77770d787f | 970 | |
Bragadeesh153 | 16:cc77770d787f | 971 | ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E; |
Bragadeesh153 | 16:cc77770d787f | 972 | |
Bragadeesh153 | 16:cc77770d787f | 973 | pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); |
Bragadeesh153 | 16:cc77770d787f | 974 | pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r"); |
Bragadeesh153 | 16:cc77770d787f | 975 | |
Bragadeesh153 | 16:cc77770d787f | 976 | ACS_DATA_ACQ_STATUS = 1; //set ACS_DATA_ACQ_STATUS flag for att sens 2 |
Bragadeesh153 | 16:cc77770d787f | 977 | return 1; |
sakthipriya | 0:7b4c00e3912f | 978 | } |
sakthipriya | 0:7b4c00e3912f | 979 | |
sakthipriya | 0:7b4c00e3912f | 980 | void FCTN_ACS_GENPWM_MAIN(float Moment[3]) |
sakthipriya | 0:7b4c00e3912f | 981 | { |
sakthipriya | 0:7b4c00e3912f | 982 | printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function |
sakthipriya | 0:7b4c00e3912f | 983 | |
sakthipriya | 0:7b4c00e3912f | 984 | float l_duty_cycle_x=0; //Duty cycle of Moment in x direction |
sakthipriya | 0:7b4c00e3912f | 985 | float l_current_x=0; //Current sent in x TR's |
sakthipriya | 0:7b4c00e3912f | 986 | float l_duty_cycle_y=0; //Duty cycle of Moment in y direction |
sakthipriya | 0:7b4c00e3912f | 987 | float l_current_y=0; //Current sent in y TR's |
sakthipriya | 0:7b4c00e3912f | 988 | float l_duty_cycle_z=0; //Duty cycle of Moment in z direction |
sakthipriya | 0:7b4c00e3912f | 989 | float l_current_z=0; //Current sent in z TR's |
sakthipriya | 0:7b4c00e3912f | 990 | |
sakthipriya | 0:7b4c00e3912f | 991 | |
sakthipriya | 0:7b4c00e3912f | 992 | for(int i = 0 ; i<3;i++) |
sakthipriya | 0:7b4c00e3912f | 993 | { |
sakthipriya | 0:7b4c00e3912f | 994 | // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs |
sakthipriya | 0:7b4c00e3912f | 995 | } |
sakthipriya | 0:7b4c00e3912f | 996 | |
sakthipriya | 0:7b4c00e3912f | 997 | //----------------------------- x-direction TR --------------------------------------------// |
sakthipriya | 0:7b4c00e3912f | 998 | |
sakthipriya | 0:7b4c00e3912f | 999 | |
sakthipriya | 0:7b4c00e3912f | 1000 | float l_moment_x = Moment[0]; //Moment in x direction |
sakthipriya | 0:7b4c00e3912f | 1001 | |
sakthipriya | 0:7b4c00e3912f | 1002 | phase_TR_x = 1; // setting the default current direction |
sakthipriya | 0:7b4c00e3912f | 1003 | if (l_moment_x <0) |
sakthipriya | 0:7b4c00e3912f | 1004 | { |
sakthipriya | 0:7b4c00e3912f | 1005 | 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 | 1006 | l_moment_x = abs(l_moment_x); |
sakthipriya | 0:7b4c00e3912f | 1007 | } |
sakthipriya | 0:7b4c00e3912f | 1008 | |
sakthipriya | 0:7b4c00e3912f | 1009 | l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship |
lakshya | 10:f93407b97750 | 1010 | printf("current in trx is %f \r \n",l_current_x); |
lakshya | 10:f93407b97750 | 1011 | 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 | 1012 | { |
lakshya | 10:f93407b97750 | 1013 | 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 | 1014 | printf("DC for trx is %f \r \n",l_duty_cycle_x); |
lakshya | 10:f93407b97750 | 1015 | PWM1.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1016 | PWM1 = l_duty_cycle_x/100 ; |
lakshya | 10:f93407b97750 | 1017 | } |
lakshya | 10:f93407b97750 | 1018 | else if (l_current_x >= 0.0016 && l_current_x < 0.0171) |
lakshya | 10:f93407b97750 | 1019 | { |
lakshya | 10:f93407b97750 | 1020 | 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 | 1021 | printf("DC for trx is %f \r \n",l_duty_cycle_x); |
lakshya | 10:f93407b97750 | 1022 | PWM1.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1023 | PWM1 = l_duty_cycle_x/100 ; |
lakshya | 10:f93407b97750 | 1024 | } |
lakshya | 10:f93407b97750 | 1025 | else if(l_current_x >= 0.0171 && l_current_x < 0.1678) |
lakshya | 10:f93407b97750 | 1026 | { |
lakshya | 10:f93407b97750 | 1027 | 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 | 1028 | printf("DC for trx is %f \r \n",l_duty_cycle_x); |
lakshya | 10:f93407b97750 | 1029 | PWM1.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1030 | PWM1 = l_duty_cycle_x/100 ; |
lakshya | 10:f93407b97750 | 1031 | } |
lakshya | 10:f93407b97750 | 1032 | else if(l_current_x==0) |
lakshya | 10:f93407b97750 | 1033 | { |
lakshya | 10:f93407b97750 | 1034 | printf("\n \r l_current_x====0"); |
lakshya | 10:f93407b97750 | 1035 | l_duty_cycle_x = 0; // default value of duty cycle |
lakshya | 10:f93407b97750 | 1036 | printf("DC for trx is %f \r \n",l_duty_cycle_x); |
lakshya | 10:f93407b97750 | 1037 | PWM1.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1038 | PWM1 = l_duty_cycle_x/100 ; |
lakshya | 10:f93407b97750 | 1039 | } |
lakshya | 10:f93407b97750 | 1040 | else //not necessary |
lakshya | 10:f93407b97750 | 1041 | { |
lakshya | 10:f93407b97750 | 1042 | g_err_flag_TR_x = 1; |
lakshya | 10:f93407b97750 | 1043 | } |
lakshya | 10:f93407b97750 | 1044 | |
lakshya | 10:f93407b97750 | 1045 | //------------------------------------- y-direction TR--------------------------------------// |
lakshya | 10:f93407b97750 | 1046 | |
lakshya | 10:f93407b97750 | 1047 | |
lakshya | 10:f93407b97750 | 1048 | float l_moment_y = Moment[1]; //Moment in y direction |
lakshya | 10:f93407b97750 | 1049 | |
lakshya | 10:f93407b97750 | 1050 | phase_TR_y = 1; // setting the default current direction |
lakshya | 10:f93407b97750 | 1051 | if (l_moment_y <0) |
lakshya | 10:f93407b97750 | 1052 | { |
lakshya | 10:f93407b97750 | 1053 | 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 | 1054 | l_moment_y = abs(l_moment_y); |
lakshya | 10:f93407b97750 | 1055 | } |
lakshya | 10:f93407b97750 | 1056 | |
lakshya | 10:f93407b97750 | 1057 | |
lakshya | 10:f93407b97750 | 1058 | l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship |
lakshya | 10:f93407b97750 | 1059 | printf("current in try is %f \r \n",l_current_y); |
lakshya | 10:f93407b97750 | 1060 | 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 | 1061 | { |
lakshya | 10:f93407b97750 | 1062 | 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 | 1063 | printf("DC for try is %f \r \n",l_duty_cycle_y); |
lakshya | 10:f93407b97750 | 1064 | PWM2.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1065 | PWM2 = l_duty_cycle_y/100 ; |
lakshya | 10:f93407b97750 | 1066 | } |
lakshya | 10:f93407b97750 | 1067 | else if (l_current_y >= 0.0016 && l_current_y < 0.0171) |
lakshya | 10:f93407b97750 | 1068 | { |
lakshya | 10:f93407b97750 | 1069 | 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 | 1070 | printf("DC for try is %f \r \n",l_duty_cycle_y); |
lakshya | 10:f93407b97750 | 1071 | PWM2.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1072 | PWM2 = l_duty_cycle_y/100 ; |
lakshya | 10:f93407b97750 | 1073 | } |
lakshya | 10:f93407b97750 | 1074 | else if(l_current_y >= 0.0171 && l_current_y < 0.1678) |
lakshya | 10:f93407b97750 | 1075 | { |
lakshya | 10:f93407b97750 | 1076 | 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 | 1077 | printf("DC for try is %f \r \n",l_duty_cycle_y); |
lakshya | 10:f93407b97750 | 1078 | PWM2.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1079 | PWM2 = l_duty_cycle_y/100 ; |
lakshya | 10:f93407b97750 | 1080 | } |
lakshya | 10:f93407b97750 | 1081 | else if(l_current_y==0) |
lakshya | 10:f93407b97750 | 1082 | { |
lakshya | 10:f93407b97750 | 1083 | printf("\n \r l_current_y====0"); |
lakshya | 10:f93407b97750 | 1084 | l_duty_cycle_y = 0; // default value of duty cycle |
lakshya | 10:f93407b97750 | 1085 | printf("DC for try is %f \r \n",l_duty_cycle_y); |
lakshya | 10:f93407b97750 | 1086 | PWM2.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1087 | PWM2 = l_duty_cycle_y/100 ; |
lakshya | 10:f93407b97750 | 1088 | } |
lakshya | 10:f93407b97750 | 1089 | else // not necessary |
lakshya | 10:f93407b97750 | 1090 | { |
lakshya | 10:f93407b97750 | 1091 | g_err_flag_TR_y = 1; |
lakshya | 10:f93407b97750 | 1092 | } |
lakshya | 10:f93407b97750 | 1093 | |
lakshya | 10:f93407b97750 | 1094 | //----------------------------------------------- z-direction TR -------------------------// |
lakshya | 10:f93407b97750 | 1095 | |
lakshya | 10:f93407b97750 | 1096 | |
lakshya | 10:f93407b97750 | 1097 | float l_moment_z = Moment[2]; //Moment in z direction |
lakshya | 10:f93407b97750 | 1098 | |
lakshya | 10:f93407b97750 | 1099 | phase_TR_z = 1; // setting the default current direction |
lakshya | 10:f93407b97750 | 1100 | if (l_moment_z <0) |
lakshya | 10:f93407b97750 | 1101 | { |
lakshya | 10:f93407b97750 | 1102 | 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 | 1103 | l_moment_z = abs(l_moment_z); |
lakshya | 10:f93407b97750 | 1104 | } |
lakshya | 10:f93407b97750 | 1105 | |
lakshya | 10:f93407b97750 | 1106 | |
lakshya | 10:f93407b97750 | 1107 | l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship |
lakshya | 10:f93407b97750 | 1108 | printf("current in trz is %f \r \n",l_current_z); |
lakshya | 10:f93407b97750 | 1109 | 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 | 1110 | { |
lakshya | 10:f93407b97750 | 1111 | 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 | 1112 | printf("DC for trz is %f \r \n",l_duty_cycle_z); |
lakshya | 10:f93407b97750 | 1113 | PWM3.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1114 | PWM3 = l_duty_cycle_z/100 ; |
lakshya | 10:f93407b97750 | 1115 | } |
lakshya | 10:f93407b97750 | 1116 | else if (l_current_z >= 0.0016 && l_current_z < 0.0171) |
lakshya | 10:f93407b97750 | 1117 | { |
lakshya | 10:f93407b97750 | 1118 | 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 | 1119 | printf("DC for trz is %f \r \n",l_duty_cycle_z); |
lakshya | 10:f93407b97750 | 1120 | PWM3.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1121 | PWM3 = l_duty_cycle_z/100 ; |
lakshya | 10:f93407b97750 | 1122 | } |
lakshya | 10:f93407b97750 | 1123 | else if(l_current_z >= 0.0171 && l_current_z < 0.1678) |
lakshya | 10:f93407b97750 | 1124 | { |
lakshya | 10:f93407b97750 | 1125 | 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 | 1126 | printf("DC for trz is %f \r \n",l_duty_cycle_z); |
lakshya | 10:f93407b97750 | 1127 | PWM3.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1128 | PWM3 = l_duty_cycle_z/100 ; |
lakshya | 10:f93407b97750 | 1129 | } |
lakshya | 10:f93407b97750 | 1130 | else if(l_current_z==0) |
lakshya | 10:f93407b97750 | 1131 | { |
lakshya | 10:f93407b97750 | 1132 | printf("\n \r l_current_z====0"); |
lakshya | 10:f93407b97750 | 1133 | l_duty_cycle_z = 0; // default value of duty cycle |
lakshya | 10:f93407b97750 | 1134 | printf("DC for trz is %f \r \n",l_duty_cycle_z); |
lakshya | 10:f93407b97750 | 1135 | PWM3.period(TIME_PERIOD); |
lakshya | 10:f93407b97750 | 1136 | PWM3 = l_duty_cycle_z/100 ; |
lakshya | 10:f93407b97750 | 1137 | } |
lakshya | 10:f93407b97750 | 1138 | else // not necessary |
lakshya | 10:f93407b97750 | 1139 | { |
lakshya | 10:f93407b97750 | 1140 | g_err_flag_TR_z = 1; |
lakshya | 10:f93407b97750 | 1141 | } |
lakshya | 10:f93407b97750 | 1142 | |
lakshya | 10:f93407b97750 | 1143 | //-----------------------------------------exiting the function-----------------------------------// |
lakshya | 10:f93407b97750 | 1144 | |
lakshya | 10:f93407b97750 | 1145 | printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function |
lakshya | 10:f93407b97750 | 1146 | |
lakshya | 10:f93407b97750 | 1147 | } |
lakshya | 10:f93407b97750 | 1148 | |
lakshya | 10:f93407b97750 | 1149 | |
lakshya | 10:f93407b97750 | 1150 | /*void FCTN_ACS_GENPWM_MAIN(float Moment[3]) |
lakshya | 10:f93407b97750 | 1151 | { |
lakshya | 10:f93407b97750 | 1152 | printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function |
lakshya | 10:f93407b97750 | 1153 | |
lakshya | 10:f93407b97750 | 1154 | float l_duty_cycle_x=0; //Duty cycle of Moment in x direction |
lakshya | 10:f93407b97750 | 1155 | float l_current_x=0; //Current sent in x TR's |
lakshya | 10:f93407b97750 | 1156 | float l_duty_cycle_y=0; //Duty cycle of Moment in y direction |
lakshya | 10:f93407b97750 | 1157 | float l_current_y=0; //Current sent in y TR's |
lakshya | 10:f93407b97750 | 1158 | float l_duty_cycle_z=0; //Duty cycle of Moment in z direction |
lakshya | 10:f93407b97750 | 1159 | float l_current_z=0; //Current sent in z TR's |
lakshya | 10:f93407b97750 | 1160 | |
lakshya | 10:f93407b97750 | 1161 | |
lakshya | 10:f93407b97750 | 1162 | for(int i = 0 ; i<3;i++) |
lakshya | 10:f93407b97750 | 1163 | { |
lakshya | 10:f93407b97750 | 1164 | printf("pwm %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs |
lakshya | 10:f93407b97750 | 1165 | } |
lakshya | 10:f93407b97750 | 1166 | |
lakshya | 10:f93407b97750 | 1167 | //----------------------------- x-direction TR --------------------------------------------// |
lakshya | 10:f93407b97750 | 1168 | |
lakshya | 10:f93407b97750 | 1169 | |
lakshya | 10:f93407b97750 | 1170 | float l_moment_x = Moment[0]; //Moment in x direction |
lakshya | 10:f93407b97750 | 1171 | |
lakshya | 10:f93407b97750 | 1172 | phase_TR_x = 1; // setting the default current direction |
lakshya | 10:f93407b97750 | 1173 | if (l_moment_x <0) |
lakshya | 10:f93407b97750 | 1174 | { |
lakshya | 10:f93407b97750 | 1175 | 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 | 1176 | l_moment_x = abs(l_moment_x); |
lakshya | 10:f93407b97750 | 1177 | } |
lakshya | 10:f93407b97750 | 1178 | |
lakshya | 10:f93407b97750 | 1179 | l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship |
sakthipriya | 0:7b4c00e3912f | 1180 | pc_acs.printf("current in trx is %f \r \n",l_current_x); |
sakthipriya | 0:7b4c00e3912f | 1181 | 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 | 1182 | { |
sakthipriya | 0:7b4c00e3912f | 1183 | 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 | 1184 | pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); |
sakthipriya | 0:7b4c00e3912f | 1185 | PWM1.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1186 | PWM1 = l_duty_cycle_x/100 ; |
sakthipriya | 0:7b4c00e3912f | 1187 | } |
sakthipriya | 0:7b4c00e3912f | 1188 | else if( l_current_x >= 0.006 && l_current_x < 0.0116) |
sakthipriya | 0:7b4c00e3912f | 1189 | { |
sakthipriya | 0:7b4c00e3912f | 1190 | 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 | 1191 | pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); |
sakthipriya | 0:7b4c00e3912f | 1192 | PWM1.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1193 | PWM1 = l_duty_cycle_x/100 ; |
sakthipriya | 0:7b4c00e3912f | 1194 | } |
sakthipriya | 0:7b4c00e3912f | 1195 | else if (l_current_x >= 0.0116 && l_current_x < 0.0624) |
sakthipriya | 0:7b4c00e3912f | 1196 | { |
sakthipriya | 0:7b4c00e3912f | 1197 | 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 | 1198 | pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); |
sakthipriya | 0:7b4c00e3912f | 1199 | PWM1.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1200 | PWM1 = l_duty_cycle_x/100 ; |
sakthipriya | 0:7b4c00e3912f | 1201 | } |
sakthipriya | 0:7b4c00e3912f | 1202 | else if(l_current_x >= 0.0624 && l_current_x < 0.555) |
sakthipriya | 0:7b4c00e3912f | 1203 | { |
sakthipriya | 0:7b4c00e3912f | 1204 | 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 | 1205 | pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); |
sakthipriya | 0:7b4c00e3912f | 1206 | PWM1.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1207 | PWM1 = l_duty_cycle_x/100 ; |
sakthipriya | 0:7b4c00e3912f | 1208 | } |
sakthipriya | 0:7b4c00e3912f | 1209 | else if(l_current_x==0) |
sakthipriya | 0:7b4c00e3912f | 1210 | { |
sakthipriya | 0:7b4c00e3912f | 1211 | printf("\n \r l_current_x====0"); |
sakthipriya | 0:7b4c00e3912f | 1212 | l_duty_cycle_x = 0; // default value of duty cycle |
sakthipriya | 0:7b4c00e3912f | 1213 | pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); |
sakthipriya | 0:7b4c00e3912f | 1214 | PWM1.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1215 | PWM1 = l_duty_cycle_x/100 ; |
sakthipriya | 0:7b4c00e3912f | 1216 | } |
sakthipriya | 0:7b4c00e3912f | 1217 | else //not necessary |
sakthipriya | 0:7b4c00e3912f | 1218 | { |
sakthipriya | 0:7b4c00e3912f | 1219 | g_err_flag_TR_x = 1; |
sakthipriya | 0:7b4c00e3912f | 1220 | } |
sakthipriya | 0:7b4c00e3912f | 1221 | |
sakthipriya | 0:7b4c00e3912f | 1222 | //------------------------------------- y-direction TR--------------------------------------// |
sakthipriya | 0:7b4c00e3912f | 1223 | |
sakthipriya | 0:7b4c00e3912f | 1224 | |
sakthipriya | 0:7b4c00e3912f | 1225 | float l_moment_y = Moment[1]; //Moment in y direction |
sakthipriya | 0:7b4c00e3912f | 1226 | |
sakthipriya | 0:7b4c00e3912f | 1227 | phase_TR_y = 1; // setting the default current direction |
sakthipriya | 0:7b4c00e3912f | 1228 | if (l_moment_y <0) |
sakthipriya | 0:7b4c00e3912f | 1229 | { |
sakthipriya | 0:7b4c00e3912f | 1230 | 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 | 1231 | l_moment_y = abs(l_moment_y); |
sakthipriya | 0:7b4c00e3912f | 1232 | } |
sakthipriya | 0:7b4c00e3912f | 1233 | |
sakthipriya | 0:7b4c00e3912f | 1234 | |
sakthipriya | 0:7b4c00e3912f | 1235 | l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship |
sakthipriya | 0:7b4c00e3912f | 1236 | pc_acs.printf("current in try is %f \r \n",l_current_y); |
sakthipriya | 0:7b4c00e3912f | 1237 | 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 | 1238 | { |
sakthipriya | 0:7b4c00e3912f | 1239 | 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 | 1240 | pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); |
sakthipriya | 0:7b4c00e3912f | 1241 | PWM2.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1242 | PWM2 = l_duty_cycle_y/100 ; |
sakthipriya | 0:7b4c00e3912f | 1243 | } |
sakthipriya | 0:7b4c00e3912f | 1244 | else if( l_current_y >= 0.006 && l_current_y < 0.0116) |
sakthipriya | 0:7b4c00e3912f | 1245 | { |
sakthipriya | 0:7b4c00e3912f | 1246 | 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 | 1247 | pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); |
sakthipriya | 0:7b4c00e3912f | 1248 | PWM2.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1249 | PWM2 = l_duty_cycle_y/100 ; |
sakthipriya | 0:7b4c00e3912f | 1250 | } |
sakthipriya | 0:7b4c00e3912f | 1251 | else if (l_current_y >= 0.0116&& l_current_y < 0.0624) |
sakthipriya | 0:7b4c00e3912f | 1252 | { |
sakthipriya | 0:7b4c00e3912f | 1253 | 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 | 1254 | pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); |
sakthipriya | 0:7b4c00e3912f | 1255 | PWM2.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1256 | PWM2 = l_duty_cycle_y/100 ; |
sakthipriya | 0:7b4c00e3912f | 1257 | } |
sakthipriya | 0:7b4c00e3912f | 1258 | else if(l_current_y >= 0.0624 && l_current_y < 0.555) |
sakthipriya | 0:7b4c00e3912f | 1259 | { |
sakthipriya | 0:7b4c00e3912f | 1260 | 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 | 1261 | pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); |
sakthipriya | 0:7b4c00e3912f | 1262 | PWM2.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1263 | PWM2 = l_duty_cycle_y/100 ; |
sakthipriya | 0:7b4c00e3912f | 1264 | } |
sakthipriya | 0:7b4c00e3912f | 1265 | else if(l_current_y==0) |
sakthipriya | 0:7b4c00e3912f | 1266 | { |
sakthipriya | 0:7b4c00e3912f | 1267 | printf("\n \r l_current_y====0"); |
sakthipriya | 0:7b4c00e3912f | 1268 | l_duty_cycle_y = 0; // default value of duty cycle |
sakthipriya | 0:7b4c00e3912f | 1269 | pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); |
sakthipriya | 0:7b4c00e3912f | 1270 | PWM2.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1271 | PWM2 = l_duty_cycle_y/100 ; |
sakthipriya | 0:7b4c00e3912f | 1272 | } |
sakthipriya | 0:7b4c00e3912f | 1273 | else // not necessary |
sakthipriya | 0:7b4c00e3912f | 1274 | { |
sakthipriya | 0:7b4c00e3912f | 1275 | g_err_flag_TR_y = 1; |
sakthipriya | 0:7b4c00e3912f | 1276 | } |
sakthipriya | 0:7b4c00e3912f | 1277 | |
sakthipriya | 0:7b4c00e3912f | 1278 | //----------------------------------------------- z-direction TR -------------------------// |
sakthipriya | 0:7b4c00e3912f | 1279 | |
sakthipriya | 0:7b4c00e3912f | 1280 | |
sakthipriya | 0:7b4c00e3912f | 1281 | float l_moment_z = Moment[2]; //Moment in z direction |
sakthipriya | 0:7b4c00e3912f | 1282 | |
sakthipriya | 0:7b4c00e3912f | 1283 | phase_TR_z = 1; // setting the default current direction |
sakthipriya | 0:7b4c00e3912f | 1284 | if (l_moment_z <0) |
sakthipriya | 0:7b4c00e3912f | 1285 | { |
sakthipriya | 0:7b4c00e3912f | 1286 | 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 | 1287 | l_moment_z = abs(l_moment_z); |
sakthipriya | 0:7b4c00e3912f | 1288 | } |
sakthipriya | 0:7b4c00e3912f | 1289 | |
sakthipriya | 0:7b4c00e3912f | 1290 | |
sakthipriya | 0:7b4c00e3912f | 1291 | l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship |
sakthipriya | 0:7b4c00e3912f | 1292 | pc_acs.printf("current in trz is %f \r \n",l_current_z); |
sakthipriya | 0:7b4c00e3912f | 1293 | 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 | 1294 | { |
sakthipriya | 0:7b4c00e3912f | 1295 | 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 | 1296 | pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); |
sakthipriya | 0:7b4c00e3912f | 1297 | PWM3.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1298 | PWM3 = l_duty_cycle_z/100 ; |
sakthipriya | 0:7b4c00e3912f | 1299 | } |
sakthipriya | 0:7b4c00e3912f | 1300 | else if( l_current_z >= 0.006 && l_current_z < 0.0116) |
sakthipriya | 0:7b4c00e3912f | 1301 | { |
sakthipriya | 0:7b4c00e3912f | 1302 | 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 | 1303 | pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); |
sakthipriya | 0:7b4c00e3912f | 1304 | PWM3.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1305 | PWM3 = l_duty_cycle_z/100 ; |
sakthipriya | 0:7b4c00e3912f | 1306 | } |
sakthipriya | 0:7b4c00e3912f | 1307 | else if (l_current_z >= 0.0116 && l_current_z < 0.0624) |
sakthipriya | 0:7b4c00e3912f | 1308 | { |
sakthipriya | 0:7b4c00e3912f | 1309 | 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 | 1310 | pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); |
sakthipriya | 0:7b4c00e3912f | 1311 | PWM3.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1312 | PWM3 = l_duty_cycle_z/100 ; |
sakthipriya | 0:7b4c00e3912f | 1313 | } |
sakthipriya | 0:7b4c00e3912f | 1314 | else if(l_current_z >= 0.0624 && l_current_z < 0.555) |
sakthipriya | 0:7b4c00e3912f | 1315 | { |
sakthipriya | 0:7b4c00e3912f | 1316 | 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 | 1317 | pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); |
sakthipriya | 0:7b4c00e3912f | 1318 | PWM3.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1319 | PWM3 = l_duty_cycle_z/100 ; |
sakthipriya | 0:7b4c00e3912f | 1320 | } |
sakthipriya | 0:7b4c00e3912f | 1321 | else if(l_current_z==0) |
sakthipriya | 0:7b4c00e3912f | 1322 | { |
sakthipriya | 0:7b4c00e3912f | 1323 | printf("\n \r l_current_z====0"); |
sakthipriya | 0:7b4c00e3912f | 1324 | l_duty_cycle_z = 0; // default value of duty cycle |
sakthipriya | 0:7b4c00e3912f | 1325 | pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); |
sakthipriya | 0:7b4c00e3912f | 1326 | PWM3.period(TIME_PERIOD); |
sakthipriya | 0:7b4c00e3912f | 1327 | PWM3 = l_duty_cycle_z/100 ; |
sakthipriya | 0:7b4c00e3912f | 1328 | } |
sakthipriya | 0:7b4c00e3912f | 1329 | else // not necessary |
sakthipriya | 0:7b4c00e3912f | 1330 | { |
sakthipriya | 0:7b4c00e3912f | 1331 | g_err_flag_TR_z = 1; |
sakthipriya | 0:7b4c00e3912f | 1332 | } |
sakthipriya | 0:7b4c00e3912f | 1333 | |
sakthipriya | 0:7b4c00e3912f | 1334 | //-----------------------------------------exiting the function-----------------------------------// |
sakthipriya | 0:7b4c00e3912f | 1335 | |
sakthipriya | 0:7b4c00e3912f | 1336 | printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function |
sakthipriya | 0:7b4c00e3912f | 1337 | |
lakshya | 10:f93407b97750 | 1338 | }*/ |
sakthipriya | 0:7b4c00e3912f | 1339 | |
sakthipriya | 0:7b4c00e3912f | 1340 | |
sakthipriya | 0:7b4c00e3912f | 1341 |