modified: Makefile

modified:   __pycache__/meControll.cpython-313.pyc
	modified:   demSequence.py
	modified:   meControll.py
	new file:   meControllMCU/config.h
	new file:   meControllMCU/meControllMCU.ino

 Changes not staged for commit:
	deleted:    controller.py
	deleted:    servoTest/config.h
	deleted:    servoTest/servoTest.ino
	deleted:    testSequence1

Makefile

@@ -2,7 +2,7 @@ BAUDRATE=9600
 FQBN='arduino:avr:uno'
 # FQBN='esp8266:esp8266:nodemcu'
 PORT=/dev/ttyACM0
-PROJ=servoTest
+PROJ=meControllMCU
 # TEST=pwmTest
 
 

demSequence.py

@@ -5,9 +5,18 @@ arm  = meControll.meArm()
 pickupLeft = [b'S1', b'T130', b'R110', b'L50', b'R150',b'G170', b'R110' ]
 drop_right = [b'S1', b'T50', b'R150', b'G90', b'R100', b'D1']
 
-arm.addMove(pickupLeft)
-arm.addMove(drop_right)
-arm.execMove(0)
-arm.execMove(1)
+def initDemo():
+    arm.addMove(pickupLeft)
+    arm.addMove(drop_right)
+
+def runDemo():
+    arm.execMove(0)
+    arm.execMove(1)
+
+if __name__ == "__main__":
+    initDemo()
+    while(1):
+        arm.execMove(0)
+        arm.execMove(1)
 
 

meControll.py

@@ -3,10 +3,10 @@ from time import sleep
 
 
 class meArm:
-    position = [90,90,90,90]
-    moves = []
-    port = ""
-    baudrate = 0
+    position = [90,90,90,90] #home position
+    moves = [] #stored moves
+    port = "" #serial port
+    baudrate = 9600
     ser = serial.Serial()
 
     def __init__(self, port = '/dev/ttyACM0', bautrate = 9600):
@@ -21,6 +21,7 @@ class meArm:
     def getMoves(self):
         [print(i, x) for i, x in enumerate(self.moves)]
 
+    #default cmd H1 sends current pos over serial
     def sendCmd(self, cmd = b'H1'):
         self.ser.flushInput() #Last change. if broken remove this
         while(1):

meControllMCU/config.h

@@ -0,0 +1,4 @@
+#ifndef CONFIG
+#define CONFIG
+#define BAUDRATE 9600
+#endif

meControllMCU/meControllMCU.ino

@@ -0,0 +1,225 @@
+#include "config.h"
+#include <Servo.h>
+// #include <avr/io.h>
+
+// mee arm Servo pins
+#define S_left_pin 10
+#define S_right_pin 5
+#define S_turn_pin 6
+#define S_grip_pin 9
+
+// Command enum for serial communication
+// L - left servo
+// 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) {
+  return (c == L) ? 'L' : (c == R) ? 'R' : (c == T) ? 'T' : 'G';
+}
+
+void error(String msg) {
+  Serial.print("error: ");
+  Serial.println(msg);
+}
+
+// Servo setup
+Servo S_left, S_right, S_turn, S_grip;
+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
+  if (dir > G) {
+    error("Not a valid Axis!!");
+    return;
+  }
+  //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(); //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; //turn axis cannot move as fast 
+
+      //Move positive / negative direction
+      if (delta > 0) {
+        axis[dir]->write(val + inc);
+        delta -= inc;
+        val += inc;
+      } else {
+        axis[dir]->write(val - inc);
+        delta += inc;
+        val -= inc;
+      }
+  
+      //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 { // 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++) {
+    Serial.print(CMD2char(i));
+    Serial.print("--");
+    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)
+//Homing sequence
+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(); //move Home (90,90,90,90)
+
+  Serial.begin(BAUDRATE);
+  absPos = true;
+  delay(500);
+}
+
+
+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]) {
+      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);
+
+    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])) {
+        numStr += serialBuf[i];
+      } else {
+        break;
+      }
+    }
+    switch (serialBuf[0]) {
+      case 'L':
+        cmd = L;
+        break;
+      case 'R':
+        cmd = R;
+        break;
+      case 'T':
+        cmd = T;
+        break;
+      case 'G':
+        cmd = G;
+        break;
+      case 'H':
+        cmd = H;
+        break;
+      case 'S': // select relative / absolute positioning
+        cmd = S;
+        break;
+      case 'D': // default pos at 90° for all servos
+        cmd = D;
+        break;
+      default:
+        error("invalid command");
+        cmd = -1;
+        break; 
+    }
+    //if command is valis call corresponding handler from handlers array 
+    if(cmd >= 0){
+      cmdVal = numStr.toInt();
+      handlers[cmd]();
+    }
+  }
+}