sakthi priya amirtharaj / Mbed 2 deprecated BAE_NEW_VR_0_0

not workin

Dependencies:   mbed-rtos mbed

Fork of BAE_FRDM_INTEGRATION by green rosh

ACS.cpp@9:6bcc165ee457, 2014-12-16 (annotated)

Committer:
sakthipriya
Date:
Tue Dec 16 11:37:09 2014 +0000
Revision:
9:6bcc165ee457
Parent:
8:667fbc82d634 
i2c not workin new integ.

Who changed what in which revision?

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