schurtisimon/meArm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

modified: servoTest/servoTest.ino

Browse Source
This commit is contained in:
ketrptr 2025-03-26 02:25:15 +01:00
parent bd1d3c7033
commit ff252c966b
1 changed files with 142 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

290
servoTest/servoTest.ino
View File

@ -1,55 +1,95 @@
#include <Servo.h>
#include <avr/io.h>
#include "config.h" #include "config.h"
#include <Servo.h>
// #include <avr/io.h>
// mee arm Servo pins
#define S_left_pin 10 #define S_left_pin 10
#define S_right_pin 5 #define S_right_pin 5
#define S_turn_pin 6 #define S_turn_pin 6
#define S_grip_pin 9 #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;
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 Servo *axis[4] = {&S_left, &S_right, &S_turn, &S_grip}; const Servo *axis[4] = {&S_left, &S_right, &S_turn, &S_grip};
const byte axCount = 4; const byte axCount = 4;
int servoPos[] = {90, 90, 90, 90};
int ServoPos[] = {90,90,90,90};
bool absPos; // absolute /relative positioning bool absPos; // absolute /relative positioning
typedef enum { // Move axis[dir] to the position set in servoPos[dir]
L, void movePos(CMD dir) {
R, // safety check
T, if (dir > G) {
G error("Not a valid Axis!!");
}ax; return;
}
if (servoPos[dir] < 20 || servoPos[dir] > 175) {
error("move out of range");
return;
}
// if actual servo pos is different to wanted move towards it
void error(String msg){ int val = axis[dir]->read();
Serial.print("error: "); if (val != servoPos[dir]) {
Serial.println(msg); int delta = servoPos[dir] - val;
} int inc;
while (delta) {
void movePos(ax dir){ inc = delta / 5;
int val = axis[dir]->read(); if (inc == 0) {
if(ServoPos[dir] < 20 || ServoPos[dir] > 175){ error("movement increment is 0?");
error("move out of range"); return;
return;
} }
if(val != ServoPos[dir]){ axis[dir]->write(val + inc);
int delta = ServoPos[dir]-val; delta -= inc;
while(delta){ val += inc;
if(delta < 0){
axis[dir]->write((val--) - 1);
delta+=1;
}else{
axis[dir]->write((val++) + 1);
delta-=1;
}
delay(50);
}
} }
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() { void setup() {
S_left.attach(S_left_pin); S_left.attach(S_left_pin);
S_right.attach(S_right_pin); S_right.attach(S_right_pin);
@ -59,129 +99,81 @@ void setup() {
Serial.begin(BAUDRATE); Serial.begin(BAUDRATE);
Serial.println("reading Servo positions"); Serial.println("reading Servo positions");
absPos = true; absPos = true;
for(int i =0; i < axCount; i++){ delay(500);
ServoPos[i] = axis[i]->read();
Serial.print("--");
Serial.println(ServoPos[i]);
}
delay(1000);
} }
//Serial Input buffer
char serialBuf[8];
int bufSize = 8;
void loop() { void loop() {
// print changes // print changes
for(int i =0; i < axCount; i++){ int val;
int val = axis[i]->read(); for (int i = 0; i < axCount; i++) {
if(val != ServoPos[i]){ val = axis[i]->read();
Serial.print("-- "); if (val != servoPos[i]) {
Serial.println(i); Serial.print("-- ");
Serial.print(val); Serial.println(i);
Serial.print("-->"); Serial.print(val);
Serial.println(ServoPos[i]); Serial.print("-->");
movePos(i); Serial.println(servoPos[i]);
} movePos(i);
delay(50); }
delay(50);
} }
//read position command from serial // read position command from serial
int serialIn = Serial.available(); if (Serial.available()) {
if(serialIn){ int r = Serial.readBytesUntil('\n', serialBuf, bufSize);
int r = Serial.readBytesUntil('\n',serialBuf, bufSize);
Serial.print("read: "); Serial.print("read: ");
Serial.println(r); Serial.println(r);
//Serial.println(serialBuf); // Serial.println(serialBuf);
if(r < 2){ if (r < 2) {
error("command to short!"); error("command to short!");
return; return;
} }
String numStr = ""; String numStr = "";
for(int i = 1; i < r; i++){ for (int i = 1; i < r; i++) {
if(isDigit(serialBuf[i]) || isPunct(serialBuf[i])){ if (isDigit(serialBuf[i]) || isPunct(serialBuf[i])) {
numStr += serialBuf[i]; numStr += serialBuf[i];
}else{ } else {
break;
}
}
Serial.print("NumStr: ");
Serial.println(numStr);
int pos = numStr.toInt();
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]);
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]);
}
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");
break; 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;
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;
}
}
}