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