Project bike
bamlor nuttymaisuay
Dependencies: mbed
Diff: main.cpp
- Revision:
- 5:738285670edf
- Parent:
- 4:9cc307f25dc9
- Child:
- 6:1de63d5df56a
--- 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;
+ }
+}
}