/* 5-Bar Linkage Planar SCARA Robot Controller - Stepper Version Fab Lab INP-HB - Fab Academy 2026 - Week 12 Dual NEMA 17 Stepper Actuation with A4988 / CNC Shield Drivers */ #include #include // Pin definitions for CNC Shield / custom driver board #define STEP_PIN_1 2 #define DIR_PIN_1 5 #define STEP_PIN_2 3 #define DIR_PIN_2 6 #define ENABLE_PIN 8 AccelStepper stepper1(AccelStepper::DRIVER, STEP_PIN_1, DIR_PIN_1); AccelStepper stepper2(AccelStepper::DRIVER, STEP_PIN_2, DIR_PIN_2); // Stepper resolution parameters const float STEPS_PER_REV = 3200.0; // 200 steps * 16 microsteps const float STEPS_PER_DEG = STEPS_PER_REV / 360.0; // Geometry (in mm) const float D = 100.0; const float L1 = 120.0; const float L2 = 140.0; bool computeIK(float x, float y, float &theta1_deg, float &theta2_deg) { float x1 = x + D / 2.0; float y1 = y; float r1_sq = x1 * x1 + y1 * y1; float r1 = sqrt(r1_sq); float x2 = x - D / 2.0; float y2 = y; float r2_sq = x2 * x2 + y2 * y2; float r2 = sqrt(r2_sq); if (r1 > (L1 + L2) || r1 < fabs(L1 - L2) || r2 > (L1 + L2) || r2 < fabs(L1 - L2)) { return false; } float cos_alpha1 = (L1 * L1 + r1_sq - L2 * L2) / (2.0 * L1 * r1); float cos_alpha2 = (L1 * L1 + r2_sq - L2 * L2) / (2.0 * L1 * r2); if (cos_alpha1 < -1.0 || cos_alpha1 > 1.0 || cos_alpha2 < -1.0 || cos_alpha2 > 1.0) return false; float theta1 = atan2(y1, x1) + acos(cos_alpha1); float theta2 = atan2(y2, x2) - acos(cos_alpha2); theta1_deg = theta1 * 180.0 / M_PI; theta2_deg = theta2 * 180.0 / M_PI; return true; } void setup() { Serial.begin(115200); pinMode(ENABLE_PIN, OUTPUT); digitalWrite(ENABLE_PIN, LOW); // Enable drivers stepper1.setMaxSpeed(2000.0); stepper1.setAcceleration(800.0); stepper2.setMaxSpeed(2000.0); stepper2.setAcceleration(800.0); Serial.println("Stepper 5-bar SCARA Robot Ready"); } void loop() { // Coordinated moves can be triggered via Serial or predefined path stepper1.run(); stepper2.run(); }