Suchakhree Srisukprom
k
Revision 1:426fbd0d126a, committed 2015-12-09
- Comitter:
- Suchakhree
- Date:
- Wed Dec 09 10:19:11 2015 +0000
- Parent:
- 0:238df339023b
- Commit message:
- ????????????????? ????????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GetAcceloroY.cpp Wed Dec 09 10:19:11 2015 +0000
@@ -0,0 +1,125 @@
+/*
+//int main()
+ t.start();
+ //___ Set up I2C: use fast (400 kHz) I2C ___
+ i2c.frequency(400000);
+ // Read the WHO_AM_I register, this is a good test of communication
+ whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
+ pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
+ if (I2Cstate != 0) // error on I2C
+ pc.printf("I2C failure while reading WHO_AM_I register");
+
+ if (whoami == 0x71) // WHO_AM_I should always be 0x71
+ {
+ pc.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 (accelerometer and gyroscope self test)
+ mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers
+ wait(2);
+ //Initialize device for active mode read of acclerometer, gyroscope, and temperature
+ mpu9250.initMPU9250();
+ pc.printf("MPU9250 initialized for active data mode....\n\r");
+ //Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
+ mpu9250.initAK8963(magCalibration);
+ wait(1);
+ }
+
+ else // Connection failure
+ {
+ pc.printf("Could not connect to MPU9250: \n\r");
+ pc.printf("%#x \n", whoami);
+ sprintf(buffer, "WHO_AM_I 0x%x", whoami);
+ //while(1) ; // Loop forever if communication doesn't happen
+ }
+
+ mpu9250.getAres(); // Get accelerometer sensitivity
+ mpu9250.getGres(); // Get gyro sensitivity
+ mpu9250.getMres(); // Get magnetometer sensitivity
+ 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
+
+
+ Ay[0]=GetAcceloroY(); pc.printf("1: %d\n",Ay[0]);
+ Ay[1]=GetAcceloroY(); pc.printf("2: %d\n",Ay[1]);
+ if( Ay[1]+Ay[0]< -20 ) pc.printf("#BR$\n\n\n");
+
+
+//Function 4 GetAcceloroY
+
+int GetAcceloroY()
+{
+ // 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
+ if (I2Cstate != 0) //error on I2C
+ pc.printf("I2C error ocurred while reading accelerometer data. I2Cstate = %d \n\r", I2Cstate);
+ else{ // I2C read or write ok
+ I2Cstate = 1;
+ 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
+ if (I2Cstate != 0) //error on I2C
+ pc.printf("I2C error ocurred while reading gyrometer data. I2Cstate = %d \n\r", I2Cstate);
+ else{ // I2C read or write ok
+ I2Cstate = 1;
+ gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
+ 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
+ if (I2Cstate != 0) //error on I2C
+ pc.printf("I2C error ocurred while reading magnetometer data. I2Cstate = %d \n\r", I2Cstate);
+ else{ // I2C read or write ok
+ I2Cstate = 1;
+ 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.getCompassOrientation(orientation);
+ }
+
+ 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++;
+
+ // Pass gyro rate as rad/s
+ // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+ 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 1.5 s rate independent of data rates
+ delt_t = t.read_ms() - count;
+ if (delt_t > 500) { // update LCD once per half-second independent of read rate
+ mpu9250.MadgwickQuaternionUpdate(ax,ay,az,gx,gy,gz,mx,my,mz);
+ pc.printf(" ay = %.2f\n", 1000*ay);
+ tempCount = mpu9250.readTempData(); // Read the adc values
+ if (I2Cstate != 0) //error on I2C
+ pc.printf("I2C error ocurred while reading sensor temp. I2Cstate = %d \n\r", I2Cstate);
+ else{ // I2C read or write ok
+ I2Cstate = 1;
+ temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
+ //pc.printf(" temperature = %f C\n\r", temperature);
+ }
+ count = t.read_ms();
+ 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;
+ }
+ return ay*1000;
+}*/
\ No newline at end of file
--- a/GetDistance2LED.cpp Mon Dec 07 12:20:46 2015 +0000
+++ b/GetDistance2LED.cpp Wed Dec 09 10:19:11 2015 +0000
@@ -1,10 +1,10 @@
//Function 4 getdistance2led
-/*DigitalOut pinled0(PC_3); DigitalOut pinled1(PC_2); DigitalOut pinled2(PH_1);
-DigitalOut pinled3(PH_0); DigitalOut pinled4(PC_15); DigitalOut pinled5(PC_14);
-DigitalOut pinled6(PC_13); DigitalOut pinled7(PB_7); DigitalOut pinled8(PA_15);
-DigitalOut pinled9(PA_14); DigitalOut pinbuzz(PA_13);
+/*DigitalOut pinled0(PC_8); DigitalOut pinled1(PC_6); DigitalOut pinled2(PC_5);
+DigitalOut pinled3(PA_12); DigitalOut pinled4(PA_11); DigitalOut pinled5(PB_12);
+DigitalOut pinled6(PB_2); DigitalOut pinled7(PB_15); DigitalOut pinled8(PB_14);
+DigitalOut pinled9(PB_13); DigitalOut pinbuzz(D3);
void getdistance2led(char step)
{
--- a/Getswitch2Send.cpp Mon Dec 07 12:20:46 2015 +0000
+++ b/Getswitch2Send.cpp Wed Dec 09 10:19:11 2015 +0000
@@ -1,10 +1,35 @@
-
-/*DigitalIn switch1(D5);
-DigitalIn switch2(D6);
-DigitalIn switch3(D7);
+/*
+//Setup
+InterruptIn switch1(D11);
+InterruptIn switch2(D10);
+InterruptIn switch3(D7);
+void switch1();
+void switch2();
+void switch3();
+int matrix_state=0;
- if(switch1==1) pc.printf("#TR$");
- if(switch2==1) pc.printf("#TL$");
- if(switch3==1) pc.printf("#BR$");
- else pc.printf("#MED$");
- */
\ No newline at end of file
+
+ // int main()
+ switch1.rise(&switch_1);
+ switch2.rise(&switch_2);
+ switch3.rise(&switch_3);
+ if(matrix_state == 0){mobile.printf("#MED$"); pc.printf("#MED$");}
+ else if(matrix_state == 1){mobile.printf("#TR$"); pc.printf("#TR$\n");}
+ else if(matrix_state == 2){mobile.printf("#TL$"); pc.printf("#TL$\n");}
+ else if(matrix_state == 3){mobile.printf("#BR$"); pc.printf("#BR$\n");}
+ if(switch1 == 0 && switch2 == 0 & switch3 == 0 ) matrix_state = 0;
+
+//Function
+void switch_1()
+{
+ matrix_state=1;
+}
+void switch_2()
+{
+ matrix_state=2;
+}
+void switch_3()
+{
+ matrix_state=3;
+}*/
+
\ No newline at end of file
--- a/SendAcceloroY.cpp Mon Dec 07 12:20:46 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,85 +0,0 @@
-
-//Function 4 SenAcceloroY
-
-/*int SenAcceloroY()
-{
- // 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
- if (I2Cstate != 0) //error on I2C
- pc.printf("I2C error ocurred while reading accelerometer data. I2Cstate = %d \n\r", I2Cstate);
- else{ // I2C read or write ok
- I2Cstate = 1;
- 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
- if (I2Cstate != 0) //error on I2C
- pc.printf("I2C error ocurred while reading gyrometer data. I2Cstate = %d \n\r", I2Cstate);
- else{ // I2C read or write ok
- I2Cstate = 1;
- gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
- 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
- if (I2Cstate != 0) //error on I2C
- pc.printf("I2C error ocurred while reading magnetometer data. I2Cstate = %d \n\r", I2Cstate);
- else{ // I2C read or write ok
- I2Cstate = 1;
- 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.getCompassOrientation(orientation);
- }
-
- 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++;
-
- // Pass gyro rate as rad/s
- // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
- 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 1.5 s rate independent of data rates
- delt_t = t.read_ms() - count;
- if (delt_t > 500) { // update LCD once per half-second independent of read rate
- mpu9250.MadgwickQuaternionUpdate(ax,ay,az,gx,gy,gz,mx,my,mz);
-
- pc.printf(" ay = %.2f\n", 1000*ay);
-
- tempCount = mpu9250.readTempData(); // Read the adc values
- if (I2Cstate != 0) //error on I2C
- pc.printf("I2C error ocurred while reading sensor temp. I2Cstate = %d \n\r", I2Cstate);
- else{ // I2C read or write ok
- I2Cstate = 1;
- temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
- //pc.printf(" temperature = %f C\n\r", temperature);
- }
-
-
- myled= !myled;
- count = t.read_ms();
-
- 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;
- }
- return ay*1000;
-}*/
\ No newline at end of file
--- a/TextLCD.lib Mon Dec 07 12:20:46 2015 +0000 +++ b/TextLCD.lib Wed Dec 09 10:19:11 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/wim/code/TextLCD/#111ca62e8a59 +https://developer.mbed.org/teams/FRA221_2015/code/TextLCD/#daa61eaff9c6
--- a/main.cpp Mon Dec 07 12:20:46 2015 +0000
+++ b/main.cpp Wed Dec 09 10:19:11 2015 +0000
@@ -2,9 +2,7 @@
#include "MPU9250.h"
#include "TextLCD.h"
-int gyrol_Break();
-
-Serial pc(USBTX, USBRX); // Huyperterminal default config: 9600 bauds, 8-bit data, 1 stop bit, no parity
+Serial pc(USBTX, USBRX); // Huyperterminal default config: 9600 bauds
Serial mobile(D8,D2);
//mobile.baud(38400);
@@ -15,31 +13,34 @@
I2C i2c_lcd(D14,D15); // SDA, SCL
TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4); // I2C bus, PCF8574 Slaveaddress, LCD Type
-char pack[8]={0};
+char pack[14]={0};
-int Ay=0;
+int Ay[2]={0};
float sum=0;
uint32_t sumCount = 0;
char buffer[14];
uint8_t dato_leido[2];
uint8_t whoami;
-int SendAcceloroY();
+int GetAcceloroY();
-DigitalOut pinled0(PC_3); DigitalOut pinled1(PC_2); DigitalOut pinled2(PC_13);
-DigitalOut pinled3(PB_7); DigitalOut pinled4(PA_15); DigitalOut pinled5(PA_13);
-DigitalOut pinled6(PC_12); DigitalOut pinled7(PC_10); DigitalOut pinled8(PB_4);
-DigitalOut pinled9(PB_10); DigitalOut pinbuzz(PA_8);
+DigitalOut pinled0(PC_8); DigitalOut pinled1(PC_6); DigitalOut pinled2(PC_5);
+DigitalOut pinled3(PA_12); DigitalOut pinled4(PA_11); DigitalOut pinled5(PB_12);
+DigitalOut pinled6(PB_2); DigitalOut pinled7(PB_15); DigitalOut pinled8(PB_14);
+DigitalOut pinled9(PB_13); DigitalOut pinbuzz(D3);
void resetled();
void getdistance2led(char step);
-DigitalIn switch1(D9);
-DigitalIn switch2(D10);
-DigitalIn switch3(D11);
-void switch_turn();
-int state = 0;
+InterruptIn switch1(D11);//TR
+InterruptIn switch2(D10);//TL
+InterruptIn switch3(D7);//BR
+void switch_1();
+void switch_2();
+void switch_3();
+int matrix_state=0;
int main() {
-
+
+
//___ Set up I2C: use fast (400 kHz) I2C ___
i2c.frequency(400000);
// Read the WHO_AM_I register, this is a good test of communication
@@ -57,10 +58,10 @@
mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values (accelerometer and gyroscope self test)
mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers
wait(2);
- // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+ //Initialize device for active mode read of acclerometer, gyroscope, and temperature
mpu9250.initMPU9250();
pc.printf("MPU9250 initialized for active data mode....\n\r");
- // Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
+ //Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
mpu9250.initAK8963(magCalibration);
wait(1);
}
@@ -70,73 +71,90 @@
pc.printf("Could not connect to MPU9250: \n\r");
pc.printf("%#x \n", whoami);
sprintf(buffer, "WHO_AM_I 0x%x", whoami);
- while(1) ; // Loop forever if communication doesn't happen
+ //while(1) ; // Loop forever if communication doesn't happen
}
- mpu9250.getAres(); // Get accelerometer sensitivity
- mpu9250.getGres(); // Get gyro sensitivity
- mpu9250.getMres(); // Get magnetometer sensitivity
- 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
+ mpu9250.getAres();
lcd.locate(0,0);
- lcd.printf("FRA241 Software-Dev.");
+ lcd.printf("FRA221 DigitalElec.");
lcd.locate(0,1);
lcd.printf("Bike_Light");
resetled();
+ t.start();
+
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
while(1)
- {
- if(switch1 == 1 || switch2 == 1 || switch3 == 1) mobile.printf("#");
- if(switch1 == 1) {mobile.printf("#TR$"); lcd.locate(0,3); lcd.printf("TurnRight"); }
- if(switch2 == 1) {mobile.printf("#TL$"); lcd.locate(0,3); lcd.printf("TurnLeft.");}
- if(switch3 == 1) {mobile.printf("#BR$"); lcd.locate(0,3); lcd.printf("Break.");}
- else {mobile.printf("#MD$"); lcd.locate(0,3); lcd.printf("None. ");}
-
- //switch_turn();
- //Read data
- /* if(mobile.readable())
+ {
+
+ if(t.read_ms()<=50)
{
- pack[0] = mobile.getc();
- for(int i=0;i<8;i++)
- pack[i] = mobile.getc();
-
- //Debug_Serial
- //for(int i=0;i<8;i++)
- // pc.printf("%c",pack[i]);
- //pc.printf("\n");
-
- lcd.locate(0,2);
- for(int i=0;i<8;i++) lcd.printf("%c",pack[i]);
-
- if(pack[1] == '#')
- {
-
- if(pack[2] == 'D' && pack[5] == 'S')
- {
- getdistance2led(pack[4]);
- lcd.locate(0,2);
- lcd.printf("SPEED : %c%c km/hr",pack[6],pack[7]);
-
- //switch1.rise(&switch_turn);
-
- //for(int i; ((switch1 == 1 || switch2 == 1 || switch3 == 1 )&& state == 0);) {mobile.printf("#"); break;}
- //for(;switch1 == 1 && state == 0;) {mobile.printf("#TR$"); lcd.locate(0,3); lcd.printf("TurnRight"); state=1; break;}
- //else if(switch2 == 1 && state == 0) {mobile.printf("#TL$"); lcd.locate(0,3); lcd.printf("TurnLeft."); state=1;}
- //else if(switch3 == 1 && state == 0) {mobile.printf("#BR$"); lcd.locate(0,3); lcd.printf("Break. "); state=1;}
- //if((switch1 == 0 || switch2 == 0 || switch3 == 0 )&& state == 1){mobile.printf("#"); mobile.printf("#MD$"); lcd.locate(0,3); lcd.printf("Clear."); state=0;}
-
- }*/
-
- //}
-
- }
-
+ //Read data
+ /*if(mobile.readable())
+ {
+ //pack[0] = mobile.getc();
+ for(int i=0;i<13;i++)
+ pack[i] = mobile.getc();
+
+ //Debug_Serial
+ for(int i=0;i<12;i++)
+ pc.printf("%c",pack[i]);
+ pc.printf("\n");
+ //lcd.locate(0,2);
+ //for(int i=0;i<14;i++) lcd.printf("%c",pack[i]);
+
+ if(pack[0] == '#')
+ {
+
+ if(pack[1] == 'D' && pack[4] == 'S' && pack[7] == 'K')
+ {
+ getdistance2led(pack[3]);
+ lcd.locate(0,2);
+ lcd.printf("SPEED : %c%c km/hr",pack[5],pack[6]);
+ lcd.locate(0,3);
+ lcd.printf("Distance: %c.%c%c km.",pack[8],pack[9],pack[10]);
+ //pc.printf("read\n");
+ }
+ }
+ }*/
+
+ Ay[0]=GetAcceloroY(); //pc.printf("1: %d\n",Ay[0]);
+ Ay[1]=GetAcceloroY(); //pc.printf("2: %d\n",Ay[1]);
+ if( Ay[1]+Ay[0]< -150 ) {matrix_state=3; pc.printf("#BR$\n\n\n"); }
+ switch1.rise(&switch_1);
+ switch2.rise(&switch_2);
+ switch3.rise(&switch_3);
+ if(matrix_state == 0){mobile.printf("#MED$"); pc.printf("#MED$");}
+ else if(matrix_state == 1){mobile.printf("#TR$"); pc.printf("#TR$\n");}
+ else if(matrix_state == 2){mobile.printf("#TL$"); pc.printf("#TL$\n");}
+ else if(matrix_state == 3){mobile.printf("#BR$"); pc.printf("#BR$\n");}
+ if(switch1 == 0 && switch2 == 0 && switch3 == 0) matrix_state = 0;
+ //if(switch1 == 0 && switch2 == 0 && switch3 == 0 && matrix_state == 0) {matrix_state = 1; mobile.printf("#MED$"); pc.printf("#MED$");}
+ t.reset();
+ }
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
+void switch_1()
+{
+ matrix_state=1;
+ //matrix_state = 0;
+ //mobile.printf("#TR$");
+}
+void switch_2()
+{
+ matrix_state=2;
+ //matrix_state = 0;
+ //mobile.printf("#TL$");
+}
+void switch_3()
+{
+ matrix_state=3;
+ //matrix_state = 0;
+ //mobile.printf("#BR$");
+}
+
void getdistance2led(char step)
{
resetled();
@@ -155,9 +173,9 @@
else if(step == '6'){
pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=0; pinled8=0; pinled9=0;}
else if(step == '7'){
- pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=0; pinled9=0; pinbuzz=1;}
+ pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=0; pinled9=0;}
else if(step == '8'){
- pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=1; pinled9=0; pinbuzz=1;}
+ pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=1; pinled9=0;}
else if(step == '9'){
pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=1; pinled9=1; pinbuzz=1;}
else { pinled0=0; pinled1=0; pinled2=0; pinled3=0; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0; pinbuzz=0; }
@@ -169,96 +187,19 @@
pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;
pinbuzz=0;
}
-
-
//Function 4 SenAcceloroY
-void switch_turn()
-{
- if(switch1 == 1 || switch2 == 1 || switch3 == 1) mobile.printf("#");
- if(switch1 == 1) {mobile.printf("#TR$"); lcd.locate(0,3); lcd.printf("TurnRight"); }
- if(switch2 == 1) {mobile.printf("#TL$"); lcd.locate(0,3); lcd.printf("TurnLeft.");}
- if(switch3 == 1) {mobile.printf("#BR$"); lcd.locate(0,3); lcd.printf("Break.");}
- else {mobile.printf("#MD$"); lcd.locate(0,3); lcd.printf("None. ");}
-}
-int SendAcceloroY()
+int GetAcceloroY()
{
// 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
- if (I2Cstate != 0) //error on I2C
- pc.printf("I2C error ocurred while reading accelerometer data. I2Cstate = %d \n\r", I2Cstate);
- else{ // I2C read or write ok
I2Cstate = 1;
- 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
- if (I2Cstate != 0) //error on I2C
- pc.printf("I2C error ocurred while reading gyrometer data. I2Cstate = %d \n\r", I2Cstate);
- else{ // I2C read or write ok
- I2Cstate = 1;
- gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
- 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
- if (I2Cstate != 0) //error on I2C
- pc.printf("I2C error ocurred while reading magnetometer data. I2Cstate = %d \n\r", I2Cstate);
- else{ // I2C read or write ok
- I2Cstate = 1;
- 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.getCompassOrientation(orientation);
- }
-
- 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++;
-
- // Pass gyro rate as rad/s
- // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
- 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 1.5 s rate independent of data rates
- delt_t = t.read_ms() - count;
- if (delt_t > 500) { // update LCD once per half-second independent of read rate
- mpu9250.MadgwickQuaternionUpdate(ax,ay,az,gx,gy,gz,mx,my,mz);
-
- pc.printf(" ay = %.2f\n", 1000*ay);
-
- tempCount = mpu9250.readTempData(); // Read the adc values
- if (I2Cstate != 0) //error on I2C
- pc.printf("I2C error ocurred while reading sensor temp. I2Cstate = %d \n\r", I2Cstate);
- else{ // I2C read or write ok
- I2Cstate = 1;
- temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
- //pc.printf(" temperature = %f C\n\r", temperature);
- }
-
-
- myled= !myled;
- count = t.read_ms();
-
- 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;
- }
+
+ mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+ mpu9250.MadgwickQuaternionUpdate(ax,ay,az,gx,gy,gz,mx,my,mz);
+ //pc.printf(" ay = %.2f\n", 1000*ay);
+
return ay*1000;
-}
\ No newline at end of file
+}
\ No newline at end of file