sakthi priya amirtharaj / Mbed 2 deprecated BAE_honeycomb1

reset prob solved n cdms reforms the structure from hk data

Dependencies:   mbed-rtos mbed

ACS.cpp@0:93514bd41116, 2014-12-18 (annotated)

Committer:
sakthipriya
Date:
Thu Dec 18 12:20:47 2014 +0000
Revision:
0:93514bd41116
reset prob solved n cdms reforms the structure from hk data

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakthipriya 0:93514bd41116 1 #include "ACS.h"
sakthipriya 0:93514bd41116 2 #include "MPU3300.h"
sakthipriya 0:93514bd41116 3
sakthipriya 0:93514bd41116 4 //PwmOut PWM1(PTD4); //Functions used to generate PWM signal
sakthipriya 0:93514bd41116 5 //PWM output comes from pins p6
sakthipriya 0:93514bd41116 6 Serial pc1(USBTX, USBRX);
sakthipriya 0:93514bd41116 7 SPI spi_acs (PTA16, PTA17, PTA15); // mosi, miso, sclk
sakthipriya 0:93514bd41116 8 DigitalOut SSN_MAG (D8); // ssn for magnetometer
sakthipriya 0:93514bd41116 9 DigitalIn DRDY (D12); // drdy for magnetometer
sakthipriya 0:93514bd41116 10 DigitalOut ssn_gyr (D13); //Slave Select pin of gyroscope
sakthipriya 0:93514bd41116 11 InterruptIn dr(D7); //Interrupt pin for gyro
sakthipriya 0:93514bd41116 12 PwmOut PWM1(A3); //Functions used to generate PWM signal
sakthipriya 0:93514bd41116 13 PwmOut PWM2(A4);
sakthipriya 0:93514bd41116 14 PwmOut PWM3(A5); //PWM output comes from pins p6
sakthipriya 0:93514bd41116 15 Ticker tr; //Ticker function to give values for limited amount of time for gyro
sakthipriya 0:93514bd41116 16 Timeout tr_mag;
sakthipriya 0:93514bd41116 17 uint8_t trflag_mag;
sakthipriya 0:93514bd41116 18 uint8_t trFlag; //ticker Flag for gyro
sakthipriya 0:93514bd41116 19 uint8_t drFlag; //data-ready interrupt flag for gyro
sakthipriya 0:93514bd41116 20
sakthipriya 0:93514bd41116 21 //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------//
sakthipriya 0:93514bd41116 22
sakthipriya 0:93514bd41116 23 void FUNC_ACS_GENPWM(float M[3])
sakthipriya 0:93514bd41116 24 {
sakthipriya 0:93514bd41116 25
sakthipriya 0:93514bd41116 26
sakthipriya 0:93514bd41116 27 printf("\nEnterd PWMGEN function\n");
sakthipriya 0:93514bd41116 28 float DCx = 0; //Duty cycle of Moment in x, y, z directions
sakthipriya 0:93514bd41116 29 float ix = 0; //Current sent in x, y, z TR's
sakthipriya 0:93514bd41116 30 float timep = 0.02 ;
sakthipriya 0:93514bd41116 31 float Mx=M[0]; //Time period is set to 0.02s
sakthipriya 0:93514bd41116 32 //Moment in x, y, z directions
sakthipriya 0:93514bd41116 33
sakthipriya 0:93514bd41116 34
sakthipriya 0:93514bd41116 35 ix = Mx * 0.3 ; //Moment and Current always have the linear relationship
sakthipriya 0:93514bd41116 36
sakthipriya 0:93514bd41116 37 if( ix>0&& ix < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:93514bd41116 38 {
sakthipriya 0:93514bd41116 39 DCx = 6*1000000*pow(ix,4) - 377291*pow(ix,3) + 4689.6*pow(ix,2) + 149.19*ix - 0.0008;
sakthipriya 0:93514bd41116 40 PWM1.period(timep);
sakthipriya 0:93514bd41116 41 PWM1 = DCx/100 ;
sakthipriya 0:93514bd41116 42 }
sakthipriya 0:93514bd41116 43 else if( ix >= 0.006&& ix < 0.0116)
sakthipriya 0:93514bd41116 44 {
sakthipriya 0:93514bd41116 45 DCx = 1*100000000*pow(ix,4) - 5*1000000*pow(ix,3) + 62603*pow(ix,2) - 199.29*ix + 0.7648;
sakthipriya 0:93514bd41116 46 PWM1.period(timep);
sakthipriya 0:93514bd41116 47 PWM1 = DCx/100 ;
sakthipriya 0:93514bd41116 48 }
sakthipriya 0:93514bd41116 49 else if (ix >= 0.0116&& ix < 0.0624)
sakthipriya 0:93514bd41116 50 {
sakthipriya 0:93514bd41116 51
sakthipriya 0:93514bd41116 52 DCx = 212444*pow(ix,4) - 33244*pow(ix,3) + 1778.4*pow(ix,2) + 120.91*ix + 0.3878;
sakthipriya 0:93514bd41116 53 PWM1.period(timep);
sakthipriya 0:93514bd41116 54 PWM1 = DCx/100 ;
sakthipriya 0:93514bd41116 55 }
sakthipriya 0:93514bd41116 56 else if(ix >= 0.0624&& ix < 0.555)
sakthipriya 0:93514bd41116 57 {
sakthipriya 0:93514bd41116 58 printf("\nACS entered if\n");
sakthipriya 0:93514bd41116 59 DCx = 331.15*pow(ix,4) - 368.09*pow(ix,3) + 140.43*pow(ix,2) + 158.59*ix + 0.0338;
sakthipriya 0:93514bd41116 60 PWM1.period(timep);
sakthipriya 0:93514bd41116 61 PWM1 = DCx/100 ;
sakthipriya 0:93514bd41116 62 }
sakthipriya 0:93514bd41116 63 else if(ix==0)
sakthipriya 0:93514bd41116 64 {
sakthipriya 0:93514bd41116 65 DCx = 0;
sakthipriya 0:93514bd41116 66 PWM1.period(timep);
sakthipriya 0:93514bd41116 67 PWM1 = DCx/100 ;
sakthipriya 0:93514bd41116 68 }
sakthipriya 0:93514bd41116 69 else
sakthipriya 0:93514bd41116 70 {
sakthipriya 0:93514bd41116 71 // printf("!!!!!!!!!!Error!!!!!!!!!");
sakthipriya 0:93514bd41116 72 }
sakthipriya 0:93514bd41116 73 float DCy = 0; //Duty cycle of Moment in x, y, z directions
sakthipriya 0:93514bd41116 74 float iy = 0; //Current sent in x, y, z TR's
sakthipriya 0:93514bd41116 75
sakthipriya 0:93514bd41116 76 float My=M[1]; //Time period is set to 0.2s
sakthipriya 0:93514bd41116 77 //Moment in x, y, z directions
sakthipriya 0:93514bd41116 78
sakthipriya 0:93514bd41116 79
sakthipriya 0:93514bd41116 80 iy = My * 0.3 ; //Moment and Current always have the linear relationship
sakthipriya 0:93514bd41116 81
sakthipriya 0:93514bd41116 82 if( iy>0&& iy < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:93514bd41116 83 {
sakthipriya 0:93514bd41116 84 DCy = 6*1000000*pow(iy,4) - 377291*pow(iy,3) + 4689.6*pow(iy,2) + 149.19*iy - 0.0008;
sakthipriya 0:93514bd41116 85 PWM2.period(timep);
sakthipriya 0:93514bd41116 86 PWM2 = DCy/100 ;
sakthipriya 0:93514bd41116 87 }
sakthipriya 0:93514bd41116 88 else if( iy >= 0.006&& iy < 0.0116)
sakthipriya 0:93514bd41116 89 {
sakthipriya 0:93514bd41116 90 DCy = 1*100000000*pow(iy,4) - 5*1000000*pow(iy,3) + 62603*pow(iy,2) - 199.29*iy + 0.7648;
sakthipriya 0:93514bd41116 91 PWM2.period(timep);
sakthipriya 0:93514bd41116 92 PWM2 = DCy/100 ;
sakthipriya 0:93514bd41116 93 }
sakthipriya 0:93514bd41116 94 else if (iy >= 0.0116&& iy < 0.0624)
sakthipriya 0:93514bd41116 95 {
sakthipriya 0:93514bd41116 96
sakthipriya 0:93514bd41116 97 DCy = 212444*pow(iy,4) - 33244*pow(iy,3) + 1778.4*pow(iy,2) + 120.91*iy + 0.3878;
sakthipriya 0:93514bd41116 98 PWM2.period(timep);
sakthipriya 0:93514bd41116 99 PWM2 = DCy/100 ;
sakthipriya 0:93514bd41116 100 }
sakthipriya 0:93514bd41116 101 else if(iy >= 0.0624&& iy < 0.555)
sakthipriya 0:93514bd41116 102 {
sakthipriya 0:93514bd41116 103 printf("\nACS entered if\n");
sakthipriya 0:93514bd41116 104 DCy = 331.15*pow(iy,4) - 368.09*pow(iy,3) + 140.43*pow(iy,2) + 158.59*iy + 0.0338;
sakthipriya 0:93514bd41116 105 PWM2.period(timep);
sakthipriya 0:93514bd41116 106 PWM2 = DCy/100 ;
sakthipriya 0:93514bd41116 107 }
sakthipriya 0:93514bd41116 108 else if(iy==0)
sakthipriya 0:93514bd41116 109 {
sakthipriya 0:93514bd41116 110 DCy = 0;
sakthipriya 0:93514bd41116 111 PWM2.period(timep);
sakthipriya 0:93514bd41116 112 PWM2 = DCy/100 ;
sakthipriya 0:93514bd41116 113 }
sakthipriya 0:93514bd41116 114 else
sakthipriya 0:93514bd41116 115 {
sakthipriya 0:93514bd41116 116 // printf("!!!!!!!!!!Error!!!!!!!!!");
sakthipriya 0:93514bd41116 117 }
sakthipriya 0:93514bd41116 118 float DCz = 0; //Duty cycle of Moment in x, y, z directions
sakthipriya 0:93514bd41116 119 float iz = 0; //Current sent in x, y, z TR's
sakthipriya 0:93514bd41116 120
sakthipriya 0:93514bd41116 121 float Mz=M[2]; //Time period is set to 0.2s
sakthipriya 0:93514bd41116 122 //Moment in x, y, z directions
sakthipriya 0:93514bd41116 123
sakthipriya 0:93514bd41116 124
sakthipriya 0:93514bd41116 125 iz = Mz * 0.3 ; //Moment and Current always have the linear relationship
sakthipriya 0:93514bd41116 126
sakthipriya 0:93514bd41116 127 if( iz>0&& iz < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:93514bd41116 128 {
sakthipriya 0:93514bd41116 129 DCz = 6*1000000*pow(iz,4) - 377291*pow(iz,3) + 4689.6*pow(iz,2) + 149.19*iz - 0.0008;
sakthipriya 0:93514bd41116 130 PWM3.period(timep);
sakthipriya 0:93514bd41116 131 PWM3 = DCz/100 ;
sakthipriya 0:93514bd41116 132 }
sakthipriya 0:93514bd41116 133 else if( iz >= 0.006&& iz < 0.0116)
sakthipriya 0:93514bd41116 134 {
sakthipriya 0:93514bd41116 135 DCz = 1*100000000*pow(iz,4) - 5*1000000*pow(iz,3) + 62603*pow(iz,2) - 199.29*iz + 0.7648;
sakthipriya 0:93514bd41116 136 PWM3.period(timep);
sakthipriya 0:93514bd41116 137 PWM3 = DCz/100 ;
sakthipriya 0:93514bd41116 138 }
sakthipriya 0:93514bd41116 139 else if (iz >= 0.0116&& iz < 0.0624)
sakthipriya 0:93514bd41116 140 {
sakthipriya 0:93514bd41116 141
sakthipriya 0:93514bd41116 142 DCz = 212444*pow(iz,4) - 33244*pow(iz,3) + 1778.4*pow(iz,2) + 120.91*iz + 0.3878;
sakthipriya 0:93514bd41116 143 PWM3.period(timep);
sakthipriya 0:93514bd41116 144 PWM3 = DCz/100 ;
sakthipriya 0:93514bd41116 145 }
sakthipriya 0:93514bd41116 146 else if(iz >= 0.0624&& iz < 0.555)
sakthipriya 0:93514bd41116 147 {
sakthipriya 0:93514bd41116 148 printf("\nACS entered if\n");
sakthipriya 0:93514bd41116 149 DCz = 331.15*pow(iz,4) - 368.09*pow(iz,3) + 140.43*pow(iz,2) + 158.59*iz + 0.0338;
sakthipriya 0:93514bd41116 150 PWM3.period(timep);
sakthipriya 0:93514bd41116 151 PWM3 = DCz/100 ;
sakthipriya 0:93514bd41116 152 }
sakthipriya 0:93514bd41116 153 else if(iz==0)
sakthipriya 0:93514bd41116 154 {
sakthipriya 0:93514bd41116 155 DCz = 0;
sakthipriya 0:93514bd41116 156 PWM3.period(timep);
sakthipriya 0:93514bd41116 157 PWM3 = DCz/100 ;
sakthipriya 0:93514bd41116 158 }
sakthipriya 0:93514bd41116 159 else
sakthipriya 0:93514bd41116 160 {
sakthipriya 0:93514bd41116 161 // printf("!!!!!!!!!!Error!!!!!!!!!");
sakthipriya 0:93514bd41116 162 }
sakthipriya 0:93514bd41116 163
sakthipriya 0:93514bd41116 164 printf("\nExited PWMGEN function\n");
sakthipriya 0:93514bd41116 165 }
sakthipriya 0:93514bd41116 166 /*-------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:93514bd41116 167 -------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/
sakthipriya 0:93514bd41116 168
sakthipriya 0:93514bd41116 169 void trsub_mag()
sakthipriya 0:93514bd41116 170 {
sakthipriya 0:93514bd41116 171 trflag_mag=0;
sakthipriya 0:93514bd41116 172 }
sakthipriya 0:93514bd41116 173
sakthipriya 0:93514bd41116 174 void FUNC_ACS_MAG_INIT()
sakthipriya 0:93514bd41116 175 {
sakthipriya 0:93514bd41116 176
sakthipriya 0:93514bd41116 177 SSN_MAG=1; //pin is disabled
sakthipriya 0:93514bd41116 178 spi_acs.format(8,0); // 8bits,Mode 0
sakthipriya 0:93514bd41116 179 spi_acs.frequency(100000); //clock frequency
sakthipriya 0:93514bd41116 180
sakthipriya 0:93514bd41116 181 SSN_MAG=0; // Selecting pin
sakthipriya 0:93514bd41116 182 wait_ms(10); //accounts for delay.can be minimised.
sakthipriya 0:93514bd41116 183
sakthipriya 0:93514bd41116 184 spi_acs.write(0x83); //
sakthipriya 0:93514bd41116 185
sakthipriya 0:93514bd41116 186 wait_ms(10);
sakthipriya 0:93514bd41116 187
sakthipriya 0:93514bd41116 188 unsigned char i;
sakthipriya 0:93514bd41116 189 for(i=0;i<3;i++)//initialising values.
sakthipriya 0:93514bd41116 190 {
sakthipriya 0:93514bd41116 191 spi_acs.write(0x00); //MSB of X,y,Z
sakthipriya 0:93514bd41116 192 spi_acs.write(0xc8); //LSB of X,Y,z;pointer increases automatically.
sakthipriya 0:93514bd41116 193 }
sakthipriya 0:93514bd41116 194 SSN_MAG=1;
sakthipriya 0:93514bd41116 195
sakthipriya 0:93514bd41116 196 }
sakthipriya 0:93514bd41116 197
sakthipriya 0:93514bd41116 198 float* FUNC_ACS_MAG_EXEC()
sakthipriya 0:93514bd41116 199 {
sakthipriya 0:93514bd41116 200 printf("\nEntered magnetometer function\n");
sakthipriya 0:93514bd41116 201 SSN_MAG=0; //enabling slave to measure the values
sakthipriya 0:93514bd41116 202 wait_ms(10);
sakthipriya 0:93514bd41116 203 spi_acs.write(0x82); //initiates measurement
sakthipriya 0:93514bd41116 204 wait_ms(10);
sakthipriya 0:93514bd41116 205 spi_acs.write(0x01); //selecting x,y and z axes, measurement starts now
sakthipriya 0:93514bd41116 206 SSN_MAG=1;
sakthipriya 0:93514bd41116 207 wait_ms(10);
sakthipriya 0:93514bd41116 208
sakthipriya 0:93514bd41116 209 trflag_mag=1;
sakthipriya 0:93514bd41116 210 tr_mag.attach(&trsub_mag,1); //runs in background,makes trflag_mag=0 after 1s
sakthipriya 0:93514bd41116 211
sakthipriya 0:93514bd41116 212 while(trflag_mag) /*initially flag is 1,so loop is executed,if DRDY is high,then data is retrieved and programme ends,else
sakthipriya 0:93514bd41116 213 loop runs for at the max 1s and if still DRDY is zero,the flag becomes 0 and loop is not executed and
sakthipriya 0:93514bd41116 214 programme is terminated*/
sakthipriya 0:93514bd41116 215 {
sakthipriya 0:93514bd41116 216 wait_ms(5);
sakthipriya 0:93514bd41116 217 if(DRDY==1)
sakthipriya 0:93514bd41116 218 {
sakthipriya 0:93514bd41116 219 SSN_MAG=0;
sakthipriya 0:93514bd41116 220 spi_acs.write(0xc9); //command byte for retrieving data
sakthipriya 0:93514bd41116 221
sakthipriya 0:93514bd41116 222 unsigned char axis;
sakthipriya 0:93514bd41116 223 float Bnewvalue[3]={0.0,0.0,0.0};
sakthipriya 0:93514bd41116 224 int32_t Bvalue[3]={0,0,0};
sakthipriya 0:93514bd41116 225 int32_t a= pow(2.0,24.0);
sakthipriya 0:93514bd41116 226 int32_t b= pow(2.0,23.0);
sakthipriya 0:93514bd41116 227
sakthipriya 0:93514bd41116 228 for(axis=0;axis<3;axis++)
sakthipriya 0:93514bd41116 229 {
sakthipriya 0:93514bd41116 230 Bvalue[axis]=spi_acs.write(0x00)<<16; //MSB 1 is send first
sakthipriya 0:93514bd41116 231 wait_ms(10);
sakthipriya 0:93514bd41116 232 Bvalue[axis]|=spi_acs.write(0x00)<<8; //MSB 2 is send next
sakthipriya 0:93514bd41116 233 wait_ms(10);
sakthipriya 0:93514bd41116 234 Bvalue[axis]|=spi_acs.write(0x00); //LSB is send.....total length is 24 bits(3*8bits)...which are appended to get actual bit configuration
sakthipriya 0:93514bd41116 235
sakthipriya 0:93514bd41116 236
sakthipriya 0:93514bd41116 237 if((Bvalue[axis]&b)==b)
sakthipriya 0:93514bd41116 238 {
sakthipriya 0:93514bd41116 239 Bvalue[axis]=Bvalue[axis]-a; //converting 2s complement to signed decimal
sakthipriya 0:93514bd41116 240
sakthipriya 0:93514bd41116 241 }
sakthipriya 0:93514bd41116 242 Bnewvalue[axis]=(float)Bvalue[axis]*22.0*pow(10.0,-3.0); //1 LSB=(22nT)...final value of field obtained in micro tesla
sakthipriya 0:93514bd41116 243
sakthipriya 0:93514bd41116 244 wait_ms(10);
sakthipriya 0:93514bd41116 245 printf("\t%lf\n",Bnewvalue[axis]);
sakthipriya 0:93514bd41116 246
sakthipriya 0:93514bd41116 247 }
sakthipriya 0:93514bd41116 248 SSN_MAG=1;
sakthipriya 0:93514bd41116 249
sakthipriya 0:93514bd41116 250 return Bnewvalue; //return here? doubt..
sakthipriya 0:93514bd41116 251 break;
sakthipriya 0:93514bd41116 252 }
sakthipriya 0:93514bd41116 253
sakthipriya 0:93514bd41116 254 }
sakthipriya 0:93514bd41116 255
sakthipriya 0:93514bd41116 256 }
sakthipriya 0:93514bd41116 257 /*------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:93514bd41116 258 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
sakthipriya 0:93514bd41116 259
sakthipriya 0:93514bd41116 260 float * FUNC_ACS_CNTRLALGO(float b[3],float omega[3])
sakthipriya 0:93514bd41116 261 {
sakthipriya 0:93514bd41116 262 float db[3]; /// inputs
sakthipriya 0:93514bd41116 263 //initialization
sakthipriya 0:93514bd41116 264 float bb[3] = {0, 0, 0};
sakthipriya 0:93514bd41116 265 float d[3] = {0, 0, 0};
sakthipriya 0:93514bd41116 266 float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}};
sakthipriya 0:93514bd41116 267 float den = 0;
sakthipriya 0:93514bd41116 268 float den2;
sakthipriya 0:93514bd41116 269 int i, j; //temporary variables
sakthipriya 0:93514bd41116 270 float Mu[2], z[2], dv[2], v[2], u[2], tauc[3] = {0, 0, 0}; //outputs
sakthipriya 0:93514bd41116 271 float invJm[3][3];
sakthipriya 0:93514bd41116 272 float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4;
sakthipriya 0:93514bd41116 273 printf("Entered cntrl algo\n");
sakthipriya 0:93514bd41116 274 //structure parameters
sakthipriya 0:93514bd41116 275
sakthipriya 0:93514bd41116 276 void inverse (float mat[3][3], float inv[3][3]);
sakthipriya 0:93514bd41116 277 void getInput (float x[9]);
sakthipriya 0:93514bd41116 278 //functions
sakthipriya 0:93514bd41116 279
sakthipriya 0:93514bd41116 280 ////////// Input from Matlab //////////////
sakthipriya 0:93514bd41116 281 while(1)
sakthipriya 0:93514bd41116 282 {
sakthipriya 0:93514bd41116 283
sakthipriya 0:93514bd41116 284 /*getInput(inputs);
sakthipriya 0:93514bd41116 285 //while(1)
sakthipriya 0:93514bd41116 286 b[0] = inputs[0];
sakthipriya 0:93514bd41116 287 b[1] = inputs[1];
sakthipriya 0:93514bd41116 288 b[2] = inputs[2];
sakthipriya 0:93514bd41116 289 db[0] = inputs[3];
sakthipriya 0:93514bd41116 290 db[1] = inputs[4];
sakthipriya 0:93514bd41116 291 db[2] = inputs[5];
sakthipriya 0:93514bd41116 292 omega[0] = inputs[6];
sakthipriya 0:93514bd41116 293 omega[1] = inputs[7];
sakthipriya 0:93514bd41116 294 omega[2] = inputs[8];*/
sakthipriya 0:93514bd41116 295 /////////// Control Algorithm //////////////////////
sakthipriya 0:93514bd41116 296 // calculate norm b, norm db
sakthipriya 0:93514bd41116 297 den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2]));
sakthipriya 0:93514bd41116 298 den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]);
sakthipriya 0:93514bd41116 299
sakthipriya 0:93514bd41116 300 for(i=0;i<3;i++)
sakthipriya 0:93514bd41116 301 {
sakthipriya 0:93514bd41116 302 db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3));
sakthipriya 0:93514bd41116 303 //db[i]/=den*den*den;
sakthipriya 0:93514bd41116 304 }
sakthipriya 0:93514bd41116 305
sakthipriya 0:93514bd41116 306 for(i=0;i<3;i++)
sakthipriya 0:93514bd41116 307 {
sakthipriya 0:93514bd41116 308 printf("\nreached here\n");
sakthipriya 0:93514bd41116 309 if(den!=0)
sakthipriya 0:93514bd41116 310 //b[i]=b[i]/den; //there is a problem here. The code gets stuck here. Maf value is required
sakthipriya 0:93514bd41116 311 ;
sakthipriya 0:93514bd41116 312 }
sakthipriya 0:93514bd41116 313
sakthipriya 0:93514bd41116 314 // select kz, kmu, gamma
sakthipriya 0:93514bd41116 315 if(b[0]>0.9 || b[0]<-0.9)
sakthipriya 0:93514bd41116 316 {
sakthipriya 0:93514bd41116 317 kz = kz2;
sakthipriya 0:93514bd41116 318 kmu = kmu2;
sakthipriya 0:93514bd41116 319 gamma = gamma2;
sakthipriya 0:93514bd41116 320 }
sakthipriya 0:93514bd41116 321 // calculate Mu, v, dv, z, u
sakthipriya 0:93514bd41116 322 for(i=0;i<2;i++)
sakthipriya 0:93514bd41116 323 {
sakthipriya 0:93514bd41116 324 Mu[i] = b[i+1];
sakthipriya 0:93514bd41116 325 v[i] = -kmu*Mu[i];
sakthipriya 0:93514bd41116 326 dv[i] = -kmu*db[i+1];
sakthipriya 0:93514bd41116 327 z[i] = db[i+1] - v[i];
sakthipriya 0:93514bd41116 328 u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma);
sakthipriya 0:93514bd41116 329 }
sakthipriya 0:93514bd41116 330 inverse(Jm, invJm);
sakthipriya 0:93514bd41116 331 // calculate cross(omega,J*omega)for(i=0;i<3;i++)
sakthipriya 0:93514bd41116 332
sakthipriya 0:93514bd41116 333 for(j=0;j<3;j++)
sakthipriya 0:93514bd41116 334 bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - omega[(i+2)%3]*Jm[(i+1)%3][j]);
sakthipriya 0:93514bd41116 335
sakthipriya 0:93514bd41116 336 // calculate invJm*cross(omega,J*omega) store in d
sakthipriya 0:93514bd41116 337 for(i=0;i<3;i++)
sakthipriya 0:93514bd41116 338 {
sakthipriya 0:93514bd41116 339 for(j=0;j<3;j++)
sakthipriya 0:93514bd41116 340 d[i] += bb[j]*invJm[i][j];
sakthipriya 0:93514bd41116 341 }
sakthipriya 0:93514bd41116 342 // calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db)
sakthipriya 0:93514bd41116 343 // bb =[0;u-d(2:3)]
sakthipriya 0:93514bd41116 344 // store in bb
sakthipriya 0:93514bd41116 345 bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]);
sakthipriya 0:93514bd41116 346 bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]);
sakthipriya 0:93514bd41116 347 bb[0] = 0;
sakthipriya 0:93514bd41116 348 // calculate N
sakthipriya 0:93514bd41116 349 // reusing invJm as N
sakthipriya 0:93514bd41116 350
sakthipriya 0:93514bd41116 351 for(i=0;i<3;i++)
sakthipriya 0:93514bd41116 352 {
sakthipriya 0:93514bd41116 353 d[i] = invJm[1][i];
sakthipriya 0:93514bd41116 354 invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i];
sakthipriya 0:93514bd41116 355 invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i];
sakthipriya 0:93514bd41116 356 invJm[0][i] = b[i];
sakthipriya 0:93514bd41116 357 }
sakthipriya 0:93514bd41116 358 // calculate inv(N) store in Jm
sakthipriya 0:93514bd41116 359 inverse(invJm, Jm);
sakthipriya 0:93514bd41116 360 // calculate tauc
sakthipriya 0:93514bd41116 361 for(i=0;i<3;i++)
sakthipriya 0:93514bd41116 362 {
sakthipriya 0:93514bd41116 363 for(j=0;j<3;j++)
sakthipriya 0:93514bd41116 364 tauc[i] += Jm[i][j]*bb[j];
sakthipriya 0:93514bd41116 365 }
sakthipriya 0:93514bd41116 366
sakthipriya 0:93514bd41116 367 return(tauc);
sakthipriya 0:93514bd41116 368 }
sakthipriya 0:93514bd41116 369 }
sakthipriya 0:93514bd41116 370 /////////// Output to Matlab //////////////////
sakthipriya 0:93514bd41116 371 /* for(i=0;i<3;i++) {
sakthipriya 0:93514bd41116 372 printf("%f\n",tauc[i]*10000000);
sakthipriya 0:93514bd41116 373 wait_ms(10);
sakthipriya 0:93514bd41116 374 }
sakthipriya 0:93514bd41116 375 }
sakthipriya 0:93514bd41116 376
sakthipriya 0:93514bd41116 377 }*/
sakthipriya 0:93514bd41116 378 void inverse(float mat[3][3], float inv[3][3])
sakthipriya 0:93514bd41116 379 {
sakthipriya 0:93514bd41116 380 int i, j;
sakthipriya 0:93514bd41116 381 float det = 0;
sakthipriya 0:93514bd41116 382 for(i=0;i<3;i++)
sakthipriya 0:93514bd41116 383 { for(j=0;j<3;j++)
sakthipriya 0:93514bd41116 384 inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3]
sakthipriya 0:93514bd41116 385 [(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
sakthipriya 0:93514bd41116 386 }
sakthipriya 0:93514bd41116 387 det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]);
sakthipriya 0:93514bd41116 388 for(i=0;i<3;i++)
sakthipriya 0:93514bd41116 389 { for(j=0;j<3;j++)
sakthipriya 0:93514bd41116 390 inv[i][j] /= det;
sakthipriya 0:93514bd41116 391 }
sakthipriya 0:93514bd41116 392 }/*
sakthipriya 0:93514bd41116 393 void getInput (float x[9]) {
sakthipriya 0:93514bd41116 394 //Functions used to generate PWM signal
sakthipriya 0:93514bd41116 395 //PWM output comes from pins p6
sakthipriya 0:93514bd41116 396 Serial pc1(USBTX, USBRX);
sakthipriya 0:93514bd41116 397 char c[10];
sakthipriya 0:93514bd41116 398 char tempchar[8];
sakthipriya 0:93514bd41116 399 int i, j;
sakthipriya 0:93514bd41116 400 //float f[9];
sakthipriya 0:93514bd41116 401 long n = 0;
sakthipriya 0:93514bd41116 402 float flval = 0;
sakthipriya 0:93514bd41116 403 for(j=0;j<9;j++) {
sakthipriya 0:93514bd41116 404 for(i=0;i<9;i++) {
sakthipriya 0:93514bd41116 405 c[i] = pc1.getc(); if(i<8) {
sakthipriya 0:93514bd41116 406 tempchar[i] = c[i];
sakthipriya 0:93514bd41116 407 }
sakthipriya 0:93514bd41116 408 }
sakthipriya 0:93514bd41116 409 sscanf (tempchar, "%8x", &n);
sakthipriya 0:93514bd41116 410 memcpy(&flval, &n, sizeof(long));
sakthipriya 0:93514bd41116 411 printf("%f\n", flval);
sakthipriya 0:93514bd41116 412 x[j] = flval;
sakthipriya 0:93514bd41116 413 }
sakthipriya 0:93514bd41116 414 }*/
sakthipriya 0:93514bd41116 415
sakthipriya 0:93514bd41116 416 void trSub();
sakthipriya 0:93514bd41116 417 void drSub();
sakthipriya 0:93514bd41116 418 void init_gyro();
sakthipriya 0:93514bd41116 419 float * FUNC_ACS_EXEC_GYR();
sakthipriya 0:93514bd41116 420
sakthipriya 0:93514bd41116 421 void drSub() //In this function we setting data-ready flag to 1
sakthipriya 0:93514bd41116 422 {
sakthipriya 0:93514bd41116 423 drFlag=1;
sakthipriya 0:93514bd41116 424 }
sakthipriya 0:93514bd41116 425 void trSub() //In this function we are setting ticker flag to 0
sakthipriya 0:93514bd41116 426 {
sakthipriya 0:93514bd41116 427 trFlag=0;
sakthipriya 0:93514bd41116 428 }
sakthipriya 0:93514bd41116 429 void FUNC_ACS_INIT_GYR()
sakthipriya 0:93514bd41116 430 {
sakthipriya 0:93514bd41116 431 uint8_t response;
sakthipriya 0:93514bd41116 432 ssn_gyr=1; //Deselecting the chip
sakthipriya 0:93514bd41116 433 spi_acs.format(8,3); // Spi format is 8 bits, and clock mode 3
sakthipriya 0:93514bd41116 434 spi_acs.frequency(1000000); //frequency to be set as 1MHz
sakthipriya 0:93514bd41116 435 drFlag=0; //Intially defining data-ready flag to be 0
sakthipriya 0:93514bd41116 436 dr.mode(PullDown);
sakthipriya 0:93514bd41116 437 dr.rise(&drSub);
sakthipriya 0:93514bd41116 438 __disable_irq();
sakthipriya 0:93514bd41116 439
sakthipriya 0:93514bd41116 440 /*Following the above mentioned algorithm for initializing the register and changing its configuration*/
sakthipriya 0:93514bd41116 441 ssn_gyr=0; //Selecting chip(Mpu-3300)
sakthipriya 0:93514bd41116 442 spi_acs.write(USER_CTRL|READFLAG); //sending USER_CTRL address with read bit
sakthipriya 0:93514bd41116 443 response=spi_acs.write(DUMMYBIT); //sending dummy bit to get default values of the register
sakthipriya 0:93514bd41116 444
sakthipriya 0:93514bd41116 445 ssn_gyr=1; //Deselecting the chip
sakthipriya 0:93514bd41116 446 wait(0.1); //waiting according the product specifications
sakthipriya 0:93514bd41116 447
sakthipriya 0:93514bd41116 448 ssn_gyr=0; //again selecting the chip
sakthipriya 0:93514bd41116 449 spi_acs.write(USER_CTRL); //sending USER_CTRL address without read bit
sakthipriya 0:93514bd41116 450 spi_acs.write(response|BIT_I2C_IF_DIS); //disabling the I2C mode in the register
sakthipriya 0:93514bd41116 451 ssn_gyr=1; //deselecting the chip
sakthipriya 0:93514bd41116 452 wait(0.1); // waiting for 100ms before going for another register
sakthipriya 0:93514bd41116 453
sakthipriya 0:93514bd41116 454 ssn_gyr=0;
sakthipriya 0:93514bd41116 455 spi_acs.write(PWR_MGMT_1|READFLAG); //Power Management register-1
sakthipriya 0:93514bd41116 456 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:93514bd41116 457 ssn_gyr=1;
sakthipriya 0:93514bd41116 458 wait(0.1);
sakthipriya 0:93514bd41116 459
sakthipriya 0:93514bd41116 460 ssn_gyr=0;
sakthipriya 0:93514bd41116 461 spi_acs.write(PWR_MGMT_1);
sakthipriya 0:93514bd41116 462 response=spi_acs.write(response|BIT_CLKSEL_X); //Selecting the X axis gyroscope as clock as mentioned above
sakthipriya 0:93514bd41116 463 ssn_gyr=1;
sakthipriya 0:93514bd41116 464 wait(0.1);
sakthipriya 0:93514bd41116 465
sakthipriya 0:93514bd41116 466 ssn_gyr=0;
sakthipriya 0:93514bd41116 467 spi_acs.write(GYRO_CONFIG|READFLAG); //sending GYRO_CONFIG address with read bit
sakthipriya 0:93514bd41116 468 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:93514bd41116 469 ssn_gyr=1;
sakthipriya 0:93514bd41116 470 wait(0.1);
sakthipriya 0:93514bd41116 471
sakthipriya 0:93514bd41116 472 ssn_gyr=0;
sakthipriya 0:93514bd41116 473 spi_acs.write(GYRO_CONFIG); //sending GYRO_CONFIG address to write to register
sakthipriya 0:93514bd41116 474 spi_acs.write(response&(~(BITS_FS_SEL_3|BITS_FS_SEL_4))); //selecting a full scale mode of +/=225 deg/sec
sakthipriya 0:93514bd41116 475 ssn_gyr=1;
sakthipriya 0:93514bd41116 476 wait(0.1);
sakthipriya 0:93514bd41116 477
sakthipriya 0:93514bd41116 478 ssn_gyr=0;
sakthipriya 0:93514bd41116 479 spi_acs.write(CONFIG|READFLAG); //sending CONFIG address with read bit
sakthipriya 0:93514bd41116 480 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:93514bd41116 481 ssn_gyr=1;
sakthipriya 0:93514bd41116 482 wait(0.1);
sakthipriya 0:93514bd41116 483
sakthipriya 0:93514bd41116 484 ssn_gyr=0;
sakthipriya 0:93514bd41116 485 spi_acs.write(CONFIG); //sending CONFIG address to write to register
sakthipriya 0:93514bd41116 486 spi_acs.write(response|BITS_DLPF_CFG); //selecting a bandwidth of 42 hz and delay of 4.8ms
sakthipriya 0:93514bd41116 487 ssn_gyr=1;
sakthipriya 0:93514bd41116 488 wait(0.1);
sakthipriya 0:93514bd41116 489
sakthipriya 0:93514bd41116 490 ssn_gyr=0;
sakthipriya 0:93514bd41116 491 spi_acs.write(SMPLRT_DIV|READFLAG); //sending SMPLRT_DIV address with read bit
sakthipriya 0:93514bd41116 492 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:93514bd41116 493 ssn_gyr=1;
sakthipriya 0:93514bd41116 494 wait(0.1);
sakthipriya 0:93514bd41116 495
sakthipriya 0:93514bd41116 496 ssn_gyr=0;
sakthipriya 0:93514bd41116 497 spi_acs.write(SMPLRT_DIV); //sending SMPLRT_DIV address to write to register
sakthipriya 0:93514bd41116 498 spi_acs.write(response&BITS_SMPLRT_DIV); //setting the sampling rate division to be 0 to make sample rate = gyroscopic output rate
sakthipriya 0:93514bd41116 499 ssn_gyr=1;
sakthipriya 0:93514bd41116 500 wait(0.1);
sakthipriya 0:93514bd41116 501
sakthipriya 0:93514bd41116 502 ssn_gyr=0;
sakthipriya 0:93514bd41116 503 spi_acs.write(INT_ENABLE|READFLAG); //sending address of INT_ENABLE with readflag
sakthipriya 0:93514bd41116 504 response=spi_acs.write(DUMMYBIT); //sending dummy byte to get the default values of the
sakthipriya 0:93514bd41116 505 // regiser
sakthipriya 0:93514bd41116 506 ssn_gyr=1;
sakthipriya 0:93514bd41116 507 wait(0.1);
sakthipriya 0:93514bd41116 508
sakthipriya 0:93514bd41116 509 ssn_gyr=0;
sakthipriya 0:93514bd41116 510 spi_acs.write(INT_ENABLE); //sending INT_ENABLE address to write to register
sakthipriya 0:93514bd41116 511 spi_acs.write(response|BIT_DATA_RDY_ENABLE); //Enbling data ready interrupt
sakthipriya 0:93514bd41116 512 ssn_gyr=1;
sakthipriya 0:93514bd41116 513 wait(0.1);
sakthipriya 0:93514bd41116 514
sakthipriya 0:93514bd41116 515 __enable_irq();
sakthipriya 0:93514bd41116 516 }
sakthipriya 0:93514bd41116 517
sakthipriya 0:93514bd41116 518 float * FUNC_ACS_EXEC_GYR()
sakthipriya 0:93514bd41116 519 {
sakthipriya 0:93514bd41116 520 printf("\nEntered gyro\n");
sakthipriya 0:93514bd41116 521 uint8_t response;
sakthipriya 0:93514bd41116 522 uint8_t MSB,LSB;
sakthipriya 0:93514bd41116 523 int16_t bit_data;
sakthipriya 0:93514bd41116 524 float data[3],error[3]={0,0,0}; //declaring error array to add to the values when required
sakthipriya 0:93514bd41116 525 float senstivity = 145.6; //senstivity is 145.6 for full scale mode of +/-225 deg/sec
sakthipriya 0:93514bd41116 526 ssn_gyr=0;
sakthipriya 0:93514bd41116 527 spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of INT_ENABLE with readflag
sakthipriya 0:93514bd41116 528 response=spi_acs.write(DUMMYBIT); //
sakthipriya 0:93514bd41116 529 ssn_gyr=1;
sakthipriya 0:93514bd41116 530 wait(0.1);
sakthipriya 0:93514bd41116 531
sakthipriya 0:93514bd41116 532 ssn_gyr=0;
sakthipriya 0:93514bd41116 533 spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
sakthipriya 0:93514bd41116 534 response=spi_acs.write(response&(~(BIT_SLEEP))); //waking up the gyroscope from sleep
sakthipriya 0:93514bd41116 535 ssn_gyr=1;
sakthipriya 0:93514bd41116 536 wait(0.1);
sakthipriya 0:93514bd41116 537
sakthipriya 0:93514bd41116 538 trFlag=1;
sakthipriya 0:93514bd41116 539 tr.attach(&trSub,1); //executes the function trSub afer 1sec
sakthipriya 0:93514bd41116 540
sakthipriya 0:93514bd41116 541 while(trFlag)
sakthipriya 0:93514bd41116 542 {
sakthipriya 0:93514bd41116 543 wait_ms(5); //This is required for this while loop to exit. I don't know why.
sakthipriya 0:93514bd41116 544 if(drFlag==1)
sakthipriya 0:93514bd41116 545 {
sakthipriya 0:93514bd41116 546 ssn_gyr=0;
sakthipriya 0:93514bd41116 547 spi_acs.write(GYRO_XOUT_H|READFLAG); //sending address of PWR_MGMT_1 with readflag
sakthipriya 0:93514bd41116 548 for(int i=0;i<3;i++)
sakthipriya 0:93514bd41116 549 {
sakthipriya 0:93514bd41116 550 MSB = spi_acs.write(DUMMYBIT); //reading the MSB values of x,y and z respectively
sakthipriya 0:93514bd41116 551 LSB = spi_acs.write(DUMMYBIT); //reading the LSB values of x,y and z respectively
sakthipriya 0:93514bd41116 552 bit_data= ((int16_t)MSB<<8)|LSB; //concatenating to get 16 bit 2's complement of the required gyroscope values
sakthipriya 0:93514bd41116 553 data[i]=(float)bit_data;
sakthipriya 0:93514bd41116 554 data[i]=data[i]/senstivity; //dividing with senstivity to get the readings in deg/sec
sakthipriya 0:93514bd41116 555 data[i]+=error[i]; //adding with error to remove offset errors
sakthipriya 0:93514bd41116 556 }
sakthipriya 0:93514bd41116 557 ssn_gyr=1;
sakthipriya 0:93514bd41116 558 for (int i=0;i<3;i++)
sakthipriya 0:93514bd41116 559 {
sakthipriya 0:93514bd41116 560 printf("%f\t",data[i]); //printing the angular velocity values
sakthipriya 0:93514bd41116 561 }
sakthipriya 0:93514bd41116 562 printf("\n");
sakthipriya 0:93514bd41116 563 break;
sakthipriya 0:93514bd41116 564 }
sakthipriya 0:93514bd41116 565 drFlag=0;
sakthipriya 0:93514bd41116 566 }
sakthipriya 0:93514bd41116 567 ssn_gyr=0;
sakthipriya 0:93514bd41116 568 spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of PWR_MGMT_1 with readflag
sakthipriya 0:93514bd41116 569 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:93514bd41116 570 ssn_gyr=1;
sakthipriya 0:93514bd41116 571 wait(0.1);
sakthipriya 0:93514bd41116 572
sakthipriya 0:93514bd41116 573 ssn_gyr=0;
sakthipriya 0:93514bd41116 574 spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
sakthipriya 0:93514bd41116 575 response=spi_acs.write(response|BIT_SLEEP); //setting the gyroscope in sleep mode
sakthipriya 0:93514bd41116 576 ssn_gyr=1;
sakthipriya 0:93514bd41116 577 wait(0.1);
sakthipriya 0:93514bd41116 578 printf("\nExited gyro\n");
sakthipriya 0:93514bd41116 579 return data;
sakthipriya 0:93514bd41116 580 }
sakthipriya 0:93514bd41116 581
sakthipriya 0:93514bd41116 582
sakthipriya 0:93514bd41116 583
sakthipriya 0:93514bd41116 584
sakthipriya 0:93514bd41116 585
sakthipriya 0:93514bd41116 586