green rosh / Mbed 2 deprecated pcb_test_v1_0

pcb test start

Dependencies:   mbed-rtos mbed

Fork of BAE_hw_test1_5 by sakthi priya amirtharaj

ACS.cpp@9:7936b618a879, 2015-04-06 (annotated)

Committer:
greenroshks
Date:
Mon Apr 06 15:53:24 2015 +0000
Revision:
9:7936b618a879
Parent:
8:6d856d863537 
pcb test start

Who changed what in which revision?

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