imporvements and host constoller

controller.py

@@ -0,0 +1,62 @@
+import serial 
+from time import sleep 
+
+move1 = [ 
+         b'D1\n',
+         b'S1\n',
+         b'L95\n',
+         b'T75\n', 
+         b'R118\n',
+         b'G185\n',
+         b'R90\n', 
+         # b'D1\n',  
+         b'L110\n',
+         b'T160\n',
+         b'R115\n',
+         b'L95\n',
+         b'G90\n',
+]
+move2 = [
+         b'D1\n',
+         b'S1\n',
+         b'L95\n',
+         b'T160\n',
+         b'R115\n',
+         b'G185\n',
+         b'R90\n', 
+         # b'D1\n',  
+         b'L110\n',
+         b'T75\n', 
+         b'R118\n',
+         b'L95\n',
+         b'G90\n',
+]
+
+if __name__ == '__main__':
+    with serial.Serial('/dev/ttyACM0', 9600, timeout=1) as ser:
+        for cmd in move1:
+            while(1):
+                line = ser.readline()
+                print(line)
+                if(line == b'READY\r\n'):
+                    break
+            ser.write(cmd)
+            sleep(1)
+    
+        sleep(5)
+        ser.write(b'D1\n')
+        sleep(5)
+    
+        for cmd in move2:
+            while(1):
+                line = ser.readline()
+                print(line)
+                if(line == b'READY\r\n'):
+                    break
+            ser.write(cmd)
+            sleep(1)
+    
+        sleep(5)
+        ser.write(b'D1\n')
+    
+    

meControll.py

@@ -0,0 +1,75 @@
+import serial 
+from time import sleep 
+
+
+class meArm:
+    position = [90,90,90,90]
+    moves = []
+    port = ""
+    baudrate = 0
+    ser = serial.Serial()
+
+    def __init__(self, port = '/dev/ttyACM0', bautrate = 9600):
+        self.port = port 
+        self.baudrate = bautrate
+        self.ser = serial.Serial(self.port, self.baudrate, timeout = 1)
+
+    def addMove(self, moves = [b'D1\n']):
+        self.moves.append(moves)
+        return len(self.moves) -1
+
+    def getMoves(self):
+        [print(i, x) for i, x in enumerate(self.moves)]
+
+    def sendCmd(self, cmd = b'H1'):
+        while(1):
+            line = self.ser.readline()
+            # print(f"sendCmd: {line}")
+            if(line == b'READY\r\n'):
+                break
+        self.ser.write(cmd)
+        self.ser.write(b'\n')
+        # print(f"sendCmd: wrote: {cmd}")
+
+    def execMove(self, idx):
+        if(idx >= len(self.moves)):
+            print("invalid movement index")
+            return
+        for cmd in self.moves[idx]:
+            print(f"sending: {cmd}")
+            self.sendCmd(cmd)
+
+
+    def getPos(self):
+        self.sendCmd()
+        sleep(0.1)
+        self.ser.flushInput()
+        c = 0
+        while(1):
+            line = self.ser.readline()
+            print(line)
+            if(line == b'READY\r\n'):
+                c+=1
+                if c ==2: break
+
+
+
+if __name__ == '__main__':
+    m1 =  [b'T70', b'L40', b'R150', b'G180', b'R90', b'D1']
+    m2 =  [b'S1', b'T70', b'L40', b'R150', b'G180', b'R90', b'T140', b'L50', b'R150', b'G90', b'D1']
+    m3 =  [b'S1', b'T140', b'L50', b'R150', b'G180', b'D1']
+    arm = meArm()
+    arm.getPos()
+    i = arm.addMove(m1)
+    i = arm.addMove(m2)
+    i = arm.addMove(m3)
+
+    arm.execMove(1)
+    arm.getPos()
+    arm.execMove(2)
+
+
+
+
+
+

servoTest/servoTest.ino

@@ -13,6 +13,9 @@
 // R - right servo
 // T - turn servo
 // G - grip servo
