diff --git a/gyrocontrol.ino b/gyrocontrol.ino new file mode 100644 index 0000000..00e4c6a --- /dev/null +++ b/gyrocontrol.ino @@ -0,0 +1,100 @@ +#include +#include +#include +#include +#include + +MPU6050 mpu6050(Wire); + +const int inactivePin = 5; +int inactivePinState = 0; +bool inactiveState = true; +const int recalibratePin = 16; +int recalibratePinState = 0; +int doRecalibrate = 0; +float offsetX = 0.0; +float offsetY = 0.0; +float offsetZ = 0.0; +float effectiveX = 0.0; +float effectiveY = 0.0; +float effectiveZ = 0.0; + + +void setup() { + pinMode(inactivePin, INPUT_PULLUP); + pinMode(recalibratePin, INPUT_PULLUP); + + Serial.begin(9600); + Wire.begin(); + mpu6050.begin(); + mpu6050.calcGyroOffsets(true); + offsetX = mpu6050.getGyroAngleX(); + offsetY = mpu6050.getGyroAngleY(); + offsetZ = mpu6050.getGyroAngleZ(); + effectiveX = mpu6050.getGyroAngleX() - offsetX; + effectiveY = mpu6050.getGyroAngleY() - offsetY; + effectiveZ = mpu6050.getGyroAngleZ() - offsetZ; + Keyboard.begin(); +} + +void loop(){ + inactivePinState = digitalRead(inactivePin); + recalibratePinState = digitalRead(recalibratePin); + mpu6050.update(); + + if(inactivePinState == LOW) { + Keyboard.releaseAll(); + inactiveState = !inactiveState; + if(!inactiveState) doRecalibrate = 1; + while(digitalRead(inactivePin) == LOW) { + delay(50); + } + } + + if(inactiveState) { + Serial.println("device inactive"); + return; + } + + if(recalibratePinState == LOW) { + Keyboard.releaseAll(); + doRecalibrate = 1; + while(digitalRead(recalibratePin) == LOW) { + delay(50); + } + } + + if(doRecalibrate) { + Keyboard.releaseAll(); + Serial.println("do recalibration, dont move!"); + delay(1000); + doRecalibrate = 0; + offsetX = mpu6050.getGyroAngleX(); + offsetY = mpu6050.getGyroAngleY(); + offsetZ = mpu6050.getGyroAngleZ(); + Serial.println("done recalibration, 1s..."); + delay(1000); + } + + if (!inactiveState) { + effectiveX = mpu6050.getGyroAngleX() - offsetX; + effectiveY = mpu6050.getGyroAngleY() - offsetY; + effectiveZ = mpu6050.getGyroAngleZ() - offsetZ; + if (effectiveX > 8) { Keyboard.press('w'); Serial.println("w"); } else { Keyboard.release('w'); } + if (effectiveX < -8) { Keyboard.press('s'); Serial.println("s"); } else { Keyboard.release('s'); } + if (effectiveZ > 8) { Keyboard.press('a'); Serial.println("a"); } else { Keyboard.release('a'); } + if (effectiveZ < -8) { Keyboard.press('d'); Serial.println("d"); } else { Keyboard.release('d'); } + } + Serial.print("angleX : "); + Serial.print(effectiveX); + Serial.print("\tangleY : "); + Serial.print(effectiveY); + Serial.print("\tangleZ : "); + Serial.print((effectiveZ)); + Serial.print("\toffsetX : "); + Serial.print(offsetX); + Serial.print("\toffsetY : "); + Serial.print(offsetY); + Serial.print("\toffsetZ : "); + Serial.println(offsetY); +} \ No newline at end of file