diff --git a/controller.py b/controller.py deleted file mode 100644 index 6913c96..0000000 --- a/controller.py +++ /dev/null @@ -1,62 +0,0 @@ -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/servoTest/config.h b/servoTest/config.h deleted file mode 100644 index 8533c18..0000000 --- a/servoTest/config.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef CONFIG -#define CONFIG -#define BAUDRATE 9600 -#endif \ No newline at end of file diff --git a/servoTest/servoTest.ino b/servoTest/servoTest.ino deleted file mode 100644 index 29c056b..0000000 --- a/servoTest/servoTest.ino +++ /dev/null @@ -1,227 +0,0 @@ -#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; - } - } - // Serial.print("NumStr: "); - // Serial.println(numStr); - 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](); - } - } -} diff --git a/testSequence1 b/testSequence1 deleted file mode 100644 index 8877192..0000000 --- a/testSequence1 +++ /dev/null @@ -1,2 +0,0 @@ -from meController import meArm -