From 0928b7f728a1872739935c13689cbc5c10f3217a Mon Sep 17 00:00:00 2001
From: Lino Schmidt <linoschmidt@lino3d.de>
Date: Fri, 27 May 2022 10:30:29 +0200
Subject: [PATCH] autoplay progress

---
 Firmware 2.0/platformio.ini |  1 +
 Firmware 2.0/src/main.cpp   | 67 ++++++++++++++++++++-----------------
 2 files changed, 37 insertions(+), 31 deletions(-)

diff --git a/Firmware 2.0/platformio.ini b/Firmware 2.0/platformio.ini
index 702351f..e7bac35 100644
--- a/Firmware 2.0/platformio.ini	
+++ b/Firmware 2.0/platformio.ini	
@@ -12,6 +12,7 @@
 platform = atmelavr
 board = leonardo
 framework = arduino
+debug_tool = simavr
 lib_deps = 
 	arminjo/ServoEasing@^2.4.1
 	arduino-libraries/Servo@^1.1.8
diff --git a/Firmware 2.0/src/main.cpp b/Firmware 2.0/src/main.cpp
index 59b2333..8ca509c 100644
--- a/Firmware 2.0/src/main.cpp	
+++ b/Firmware 2.0/src/main.cpp	
@@ -20,11 +20,14 @@ bool inAutoplay = false;
 int buzzTime = 0;
 bool cal = true;
 
