modified: servoTest/servoTest.ino
This commit is contained in:
ketrptr
parent
bd1d3c7033
commit
ff252c966b
@ -1,55 +1,95 @@
|
||||
#include <Servo.h>
|
||||
#include <avr/io.h>
|
||||
#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
|
||||
|
||||
Servo S_left,S_right,S_turn,S_grip;
|
||||
// Command enum for serial communication
|
||||
// L - left servo
|
||||
// R - right servo
|
||||
// T - turn servo
|
||||
// G - grip servo
|
||||
typedef enum { L, R, T, G, H, S, D } CMD;
|
||||
|
||||
const Servo *axis[4] = {&S_left, &S_right, &S_turn, &S_grip};
|
||||
const byte axCount = 4;
|
||||
char CMD2char(CMD c) {
|
||||
return (c == L) ? 'L' : (c == R) ? 'R' : (c == T) ? 'T' : 'G';
|
||||
}
|
||||
|
||||
int ServoPos[] = {90,90,90,90};
|
||||
bool absPos; // absolute /relative positioning
|
||||
|
||||
typedef enum {
|
||||
L,
|
||||
R,
|
||||
T,
|
||||
G
|
||||
}ax;
|
||||
|
||||
|
||||
void error(String msg){
|
||||
void error(String msg) {
|
||||
Serial.print("error: ");
|
||||
Serial.println(msg);
|
||||
}
|
||||
|
||||
void movePos(ax dir){
|
||||
int val = axis[dir]->read();
|
||||
if(ServoPos[dir] < 20 || ServoPos[dir] > 175){
|
||||
// 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};
|
||||
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;
|
||||
}
|
||||
if (servoPos[dir] < 20 || servoPos[dir] > 175) {
|
||||
error("move out of range");
|
||||
return;
|
||||
}
|
||||
if(val != ServoPos[dir]){
|
||||
int delta = ServoPos[dir]-val;
|
||||
while(delta){
|
||||
if(delta < 0){
|
||||
axis[dir]->write((val--) - 1);
|
||||
delta+=1;
|
||||
}else{
|
||||
axis[dir]->write((val++) + 1);
|
||||
delta-=1;
|
||||
}
|
||||
delay(50);
|
||||
}
|
||||
|
||||
// if actual servo pos is different to wanted move towards it
|
||||
int val = axis[dir]->read();
|
||||
if (val != servoPos[dir]) {
|
||||
int delta = servoPos[dir] - val;
|
||||
int inc;
|
||||
while (delta) {
|
||||
inc = delta / 5;
|
||||
if (inc == 0) {
|
||||
error("movement increment is 0?");
|
||||
return;
|
||||
}
|
||||
axis[dir]->write(val + inc);
|
||||
delta -= inc;
|
||||
val += inc;
|
||||
}
|
||||
delay(50); // TODO no delay
|
||||
}
|
||||
}
|
||||
|
||||
int cmdVal;
|
||||
CMD cmd;
|
||||
|
||||
void moveCmdHandler() {
|
||||
Serial.print("Servo_Command: ");
|
||||
Serial.print(CMD2char(cmd));
|
||||
Serial.println(cmdVal);
|
||||
if (absPos) {
|
||||
servoPos[cmd] = constrain(cmdVal, 20, 165);
|
||||
} else {
|
||||
servoPos[cmd] = constrain(servoPos[cmd] + cmdVal, 20, 165);
|
||||
}
|
||||
Serial.print("new val: ");
|
||||
Serial.println(servoPos[cmd]);
|
||||
}
|
||||
|
||||
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]);
|
||||
}
|
||||
}
|
||||
// Serial Input buffer
|
||||
char serialBuf[8];
|
||||
int bufSize = 8;
|
||||
|
||||
void setup() {
|
||||
S_left.attach(S_left_pin);
|
||||
S_right.attach(S_right_pin);
|
||||
@ -59,129 +99,81 @@ void setup() {
|
||||
Serial.begin(BAUDRATE);
|
||||
Serial.println("reading Servo positions");
|
||||
absPos = true;
|
||||
for(int i =0; i < axCount; i++){
|
||||
ServoPos[i] = axis[i]->read();
|
||||
Serial.print("--");
|
||||
Serial.println(ServoPos[i]);
|
||||
}
|
||||
delay(1000);
|
||||
delay(500);
|
||||
}
|
||||
|
||||
|
||||
//Serial Input buffer
|
||||
char serialBuf[8];
|
||||
int bufSize = 8;
|
||||
|
||||
|
||||
void loop() {
|
||||
|
||||
// print changes
|
||||
for(int i =0; i < axCount; i++){
|
||||
int val = axis[i]->read();
|
||||
if(val != ServoPos[i]){
|
||||
int val;
|
||||
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.println(servoPos[i]);
|
||||
movePos(i);
|
||||
}
|
||||
delay(50);
|
||||
}
|
||||
|
||||
//read position command from serial
|
||||
int serialIn = Serial.available();
|
||||
if(serialIn){
|
||||
int r = Serial.readBytesUntil('\n',serialBuf, bufSize);
|
||||
// read position command from serial
|
||||
if (Serial.available()) {
|
||||
int r = Serial.readBytesUntil('\n', serialBuf, bufSize);
|
||||
Serial.print("read: ");
|
||||
Serial.println(r);
|
||||
//Serial.println(serialBuf);
|
||||
if(r < 2){
|
||||
// Serial.println(serialBuf);
|
||||
if (r < 2) {
|
||||
error("command to short!");
|
||||
return;
|
||||
}
|
||||
String numStr = "";
|
||||
for(int i = 1; i < r; i++){
|
||||
if(isDigit(serialBuf[i]) || isPunct(serialBuf[i])){
|
||||
for (int i = 1; i < r; i++) {
|
||||
if (isDigit(serialBuf[i]) || isPunct(serialBuf[i])) {
|
||||
numStr += serialBuf[i];
|
||||
}else{
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
Serial.print("NumStr: ");
|
||||
Serial.println(numStr);
|
||||
int pos = numStr.toInt();
|
||||
// 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;
|
||||
|
||||
switch(serialBuf[0]){
|
||||
switch (serialBuf[0]) {
|
||||
case 'L':
|
||||
Serial.println("Servo_Command L");
|
||||
Serial.println(pos);
|
||||
if(absPos){
|
||||
ServoPos[L] = constrain(pos, 20, 165);
|
||||
}else{
|
||||
ServoPos[L] += pos;
|
||||
}
|
||||
Serial.println(ServoPos[L]);
|
||||
break;
|
||||
case 'R':
|
||||
Serial.println("Servo_Command R");
|
||||
Serial.println(pos);
|
||||
|
||||
if(absPos){
|
||||
ServoPos[R] = constrain(pos, 20, 170);
|
||||
}else{
|
||||
ServoPos[R] += pos;
|
||||
}
|
||||
|
||||
Serial.println(ServoPos[R]);
|
||||
|
||||
break;
|
||||
case 'T':
|
||||
Serial.println("Servo_Command T");
|
||||
Serial.println(pos);
|
||||
if(absPos){
|
||||
ServoPos[T] = constrain(pos, 20, 170);
|
||||
}else{
|
||||
ServoPos[T] += pos;
|
||||
}
|
||||
|
||||
Serial.println(ServoPos[T]);
|
||||
break;
|
||||
case 'G':
|
||||
Serial.println("Servo_Command G");
|
||||
Serial.println(pos);
|
||||
if(absPos){
|
||||
ServoPos[G] = constrain(pos, 90, 170);
|
||||
}else{
|
||||
ServoPos[G] += pos;
|
||||
}
|
||||
|
||||
Serial.println(ServoPos[G]);
|
||||
moveCmdHandler();
|
||||
break;
|
||||
case 'H':
|
||||
Serial.println((absPos)?"Absolute Positioning" : "Relative Positioning");
|
||||
for(int i =0; i < axCount; i++){
|
||||
char axSym = (i==L)?'L':(i==R)?'R':(i==T)?'T':'G';
|
||||
Serial.print(axSym);
|
||||
Serial.print("--");
|
||||
Serial.println(ServoPos[i]);
|
||||
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;
|
||||
case 'S':
|
||||
if(serialBuf[1] == '0') absPos = false;
|
||||
else absPos = true;
|
||||
break;
|
||||
case 'D': //default
|
||||
for(int i = 0; i < axCount;i++){
|
||||
ServoPos[i] = 90;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
|
||||
default:
|
||||
error("invalid axis description");
|
||||
error("invalid command");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user