deleted: controller.py
deleted: servoTest/config.h deleted: servoTest/servoTest.ino deleted: testSequence1
This commit is contained in:
ketrptr
parent
02f6dbef49
commit
4d8b5097c5
@ -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')
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,4 +0,0 @@
|
|||||||
#ifndef CONFIG
|
|
||||||
#define CONFIG
|
|
||||||
#define BAUDRATE 9600
|
|
||||||
#endif
|
|
||||||
@ -1,227 +0,0 @@
|
|||||||
#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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// 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]();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,2 +0,0 @@
|
|||||||
from meController import meArm
|
|
||||||
|
|
||||||
Loading…
x
Reference in New Issue
Block a user