modified: servoTest/servoTest.ino

servoTest/servoTest.ino

@@ -1,54 +1,94 @@
-#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
-const Servo *axis[4] = {&S_left, &S_right, &S_turn, &S_grip};
+// R - right servo
-const byte axCount = 4;
+// T - turn servo
-
+// G - grip servo
-int ServoPos[] = {90,90,90,90};
+typedef enum { L, R, T, G, H, S, D } CMD;
-bool absPos; // absolute /relative positioning
-
-typedef enum {
-    L,
-    R,
-    T,
-    G
-}ax;
 
+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);
 }
 
-void movePos(ax dir){
+// Servo setup
-    int val = axis[dir]->read();
+Servo S_left, S_right, S_turn, S_grip;
-    if(ServoPos[dir] < 20 || ServoPos[dir] > 175){
+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;
+  // 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(delta < 0){
+      inc = delta / 5;
-          axis[dir]->write((val--) - 1);
+      if (inc == 0) {
-          delta+=1;
+        error("movement increment is 0?");
-        }else{
+        return;
-          axis[dir]->write((val++) + 1);
+      }
-          delta-=1;
+      axis[dir]->write(val + inc);
+      delta -= inc;
+      val += inc;
+    }
+    delay(50); // TODO  no delay
   }
-          delay(50);
 }
 
+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);
@@ -59,39 +99,27 @@ void setup() {
   Serial.begin(BAUDRATE);
   Serial.println("reading Servo positions");
   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() {
 
   // print changes
+  int val;
   for (int i = 0; i < axCount; i++) {
-      int val = axis[i]->read();
+    val = axis[i]->read();
-      if(val != ServoPos[i]){
+    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 (Serial.available()) {
-  if(serialIn){
     int r = Serial.readBytesUntil('\n', serialBuf, bufSize);
     Serial.print("read: ");
     Serial.println(r);
@@ -108,80 +136,44 @@ void loop() {
         break;
       }
     }
-    Serial.print("NumStr: ");
+    // Serial.print("NumStr: ");
-    Serial.println(numStr);
+    // Serial.println(numStr);
-    int pos = numStr.toInt();
+    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':
-          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");
+      moveCmdHandler();
-          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");
+      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++) {
-              char axSym = (i==L)?'L':(i==R)?'R':(i==T)?'T':'G';
+        servoPos[i] = 90;
-              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");
+      error("invalid command");
       break;
     }
   }
-
+}