meArm/servoTest/servoTest.ino

#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;
  }
  if (servoPos[dir] < 20 || servoPos[dir] > 175) {
    error("move out of range");
    return;
  }

  // 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) {
      if(dir!=T) inc = (abs(delta) > 20) ? constrain((abs(delta) / 10), 1, 5) : 1;
      else inc =1;
      if (delta > 0) {
        axis[dir]->write(val + inc);
        delta -= inc;
        val += inc;
      } else {
        axis[dir]->write(val - inc);
        delta += inc;
        val -= inc;
      }

      delay(20); // TODO  no delay
    }
  }
}

//Global current command value and type
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]);
  }
}

//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)
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();

  Serial.begin(BAUDRATE);
  absPos = true;
  delay(500);
}


void loop() {

  // print changes
  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]);
      movePos(i);
    }
    delay(50);
  }
  Serial.println("READY");

  // 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) {
      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);
    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(cmd >= 0){
      cmdVal = numStr.toInt();
      handlers[cmd]();
    }
  }
}