-int autoplayPos[][4] = {{0,0,0,0}};
-bool autoplayNull = true;
+int autoplayPosDrehung[] = {};
+int autoplayPosArm[] = {};
+int autoplayPosOberarm[] = {};
+int autoplayPosHand[] = {};
+int autoplayLenght = 0;
 
 void attached(bool attach){
-  if(!attach){
+  if(!attach) {
     Drehung.detach();
     Arm.detach();
     Oberarm.detach();
@@ -206,37 +209,24 @@ void setup(){
 }
 
 void savePos() {
-  if(autoplayNull) {
-    autoplayPos[0][0] = DrehungPos;
-    autoplayPos[0][1] = ArmPos;
-    autoplayPos[0][2] = OberarmPos;
-    autoplayPos[0][3] = HandPos;
-    autoplayNull = false;
-  } else {
-    Serial.println(sizeof(autoplayPos)/sizeof(int));
-    autoplayPos[sizeof(autoplayPos)/sizeof(int)][0] = DrehungPos;
-    autoplayPos[sizeof(autoplayPos)/sizeof(int)][1] = ArmPos;
-    autoplayPos[sizeof(autoplayPos)/sizeof(int)][2] = OberarmPos;
-    autoplayPos[sizeof(autoplayPos)/sizeof(int)][3] = HandPos;
-  }
+  autoplayPosDrehung[autoplayLenght] = DrehungPos;
+  autoplayPosArm[autoplayLenght] = ArmPos;
+  autoplayPosOberarm[autoplayLenght] = OberarmPos;
+  autoplayPosHand[autoplayLenght] = HandPos;
+  autoplayLenght++;
 }
 
 void clearPos() {
-  memset(autoplayPos, 0, sizeof(autoplayPos));
-  autoplayNull = true;
+  autoplayLenght = 0;
 }
 
 void joystickButtonPress() {
-  int Code[30] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; //0 = short, 1 = long
+  int Code[] = {}; //0 = short, 1 = long
   int CodeLenght = 0;
   bool Finished = false;
   bool abort = false;
 
   while(!Finished && !abort){
-    if(CodeLenght > 29){
-      abort = true;
-      Serial.println("Code aborted!");
-    }
     int buttonTime = 0;
     while(digitalRead(joystick_button_Pin) == LOW){
       buttonTime+= 10;
@@ -271,7 +261,11 @@ void joystickButtonPress() {
   }
 
   if(!abort) {
-    Serial.println(String(Code[0]) + String(Code[1]) + String(Code[2]) + String(Code[3]) + String(Code[4]) + " Len:" + String(CodeLenght));
+    for(int i = 0; i < CodeLenght; i++) {
+      Serial.print(String(Code[i]));
+    }
+    Serial.println(" Len:" + String(CodeLenght));
+    
     if(Code[0] == 0 && Code[1] == 1 && Code[2] == 0 && Code[3] == 1 && CodeLenght == 4) {
       Serial.println("Calibrate...");
       calibrateMaxMin();
@@ -283,6 +277,7 @@ void joystickButtonPress() {
       autoplayStart = true;
       autoplayLoop = 0;
       inAutoplay = true;
+      attached(true);
     } else if(Code[0] == 0 && Code[1] == 0 && CodeLenght == 2) {
       Serial.println("Clear positions...");
       clearPos();
@@ -293,16 +288,16 @@ void joystickButtonPress() {
 void Autoplay(){
   if(autoplayStart) {
     autoplayStart = false;
-    servoEase(autoplayPos[autoplayLoop][0], autoplayPos[autoplayLoop][1], autoplayPos[autoplayLoop][2], autoplayPos[autoplayLoop][3], AutoplayEaseSpeed);
+    servoEase(autoplayPosDrehung[autoplayLoop], autoplayPosArm[autoplayLoop], autoplayPosOberarm[autoplayLoop], autoplayPosHand[autoplayLoop], AutoplayEaseSpeed);
   }
   
-  if(DrehungPos == autoplayPos[autoplayLoop][0] && ArmPos == autoplayPos[autoplayLoop][1] && OberarmPos == autoplayPos[autoplayLoop][2] && HandPos == autoplayPos[autoplayLoop][3]) {
-    if((unsigned long long)(autoplayLoop+1) >= sizeof(autoplayPos)/sizeof(int)) {
+  if(DrehungPos == autoplayPosDrehung[autoplayLoop] && ArmPos == autoplayPosArm[autoplayLoop] && OberarmPos == autoplayPosOberarm[autoplayLoop] && HandPos == autoplayPosHand[autoplayLoop]) {
+    if(autoplayLoop >= autoplayLenght-1) {
       autoplayLoop = 0;
     } else {
       autoplayLoop++;
     }
-    servoEase(autoplayPos[autoplayLoop][0], autoplayPos[autoplayLoop][1], autoplayPos[autoplayLoop][2], autoplayPos[autoplayLoop][3], AutoplayEaseSpeed);
+    servoEase(autoplayPosDrehung[autoplayLoop], autoplayPosArm[autoplayLoop], autoplayPosOberarm[autoplayLoop], autoplayPosHand[autoplayLoop], AutoplayEaseSpeed);
   }
   
   DrehungPos = Drehung.getCurrentAngle();
@@ -328,6 +323,17 @@ void buzz(){
   buzzTime++;
 }
 
+void autoplayStop() {
+  if(inAutoplay) {
+    Drehung.stop();
+    Arm.stop();
+    Oberarm.stop();
+    Hand.stop();
+    
+    inAutoplay = false;
+  }
+}
+
 void loop(){
   joystick_LX = joystick_position(analogRead(joystick_LX_Pin), joystick_LX_middle, joystick_LX_min, joystick_LX_max, joystick_LX_MinSpeed, joystick_LX_MaxSpeed);
   joystick_LY = joystick_position(analogRead(joystick_LY_Pin), joystick_LY_middle, joystick_LY_min, joystick_LY_max, joystick_LY_MinSpeed, joystick_LY_MaxSpeed);
@@ -352,7 +358,7 @@ void loop(){
   }
   else{
     offtime = 0;
-    inAutoplay = false;
+    autoplayStop();
     cal = true;
 
     if(joystick_LX != 0){
@@ -372,7 +378,6 @@ void loop(){
       else ArmPos += joystick_RY;
     }
     
-    
     if(HandPos > HandMax) {
       HandPos = HandMax;
       buzz();
@@ -406,7 +411,7 @@ void loop(){
   }
 
   if(digitalRead(joystick_button_Pin) == LOW){
-    inAutoplay = false;
+    autoplayStop();
     joystickButtonPress();
   }