sakthi priya amirtharaj / Mbed 2 deprecated BAE_working_always

i2c working version

Dependencies:   mbed-rtos mbed

ACS.cpp@0:7882d03f59e2, 2015-04-09 (annotated)

Committer:
sakthipriya
Date:
Thu Apr 09 22:44:39 2015 +0000
Revision:
0:7882d03f59e2
i2c working version

Who changed what in which revision?

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