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