Ti
Dependencies: mbed MPU6050 HMC5883L ledControl2
Diff: main.cpp
- Revision:
- 6:6d4e8461b60a
- Parent:
- 5:d760e9c88de4
diff -r d760e9c88de4 -r 6d4e8461b60a main.cpp --- a/main.cpp Mon Nov 05 13:37:54 2018 +0000 +++ b/main.cpp Sat Dec 15 13:30:06 2018 +0000 @@ -37,6 +37,22 @@ #include "MPU6050.h" #include "ledControl.h" +int Mode =0; +int degree3=4; +int degree = 0; +int previous_state1 = 0; +int state=0; + +InterruptIn button(USER_BUTTON); +void released() +{ + Mode+=1; + if(Mode%4==0){Mode%=4;Mode+=1;} + degree=0; + degree3=4; + state=0; + previous_state1 = 0; +} Serial pc(D1,D0); MPU6050 mpu6050; HMC5883L hmc5883l; @@ -53,7 +69,9 @@ float yawAngle = 0; int main() -{ +{ + char data[5]="@122"; + button.rise(&released); pc.baud(9600); // baud rate: 9600 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers @@ -61,48 +79,68 @@ hmc5883l.init(); filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) compass.attach(&tiltCompensatedAngle, 0.015); // Call the tiltCompensatedAngle func. every 15 ms (75 Hz sampling period) - int degree = 0; - int previous_state1 = 0; - int state = 0; - int previous_state2 = 0; - while(1) - { //pc.printf("%.1f,%.1f,%.1f\r\n",rollAngle,pitchAngle,yawAngle); // send data to matlab // wait_ms(500); // mpu6050.reset(); - if((rollAngle > 60)&&(previous_state1 == 0)) + while(Mode%4==1){ + if((rollAngle > 45)&&(previous_state1 == 0)&&(-7<pitchAngle<10)) { - if(degree>0){ - degree -= 1; - previous_state1 = 1; - wait_ms(100);} - else{previous_state1 = 1;} + if(degree>=0){degree-=1;previous_state1 = 1;} + } + if ((rollAngle < -45)&&(previous_state1 == 0)&&(-7<pitchAngle<10)) + { + if(degree<=8){degree+=1;previous_state1 = 1;} + } + if((-7<rollAngle<10)&&(previous_state1 == 1)&&(-7<pitchAngle<10)) + { + previous_state1 = 0; } - else if ((rollAngle < -60)&&(previous_state1 == 0)) + if((pitchAngle<-45)&&(previous_state1 == 0)&&(-7<rollAngle<10)) { - degree += 1; - previous_state1 = 1; - wait_ms(100); + if(state>=0){state+=1;previous_state1 = 1;} + } + data[1]='1';data[2]=degree+'0'; } - if((pitchAngle < -20)&&(previous_state2 == 0)) + + while(Mode%4==2){ + if((rollAngle > 45)&&(previous_state1 == 0)&&(-7<pitchAngle<10)) + { + if(degree>=0){degree-=1;previous_state1 = 1;} + } + if ((rollAngle < -45)&&(previous_state1 == 0)&&(-7<pitchAngle<10)) { - if(state>0){ - state -= 1; - previous_state2 = 1; - wait_ms(100);} - else{previous_state2 = 1;} + if(degree<=8){degree+=1;previous_state1 = 1;} + } + if((-7<rollAngle<10)&&(previous_state1 == 1)&&(-7<pitchAngle<10)) + { + previous_state1 = 0; } - else if((pitchAngle > 60)&&(previous_state2 == 0)) + if((pitchAngle<-45)&&(previous_state1 == 0)&&(-7<rollAngle<10)) + { + if(state>=0){state+=1;previous_state1 = 1;if(state%4==0){state%=4;state+=1;}} + } + data[1]='2';data[2]=degree+'0';data[3]=state+'0'; + } + while(Mode%4==3){ + if((rollAngle > 45)&&(previous_state1 == 0)&&(-7<pitchAngle<10)) { - state += 1; - previous_state2 = 1; - wait_ms(100); + if(degree3>=0){degree3-=1;previous_state1 = 1;} + } + if ((rollAngle < -45)&&(previous_state1 == 0)&&(-7<pitchAngle<10)) + { + if(degree3<=8){degree3+=1;previous_state1 = 1;} } - if (-40<rollAngle<40){previous_state1 = 0;pc.printf("degree = %d\n",degree);wait_ms(400);} - if (-10<pitchAngle<50){previous_state2 = 0;pc.printf("state = %d\n",state);wait_ms(400);} - } -} - + if((-7<rollAngle<10)&&(previous_state1 == 1)&&(-7<pitchAngle<10)) + { + previous_state1 = 0; + } + if((pitchAngle<-45)&&(previous_state1 == 0)&&(-7<rollAngle<10)) + { + if(state>=0){state+=1;previous_state1 = 1;if(state%3==0){state%=3;state+=1;}} + } + data[1]='2';data[2]=degree3+'0';data[3]=state+'0'; + } + } void toggle_led1() {ledToggle(1);} void toggle_led2() {ledToggle(2);}