diff --git a/__pycache__/meControll.cpython-313.pyc b/__pycache__/meControll.cpython-313.pyc new file mode 100644 index 0000000..3004f99 Binary files /dev/null and b/__pycache__/meControll.cpython-313.pyc differ diff --git a/controller.py b/controller.py new file mode 100644 index 0000000..6913c96 --- /dev/null +++ b/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') + + diff --git a/meControll.py b/meControll.py new file mode 100644 index 0000000..eb756a7 --- /dev/null +++ b/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) + + + + + + diff --git a/servoTest/servoTest.ino b/servoTest/servoTest.ino index d4d8328..6aff4fa 100644 --- a/servoTest/servoTest.ino +++ b/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 (inc == 0) { - error("movement increment is 0?"); - return; + if(dir!=T) inc = (abs(delta) > 20) ? constrain((abs(delta) / 10), 1, 10) : 1; + else inc =1; + if (delta > 0) { + 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; - val += inc; + + delay(30); // TODO no delay } - 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.println(servoPos[cmd]); + //Serial.print("new val: "); + //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.println(i); - Serial.print(val); - Serial.print("-->"); - Serial.println(servoPos[i]); + // Serial.print("-- "); + // Serial.println(i); + // Serial.print(val); + // Serial.print("-->"); + // 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.println(r); +// Serial.print("read: "); +// Serial.println(r); // Serial.println(serialBuf); if (r < 2) { error("command to short!"); @@ -136,44 +170,38 @@ void loop() { break; } } - // Serial.print("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; - + // Serial.print("NumStr: "); + // Serial.println(numStr); switch (serialBuf[0]) { - case 'L': - case 'R': - case 'T': - case 'G': - moveCmdHandler(); - break; - case 'H': - statusHandler(); - break; - case 'S': // select relative / absolute positioning - if (serialBuf[1] == '0') - absPos = false; - else - absPos = true; - break; - case 'D': // default pos at 90° for all servos - for (int i = 0; i < axCount; i++) { - servoPos[i] = 90; - } - break; - - default: - error("invalid command"); - break; + 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(cmd >= 0){ + cmdVal = numStr.toInt(); + handlers[cmd](); } } }