Save before servo rebuild

Firmware 2.0/platformio.ini

@@ -12,4 +12,6 @@
 platform = atmelavr
 board = leonardo
 framework = arduino
-lib_deps = arduino-libraries/Servo @ ^1.1.8
+lib_deps = 
+	arduino-libraries/Servo @ ^1.1.8
+	arminjo/ServoEasing@^2.4.1

Firmware 2.0/src/main.cpp

@@ -1,6 +1,7 @@
 #include <Arduino.h>
 #include <Servo.h>
 #include <EEPROM.h>
+#include <ServoEasing.h>
 #include "config.h"
 
 Servo Drehung, Arm, Oberarm, Hand;
@@ -10,11 +11,53 @@ int joystick_LX_middle, joystick_LY_middle, joystick_RX_middle, joystick_RY_midd
 int joystick_LX_max, joystick_LY_max, joystick_RX_max, joystick_RY_max;
 int joystick_LX_min, joystick_LY_min, joystick_RX_min, joystick_RY_min;
 int DrehungPos = Drehung_Start, ArmPos = Arm_Start, OberarmPos = Oberarm_Start, HandPos = Hand_Start;
+int oldPos = -1;
+
+int autoAblauf[][4] = {
+  {135,180,111,180},
+  {100,120,90,100},
+  {100,60,50,180},
+  {145,180,140,180},
+  {135,110,131,180}
+};
+int ablauf = 0;
 
 int offtime = 0;
 bool detached = true;
 int eeprom = 0;
 
+void servoWrite() {
+  Drehung.write(DrehungPos);
+  Arm.write(ArmPos);
+  Oberarm.write(OberarmPos);
+  Hand.write(HandPos);
+}
+void servoWrite(int DrehungP, int ArmP, int OberarmP, int HandP) {
+  Drehung.write(DrehungP);
+  Arm.write(ArmP);
+  Oberarm.write(OberarmP);
+  Hand.write(HandP);
+}
+
+int goTo(int Pos, int newPos, int time) {
+  
+  float aufloesung = 1 / time * LoopTime;
+  float steigung = 2;
+  
+  if(oldPos == -1) {
+    oldPos = Pos;
+  }
+  
+  float sendPos = (int)((pow(aufloesung, steigung) / pow(aufloesung, steigung) + pow((1 - aufloesung), steigung)) + oldPos) + (newPos - oldPos - 1)*Pos;
+  
+  if(sendPos == newPos) {
+    oldPos = -1;
+    return -1;
+  }
+  
+  return sendPos;
+}
+
 void calibrateMiddle(){
   Serial.print("Get Middle Positions: ");
 
@@ -230,7 +273,33 @@ void detachAttach(bool detach){
 }
 
 void Autoplay(){
+  int DrehungPs = goTo(DrehungPos, autoAblauf[ablauf][0], 3000);
+  int ArmPs = goTo(ArmPos, autoAblauf[ablauf][1], 3000);
+  int OberarmPs = goTo(OberarmPos, autoAblauf[ablauf][2], 3000);
+  int HandPs = goTo(HandPos, autoAblauf[ablauf][3], 3000);
   
+  if(DrehungPs != -1) {
+    DrehungPos = DrehungPs;
+  }
+  if(ArmPs != -1) {
+    ArmPos = ArmPs;
+  }
+  if(OberarmPs != -1) {
+    OberarmPos = OberarmPs;
+  }
+  if(HandPs != -1) {
+    HandPos = HandPs;
+  }
+  
+  if(DrehungPs == -1 && ArmPs == -1 && OberarmPs == -1 && HandPs == -1) {
+    if(ablauf < sizeof(autoAblauf)) {
+      ablauf++;
+    } else {
+      ablauf = 0;
+    }
+  }
+  
+  servoWrite();
 }
 
 int joystick_position(int joystick, int joystick_middle, int joystick_min, int joystick_max, int joystick_MinSpeed, int joystick_MaxSpeed){
@@ -286,13 +355,11 @@ void loop(){
     if(DrehungPos < DrehungMin)DrehungPos = DrehungMin;
     if(ArmPos < ArmMin)ArmPos = ArmMin;
 
-    Drehung.write(DrehungPos);
+    servoWrite();
-    Arm.write(ArmPos);
-    Oberarm.write(OberarmPos);
-    Hand.write(HandPos);
   }
 
   if(offtime >= TimeToAutoplay){
+    detachAttach(false);
     Autoplay();
   }
   else if(offtime >= TimeToDetach){