Add robot_arm.ino

robot_arm.ino

@@ -0,0 +1,138 @@
+#include <SoftwareSerial.h>
+#include <Servo.h>
+
+SoftwareSerial BT(3, 2); // RX, TX
+
+Servo servo1, servo2, servo3, servo4;
+
+int cur1, cur2, cur3, cur4;
+
+void forceHomePose() {
+  servo1.write(50);
+  servo2.write(0);
+  servo3.write(140);
+  servo4.write(10);
+
+  delay(1200);
+
+  cur1 = 50; cur2 = 0; cur3 = 140; cur4 = 10;
+}
+
+void moveServoSmooth(Servo &s, int &cur, int target, int step, int dly) {
+  if (cur == target) return;
+
+  int dir = (target > cur) ? 1 : -1;
+  step = abs(step);
+
+  while (cur != target) {
+    cur += dir * step;
+    if ((dir > 0 && cur > target) || (dir < 0 && cur < target)) cur = target;
+    s.write(cur);
+    delay(dly);
+  }
+}
+
+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 action1() {
+  for (int i = 0; i <= 100; i += 4) {
+    int a3 = map(i, 0, 100, 140, 0);
+    servo3.write(a3);
+    cur3 = a3;
+    delay(10);
+  }
+  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);
+  }
+
+  delay(2000);
+  setHomePoseSmooth();
+}
+
+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);
+  }
+
+  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();
+}
+
+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);
+
+  // Home after start
+  forceHomePose();
+
+  Serial.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());
+  }
+}