Improve arm control with IK validation, synchronized movements, and vision tracking

ArmParts.h

@@ -1,13 +1,17 @@
 #include <Servo.h>
 
+// Servo type: PWM (standard) or BUS (Hiwonder LX serial protocol)
+enum ServoType { SERVO_PWM, SERVO_BUS };
+
 // Define a structure for each part of the arm
 struct ArmPart {
     String name;
-    int pin;
+    int pin;          // PWM: Arduino pin number, BUS: servo ID
     int minAngle;
     int maxAngle;
     int defaultAngle;
-    Servo servo;
+    ServoType type;
+    Servo servo;      // Only used for SERVO_PWM
 };
 
 // Define a structure for the robot arm
@@ -21,12 +25,14 @@ struct RobotArm {
 };
 
 // Initialize the arm
+// PWM servos: base (pin 3), shoulder (pin 5), gripper (pin 11)
+// Bus servos: elbow (ID 1), wrist (ID 2), hand (ID 3)
 RobotArm arm = {
-// part name, pin on the board, min angle, max angle, default angle
-    {"base",     3, 35, 150, 90},
-    {"shoulder", 5, 0, 180, 140},
-    {"elbow",    6, 0, 140, 100},
-    {"wrist",    9, 89, 180, 135},
-    {"hand",     10, 0, 180, 90},
-    {"gripper",  11, 20, 90, 20}
+// name,       pin/ID, min, max, default, type
+    {"base",     3,  35, 150,  90, SERVO_PWM},
+    {"shoulder", 5,  0,  180, 140, SERVO_PWM},
+    {"elbow",    1,  0,  140, 100, SERVO_BUS},
+    {"wrist",    2,  89, 180, 135, SERVO_BUS},
+    {"hand",     3,  0,  180,  90, SERVO_BUS},
+    {"gripper",  11, 20,  90,  20, SERVO_PWM}
 };

BusServo.h

@@ -0,0 +1,110 @@
+#include <SoftwareSerial.h>
+
+// Bus servo serial pins (connect to BusLinker board)
+// Wiring: Arduino TX (pin 7) --> BusLinker RX
+//         Arduino RX (pin 6) <-- BusLinker TX
+//         Arduino GND -------- BusLinker GND
+//         BusLinker servo interface --> daisy-chained servos (PH2.0/3P cables)
+#define BUS_SERVO_RX_PIN 6
+#define BUS_SERVO_TX_PIN 7
+
+// Bus servo command IDs (Hiwonder LX protocol)
+#define CMD_MOVE_TIME_WRITE    1
+#define CMD_MOVE_TIME_READ     2
+#define CMD_MOVE_STOP          12
+#define CMD_POS_READ           28
+#define CMD_LOAD_UNLOAD_WRITE  31
+
+// Position range: 0-1000 maps to 0-240 degrees
+#define BUS_POS_MAX   1000
+#define BUS_DEG_MAX   240
+
+SoftwareSerial busSerial(BUS_SERVO_RX_PIN, BUS_SERVO_TX_PIN);
+
+void initBusServos() {
+    busSerial.begin(115200);
+}
+
+// Send a command packet to a bus servo
+void busServoWrite(uint8_t id, uint8_t cmd, const uint8_t *params, uint8_t paramLen) {
+    uint8_t length = paramLen + 3;
+    uint8_t buf[10];
+    buf[0] = 0x55;
+    buf[1] = 0x55;
+    buf[2] = id;
+    buf[3] = length;
+    buf[4] = cmd;
+    for (uint8_t i = 0; i < paramLen; i++) {
+        buf[5 + i] = params[i];
+    }
+    // Checksum: ~(ID + Length + Cmd + Params)
+    uint8_t cksum = 0;
+    for (uint8_t i = 2; i < 5 + paramLen; i++) {
+        cksum += buf[i];
+    }
+    buf[5 + paramLen] = ~cksum;
+
+    busSerial.write(buf, 6 + paramLen);
+    busSerial.flush();
+}
+
+// Move servo to position (0-1000) over time (0-30000 ms)
+void busServoMove(uint8_t id, int position, int timeMs) {
+    position = constrain(position, 0, BUS_POS_MAX);
+    timeMs = constrain(timeMs, 0, 30000);
+    uint8_t params[] = {
+        (uint8_t)(position & 0xFF),
+        (uint8_t)((position >> 8) & 0xFF),
+        (uint8_t)(timeMs & 0xFF),
+        (uint8_t)((timeMs >> 8) & 0xFF)
+    };
+    busServoWrite(id, CMD_MOVE_TIME_WRITE, params, 4);
+}
+
+// Stop servo movement
+void busServoStop(uint8_t id) {
+    busServoWrite(id, CMD_MOVE_STOP, NULL, 0);
+}
+
+// Enable or disable torque (true = hold position, false = free to move by hand)
+void busServoSetTorque(uint8_t id, bool enabled) {
+    uint8_t param = enabled ? 1 : 0;
+    busServoWrite(id, CMD_LOAD_UNLOAD_WRITE, &param, 1);
+}
+
+// Read current position from servo. Returns 0-1000, or -1 on failure
+int busServoReadPosition(uint8_t id) {
+    // Flush any pending data
+    while (busSerial.available()) busSerial.read();
+
+    busServoWrite(id, CMD_POS_READ, NULL, 0);
+
+    // Wait for response (8 bytes: header(2) + id + len + cmd + posLow + posHigh + checksum)
+    unsigned long start = millis();
+    while (busSerial.available() < 8 && millis() - start < 50) {
+        // timeout after 50ms
+    }
+
+    if (busSerial.available() >= 8) {
+        if (busSerial.read() != 0x55) return -1;
+        if (busSerial.read() != 0x55) return -1;
+        busSerial.read(); // id
+        busSerial.read(); // length
+        busSerial.read(); // cmd
+        uint8_t posLow = busSerial.read();
+        uint8_t posHigh = busSerial.read();
+        busSerial.read(); // checksum
+        return (posHigh << 8) | posLow;
+    }
+    return -1;
+}
+
+// Convert degrees (0-240) to bus servo position units (0-1000)
+int degreesToBusPos(int degrees) {
+    return map(constrain(degrees, 0, BUS_DEG_MAX), 0, BUS_DEG_MAX, 0, BUS_POS_MAX);
+}
+
+// Convert bus servo position units (0-1000) to degrees (0-240)
+int busPosToDegrees(int position) {
+    return map(constrain(position, 0, BUS_POS_MAX), 0, BUS_POS_MAX, 0, BUS_DEG_MAX);
+}