+// H - Help/status 
+// S - set relative / abs pos (0 -> relPos !0 -> absPos)
+// D - goto default pos (90,90,90,90)
 typedef enum { L, R, T, G, H, S, D } CMD;
 
 char CMD2char(CMD c) {
@@ -26,9 +29,12 @@ void error(String msg) {
 
 // Servo setup
 Servo S_left, S_right, S_turn, S_grip;
-const Servo *axis[4] = {&S_left, &S_right, &S_turn, &S_grip};
 const byte axCount = 4;
-int servoPos[] = {90, 90, 90, 90};
+
+// 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]
@@ -49,20 +55,25 @@ void movePos(CMD dir) {
     int delta = servoPos[dir] - val;
     int inc;
     while (delta) {
-      inc = delta / 5;
+      if(dir!=T) inc = (abs(delta) > 20) ? constrain((abs(delta) / 10), 1, 10) : 1;
-      if (inc == 0) {
+      else inc =1;
-        error("movement increment is 0?");
+      if (delta > 0) {
-        return;
+        axis[dir]->write(val + inc);
+        delta -= inc;
+        val += inc;
+      } else {
+        axis[dir]->write(val - inc);
+        delta += inc;
+        val -= inc;
       }
-      axis[dir]->write(val + inc);
+
-      delta -= inc;
+      delay(30); // TODO  no delay
-      val += inc;
     }
-    delay(50); // TODO  no delay
   }
 }
 
-int cmdVal;
+//Global current command value and type
+int cmdVal; 
 CMD cmd;
 
 void moveCmdHandler() {
@@ -74,8 +85,8 @@ void moveCmdHandler() {
   } else {
     servoPos[cmd] = constrain(servoPos[cmd] + cmdVal, 20, 165);
   }
-  Serial.print("new val:  ");
+  //Serial.print("new val:  ");
-  Serial.println(servoPos[cmd]);
+  //Serial.println(servoPos[cmd]);
 }
 
 void statusHandler() {
@@ -86,21 +97,43 @@ void statusHandler() {
     Serial.println(servoPos[i]);
   }
 }
+
+//switch between absulute and Relative posiitoning 
+//0 == Relative
+//!0 == absulute
+void setPositioningHandler(){
+    if (cmdVal) absPos = true;
+    else absPos = false;
+    Serial.println((absPos) ? "Absolute Positioning" : "Relative Positioning");
+  }
+
+//move arm to (90,90,90,90)
+void defaultPosHandler(){
+  memcpy(servoPos, defaultPos, sizeof(int)*axCount);
+}
+
+//CMD handler functions array to map command to handler, order must be same as in CMD enum!!
+void (*handlers[])(void) = {&moveCmdHandler, &moveCmdHandler, &moveCmdHandler, &moveCmdHandler, &statusHandler, &setPositioningHandler, &defaultPosHandler };
+
 // Serial Input buffer
 char serialBuf[8];
 int bufSize = 8;
 
 void setup() {
+  //attach servos and move to default pos
   S_left.attach(S_left_pin);
   S_right.attach(S_right_pin);
   S_turn.attach(S_turn_pin);
   S_grip.attach(S_grip_pin);
 
+  defaultPosHandler();
+
   Serial.begin(BAUDRATE);
-  Serial.println("reading Servo positions");
   absPos = true;
   delay(500);
 }
+
+
 void loop() {
 
   // print changes
@@ -108,21 +141,22 @@ void loop() {
   for (int i = 0; i < axCount; i++) {
     val = axis[i]->read();
     if (val != servoPos[i]) {
-      Serial.print("-- ");
+    //  Serial.print("-- ");
-      Serial.println(i);
+    //  Serial.println(i);
-      Serial.print(val);
+    //  Serial.print(val);
-      Serial.print("-->");
+    //  Serial.print("-->");
-      Serial.println(servoPos[i]);
+    //  Serial.println(servoPos[i]);
       movePos(i);
     }
     delay(50);
   }
+  Serial.println("READY");
 
   // read position command from serial
   if (Serial.available()) {
     int r = Serial.readBytesUntil('\n', serialBuf, bufSize);
-    Serial.print("read: ");
+//    Serial.print("read: ");
-    Serial.println(r);
+//    Serial.println(r);
     // Serial.println(serialBuf);
     if (r < 2) {
       error("command to short!");
@@ -136,44 +170,38 @@ void loop() {
         break;
       }
     }
-    // Serial.print("NumStr: ");
+   // Serial.print("NumStr: ");
-    // Serial.println(numStr);
+   // Serial.println(numStr);
-    cmdVal = numStr.toInt();
-    char c = serialBuf[0];
-    cmd = (c == 'L')   ? L
-          : (c == 'R') ? R
-          : (c == 'T') ? T
-          : (c == 'G') ? G
-          : (c == 'H') ? H
-          : (c == 'S') ? S
-          : (c == 'D') ? D
-                       : -1;
-
     switch (serialBuf[0]) {
-    case 'L':
+      case 'L':
-    case 'R':
+        cmd = L;
-    case 'T':
+        break;
-    case 'G':
+      case 'R':
-      moveCmdHandler();
+        cmd = R;
-      break;
+        break;
-    case 'H':
+      case 'T':
-      statusHandler();
+        cmd = T;
-      break;
+        break;
-    case 'S': // select relative / absolute positioning
+      case 'G':
-      if (serialBuf[1] == '0')
+        cmd = G;
-        absPos = false;
+        break;
-      else
+      case 'H':
-        absPos = true;
+        cmd = H;
-      break;
+        break;
-    case 'D': // default pos at 90° for all servos
+      case 'S': // select relative / absolute positioning
-      for (int i = 0; i < axCount; i++) {
+        cmd = S;
-        servoPos[i] = 90;
+        break;
-      }
+      case 'D': // default pos at 90° for all servos
-      break;
+        cmd = D;
-
+        break;
-    default:
+      default:
-      error("invalid command");
+        error("invalid command");
-      break;
+        cmd = -1;
+        break; 
+    }
+    if(cmd >= 0){
+      cmdVal = numStr.toInt();
+      handlers[cmd]();
     }
   }
 }