![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Filter for 9250
main.cpp@1:c9547742263c, 2019-08-06 (annotated)
- Committer:
- Edrum_x
- Date:
- Tue Aug 06 18:37:41 2019 +0000
- Revision:
- 1:c9547742263c
- Parent:
- 0:ccea261dce7a
to export into mbed studio
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
imanyonok | 0:ccea261dce7a | 1 | /* MPU9250 Basic Example Code |
imanyonok | 0:ccea261dce7a | 2 | by: Kris Winer |
imanyonok | 0:ccea261dce7a | 3 | date: April 1, 2014 |
imanyonok | 0:ccea261dce7a | 4 | license: Beerware - Use this code however you'd like. If you |
imanyonok | 0:ccea261dce7a | 5 | find it useful you can buy me a beer some time. |
imanyonok | 0:ccea261dce7a | 6 | |
imanyonok | 0:ccea261dce7a | 7 | Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, |
imanyonok | 0:ccea261dce7a | 8 | getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to |
imanyonok | 0:ccea261dce7a | 9 | allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and |
imanyonok | 0:ccea261dce7a | 10 | Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. |
imanyonok | 0:ccea261dce7a | 11 | |
imanyonok | 0:ccea261dce7a | 12 | SDA and SCL should have external pull-up resistors (to 3.3V). |
imanyonok | 0:ccea261dce7a | 13 | 10k resistors are on the EMSENSR-9250 breakout board. |
imanyonok | 0:ccea261dce7a | 14 | |
imanyonok | 0:ccea261dce7a | 15 | Hardware setup: |
imanyonok | 0:ccea261dce7a | 16 | MPU9250 Breakout --------- Arduino |
imanyonok | 0:ccea261dce7a | 17 | VDD ---------------------- 3.3V |
imanyonok | 0:ccea261dce7a | 18 | VDDI --------------------- 3.3V |
imanyonok | 0:ccea261dce7a | 19 | SDA ----------------------- A4 |
imanyonok | 0:ccea261dce7a | 20 | SCL ----------------------- A5 |
imanyonok | 0:ccea261dce7a | 21 | GND ---------------------- GND |
imanyonok | 0:ccea261dce7a | 22 | |
imanyonok | 0:ccea261dce7a | 23 | Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. |
imanyonok | 0:ccea261dce7a | 24 | Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1. |
imanyonok | 0:ccea261dce7a | 25 | We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. |
imanyonok | 0:ccea261dce7a | 26 | We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. |
imanyonok | 0:ccea261dce7a | 27 | */ |
imanyonok | 0:ccea261dce7a | 28 | |
imanyonok | 0:ccea261dce7a | 29 | #include "mbed.h" |
imanyonok | 0:ccea261dce7a | 30 | #include "MPU9250.h" |
Edrum_x | 1:c9547742263c | 31 | //#include "ST_F401_84MHZ.h" |
Edrum_x | 1:c9547742263c | 32 | #include "f_trapf.h" |
Edrum_x | 1:c9547742263c | 33 | #include "f_trapi.h" |
Edrum_x | 1:c9547742263c | 34 | #include "f_tri.h" |
Edrum_x | 1:c9547742263c | 35 | #include "max_.h" |
Edrum_x | 1:c9547742263c | 36 | #include "min_.h" |
Edrum_x | 1:c9547742263c | 37 | |
Edrum_x | 1:c9547742263c | 38 | |
Edrum_x | 1:c9547742263c | 39 | ////////////////GPIO///////////////////////////////////////////////////////////// |
Edrum_x | 1:c9547742263c | 40 | PwmOut PWM1(A2); |
Edrum_x | 1:c9547742263c | 41 | PwmOut PWM2(PB_1); |
Edrum_x | 1:c9547742263c | 42 | PwmOut PWM3(PB_0); |
Edrum_x | 1:c9547742263c | 43 | |
Edrum_x | 1:c9547742263c | 44 | DigitalOut EN1(D2); |
Edrum_x | 1:c9547742263c | 45 | DigitalOut EN2(A6); |
Edrum_x | 1:c9547742263c | 46 | DigitalOut EN3(D9); |
Edrum_x | 1:c9547742263c | 47 | |
Edrum_x | 1:c9547742263c | 48 | DigitalOut INA1(D10); |
Edrum_x | 1:c9547742263c | 49 | DigitalOut INB1(D11); |
Edrum_x | 1:c9547742263c | 50 | DigitalOut INA2(D12); |
Edrum_x | 1:c9547742263c | 51 | DigitalOut INB2(A0); |
Edrum_x | 1:c9547742263c | 52 | DigitalOut INA3(A1); |
Edrum_x | 1:c9547742263c | 53 | DigitalOut INB3(A3); |
Edrum_x | 1:c9547742263c | 54 | |
Edrum_x | 1:c9547742263c | 55 | ///////////////FIN GPIO////////////////////////////////////////////////////////// |
Edrum_x | 1:c9547742263c | 56 | int Max_degree=8; |
Edrum_x | 1:c9547742263c | 57 | uint8_t area; |
Edrum_x | 1:c9547742263c | 58 | int salida; |
Edrum_x | 1:c9547742263c | 59 | |
Edrum_x | 1:c9547742263c | 60 | //funcion de pertenencia para el error |
Edrum_x | 1:c9547742263c | 61 | int Ng_e,Np_e,Z_e,Pp_e,Pg_e; |
Edrum_x | 1:c9547742263c | 62 | |
Edrum_x | 1:c9547742263c | 63 | //funcion de pertenencia de delta error |
Edrum_x | 1:c9547742263c | 64 | int Ng_de,Np_de,Z_de,Pp_de,Pg_de; |
Edrum_x | 1:c9547742263c | 65 | |
Edrum_x | 1:c9547742263c | 66 | int Ng_u,Ng_u1,Ng_u2,Ng_u3,Ng_u4,Ng_u5,Ng_u6; |
Edrum_x | 1:c9547742263c | 67 | int Np_u,Np_u1,Np_u2,Np_u3,Np_u4; |
Edrum_x | 1:c9547742263c | 68 | int Z_u,Z_u1,Z_u2,Z_u3,Z_u4,Z_u5; |
Edrum_x | 1:c9547742263c | 69 | int Pp_u,Pp_u1,Pp_u2,Pp_u3,Pp_u4; |
Edrum_x | 1:c9547742263c | 70 | int Pg_u,Pg_u1,Pg_u2,Pg_u3,Pg_u4,Pg_u5,Pg_u6; |
Edrum_x | 1:c9547742263c | 71 | |
Edrum_x | 1:c9547742263c | 72 | |
Edrum_x | 1:c9547742263c | 73 | |
Edrum_x | 1:c9547742263c | 74 | |
Edrum_x | 1:c9547742263c | 75 | float Modulo=0,error1=0,error2=0,delta_error=0; |
Edrum_x | 1:c9547742263c | 76 | float phi=(2*PI)/3; |
Edrum_x | 1:c9547742263c | 77 | float theta; |
Edrum_x | 1:c9547742263c | 78 | |
Edrum_x | 1:c9547742263c | 79 | int a,b,c; |
Edrum_x | 1:c9547742263c | 80 | |
imanyonok | 0:ccea261dce7a | 81 | |
imanyonok | 0:ccea261dce7a | 82 | |
imanyonok | 0:ccea261dce7a | 83 | float sum = 0; |
imanyonok | 0:ccea261dce7a | 84 | uint32_t sumCount = 0; |
imanyonok | 0:ccea261dce7a | 85 | |
imanyonok | 0:ccea261dce7a | 86 | MPU9250 mpu9250; |
imanyonok | 0:ccea261dce7a | 87 | |
imanyonok | 0:ccea261dce7a | 88 | Timer t; |
imanyonok | 0:ccea261dce7a | 89 | |
imanyonok | 0:ccea261dce7a | 90 | Serial pc(USBTX, USBRX); // tx, rx |
imanyonok | 0:ccea261dce7a | 91 | |
imanyonok | 0:ccea261dce7a | 92 | |
imanyonok | 0:ccea261dce7a | 93 | int main() |
imanyonok | 0:ccea261dce7a | 94 | { |
imanyonok | 0:ccea261dce7a | 95 | pc.baud(9600); |
Edrum_x | 1:c9547742263c | 96 | |
imanyonok | 0:ccea261dce7a | 97 | //Set up I2C |
imanyonok | 0:ccea261dce7a | 98 | i2c.frequency(400000); // use fast (400 kHz) I2C |
imanyonok | 0:ccea261dce7a | 99 | |
Edrum_x | 1:c9547742263c | 100 | PWM1.period_us(500); |
Edrum_x | 1:c9547742263c | 101 | PWM2.period_us(500); |
Edrum_x | 1:c9547742263c | 102 | PWM3.period_us(500); |
Edrum_x | 1:c9547742263c | 103 | |
Edrum_x | 1:c9547742263c | 104 | |
Edrum_x | 1:c9547742263c | 105 | |
imanyonok | 0:ccea261dce7a | 106 | pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); |
imanyonok | 0:ccea261dce7a | 107 | |
imanyonok | 0:ccea261dce7a | 108 | t.start(); |
imanyonok | 0:ccea261dce7a | 109 | |
imanyonok | 0:ccea261dce7a | 110 | |
imanyonok | 0:ccea261dce7a | 111 | |
imanyonok | 0:ccea261dce7a | 112 | // Read the WHO_AM_I register, this is a good test of communication |
imanyonok | 0:ccea261dce7a | 113 | uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 |
imanyonok | 0:ccea261dce7a | 114 | pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r"); |
imanyonok | 0:ccea261dce7a | 115 | |
Edrum_x | 1:c9547742263c | 116 | if (whoami == whoami) // WHO_AM_I should always be 0x68 |
imanyonok | 0:ccea261dce7a | 117 | { |
imanyonok | 0:ccea261dce7a | 118 | pc.printf("MPU9250 is online...\n\r"); |
imanyonok | 0:ccea261dce7a | 119 | wait(1); |
imanyonok | 0:ccea261dce7a | 120 | |
imanyonok | 0:ccea261dce7a | 121 | |
imanyonok | 0:ccea261dce7a | 122 | mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration |
imanyonok | 0:ccea261dce7a | 123 | mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
imanyonok | 0:ccea261dce7a | 124 | //pc.printf("x gyro bias = %f\n\r", gyroBias[0]); |
imanyonok | 0:ccea261dce7a | 125 | //pc.printf("y gyro bias = %f\n\r", gyroBias[1]); |
imanyonok | 0:ccea261dce7a | 126 | //pc.printf("z gyro bias = %f\n\r", gyroBias[2]); |
imanyonok | 0:ccea261dce7a | 127 | //pc.printf("x accel bias = %f\n\r", accelBias[0]); |
imanyonok | 0:ccea261dce7a | 128 | //pc.printf("y accel bias = %f\n\r", accelBias[1]); |
imanyonok | 0:ccea261dce7a | 129 | //pc.printf("z accel bias = %f\n\r", accelBias[2]); |
imanyonok | 0:ccea261dce7a | 130 | wait(2); |
imanyonok | 0:ccea261dce7a | 131 | mpu9250.initMPU9250(); |
imanyonok | 0:ccea261dce7a | 132 | pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature |
imanyonok | 0:ccea261dce7a | 133 | mpu9250.initAK8963(magCalibration); |
imanyonok | 0:ccea261dce7a | 134 | pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer |
imanyonok | 0:ccea261dce7a | 135 | pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); |
imanyonok | 0:ccea261dce7a | 136 | pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); |
imanyonok | 0:ccea261dce7a | 137 | if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r"); |
imanyonok | 0:ccea261dce7a | 138 | if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r"); |
imanyonok | 0:ccea261dce7a | 139 | if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r"); |
imanyonok | 0:ccea261dce7a | 140 | if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r"); |
Edrum_x | 1:c9547742263c | 141 | |
imanyonok | 0:ccea261dce7a | 142 | wait(2); |
imanyonok | 0:ccea261dce7a | 143 | } |
imanyonok | 0:ccea261dce7a | 144 | else |
imanyonok | 0:ccea261dce7a | 145 | { |
imanyonok | 0:ccea261dce7a | 146 | pc.printf("Could not connect to MPU9250: \n\r"); |
imanyonok | 0:ccea261dce7a | 147 | pc.printf("%#x \n", whoami); |
imanyonok | 0:ccea261dce7a | 148 | |
imanyonok | 0:ccea261dce7a | 149 | |
imanyonok | 0:ccea261dce7a | 150 | |
imanyonok | 0:ccea261dce7a | 151 | while(1) ; // Loop forever if communication doesn't happen |
imanyonok | 0:ccea261dce7a | 152 | } |
imanyonok | 0:ccea261dce7a | 153 | |
imanyonok | 0:ccea261dce7a | 154 | mpu9250.getAres(); // Get accelerometer sensitivity |
imanyonok | 0:ccea261dce7a | 155 | mpu9250.getGres(); // Get gyro sensitivity |
imanyonok | 0:ccea261dce7a | 156 | mpu9250.getMres(); // Get magnetometer sensitivity |
imanyonok | 0:ccea261dce7a | 157 | //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes); |
imanyonok | 0:ccea261dce7a | 158 | //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes); |
imanyonok | 0:ccea261dce7a | 159 | //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes); |
imanyonok | 0:ccea261dce7a | 160 | magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated |
imanyonok | 0:ccea261dce7a | 161 | magbias[1] = +120.; // User environmental x-axis correction in milliGauss |
imanyonok | 0:ccea261dce7a | 162 | magbias[2] = +125.; // User environmental x-axis correction in milliGauss |
imanyonok | 0:ccea261dce7a | 163 | |
imanyonok | 0:ccea261dce7a | 164 | while(1) { |
imanyonok | 0:ccea261dce7a | 165 | |
imanyonok | 0:ccea261dce7a | 166 | // If intPin goes high, all data registers have new data |
imanyonok | 0:ccea261dce7a | 167 | if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt |
imanyonok | 0:ccea261dce7a | 168 | |
imanyonok | 0:ccea261dce7a | 169 | mpu9250.readAccelData(accelCount); // Read the x/y/z adc values |
imanyonok | 0:ccea261dce7a | 170 | // Now we'll calculate the accleration value into actual g's |
imanyonok | 0:ccea261dce7a | 171 | ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
imanyonok | 0:ccea261dce7a | 172 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
imanyonok | 0:ccea261dce7a | 173 | az = (float)accelCount[2]*aRes - accelBias[2]; |
imanyonok | 0:ccea261dce7a | 174 | |
imanyonok | 0:ccea261dce7a | 175 | mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values |
imanyonok | 0:ccea261dce7a | 176 | // Calculate the gyro value into actual degrees per second |
imanyonok | 0:ccea261dce7a | 177 | gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set |
imanyonok | 0:ccea261dce7a | 178 | gy = (float)gyroCount[1]*gRes - gyroBias[1]; |
imanyonok | 0:ccea261dce7a | 179 | gz = (float)gyroCount[2]*gRes - gyroBias[2]; |
imanyonok | 0:ccea261dce7a | 180 | |
imanyonok | 0:ccea261dce7a | 181 | mpu9250.readMagData(magCount); // Read the x/y/z adc values |
imanyonok | 0:ccea261dce7a | 182 | // Calculate the magnetometer values in milliGauss |
imanyonok | 0:ccea261dce7a | 183 | // Include factory calibration per data sheet and user environmental corrections |
imanyonok | 0:ccea261dce7a | 184 | mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set |
imanyonok | 0:ccea261dce7a | 185 | my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; |
imanyonok | 0:ccea261dce7a | 186 | mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; |
imanyonok | 0:ccea261dce7a | 187 | } |
imanyonok | 0:ccea261dce7a | 188 | |
imanyonok | 0:ccea261dce7a | 189 | Now = t.read_us(); |
imanyonok | 0:ccea261dce7a | 190 | deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
imanyonok | 0:ccea261dce7a | 191 | lastUpdate = Now; |
imanyonok | 0:ccea261dce7a | 192 | |
imanyonok | 0:ccea261dce7a | 193 | sum += deltat; |
imanyonok | 0:ccea261dce7a | 194 | sumCount++; |
imanyonok | 0:ccea261dce7a | 195 | |
imanyonok | 0:ccea261dce7a | 196 | // if(lastUpdate - firstUpdate > 10000000.0f) { |
imanyonok | 0:ccea261dce7a | 197 | // beta = 0.04; // decrease filter gain after stabilized |
imanyonok | 0:ccea261dce7a | 198 | // zeta = 0.015; // increasey bias drift gain after stabilized |
imanyonok | 0:ccea261dce7a | 199 | // } |
imanyonok | 0:ccea261dce7a | 200 | |
imanyonok | 0:ccea261dce7a | 201 | // Pass gyro rate as rad/s |
Edrum_x | 1:c9547742263c | 202 | mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
Edrum_x | 1:c9547742263c | 203 | mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
imanyonok | 0:ccea261dce7a | 204 | |
imanyonok | 0:ccea261dce7a | 205 | // Serial print and/or display at 0.5 s rate independent of data rates |
Edrum_x | 1:c9547742263c | 206 | //delt_t = (t.read_ms()) - count; |
Edrum_x | 1:c9547742263c | 207 | // if (delt_t > 100) { // update LCD once per half-second independent of read rate |
imanyonok | 0:ccea261dce7a | 208 | |
Edrum_x | 1:c9547742263c | 209 | //pc.printf("ax = %f", 1000*ax); |
Edrum_x | 1:c9547742263c | 210 | //pc.printf(" ay = %f", 1000*ay); |
Edrum_x | 1:c9547742263c | 211 | //pc.printf(" az = %f mg ", 1000*az); |
imanyonok | 0:ccea261dce7a | 212 | |
imanyonok | 0:ccea261dce7a | 213 | //pc.printf("gx = %f", gx); |
imanyonok | 0:ccea261dce7a | 214 | //pc.printf(" gy = %f", gy); |
imanyonok | 0:ccea261dce7a | 215 | //pc.printf(" gz = %f deg/s\n\r", gz); |
imanyonok | 0:ccea261dce7a | 216 | |
imanyonok | 0:ccea261dce7a | 217 | //pc.printf("gx = %f", mx); |
imanyonok | 0:ccea261dce7a | 218 | //pc.printf(" gy = %f", my); |
imanyonok | 0:ccea261dce7a | 219 | //pc.printf(" gz = %f mG\n\r", mz); |
imanyonok | 0:ccea261dce7a | 220 | |
imanyonok | 0:ccea261dce7a | 221 | tempCount = mpu9250.readTempData(); // Read the adc values |
imanyonok | 0:ccea261dce7a | 222 | temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade |
imanyonok | 0:ccea261dce7a | 223 | //pc.printf(" temperature = %f C\n\r", temperature); |
imanyonok | 0:ccea261dce7a | 224 | |
imanyonok | 0:ccea261dce7a | 225 | //pc.printf("q0 = %f\n\r", q[0]); |
imanyonok | 0:ccea261dce7a | 226 | //pc.printf("q1 = %f\n\r", q[1]); |
imanyonok | 0:ccea261dce7a | 227 | //pc.printf("q2 = %f\n\r", q[2]); |
imanyonok | 0:ccea261dce7a | 228 | //pc.printf("q3 = %f\n\r", q[3]); |
imanyonok | 0:ccea261dce7a | 229 | |
imanyonok | 0:ccea261dce7a | 230 | |
imanyonok | 0:ccea261dce7a | 231 | // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. |
imanyonok | 0:ccea261dce7a | 232 | // In this coordinate system, the positive z-axis is down toward Earth. |
imanyonok | 0:ccea261dce7a | 233 | // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. |
imanyonok | 0:ccea261dce7a | 234 | // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. |
imanyonok | 0:ccea261dce7a | 235 | // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. |
imanyonok | 0:ccea261dce7a | 236 | // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. |
imanyonok | 0:ccea261dce7a | 237 | // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be |
imanyonok | 0:ccea261dce7a | 238 | // applied in the correct order which for this configuration is yaw, pitch, and then roll. |
imanyonok | 0:ccea261dce7a | 239 | // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. |
imanyonok | 0:ccea261dce7a | 240 | yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); |
imanyonok | 0:ccea261dce7a | 241 | pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
imanyonok | 0:ccea261dce7a | 242 | roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); |
imanyonok | 0:ccea261dce7a | 243 | pitch *= 180.0f / PI; |
imanyonok | 0:ccea261dce7a | 244 | yaw *= 180.0f / PI; |
imanyonok | 0:ccea261dce7a | 245 | yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 |
imanyonok | 0:ccea261dce7a | 246 | roll *= 180.0f / PI; |
imanyonok | 0:ccea261dce7a | 247 | |
Edrum_x | 1:c9547742263c | 248 | //pc.printf("Yaw, Pitch, Roll: %f %f %f \n\r", yaw, pitch, roll); |
imanyonok | 0:ccea261dce7a | 249 | //pc.printf("average rate = %f\n\r", (float) sumCount/sum); |
imanyonok | 0:ccea261dce7a | 250 | |
imanyonok | 0:ccea261dce7a | 251 | myled= !myled; |
Edrum_x | 1:c9547742263c | 252 | //count = t.read_ms(); |
imanyonok | 0:ccea261dce7a | 253 | sum = 0; |
imanyonok | 0:ccea261dce7a | 254 | sumCount = 0; |
Edrum_x | 1:c9547742263c | 255 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 256 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 257 | //-----------------------------------------------MY CODE-------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 258 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 259 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 260 | |
Edrum_x | 1:c9547742263c | 261 | Modulo=sqrt(((pitch)*(pitch))+((roll)*(roll))); |
Edrum_x | 1:c9547742263c | 262 | |
Edrum_x | 1:c9547742263c | 263 | theta = atan2(pitch,roll); |
Edrum_x | 1:c9547742263c | 264 | |
Edrum_x | 1:c9547742263c | 265 | |
Edrum_x | 1:c9547742263c | 266 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 267 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 268 | //-----------------------------------------------FUZZY-------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 269 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 270 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 271 | |
Edrum_x | 1:c9547742263c | 272 | error1=Modulo; |
Edrum_x | 1:c9547742263c | 273 | delta_error=(error1-error2)*100; |
Edrum_x | 1:c9547742263c | 274 | |
Edrum_x | 1:c9547742263c | 275 | error2=error1; |
Edrum_x | 1:c9547742263c | 276 | |
Edrum_x | 1:c9547742263c | 277 | |
Edrum_x | 1:c9547742263c | 278 | //funcion de pertenencia del error |
Edrum_x | 1:c9547742263c | 279 | Ng_e=f_trapi(-Max_degree/2,-Max_degree/4,error1); |
Edrum_x | 1:c9547742263c | 280 | Np_e=f_tri(-Max_degree/2,0,error1); |
Edrum_x | 1:c9547742263c | 281 | Z_e=f_tri(-Max_degree/4,Max_degree/4,error1); |
Edrum_x | 1:c9547742263c | 282 | Pp_e=f_tri(0,Max_degree/2,error1); |
Edrum_x | 1:c9547742263c | 283 | Pg_e=f_trapf(Max_degree/4,Max_degree/2,error1); |
Edrum_x | 1:c9547742263c | 284 | |
Edrum_x | 1:c9547742263c | 285 | //funcion de pertenencia de delta error |
Edrum_x | 1:c9547742263c | 286 | Ng_de=f_trapi(-Max_degree/4,-Max_degree/8,delta_error); |
Edrum_x | 1:c9547742263c | 287 | Np_de=f_tri(-Max_degree/4,0,delta_error); |
Edrum_x | 1:c9547742263c | 288 | Z_de=f_tri(-Max_degree/8,Max_degree/8,delta_error); |
Edrum_x | 1:c9547742263c | 289 | Pp_de=f_tri(0,Max_degree/4,delta_error); |
Edrum_x | 1:c9547742263c | 290 | Pg_de=f_trapf(Max_degree/8,Max_degree/4,delta_error); |
Edrum_x | 1:c9547742263c | 291 | |
Edrum_x | 1:c9547742263c | 292 | //estados de salida Ng |
Edrum_x | 1:c9547742263c | 293 | |
Edrum_x | 1:c9547742263c | 294 | Ng_u1=min_(Ng_e,Ng_de); |
Edrum_x | 1:c9547742263c | 295 | Ng_u2=min_(Np_e,Ng_de); |
Edrum_x | 1:c9547742263c | 296 | Ng_u3=min_(Z_e,Ng_de); |
Edrum_x | 1:c9547742263c | 297 | Ng_u4=min_(Ng_e,Np_de); |
Edrum_x | 1:c9547742263c | 298 | Ng_u5=min_(Np_e,Np_de); |
Edrum_x | 1:c9547742263c | 299 | Ng_u6=min_(Ng_e,Z_de); |
Edrum_x | 1:c9547742263c | 300 | |
Edrum_x | 1:c9547742263c | 301 | Ng_u=max_(Ng_u1,max_(Ng_u2,max_(Ng_u3,max_(Ng_u4,max_(Ng_u5,Ng_u6))))); |
Edrum_x | 1:c9547742263c | 302 | |
Edrum_x | 1:c9547742263c | 303 | Np_u1=min_(Ng_e,Pp_de); |
Edrum_x | 1:c9547742263c | 304 | Np_u2=min_(Np_e,Z_de); |
Edrum_x | 1:c9547742263c | 305 | Np_u3=min_(Z_e,Np_de); |
Edrum_x | 1:c9547742263c | 306 | Np_u4=min_(Pp_e,Ng_de); |
Edrum_x | 1:c9547742263c | 307 | |
Edrum_x | 1:c9547742263c | 308 | Np_u=max_(Np_u1,max_(Np_u2,max_(Np_u3,Np_u4))); |
Edrum_x | 1:c9547742263c | 309 | |
Edrum_x | 1:c9547742263c | 310 | Z_u1=min_(Ng_e,Pg_de); |
Edrum_x | 1:c9547742263c | 311 | Z_u2=min_(Np_e,Pp_de); |
Edrum_x | 1:c9547742263c | 312 | Z_u3=min_(Z_e,Z_de); |
Edrum_x | 1:c9547742263c | 313 | Z_u4=min_(Pp_e,Np_de); |
Edrum_x | 1:c9547742263c | 314 | Z_u5=min_(Pg_e,Ng_de); |
Edrum_x | 1:c9547742263c | 315 | |
Edrum_x | 1:c9547742263c | 316 | Z_u=max_(Z_u1,max_(Z_u2,max_(Z_u3,max_(Z_u4,Z_u5)))); |
Edrum_x | 1:c9547742263c | 317 | |
Edrum_x | 1:c9547742263c | 318 | Pp_u1=min_(Pg_e,Np_de); |
Edrum_x | 1:c9547742263c | 319 | Pp_u2=min_(Pp_e,Z_de); |
Edrum_x | 1:c9547742263c | 320 | Pp_u3=min_(Z_e,Pp_de); |
Edrum_x | 1:c9547742263c | 321 | Pp_u4=min_(Np_e,Pg_de); |
Edrum_x | 1:c9547742263c | 322 | |
Edrum_x | 1:c9547742263c | 323 | Pp_u=max_(Pp_u1,max_(Pp_u2,max_(Pp_u3,Pp_u4))); |
Edrum_x | 1:c9547742263c | 324 | |
Edrum_x | 1:c9547742263c | 325 | Pg_u1=min_(Pg_e,Pg_de); |
Edrum_x | 1:c9547742263c | 326 | Pg_u2=min_(Pp_e,Pg_de); |
Edrum_x | 1:c9547742263c | 327 | Pg_u3=min_(Z_e,Pg_de); |
Edrum_x | 1:c9547742263c | 328 | Pg_u4=min_(Pg_e,Pp_de); |
Edrum_x | 1:c9547742263c | 329 | Pg_u5=min_(Pp_e,Pp_de); |
Edrum_x | 1:c9547742263c | 330 | Pg_u6=min_(Pg_e,Z_de); |
Edrum_x | 1:c9547742263c | 331 | |
Edrum_x | 1:c9547742263c | 332 | Pg_u=max_(Pg_u1,max_(Pg_u2,max_(Pg_u3,max_(Pg_u4,max_(Pg_u5,Pg_u6))))); |
Edrum_x | 1:c9547742263c | 333 | |
Edrum_x | 1:c9547742263c | 334 | area = (25*Ng_u)+(25*Np_u)+(25*Z_u)+(25*Pp_u)+(25*Pg_u)-(12.5*(25/((25/Ng_u)+(25/Np_u))))-(12.5*(25/((25/Np_u)+(25/Z_u))))-(12.5*(25/((25/Z_u)+(25/Pp_u))))-(12.5*(25/((25/Pp_u)+(25/Pg_u)))); |
Edrum_x | 1:c9547742263c | 335 | salida= ((-50*Ng_u)+(-25*Np_u)+(25*Pp_u)+(50*Pg_u))*25/area; |
Edrum_x | 1:c9547742263c | 336 | |
Edrum_x | 1:c9547742263c | 337 | |
Edrum_x | 1:c9547742263c | 338 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 339 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 340 | //-----------------------------------------------FUZZY END-------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 341 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 342 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 343 | |
Edrum_x | 1:c9547742263c | 344 | |
Edrum_x | 1:c9547742263c | 345 | a= (int)(Modulo * sin(theta))*40; |
Edrum_x | 1:c9547742263c | 346 | b= (int)(Modulo * sin(theta-phi))*40; |
Edrum_x | 1:c9547742263c | 347 | c= (int)(Modulo * sin(theta+phi))*40; |
Edrum_x | 1:c9547742263c | 348 | |
Edrum_x | 1:c9547742263c | 349 | |
Edrum_x | 1:c9547742263c | 350 | |
Edrum_x | 1:c9547742263c | 351 | |
Edrum_x | 1:c9547742263c | 352 | if(a<0){a=-a; EN1=1; INA1=0; INB1=1;} |
Edrum_x | 1:c9547742263c | 353 | else{ EN1=1; INA1=1; INB1=0;} |
Edrum_x | 1:c9547742263c | 354 | |
Edrum_x | 1:c9547742263c | 355 | |
Edrum_x | 1:c9547742263c | 356 | if(b<0){b=-b; EN2=1; INA2=0; INB2=1;} |
Edrum_x | 1:c9547742263c | 357 | else{ EN2=1; INA2=1; INB2=0;} |
Edrum_x | 1:c9547742263c | 358 | |
Edrum_x | 1:c9547742263c | 359 | |
Edrum_x | 1:c9547742263c | 360 | if(c<0){c=-c; EN3=1; INA3=0; INB3=1;} |
Edrum_x | 1:c9547742263c | 361 | else{ EN3=1; INA3=1; INB3=0;} |
Edrum_x | 1:c9547742263c | 362 | |
Edrum_x | 1:c9547742263c | 363 | if(a>500){ a=500;} |
Edrum_x | 1:c9547742263c | 364 | if(b>500){ b=500;} |
Edrum_x | 1:c9547742263c | 365 | if(c>500){ c=500;} |
Edrum_x | 1:c9547742263c | 366 | |
Edrum_x | 1:c9547742263c | 367 | |
Edrum_x | 1:c9547742263c | 368 | PWM1.pulsewidth_us(a); |
Edrum_x | 1:c9547742263c | 369 | PWM2.pulsewidth_us(b); |
Edrum_x | 1:c9547742263c | 370 | PWM3.pulsewidth_us(c); |
Edrum_x | 1:c9547742263c | 371 | |
Edrum_x | 1:c9547742263c | 372 | /* |
Edrum_x | 1:c9547742263c | 373 | PWM1.pulsewidth_us(250); |
Edrum_x | 1:c9547742263c | 374 | PWM2.pulsewidth_us(250); |
Edrum_x | 1:c9547742263c | 375 | PWM3.pulsewidth_us(250); |
Edrum_x | 1:c9547742263c | 376 | |
Edrum_x | 1:c9547742263c | 377 | wait(2); |
Edrum_x | 1:c9547742263c | 378 | |
Edrum_x | 1:c9547742263c | 379 | EN1=1; INA1=1; INB1=0; |
Edrum_x | 1:c9547742263c | 380 | EN2=1; INA2=1; INB2=0; |
Edrum_x | 1:c9547742263c | 381 | EN3=1; INA3=1; INB3=0; |
Edrum_x | 1:c9547742263c | 382 | |
Edrum_x | 1:c9547742263c | 383 | wait(2); |
Edrum_x | 1:c9547742263c | 384 | */ |
Edrum_x | 1:c9547742263c | 385 | //pc.printf("Yaw, Pitch, Roll, mod, theta, a, b, c : %f %f %f %f %f %i %i %i\n\r", yaw, pitch, roll, Modulo, theta,a,b,c); |
Edrum_x | 1:c9547742263c | 386 | |
Edrum_x | 1:c9547742263c | 387 | |
Edrum_x | 1:c9547742263c | 388 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 389 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 390 | //--------------------------------------------END OF MY CODE---------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 391 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 392 | //-------------------------------------------------------------------------------------------------------------------------------- |
Edrum_x | 1:c9547742263c | 393 | |
Edrum_x | 1:c9547742263c | 394 | //} |
imanyonok | 0:ccea261dce7a | 395 | } |
imanyonok | 0:ccea261dce7a | 396 | |
imanyonok | 0:ccea261dce7a | 397 | } |