Description: I wanted to build a 5-axis robotic arm to gain experience in robotics and trying to encourage developing independence when creating cool projects :)

Milestone3 Code

The following is the code for the entire arm. 

#define BLYNK_TEMPLATE_ID "TMPL2yiEx1ynl"

#define BLYNK_TEMPLATE_NAME "Robotic Arm"

#define BLYNK_AUTH_TOKEN "X_9BIgx9QAXB9089_V1zSDwg0WPkKHlz"

#include <WiFiS3.h>

#include <AccelStepper.h>

#include <Servo.h>

#include <math.h>

#include <BlynkSimpleWifi.h>

//char ssid[] = "22000_RollingHills_Eero";

//char pass[] = "northstar";

char ssid[] = "J11";

char pass[] = "Blue@J11";

const int stepPin = 3;

const int dirPin = 2;

const int shoulderPin = 4;

const int elbowPin = 5;

const int wristPin = 6;

// Button states coming from the Blynk app

int BASE_CW = 0;     // V0

int BASE_CCW = 0;    // V1

int HORIZ_OUT = 0;   // V2

int HORIZ_IN = 0;    // V3

int VERT_UP = 0;     // V4

int VERT_DOWN = 0;   // V5

int PITCH_UP = 0;    // V6

int PITCH_DOWN = 0;  // V7

unsigned long runCount = 0;

// Timing variables for smooth, non-blocking execution

unsigned long lastMoveTime = 0;

const unsigned long moveInterval = 20;  // Run movement math every 30 milliseconds

// Miscellaneous variables

double Smoothness = 0.2;

double Stepper_Smoothness = 2.0;

double Wrist_smoothness = 2.0;

double Shoulder_angle = 90.0;

double Elbow_angle = 0.0;

double Wrist_angle = 180.0;

double Wrist_target = 90.0;

double L1 = 13.432;  // cm

double L2 = 13.467;  // cm

double MaxReach = L1 + L2;

double MinReach = 1.5;

double rotation = 0.0;

double radius = 0.0;

double height = MaxReach - 1.0;  // Start slightly below absolute max to prevent instant lock

AccelStepper motor(1, stepPin, dirPin);

Servo shoulderServo;

Servo elbowServo;

Servo wristServo;

BLYNK_WRITE(V0) {

  BASE_CW = param.asInt();

}

BLYNK_WRITE(V1) {

  BASE_CCW = param.asInt();

}

BLYNK_WRITE(V2) {

  HORIZ_OUT = param.asInt();

}

BLYNK_WRITE(V3) {

  HORIZ_IN = param.asInt();

}

BLYNK_WRITE(V4) {

  VERT_UP = param.asInt();

}

BLYNK_WRITE(V5) {

  VERT_DOWN = param.asInt();

}

BLYNK_WRITE(V6) {

  PITCH_UP = param.asInt();

}

BLYNK_WRITE(V7) {

  PITCH_DOWN = param.asInt();

}

void calculations(double desired_x, double desired_y, double desired_z, double L1, double L2) {

  double HyptnsT = sqrt((desired_x * desired_x) + (desired_y * desired_y));

  double Phi = atan2(desired_z, HyptnsT) * (180 / PI);

  double HyptnsS = sqrt((HyptnsT * HyptnsT) + (desired_z * desired_z));

  double a = ((HyptnsS * HyptnsS) + (L1 * L1) - (L2 * L2)) / (2 * L1 * HyptnsS);

  a = constrain(a, -1.0, 1.0);

  double Theta = acos(a) * (180 / PI);

  Shoulder_angle = Phi + Theta;

  double a1 = ((L2 * L2) + (L1 * L1) - (HyptnsS * HyptnsS)) / (2 * L1 * L2);

  a1 = constrain(a1, -1.0, 1.0);

  Elbow_angle = acos(a1) * (180 / PI);

}

void setup() {

  Serial.begin(115200);

  delay(10);

  Serial.println("Connecting to Blynk...");

  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);

  Serial.println("Connected!");

  motor.setMaxSpeed(300);  // Increased speed slightly for smoother response

  motor.setAcceleration(300);

  motor.setCurrentPosition(0);

  shoulderServo.attach(shoulderPin);

  elbowServo.attach(elbowPin);

  wristServo.attach(wristPin);

  // Initialize position

  double desired_x = radius * cos(rotation * PI / 180.0);

  double desired_y = radius * sin(rotation * PI / 180.0);

  double desired_z = height;

  calculations(desired_x, desired_y, desired_z, L1, L2);

  Shoulder_angle = constrain(Shoulder_angle, 0, 180);

  Elbow_angle = constrain(Elbow_angle, 0, 180);

  shoulderServo.write(Shoulder_angle);

  elbowServo.write(Elbow_angle);

  Wrist_angle = constrain(Wrist_target - Elbow_angle - Shoulder_angle + 90, 0, 180);

  wristServo.write(Wrist_angle);

}

void loop() {

  Blynk.run();

  motor.run();  // Keeps stepper moving smoothly by running unthrottled every loop iteration

  // Only evaluate controls and servo angles at a defined millisecond interval

  if (millis() - lastMoveTime >= moveInterval) {

    lastMoveTime = millis();

    // Cache the previous positions in case the new inputs break geometry rules

    double next_rotation = rotation;

    double next_radius = radius;

    double next_height = height;

    double next_wrist_target = Wrist_target;

    if (BASE_CW == 1) { next_rotation += Stepper_Smoothness; }

    if (BASE_CCW == 1) { next_rotation -= Stepper_Smoothness; }

    if (HORIZ_OUT == 1) { next_radius += Smoothness; }

    if (HORIZ_IN == 1) { next_radius -= Smoothness; }

    if (VERT_UP == 1) { next_height += Smoothness; }

    if (VERT_DOWN == 1) { next_height -= Smoothness; }

    if (PITCH_UP == 1) { next_wrist_target += Wrist_smoothness; }

    if (PITCH_DOWN == 1) { next_wrist_target -= Wrist_smoothness; }

    // Test reach geometry before applying updates

    double currentReach = sqrt(next_radius * next_radius + next_height * next_height);

    if (currentReach >= MinReach && currentReach <= MaxReach) {

      // Commit changes if boundaries pass

      rotation = next_rotation;

      radius = next_radius;

      height = next_height;

      Wrist_target = next_wrist_target;

      double desired_x = radius * cos(rotation * PI / 180.0);

      double desired_y = radius * sin(rotation * PI / 180.0);

      double desired_z = height;

      calculations(desired_x, desired_y, desired_z, L1, L2);

      Shoulder_angle = constrain(Shoulder_angle, 0, 180);

      Elbow_angle = constrain(Elbow_angle, 0, 180);

      motor.moveTo(rotation / 1.8);

      shoulderServo.write(Shoulder_angle);

      elbowServo.write(Elbow_angle);

      Wrist_angle = Wrist_target - Elbow_angle - Shoulder_angle + 90;

      Wrist_angle = 180 - constrain(Wrist_angle, 0, 180);

      wristServo.write(Wrist_angle);

    }

    // If out of bounds, variables hold their previous safe states completely intact

  }

  runCount++;

  if (runCount % 5000 == 0) {

    Serial.print("rot: ");

    Serial.print(rotation);

    Serial.print("  rad: ");

    Serial.print(radius);

    Serial.print("  h: ");

    Serial.println(height);

  }

}