green rosh / Mbed 2 deprecated BAE_vr2_1_1

The one with the new HK

Dependencies:   mbed-rtos mbed

Fork of BAE_vr2_1_1 by Seeker of Truth ,

ACS.cpp@13:1b37d98840d3, 2014-12-17 (annotated)

Committer:
greenroshks
Date:
Wed Dec 17 05:25:04 2014 +0000
Revision:
13:1b37d98840d3
Parent:
0:8b0d43fe6c05 
The one with the new HK

Who changed what in which revision?

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