From 024c0ff9e3dead0493c4c71d0e0d6950abd3134f Mon Sep 17 00:00:00 2001
From: ketrptr <ketrptr@cocaine.ninja>
Date: Tue, 1 Apr 2025 15:49:44 +0200
Subject: [PATCH] 	modified:   servoTest/servoTest.ino 	new file:  
 testSequence2.py

---
 servoTest/servoTest.ino | 56 ++++++++++++++++++++++++++++-------------
 testSequence2.py        | 13 ++++++++++
 2 files changed, 51 insertions(+), 18 deletions(-)
 create mode 100644 testSequence2.py

diff --git a/servoTest/servoTest.ino b/servoTest/servoTest.ino
index 02e91d4..29c056b 100644
--- a/servoTest/servoTest.ino
+++ b/servoTest/servoTest.ino
@@ -34,9 +34,11 @@ const byte axCount = 4;
 // serPos and axis array must have same order as CMD enum!!
 int servoPos[axCount];
 const Servo *axis[axCount] = {&S_left, &S_right, &S_turn, &S_grip};
+
 int defaultPos[] = {90, 90, 90, 90};
 bool absPos; // absolute /relative positioning
 
+
 // Move axis[dir] to the position set in servoPos[dir]
 void movePos(CMD dir) {
   // safety check
@@ -44,19 +46,24 @@ void movePos(CMD dir) {
     error("Not a valid Axis!!");
     return;
   }
-  if (servoPos[dir] < 20 || servoPos[dir] > 175) {
+  //arm can crash with small or large angles
+  if (servoPos[dir] < 20 || servoPos[dir] > 170) {
     error("move out of range");
     return;
   }
 
   // if actual servo pos is different to wanted move towards it
-  int val = axis[dir]->read();
-  if (val != servoPos[dir]) {
-    int delta = servoPos[dir] - val;
-    int inc;
+  int val = axis[dir]->read(); //get current position of dir
+  if (val != servoPos[dir]) { 
+    int delta = servoPos[dir] - val; //wantPos - havePos
+    int inc; //movement sptep size, depends on distance to wantPos
     while (delta) {
+
+      //calculate movement increment 
       if(dir!=T) inc = (abs(delta) > 20) ? constrain((abs(delta) / 10), 1, 5) : 1;
-      else inc =1;
+      else inc =1; //turn axis cannot move as fast 
+
+      //Move positive / negative direction
       if (delta > 0) {
         axis[dir]->write(val + inc);
         delta -= inc;
@@ -66,29 +73,40 @@ void movePos(CMD dir) {
         delta += inc;
         val -= inc;
       }
-
-      delay(20); // TODO  no delay
+  
+      //step delay 
+      //short --> fast movement 
+      //long --> slow movement, noticable steps
+      delay(20);
     }
   }
 }
 
 //Global current command value and type
+//Commands have form <CMD><Val> 
 int cmdVal; 
 CMD cmd;
 
+// Handle move command 
+// L, R, T, G
 void moveCmdHandler() {
   Serial.print("Servo_Command: ");
   Serial.print(CMD2char(cmd));
   Serial.println(cmdVal);
+  
+  //if abolute positioning set wantPos for current axis (cmd) to cmd val
   if (absPos) {
     servoPos[cmd] = constrain(cmdVal, 20, 165);
-  } else {
+  } else { // if relative positioning add val to wantPos
     servoPos[cmd] = constrain(servoPos[cmd] + cmdVal, 20, 165);
   }
   //Serial.print("new val:  ");
   //Serial.println(servoPos[cmd]);
 }
 
+// Status command handler
+// H1 command 
+// print posiotion and abs/rel positioning
 void statusHandler() {
   Serial.println((absPos) ? "Absolute Positioning" : "Relative Positioning");
   for (int i = 0; i < axCount; i++) {
@@ -108,6 +126,7 @@ void setPositioningHandler(){
   }
 
 //move arm to (90,90,90,90)
+//Homing sequence
 void defaultPosHandler(){
   memcpy(servoPos, defaultPos, sizeof(int)*axCount);
 }
@@ -126,7 +145,7 @@ void setup() {
   S_turn.attach(S_turn_pin);
   S_grip.attach(S_grip_pin);
 
-  defaultPosHandler();
+  defaultPosHandler(); //move Home (90,90,90,90)
 
   Serial.begin(BAUDRATE);
   absPos = true;
@@ -138,30 +157,30 @@ void loop() {
 
   // print changes
   int val;
+
+  //check if all axis are where they should be 
+  //if not move to position
   for (int i = 0; i < axCount; i++) {
     val = axis[i]->read();
     if (val != servoPos[i]) {
-    //  Serial.print("-- ");
-    //  Serial.println(i);
-    //  Serial.print(val);
-    //  Serial.print("-->");
-    //  Serial.println(servoPos[i]);
       movePos(i);
     }
     delay(50);
   }
+
+  //IMPORTANT! This tells controller that move is done and next command can be sent
   Serial.println("READY");
 
   // read position command from serial
   if (Serial.available()) {
     int r = Serial.readBytesUntil('\n', serialBuf, bufSize);
-//    Serial.print("read: ");
-//    Serial.println(r);
-    // Serial.println(serialBuf);
+
     if (r < 2) {
       error("command to short!");
       return;
     }
+
+    //parse value after cmd
     String numStr = "";
     for (int i = 1; i < r; i++) {
       if (isDigit(serialBuf[i]) || isPunct(serialBuf[i])) {
@@ -199,6 +218,7 @@ void loop() {
         cmd = -1;
         break; 
     }
+    //if command is valis call corresponding handler from handlers array 
     if(cmd >= 0){
       cmdVal = numStr.toInt();
       handlers[cmd]();
diff --git a/testSequence2.py b/testSequence2.py
new file mode 100644
index 0000000..cb67275
--- /dev/null
+++ b/testSequence2.py
@@ -0,0 +1,13 @@
+import meControll
+
+arm = meControll.meArm()
+
+pickupDouble = [b'D1', b'S1', b'T70', b'L50', b'R120', b'L20', b'R140', b'G180', b'R110' ] 
+m2 = [b'T130', b'R130', b'L30', b'G90', b'R100', b'D1']
+
+arm.addMove(pickupDouble)
+arm.addMove(m2)
+arm.execMove(0)
+arm.execMove(1)
+
+