bamlor nuttymaisuay
Dependencies: mbed
Diff: main.cpp
- Revision:
- 5:738285670edf
- Parent:
- 4:9cc307f25dc9
- Child:
- 6:1de63d5df56a
diff -r 9cc307f25dc9 -r 738285670edf main.cpp --- a/main.cpp Mon Dec 11 01:12:18 2017 +0000 +++ b/main.cpp Mon Dec 11 11:56:44 2017 +0000 @@ -33,7 +33,7 @@ -float x = 10,y = 0; +float x; float sum = 0; uint32_t sumCount = 0; char buffer[14]; @@ -43,67 +43,69 @@ Timer t; Serial pc(PA_15, PB_7); // tx, rx - - +float v, delt_t2 = 0, count2 = 0; +int mode; +char cmode; +Serial bam(D1,D0); BusOut B(PA_0,PA_1,PA_4,PB_0,PC_1,PC_2,PC_3), A(PC_10,PC_12,PA_13,PA_14,PC_13,PC_11,PD_2), C(PA_5,PA_6,PA_7,PC_7,PB_2,PB_1,PB_15); -float posA,posB,posC; +float posA = 0,posB,posC; int main() { - - pc.baud(9600); + mode = 0; + pc.baud(38400); //Set up I2C i2c.frequency(400000); // use fast (400 kHz) I2C - pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); + bam.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); t.start(); // Read the WHO_AM_I register, this is a good test of communication uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r"); + pc.printf("I AM 0x%x\n\r", whoami); bam.printf("I SHOULD BE 0x71\n\r"); if (whoami == 0x71) // WHO_AM_I should always be 0x68 { - pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami); - pc.printf("MPU9250 is online...\n\r"); + bam.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami); + bam.printf("MPU9250 is online...\n\r"); sprintf(buffer, "0x%x", whoami); wait(1); mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values - pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]); - pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]); - pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]); - pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]); - pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]); - pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]); + bam.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]); + bam.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]); + bam.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]); + bam.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]); + bam.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]); + bam.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]); mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - pc.printf("x gyro bias = %f\n\r", gyroBias[0]); - pc.printf("y gyro bias = %f\n\r", gyroBias[1]); - pc.printf("z gyro bias = %f\n\r", gyroBias[2]); - pc.printf("x accel bias = %f\n\r", accelBias[0]); - pc.printf("y accel bias = %f\n\r", accelBias[1]); - pc.printf("z accel bias = %f\n\r", accelBias[2]); + bam.printf("x gyro bias = %f\n\r", gyroBias[0]); + bam.printf("y gyro bias = %f\n\r", gyroBias[1]); + bam.printf("z gyro bias = %f\n\r", gyroBias[2]); + bam.printf("x accel bias = %f\n\r", accelBias[0]); + bam.printf("y accel bias = %f\n\r", accelBias[1]); + bam.printf("z accel bias = %f\n\r", accelBias[2]); wait(2); mpu9250.initMPU9250(); - pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature + bam.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature mpu9250.initAK8963(magCalibration); - pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer - pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); - pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); - if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r"); - if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r"); - if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r"); - if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r"); + bam.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer + bam.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); + bam.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); + if(Mscale == 0) bam.printf("Magnetometer resolution = 14 bits\n\r"); + if(Mscale == 1) bam.printf("Magnetometer resolution = 16 bits\n\r"); + if(Mmode == 2) bam.printf("Magnetometer ODR = 8 Hz\n\r"); + if(Mmode == 6) bam.printf("Magnetometer ODR = 100 Hz\n\r"); wait(1); } else { - pc.printf("Could not connect to MPU9250: \n\r"); - pc.printf("%#x \n", whoami); + bam.printf("Could not connect to MPU9250: \n\r"); + bam.printf("%#x \n", whoami); sprintf(buffer, "WHO_AM_I 0x%x", whoami); @@ -114,15 +116,16 @@ mpu9250.getAres(); // Get accelerometer sensitivity mpu9250.getGres(); // Get gyro sensitivity mpu9250.getMres(); // Get magnetometer sensitivity - pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes); - pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes); - pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes); + // pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes); +// pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes); +// pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes); magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated magbias[1] = +120.; // User environmental x-axis correction in milliGauss magbias[2] = +125.; // User environmental x-axis correction in milliGauss while(1) { + // If intPin goes high, all data registers have new data if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt @@ -180,15 +183,20 @@ // x = 13; // } // x=35; - if(gz<0) - { - x = 0; - } - x = 89.1 ; + // if(gz<0) +// { +// x = 0; +// } +// else{ +// x = 40 ; +// } + x = 97.5; posA += (gz*delt_t*x/1000000); posA = fmod(posA,360); posB = fmod(posA + 240,360); posC = fmod(posA + 120,360); + if (mode == 1) + { //pc.printf("x = %f", x); @@ -233,23 +241,43 @@ // break; // } - if (posA > 0 && posA < 12) + if (posA > 0 && posA <= 6) { A = 0x70; } - else if(posA > 12 && posA < 24 ) + else if (posA > 6 && posA <= 12) + { + A = 0x70; + } + else if(posA > 12 && posA <= 18 ) + { + A = 0x08; + } + else if(posA > 18 && posA <= 24 ) { A = 0x08; } - else if(posA > 24 && posA < 36 ) + else if(posA > 24 && posA <= 30 ) + { + A = 0x07; + } + else if(posA > 30 && posA <= 36 ) { A = 0x07; } - else if(posA > 36 && posA < 48 ) + else if(posA > 36 && posA <= 42 ) + { + A = 0x08; + } + else if(posA > 42 && posA <= 48 ) { A = 0x08; } - else if(posA > 48 && posA < 60 ) + else if(posA > 48 && posA <= 54 ) + { + A = 0x70; + } + else if(posA > 54 && posA <= 59 ) { A = 0x70; } @@ -257,23 +285,43 @@ { A = 0x00; } - if (posB > 0 && posB < 30) + if (posB > 0 && posB <= 6) { B = 0x70; } - else if(posB > 12 && posB < 24 ) + else if (posB > 6 && posB <= 12) + { + B = 0x70; + } + else if(posB > 12 && posB <= 18 ) + { + B = 0x08; + } + else if(posB > 18 && posB <= 24 ) { B = 0x08; } - else if(posB > 24 && posB < 36 ) + else if(posB > 24 && posB <= 30 ) + { + B = 0x07; + } + else if(posB > 30 && posB <= 36 ) { B = 0x07; } - else if(posB > 36 && posB < 48 ) + else if(posB > 36 && posB <= 42 ) + { + B = 0x08; + } + else if(posB > 42 && posB <= 48 ) { B = 0x08; } - else if(posB > 48 && posB < 60 ) + else if(posB > 48 && posB <= 54 ) + { + B = 0x70; + } + else if(posB > 54 && posB <= 59 ) { B = 0x70; } @@ -282,23 +330,43 @@ B = 0x00; } - if (posC > 0 && posC < 30) + if (posC > 0 && posC <= 6) { C = 0x70; } - else if(posC > 12 && posC < 24 ) + else if (posC > 6 && posC <= 12) + { + C = 0x70; + } + else if(posC > 12 && posC <= 18 ) + { + C = 0x08; + } + else if(posC > 18 && posC <= 24 ) { C = 0x08; } - else if(posC > 24 && posC < 36 ) + else if(posC > 24 && posC <= 30 ) + { + C = 0x07; + } + else if(posC > 30 && posC <= 36 ) { C = 0x07; - } - else if(posC > 36 && posC < 48 ) + } + else if(posC > 36 && posC <= 42 ) + { + C = 0x08; + } + else if(posC > 42 && posC <= 48 ) { C = 0x08; } - else if(posC > 48 && posC < 60 ) + else if(posC > 48 && posC <= 54 ) + { + C = 0x70; + } + else if(posC > 54&& posC <= 59 ) { C = 0x70; } @@ -306,14 +374,19 @@ { C = 0x00; } + } + else if (mode == 2) + { + + } // pc.printf("ax = %f", 1000*ax); // pc.printf(" ay = %f", 1000*ay); // pc.printf(" az = %f mg\n\r", 1000*az); // pc.printf("gx = %f", gx); // pc.printf(" gy = %f", gy); - pc.printf(" %.2f\r", gz); - pc.printf(" %.2f %.2f %.2f\n", posA, posB, posC); +// pc.printf(" %.2f\r", gz); +// pc.printf(" %.2f %.2f %.2f\n", posA, posB, posC); // pc.printf("gx = %f", mx); // pc.printf(" gy = %f", my); @@ -364,6 +437,27 @@ // sum = 0; // sumCount = 0; } +delt_t2 = t.read_us() - count2; +if (delt_t2 >= 1000000) +{ + v = (gz*x*PI*0.35*3600)/(180*1000*100); + pc.printf("%0.1f",v); + count2 = t.read_us(); +} +if (pc.readable()) +{ + cmode = pc.getc(); + switch (cmode) + { + case '?': + mode = 1; + break; + + case 'j' + mode = 2; + break; + } +} }