Fixes

Author: danic85

arduino_sketch/Config.h

@@ -46,7 +46,7 @@
 
 #define NOVAL 1000
 
-// #define MPU6050_ENABLED // Enable MPU6050
+#define MPU6050_ENABLED // Enable MPU6050
 //#define MPU6050_DEBUG // Debug in serial plotter
 #define ANIMATE_ENABLED // Enable random animations
 
@@ -70,7 +70,7 @@ int PosRest[SERVO_COUNT] = {60, 0, 165, 120, 180, 10, 90, 90};
 int PosConfig[SERVO_COUNT] = {90, 90, 90, 90, 90, 90, 90, 90};
 
 // Poses
-int PosStand[SERVO_COUNT] = {45, 75, 95, 135, 105, 85, NOVAL, NOVAL};
+int PosStand[SERVO_COUNT] = {45, 75, 90, 135, 105, 90, NOVAL, NOVAL};
 int PosLookLeft[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 180};
 int PosLookRight[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL,  NOVAL, NOVAL, 0};
 int PosLookRandom[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, -1, -1}; // Made random by calling the function moveRandom() if the value is -1

arduino_sketch/arduino_sketch.ino

@@ -45,9 +45,6 @@ void setup()
   bootTime = millis();
   // Init serial
   Serial.begin(115200);
-  // Init ServoManager
-  servoManager.doInit();
-  servoManager.setSpeed(SERVO_SPEED_MIN);
 
 
   pinMode(backpackPin, INPUT_PULLUP); // sets the digital pin as input
@@ -64,6 +61,10 @@ void setup()
     restrainingBolt = true;
   }
 
+  // Init ServoManager
+  servoManager.doInit();
+  servoManager.setSpeed(SERVO_SPEED_MIN);
+
   #ifdef MPU6050_ENABLED
   tilt.doInit();
   #endif
@@ -158,6 +159,7 @@ void hipAdjust()
   tilt.read();
   //Serial.println(tilt.getPitch());
   servoManager.hipAdjust(tilt.getPitch());
+  setEaseToForAllServosSynchronizeAndStartInterrupt(servoManager.getSpeed());
 }
 #endif