us final version

2026-08-12 13:12:24 +09:00
parent dc1e67bf62
commit f0b4997b9c
2 changed files with 105 additions and 105 deletions
@@ -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() {
-  servo1.write(50);
-  servo2.write(0);
-  servo3.write(140);
-  servo4.write(10);
+struct LineBuf {
+  char buf[48];
+  byte len;
+};

-  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) {
-  if (cur == target) return;
+bool stopRequested() {
+  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;
-  step = abs(step);
+bool moveTo(const int target[N], unsigned long ms) {
+  int start[N];
+  for (byte i = 0; i < N; i++) start[i] = cur[i];

-  while (cur != target) {
-    cur += dir * step;
-    if ((dir > 0 && cur > target) || (dir < 0 && cur < target)) cur = target;
-    s.write(cur);
-    delay(dly);
+  unsigned long t0 = millis();
+  while (true) {
+    unsigned long t = millis() - t0;
+    float p = (ms == 0 || t >= ms) ? 1.0 : (float)t / ms;
+    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() {
-  moveServoSmooth(servo1, cur1, 50, 4, 20);
-  moveServoSmooth(servo2, cur2, 0,  4, 20);
-  moveServoSmooth(servo3, cur3, 140,4, 20);
-  moveServoSmooth(servo4, cur4, 10, 4, 20);
+void report(Stream &out, const char *word) {
+  out.print(word);
+  for (byte i = 0; i < N; i++) {
+    out.print(' ');
+    out.print(cur[i]);
+  }
+  out.println();
 }

-void action1() {
-  for (int i = 0; i <= 100; i += 4) {
-    int a3 = map(i, 0, 100, 140, 0);
-    servo3.write(a3);
-    cur3 = a3;
-    delay(10);
+bool parseMove(char *p, int target[N], long &ms) {
+  char *end;
+  for (byte i = 0; i < N; i++) {
+    long v = strtol(p, &end, 10);
+    if (end == p) return false;
+    target[i] = constrain(v, LIMITS[i][0], LIMITS[i][1]);
+    p = end;
  }
-  for (int i = 0; i <= 100; i += 4) {
-    int a2 = map(i, 0, 100, 0, 120);
-    int a3 = map(i, 0, 100, 0, 60);
-    servo2.write(a2);
-    servo3.write(a3);
-    cur2 = a2;
-    cur3 = a3;
-    delay(10);
+  ms = strtol(p, &end, 10);
+  if (end == p) return false;
+  ms = constrain(ms, 0, MAX_MS);
+  return true;
 }

-  delay(2000);
-  setHomePoseSmooth();
+void handle(char *line, Stream &out) {
+  switch (toupper(line[0])) {
+    case 'P':
+      out.println("PONG");
+      break;
+    case 'S':
+      report(out, "STOPPED");
+      break;
+    case 'H':
+      report(out, moveTo(HOME_POSE, 1000) ? "OK" : "STOPPED");
+      break;
+    case 'M': {
+      int target[N];
+      long ms;
+      if (!parseMove(line + 1, target, ms)) {
+        out.println("ERR bad move");
+        break;
      }
-
-void action2() {
-  int start2 = cur2;
-  int start3 = cur3;
-  int start4 = cur4;
-
-  for (int t = 0; t <= 100; t += 4) {
-    int a2 = map(t, 0, 100, start2, 80);
-    int a3 = map(t, 0, 100, start3, 60);
-    int a4 = map(t, 0, 100, start4, 60);
-    servo2.write(a2);
-    servo3.write(a3);
-    servo4.write(a4);
-    cur2 = a2;
-    cur3 = a3;
-    cur4 = a4;
-    delay(10);
+      report(out, moveTo(target, ms) ? "OK" : "STOPPED");
+      break;
    }
-
-  moveServoSmooth(servo1, cur1, 180, 4, 30);
-  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();
+    case 0:
+      break;
+    default:
+      out.println("ERR unknown");
  }
-
-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);
-  for (int i = 0; i <= 100; i += 4) {
-    int a2 = map(i, 0, 100, 0, 120);
-    servo2.write(a2);
-    cur2 = a2;
-    delay(5);
-  }
-
-  delay(2000);
-  setHomePoseSmooth();
 }

 void setup() {
  Serial.begin(9600);
  BT.begin(9600);

-  servo1.attach(4);
-  servo2.attach(5);
-  servo3.attach(6);
-  servo4.attach(7);
+  for (byte i = 0; i < N; i++) {
+    servos[i].attach(PINS[i]);
+    servos[i].write(HOME_POSE[i]);
+    cur[i] = HOME_POSE[i];
+  }
+  delay(1200);

-  // Home after start
-  forceHomePose();
-
-  Serial.println("Ready");
+  Serial.println("READY");
+  BT.println("READY");
 }

 void loop() {
-  if (BT.available()) {
-    char c = BT.read();
-    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());
-  }
+  if (readLine(BT, btLine)) handle(btLine.buf, BT);
+  if (readLine(Serial, usbLine)) handle(usbLine.buf, Serial);
 }