InverseKinematics.h

@@ -1,7 +1,9 @@
 #include <math.h>
 
-// Define PI for trigonometric calculations
+// Define PI for trigonometric calculations (guard against redefinition)
+#ifndef PI
 #define PI 3.14159265358979323846
+#endif
 
 const int baseHeight = 81;
 const int armLength1 = 104; // Shoulder to elbow
@@ -15,12 +17,20 @@ void solveXYZ(float x, float y, float &baseAngle, float &r) {
 }
 
 // Function to calculate the shoulder, elbow, and wrist angles
-void solveRZ(float r, float z, float gripAngle, float &shoulderAngle, float &elbowAngle, float &wristAngle) {
+// Returns true if the position is reachable, false otherwise
+bool solveRZ(float r, float z, float gripAngle, float &shoulderAngle, float &elbowAngle, float &wristAngle) {
     float r_prime = r - (sin(gripAngle) * gripLengthOpen);
     float z_prime = z - baseHeight + (cos(gripAngle) * gripLengthOpen);
     float h = sqrt(r_prime * r_prime + z_prime * z_prime) / 2;
-
-    elbowAngle = asin(h / armLength1) * 2;
+
+    // Reachability check: asin requires input in [-1.0, 1.0]
+    float ratio = h / armLength1;
+    if (ratio > 1.0f || ratio < -1.0f) {
+        return false;
+    }
+
+    elbowAngle = asin(ratio) * 2;
     shoulderAngle = atan2(z_prime, r_prime) + ((PI - elbowAngle) / 2);
     wristAngle = PI + gripAngle - shoulderAngle - elbowAngle;
+    return true;
 }

Movements.h

@@ -32,36 +32,166 @@ void openGripper() {
     setServoPosition(arm.gripper, 0);
 }
 
+// Move to initial position with all joints synchronized
 void moveInitialPosition() {
-    moveBase(90);
-    moveShoulder(140);
-    moveElbow(100);
-    moveWrist(135);
-    moveHand(90);
+    ArmPart *parts[] = {&arm.base, &arm.shoulder, &arm.elbow, &arm.wrist, &arm.hand};
+    int targets[] = {90, 140, 100, 135, 90};
+    moveJointsSynchronized(parts, targets, 5);
 }
 
+// Coordinated grab sequence using synchronized multi-joint movements
 void grab() {
-    moveShoulder(90);//90
-    moveElbow(130);
-    moveWrist(60);
+    // Phase 1: Move arm to pre-grab posture (shoulder + elbow + wrist together)
+    {
+        ArmPart *parts[] = {&arm.shoulder, &arm.elbow, &arm.wrist};
+        int targets[] = {90, 130, 150};
+        moveJointsSynchronized(parts, targets, 3);
+    }
     delay(200);
+
+    // Phase 2: Rotate hand and open gripper
     moveHand(180);
     openGripper();
-    moveElbow(150);
-    moveShoulder(50);
-    moveWrist(80);
-    moveElbow(160);
+
+    // Phase 3: Extend arm to reach object (shoulder + elbow + wrist together)
+    {
+        ArmPart *parts[] = {&arm.shoulder, &arm.elbow, &arm.wrist};
+        int targets[] = {80, 130, 160};
+        moveJointsSynchronized(parts, targets, 3);
+    }
     delay(500);
+
+    // Phase 4: Grip the object
     closeGripper();
     delay(500);
+
+    // Phase 5: Lift to safe position
     moveInitialPosition();
     delay(300);
-    moveBase(60);
-    moveShoulder(100);
-    moveWrist(30);
-    moveHand(90);
+
+    // Phase 6: Move to drop position (base + shoulder + wrist + hand together)
+    {
+        ArmPart *parts[] = {&arm.base, &arm.shoulder, &arm.wrist, &arm.hand};
+        int targets[] = {60, 100, 160, 90};
+        moveJointsSynchronized(parts, targets, 4);
+    }
     delay(1000);
+
+    // Phase 7: Release object
     openGripper();
     delay(1000);
+
+    // Phase 8: Return to initial position
     moveInitialPosition();
-}
+}
+
+// --- Teach mode: record and replay arm positions ---
+
+#define MAX_WAYPOINTS 20
+
+struct Waypoint {
+    int angles[6]; // base, shoulder, elbow, wrist, hand, gripper
+};
+
+Waypoint teachWaypoints[MAX_WAYPOINTS];
+int teachCount = 0;
+bool teachMode = false;
+
+// Enter teach mode: disable torque on bus servos so they can be moved by hand
+void teachStart() {
+    teachMode = true;
+    teachCount = 0;
+
+    // Disable torque on bus servos (they become free to move)
+    if (arm.elbow.type == SERVO_BUS) busServoSetTorque(arm.elbow.pin, false);
+    if (arm.wrist.type == SERVO_BUS) busServoSetTorque(arm.wrist.pin, false);
+    if (arm.hand.type == SERVO_BUS) busServoSetTorque(arm.hand.pin, false);
+
+    Serial.println(F("Teach mode on. Move bus servos (elbow, wrist, hand) by hand."));
+    Serial.println(F("Use commands for PWM servos (base, shoulder, gripper)."));
+    Serial.println(F("  teach capture - save current position as waypoint"));
+    Serial.println(F("  teach play    - replay recorded waypoints"));
+    Serial.println(F("  teach off     - exit teach mode"));
+}
+
+// Capture current arm position as a waypoint
+void teachCapture() {
+    if (!teachMode) {
+        Serial.println(F("Not in teach mode. Send 'teach on' first."));
+        return;
+    }
+    if (teachCount >= MAX_WAYPOINTS) {
+        Serial.println(F("Waypoint storage full (max 20)."));
+        return;
+    }
+
+    Waypoint &wp = teachWaypoints[teachCount];
+    wp.angles[0] = getServoAngle(arm.base);
+    wp.angles[1] = getServoAngle(arm.shoulder);
+    wp.angles[2] = getServoAngle(arm.elbow);
+    wp.angles[3] = getServoAngle(arm.wrist);
+    wp.angles[4] = getServoAngle(arm.hand);
+    wp.angles[5] = getServoAngle(arm.gripper);
+
+    teachCount++;
+
+    Serial.print(F("Waypoint "));
+    Serial.print(teachCount);
+    Serial.print(F(" saved: "));
+    for (int i = 0; i < 6; i++) {
+        Serial.print(wp.angles[i]);
+        if (i < 5) Serial.print(F(", "));
+    }
+    Serial.println();
+}
+
+// Replay all recorded waypoints
+void teachPlay() {
+    if (teachCount == 0) {
+        Serial.println(F("No waypoints recorded."));
+        return;
+    }
+
+    // Re-enable torque on bus servos for playback
+    if (arm.elbow.type == SERVO_BUS) busServoSetTorque(arm.elbow.pin, true);
+    if (arm.wrist.type == SERVO_BUS) busServoSetTorque(arm.wrist.pin, true);
+    if (arm.hand.type == SERVO_BUS) busServoSetTorque(arm.hand.pin, true);
+    delay(100);
+
+    Serial.print(F("Playing "));
+    Serial.print(teachCount);
+    Serial.println(F(" waypoints..."));
+
+    for (int i = 0; i < teachCount; i++) {
+        Serial.print(F("Waypoint "));
+        Serial.println(i + 1);
+
+        ArmPart *parts[] = {&arm.base, &arm.shoulder, &arm.elbow,
+                            &arm.wrist, &arm.hand, &arm.gripper};
+        int targets[] = {
+            teachWaypoints[i].angles[0],
+            teachWaypoints[i].angles[1],
+            teachWaypoints[i].angles[2],
+            teachWaypoints[i].angles[3],
+            teachWaypoints[i].angles[4],
+            teachWaypoints[i].angles[5]
+        };
+        moveJointsSynchronized(parts, targets, 6);
+        delay(500);
+    }
+
+    Serial.println(F("Playback complete."));
+}
+
+// Exit teach mode: re-enable torque on bus servos
+void teachStop() {
+    if (arm.elbow.type == SERVO_BUS) busServoSetTorque(arm.elbow.pin, true);
+    if (arm.wrist.type == SERVO_BUS) busServoSetTorque(arm.wrist.pin, true);
+    if (arm.hand.type == SERVO_BUS) busServoSetTorque(arm.hand.pin, true);
+
+    teachMode = false;
+
+    Serial.print(F("Teach mode off. "));
+    Serial.print(teachCount);
+    Serial.println(F(" waypoints stored. Send 'teach play' to replay."));
+}