/* 5-Bar Linkage Planar SCARA Robot Controller Fab Lab INP-HB - Fab Academy 2026 - Week 12 Dual Servo Actuation with Basic Geometric Kinematics */ #include #include // Servo objects Servo servoLeft; Servo servoRight; // GPIO Pins for ESP32 const int SERVO_LEFT_PIN = 18; const int SERVO_RIGHT_PIN = 19; // Geometric parameters of the 5-bar linkage (in mm) const float D = 80.0; // Distance between the two motor pivot axes const float L1 = 90.0; // Length of proximal arms (attached to motors) const float L2 = 120.0; // Length of distal arms (holding the pen) // Calibration offsets (adjust to align 90 deg horizontal/vertical) int offsetLeft = 0; int offsetRight = 0; // Function to compute Inverse Kinematics bool computeIK(float x, float y, float &theta1_deg, float &theta2_deg) { // Left arm pivot at (-D/2, 0) float x1 = x + D / 2.0; float y1 = y; float r1_sq = x1 * x1 + y1 * y1; float r1 = sqrt(r1_sq); // Right arm pivot at (+D/2, 0) float x2 = x - D / 2.0; float y2 = y; float r2_sq = x2 * x2 + y2 * y2; float r2 = sqrt(r2_sq); // Check workspace limits if (r1 > (L1 + L2) || r1 < fabs(L1 - L2) || r2 > (L1 + L2) || r2 < fabs(L1 - L2)) { return false; // Point unreachable } // Law of cosines for left triangle float cos_alpha1 = (L1 * L1 + r1_sq - L2 * L2) / (2.0 * L1 * r1); if (cos_alpha1 < -1.0 || cos_alpha1 > 1.0) return false; float alpha1 = acos(cos_alpha1); float beta1 = atan2(y1, x1); float theta1 = beta1 + alpha1; // Elbow-left // Law of cosines for right triangle float cos_alpha2 = (L1 * L1 + r2_sq - L2 * L2) / (2.0 * L1 * r2); if (cos_alpha2 < -1.0 || cos_alpha2 > 1.0) return false; float alpha2 = acos(cos_alpha2); float beta2 = atan2(y2, x2); float theta2 = beta2 - alpha2; // Elbow-right theta1_deg = theta1 * 180.0 / M_PI; theta2_deg = theta2 * 180.0 / M_PI; return true; } void moveToXY(float x, float y, int delayMs = 20) { float th1, th2; if (computeIK(x, y, th1, th2)) { servoLeft.write(constrain((int)th1 + offsetLeft, 0, 180)); servoRight.write(constrain((int)th2 + offsetRight, 0, 180)); delay(delayMs); } } void setup() { Serial.begin(115200); ESP32PWM::allocateTimer(0); ESP32PWM::allocateTimer(1); servoLeft.setPeriodHertz(50); servoRight.setPeriodHertz(50); servoLeft.attach(SERVO_LEFT_PIN, 500, 2500); servoRight.attach(SERVO_RIGHT_PIN, 500, 2500); Serial.println("SCARA 5-bar robot initialized."); // Home position (center workspace) moveToXY(0, 130, 1000); } void loop() { // Demonstration: draw a horizontal line back and forth for (float x = -30; x <= 30; x += 2.0) { moveToXY(x, 130, 30); } delay(200); for (float x = 30; x >= -30; x -= 2.0) { moveToXY(x, 130, 30); } delay(500); // Draw a circle float cx = 0, cy = 130, radius = 25; for (float angle = 0; angle <= 360; angle += 5) { float rad = angle * M_PI / 180.0; float px = cx + radius * cos(rad); float py = cy + radius * sin(rad); moveToXY(px, py, 25); } delay(1000); }