From 02f6dbef4971368adfefd89a8d4c58374cac7a8d Mon Sep 17 00:00:00 2001 From: ketrptr Date: Tue, 15 Apr 2025 18:09:58 +0200 Subject: [PATCH] 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 +- __pycache__/meControll.cpython-313.pyc | Bin 3614 -> 3687 bytes demSequence.py | 17 +- meControll.py | 9 +- meControllMCU/config.h | 4 + meControllMCU/meControllMCU.ino | 225 +++++++++++++++++++++++++ 6 files changed, 248 insertions(+), 9 deletions(-) create mode 100644 meControllMCU/config.h create mode 100644 meControllMCU/meControllMCU.ino diff --git a/Makefile b/Makefile index 6d08a27..b82095b 100644 --- a/Makefile +++ b/Makefile @@ -2,7 +2,7 @@ BAUDRATE=9600 FQBN='arduino:avr:uno' # FQBN='esp8266:esp8266:nodemcu' PORT=/dev/ttyACM0 -PROJ=servoTest +PROJ=meControllMCU # TEST=pwmTest diff --git a/__pycache__/meControll.cpython-313.pyc b/__pycache__/meControll.cpython-313.pyc index c1a97bef391d828a7085643294468a67c5f54644..647a96735e1e0895bc2402d1a0f09cfb0b1eeb3a 100644 GIT binary patch delta 247 zcmbOy^IV4aGcPX}0}%XN@;NiKOmAgV5n?G40P4KOm6lUloZ*>QP+BtiKAW#tBv7z{;RXj!KSw9Wbq?u^ z9MV@fWH%eJw=fFn0Od59iv&UXm@`uh;k@puX>kc4q>SPBtsmbgt!jp5^j3-C22~U<{;hDUj zO&rXA$)>`_TqFQgH(8$Dm(3H%XkeHez%9CYI(t5&kT#I7$y_7|q;4^1rc@MZPL|-* zVpQ7f%-P3i0Fo_Y1`!|wZgHjMlon@r<`tBd6sdzm<$(lDH)C><*5qGYYK%IY6}kBs z*}!`BC!6wAu(GqteG-|xiDwt1(d1lS2S)wL>v+w%4H*?aG60DWS&WlE@RkDrAi_4; diff --git a/demSequence.py b/demSequence.py index bfef2ff..b7efec3 100644 --- a/demSequence.py +++ b/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) diff --git a/meControll.py b/meControll.py index 4beaaf5..aae6916 100644 --- a/meControll.py +++ b/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): diff --git a/meControllMCU/config.h b/meControllMCU/config.h new file mode 100644 index 0000000..8533c18 --- /dev/null +++ b/meControllMCU/config.h @@ -0,0 +1,4 @@ +#ifndef CONFIG +#define CONFIG +#define BAUDRATE 9600 +#endif \ No newline at end of file diff --git a/meControllMCU/meControllMCU.ino b/meControllMCU/meControllMCU.ino new file mode 100644 index 0000000..4abe482 --- /dev/null +++ b/meControllMCU/meControllMCU.ino @@ -0,0 +1,225 @@ +#include "config.h" +#include +// #include + +// 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 +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](); + } + } +}