us final version

robot_arm.ino

@@ -3,136 +3,136 @@
 
 SoftwareSerial BT(3, 2); // RX, TX
 
-Servo servo1, servo2, servo3, servo4;
+const byte N = 4;
+const byte PINS[N] = {4, 5, 6, 7};
+const int HOME_POSE[N] = {50, 0, 140, 10};
+const int LIMITS[N][2] = {{0, 180}, {0, 180}, {0, 180}, {0, 180}};
+const unsigned long TICK_MS = 15;
+const long MAX_MS = 10000;
 
-int cur1, cur2, cur3, cur4;
+Servo servos[N];
+int cur[N];
 
-void forceHomePose() {
+struct LineBuf {
-  servo1.write(50);
+  char buf[48];
-  servo2.write(0);
+  byte len;
-  servo3.write(140);
+};
-  servo4.write(10);
 
-  delay(1200);
+LineBuf btLine, usbLine;
 
-  cur1 = 50; cur2 = 0; cur3 = 140; cur4 = 10;
+bool readLine(Stream &s, LineBuf &l) {
+  while (s.available()) {
+    char c = s.read();
+    if (c == '\r') continue;
+    if (c == '\n') {
+      l.buf[l.len] = 0;
+      l.len = 0;
+      return true;
+    }
+    if (l.len < sizeof(l.buf) - 1) l.buf[l.len++] = c;
+  }
+  return false;
 }
 
-void moveServoSmooth(Servo &s, int &cur, int target, int step, int dly) {
+bool stopRequested() {
-  if (cur == target) return;
+  if (readLine(BT, btLine) && toupper(btLine.buf[0]) == 'S') return true;
+  if (readLine(Serial, usbLine) && toupper(usbLine.buf[0]) == 'S') return true;
+  return false;
+}
 
-  int dir = (target > cur) ? 1 : -1;
+bool moveTo(const int target[N], unsigned long ms) {
-  step = abs(step);
+  int start[N];
+  for (byte i = 0; i < N; i++) start[i] = cur[i];
 
-  while (cur != target) {
+  unsigned long t0 = millis();
-    cur += dir * step;
+  while (true) {
-    if ((dir > 0 && cur > target) || (dir < 0 && cur < target)) cur = target;
+    unsigned long t = millis() - t0;
-    s.write(cur);
+    float p = (ms == 0 || t >= ms) ? 1.0 : (float)t / ms;
-    delay(dly);
+    float eased = p * p * (3 - 2 * p);
+
+    for (byte i = 0; i < N; i++) {
+      int a = round(start[i] + (target[i] - start[i]) * eased);
+      if (a != cur[i]) {
+        servos[i].write(a);
+        cur[i] = a;
+      }
+    }
+    if (p >= 1.0) return true;
+
+    unsigned long next = millis() + TICK_MS;
+    while ((long)(next - millis()) > 0) {
+      if (stopRequested()) return false;
+    }
   }
 }
 
-void setHomePoseSmooth() {
+void report(Stream &out, const char *word) {
-  moveServoSmooth(servo1, cur1, 50, 4, 20);
+  out.print(word);
-  moveServoSmooth(servo2, cur2, 0,  4, 20);
+  for (byte i = 0; i < N; i++) {
-  moveServoSmooth(servo3, cur3, 140,4, 20);
+    out.print(' ');
-  moveServoSmooth(servo4, cur4, 10, 4, 20);
+    out.print(cur[i]);
+  }
+  out.println();
 }
 
-void action1() {
+bool parseMove(char *p, int target[N], long &ms) {
-  for (int i = 0; i <= 100; i += 4) {
+  char *end;
-    int a3 = map(i, 0, 100, 140, 0);
+  for (byte i = 0; i < N; i++) {
-    servo3.write(a3);
+    long v = strtol(p, &end, 10);
-    cur3 = a3;
+    if (end == p) return false;
-    delay(10);
+    target[i] = constrain(v, LIMITS[i][0], LIMITS[i][1]);
+    p = end;
   }
-  for (int i = 0; i <= 100; i += 4) {
+  ms = strtol(p, &end, 10);
-    int a2 = map(i, 0, 100, 0, 120);
+  if (end == p) return false;
-    int a3 = map(i, 0, 100, 0, 60);
+  ms = constrain(ms, 0, MAX_MS);
-    servo2.write(a2);
+  return true;
-    servo3.write(a3);
-    cur2 = a2;
-    cur3 = a3;
-    delay(10);
-  }
-
-  delay(2000);
-  setHomePoseSmooth();
 }
 
-void action2() {
+void handle(char *line, Stream &out) {
-  int start2 = cur2;
+  switch (toupper(line[0])) {
-  int start3 = cur3;
+    case 'P':
-  int start4 = cur4;
+      out.println("PONG");
-
+      break;
-  for (int t = 0; t <= 100; t += 4) {
+    case 'S':
-    int a2 = map(t, 0, 100, start2, 80);
+      report(out, "STOPPED");
-    int a3 = map(t, 0, 100, start3, 60);
+      break;
-    int a4 = map(t, 0, 100, start4, 60);
+    case 'H':
-    servo2.write(a2);
+      report(out, moveTo(HOME_POSE, 1000) ? "OK" : "STOPPED");
-    servo3.write(a3);
+      break;
-    servo4.write(a4);
+    case 'M': {
-    cur2 = a2;
+      int target[N];
-    cur3 = a3;
+      long ms;
-    cur4 = a4;
+      if (!parseMove(line + 1, target, ms)) {
-    delay(10);
+        out.println("ERR bad move");
+        break;
       }
-
+      report(out, moveTo(target, ms) ? "OK" : "STOPPED");
-  moveServoSmooth(servo1, cur1, 180, 4, 30);
+      break;
-  moveServoSmooth(servo1, cur1, 0,   4, 30);
-  moveServoSmooth(servo1, cur1, 180, 4, 30);
-  moveServoSmooth(servo1, cur1, 0,   4, 30);
-  moveServoSmooth(servo1, cur1, 180, 4, 30);
-  moveServoSmooth(servo1, cur1, 50,  4, 30);
-
-  delay(2000);
-  setHomePoseSmooth();
-}
-
-void action3() {
-  for (int i = 0; i <= 100; i += 4) {
-    int a3 = map(i, 0, 100, 140, 0);
-    servo3.write(a3);
-    cur3 = a3;
-    delay(10);
     }
-  delay(500);
+    case 0:
-  for (int i = 0; i <= 100; i += 4) {
+      break;
-    int a2 = map(i, 0, 100, 0, 120);
+    default:
-    servo2.write(a2);
+      out.println("ERR unknown");
-    cur2 = a2;
-    delay(5);
   }
-
-  delay(2000);
-  setHomePoseSmooth();
 }
 
 void setup() {
   Serial.begin(9600);
   BT.begin(9600);
 
-  servo1.attach(4);
+  for (byte i = 0; i < N; i++) {
-  servo2.attach(5);
+    servos[i].attach(PINS[i]);
-  servo3.attach(6);
+    servos[i].write(HOME_POSE[i]);
-  servo4.attach(7);
+    cur[i] = HOME_POSE[i];
+  }
+  delay(1200);
 
-  // Home after start
+  Serial.println("READY");
-  forceHomePose();
+  BT.println("READY");
-
-  Serial.println("Ready");
 }
 
 void loop() {
-  if (BT.available()) {
+  if (readLine(BT, btLine)) handle(btLine.buf, BT);
-    char c = BT.read();
+  if (readLine(Serial, usbLine)) handle(usbLine.buf, Serial);
-    if (c == '1') action1();
-    else if (c == '2') action2();
-    else if (c == '3') action3();
-    else if (c == 'H' || c == 'h') setHomePoseSmooth();
-  }
-
-  if (Serial.available()) {
-    BT.write(Serial.read());
-  }
 }