Project bike
bamlor nuttymaisuay
Dependencies: mbed
Diff: main.cpp
- Revision:
- 4:9cc307f25dc9
- Parent:
- 3:46cc9d386ff4
- Child:
- 5:738285670edf
--- a/main.cpp Sun Dec 10 11:45:47 2017 +0000
+++ b/main.cpp Mon Dec 11 01:12:18 2017 +0000
@@ -33,7 +33,7 @@
-float x = 9,y = 0;
+float x = 10,y = 0;
float sum = 0;
uint32_t sumCount = 0;
char buffer[14];
@@ -46,11 +46,11 @@
-BusOut C(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), B(PA_5,PA_6,PA_7,PC_7,PB_2,PB_1,PB_15);
+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;
int main()
{
- A = 0x7F;
+
pc.baud(9600);
//Set up I2C
i2c.frequency(400000); // use fast (400 kHz) I2C
@@ -126,11 +126,11 @@
// 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
- mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
- // Now we'll calculate the accleration value into actual g's
- ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
- ay = (float)accelCount[1]*aRes - accelBias[1];
- az = (float)accelCount[2]*aRes - accelBias[2];
+ // mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
+// // Now we'll calculate the accleration value into actual g's
+// ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+// ay = (float)accelCount[1]*aRes - accelBias[1];
+// az = (float)accelCount[2]*aRes - accelBias[2];
mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
// Calculate the gyro value into actual degrees per second
@@ -138,20 +138,20 @@
gy = (float)gyroCount[1]*gRes - gyroBias[1];
gz = (float)gyroCount[2]*gRes - gyroBias[2];
- mpu9250.readMagData(magCount); // Read the x/y/z adc values
- // Calculate the magnetometer values in milliGauss
- // Include factory calibration per data sheet and user environmental corrections
- mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
- my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
- mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
- }
+// mpu9250.readMagData(magCount); // Read the x/y/z adc values
+// // Calculate the magnetometer values in milliGauss
+// // Include factory calibration per data sheet and user environmental corrections
+// mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
+// my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
+// mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+// }
- Now = t.read_us();
- deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
- lastUpdate = Now;
+ // Now = t.read_us();
+// deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+// lastUpdate = Now;
- sum += deltat;
- sumCount++;
+ //sum += deltat;
+// sumCount++;
// if(lastUpdate - firstUpdate > 10000000.0f) {
// beta = 0.04; // decrease filter gain after stabilized
@@ -163,19 +163,34 @@
mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
// Serial print and/or display at 0.5 s rate independent of data rates
- delt_t = t.read_ms() - count;
- if (delt_t > 5) { // update LCD once per half-second independent of read rate
- pc.printf("delt_t = %f\n",delt_t);
- pc.printf(" x = %f", x);
- y += 1;
- if (y == 2000)
+ delt_t = t.read_us() - count;
+ if (delt_t > 1) { // update LCD once per half-second independent of read rate
+ // pc.printf("delt_t = %f\n",delt_t);
+
+ // if (gz < 1)
+// {
+// x = 0;
+// }
+// else if (gz < 50)
+// {
+// x = 19;
+// }
+// else
+// {
+// x = 13;
+// }
+ // x=35;
+ if(gz<0)
{
- x+=0.5;
- y = 0;
- }
- posA += (gz*delt_t*x/1000);
- posB = posA + 120;
- posC = posA + 240;
+ x = 0;
+ }
+ x = 89.1 ;
+ posA += (gz*delt_t*x/1000000);
+ posA = fmod(posA,360);
+ posB = fmod(posA + 240,360);
+ posC = fmod(posA + 120,360);
+ //pc.printf("x = %f", x);
+
// if (posA >= 360 && posA < 720)
// {
@@ -217,38 +232,76 @@
// {
// break;
// }
- if (posA >= 360)
+
+ if (posA > 0 && posA < 12)
{
- posA-=360;
+ A = 0x70;
+ }
+ else if(posA > 12 && posA < 24 )
+ {
+ A = 0x08;
}
- if (posA > 0 && posA < 30)
+ else if(posA > 24 && posA < 36 )
+ {
+ A = 0x07;
+ }
+ else if(posA > 36 && posA < 48 )
{
- A = 0x7F;
- }
+ A = 0x08;
+ }
+ else if(posA > 48 && posA < 60 )
+ {
+ A = 0x70;
+ }
else
{
A = 0x00;
}
- if (posB >= 360)
- {
- posB-=360;
- }
if (posB > 0 && posB < 30)
{
- B = 0x7F;
+ B = 0x70;
+ }
+ else if(posB > 12 && posB < 24 )
+ {
+ B = 0x08;
}
+ else if(posB > 24 && posB < 36 )
+ {
+ B = 0x07;
+ }
+ else if(posB > 36 && posB < 48 )
+ {
+ B = 0x08;
+ }
+ else if(posB > 48 && posB < 60 )
+ {
+ B = 0x70;
+ }
else
{
B = 0x00;
}
- if (posC >= 360)
- {
- posC-=360;
- }
+
if (posC > 0 && posC < 30)
{
- C = 0x7F;
+ C = 0x70;
+ }
+ else if(posC > 12 && posC < 24 )
+ {
+ C = 0x08;
}
+ else if(posC > 24 && posC < 36 )
+ {
+ C = 0x07;
+ }
+ else if(posC > 36 && posC < 48 )
+ {
+ C = 0x08;
+ }
+ else if(posC > 48 && posC < 60 )
+ {
+ C = 0x70;
+ }
else
{
C = 0x00;
@@ -259,15 +312,15 @@
// pc.printf("gx = %f", gx);
// pc.printf(" gy = %f", gy);
- pc.printf(" gz = %f deg/s\n\r", gz);
- pc.printf(" posA = %f\n", posA);
+ pc.printf(" %.2f\r", gz);
+ pc.printf(" %.2f %.2f %.2f\n", posA, posB, posC);
// pc.printf("gx = %f", mx);
// pc.printf(" gy = %f", my);
// pc.printf(" gz = %f mG\n\r", mz);
- tempCount = mpu9250.readTempData(); // Read the adc values
- temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
+ // tempCount = mpu9250.readTempData(); // Read the adc values
+// temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
// pc.printf(" temperature = %f C\n\r", temperature);
// pc.printf("q0 = %f\n\r", q[0]);
@@ -286,13 +339,13 @@
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
- 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]);
- pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
- 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]);
- pitch *= 180.0f / PI;
- yaw *= 180.0f / PI;
- yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
- roll *= 180.0f / PI;
+// 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]);
+// pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+// 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]);
+// pitch *= 180.0f / PI;
+// yaw *= 180.0f / PI;
+// yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
+// roll *= 180.0f / PI;
// pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
// pc.printf("average rate = %f\n\r", (float) sumCount/sum);
@@ -300,18 +353,19 @@
// sprintf(buffer, "rate = %f", (float) sumCount/sum);
//
- count = t.read_ms();
+ count = t.read_us();
- if(count > 1<<21) {
- t.start(); // start the timer over again if ~30 minutes has passed
- count = 0;
- deltat= 0;
- lastUpdate = t.read_us();
- }
- sum = 0;
- sumCount = 0;
+ // if(count > 1<<21) {
+// t.start(); // start the timer over again if ~30 minutes has passed
+// count = 0;
+// deltat= 0;
+// lastUpdate = t.read_us();
+// }
+// sum = 0;
+// sumCount = 0;
}
}
- pc.printf("fuck you");
+
+}
}
\ No newline at end of file