188 lines
4.1 KiB
C++
188 lines
4.1 KiB
C++
#include <Servo.h>
|
|
#include <avr/io.h>
|
|
#include "config.h"
|
|
|
|
#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;
|
|
|
|
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
|
|
|
|
typedef enum {
|
|
L,
|
|
R,
|
|
T,
|
|
G
|
|
}ax;
|
|
|
|
|
|
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){
|
|
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);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
void setup() {
|
|
S_left.attach(S_left_pin);
|
|
S_right.attach(S_right_pin);
|
|
S_turn.attach(S_turn_pin);
|
|
S_grip.attach(S_grip_pin);
|
|
|
|
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);
|
|
}
|
|
|
|
|
|
//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]){
|
|
Serial.print("-- ");
|
|
Serial.println(i);
|
|
Serial.print(val);
|
|
Serial.print("-->");
|
|
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);
|
|
Serial.print("read: ");
|
|
Serial.println(r);
|
|
//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])){
|
|
numStr += serialBuf[i];
|
|
}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;
|
|
}
|
|
}
|
|
|