From 18a93c78e2841a8d2d57e956fd1df173efd1a3b2 Mon Sep 17 00:00:00 2001
From: Lino Schmidt <linoschmidt@lino3d.de>
Date: Sun, 15 May 2022 13:59:51 +0200
Subject: [PATCH] Fixed Autoplay

---
 Firmware 2.0/src/config.h |  1 +
 Firmware 2.0/src/main.cpp | 63 ++++++++++++++++++++++-----------------
 2 files changed, 37 insertions(+), 27 deletions(-)

diff --git a/Firmware 2.0/src/config.h b/Firmware 2.0/src/config.h
index 4eb34b0..a37afb5 100644
--- a/Firmware 2.0/src/config.h	
+++ b/Firmware 2.0/src/config.h	
@@ -51,6 +51,7 @@
 //Timing
 #define LoopTime 50
 #define TimeToAutoplay 1000
+#define AutoplayEaseSpeed 50
 #define TimeToDetach 70
 #define Calibration_TimeToMiddle 3000
 #define Calibration_messureTime 5
diff --git a/Firmware 2.0/src/main.cpp b/Firmware 2.0/src/main.cpp
index be63b72..59569b5 100644
--- a/Firmware 2.0/src/main.cpp	
+++ b/Firmware 2.0/src/main.cpp	
@@ -15,6 +15,7 @@ int offtime = 0;
 bool detached = true;
 int eeprom = 0;
 int autoplayLoop = 0;
+bool autoplayStart = true;
 
 int autoplayPos[][4] = {
   {20, 100, 50, 180},
@@ -23,13 +24,35 @@ int autoplayPos[][4] = {
   {50, 180, 50, 150}
 };
 
+void detachAttach(bool detach){
+  if(detach){
+    Drehung.detach();
+    Arm.detach();
+    Oberarm.detach();
+    Hand.detach();
+
+    detached = true;
+  }
+  else{
+    Drehung.attach(DrehungPin, DrehungPos);
+    Arm.attach(ArmPin, ArmPos);
+    Oberarm.attach(OberarmPin, OberarmPos);
+    Hand.attach(HandPin, HandPos);
+
+    detached = false;
+  }
+}
+
 void servoWrite() {
+  if(detached) detachAttach(false);
   Drehung.write(DrehungPos);
   Arm.write(ArmPos);
   Oberarm.write(OberarmPos);
   Hand.write(HandPos);
 }
 void servoEase(int DrehungP, int ArmP, int OberarmP, int HandP, float easeTime, uint_fast8_t easeType = EASE_QUADRATIC_IN_OUT) {
+  if(detached) detachAttach(false);
+  
   Drehung.setEasingType(easeType);
   Arm.setEasingType(easeType);
   Oberarm.setEasingType(easeType);
@@ -236,31 +259,21 @@ void joystickButtonPress(){
   }
 }
 
-void detachAttach(bool detach){
-  if(detach){
-    Drehung.detach();
-    Arm.detach();
-    Oberarm.detach();
-    Hand.detach();
-
-    detached = true;
-  }
-  else{
-    Drehung.attach(DrehungPin);
-    Arm.attach(ArmPin);
-    Oberarm.attach(OberarmPin);
-    Hand.attach(HandPin);
-
-    detached = false;
-  }
-}
-
 void Autoplay(){
-  if(autoplayLoop == sizeof(autoplayPos)) autoplayLoop = 0;
+  if(autoplayLoop+1 >= sizeof(autoplayPos)/sizeof(autoplayPos[0])) autoplayLoop = 0;
+  
+  if(autoplayStart) {
+    autoplayStart = false;
+    servoEase(autoplayPos[autoplayLoop][0], autoplayPos[autoplayLoop][1], autoplayPos[autoplayLoop][2], autoplayPos[autoplayLoop][3], AutoplayEaseSpeed);
+  }
   
   if(DrehungPos == autoplayPos[autoplayLoop][0] && ArmPos == autoplayPos[autoplayLoop][1] && OberarmPos == autoplayPos[autoplayLoop][2] && HandPos == autoplayPos[autoplayLoop][3]) {
-    servoEase(autoplayPos[autoplayLoop][0], autoplayPos[autoplayLoop][1], autoplayPos[autoplayLoop][2], autoplayPos[autoplayLoop][3], 50);
-    autoplayLoop++;
+    if(autoplayLoop+1 >= sizeof(autoplayPos)/sizeof(autoplayPos[0])) {
+      autoplayLoop = 0;
+    } else {
+      autoplayLoop++;
+    }
+    servoEase(autoplayPos[autoplayLoop][0], autoplayPos[autoplayLoop][1], autoplayPos[autoplayLoop][2], autoplayPos[autoplayLoop][3], AutoplayEaseSpeed);
   }
   
   DrehungPos = Drehung.getCurrentAngle();
@@ -291,9 +304,7 @@ void loop(){
   }
   else{
     offtime = 0;
-    if(detached){
-      detachAttach(false);
-    }
+    autoplayStart = true;
 
     if(joystick_LX != 0){
       if(InvertHand)HandPos -= joystick_LX;
@@ -342,12 +353,10 @@ void loop(){
       noTone(BuzzerPin);
     }
     
-
     servoWrite();
   }
 
   if(offtime >= TimeToAutoplay){
-    detachAttach(false);
     Autoplay();
   }
   else if(offtime >= TimeToDetach){