Categories
Robotics Stepper Motors

A drawing robot

Right smack in the middle, my first blog. Not about what I’ve done in the past, not about what I want to do in the near future. None of that. Just a first blog on what’s been on my mind over the last few weeks: how do stepper motors work and how can I effectively drive them.

So here is what started this all:

Robot Arm
Robotic Arm

This is my Robotic Arm, 3D-printed on my very own 3D printer, courtesy of Zortrax. It is really nice looking, and although axes 4 and 5 are not actuated, great for experiments and getting acquainted with robotics. Should you want to print one yourself, you can find the design here. The design was already placed on Zortrax website in 2015, but has held up great over the last five years. (I may write a blog on building this robotic arm later. Lot’s to learn there too).

Now, the people of Zortrax recommend the use of NEMA 17 Stepper Motors, a RAMPS board with three A4988 stepper drivers, an Arduino Mega with Marlin Firmware, Pronterface as control system, and a 12V minimum 100W power supply. As it so happens, I have those parts waiting for me in my now obsolete Prusa Mendel 3D printer that I build eight years ago. If you don’t have an old Prusa Mendel waiting for you, you could of course check out Amazon or equivalent for a reasonably prized kit containing all required items apart from the stepper motors. If you were planning on shopping on Amazon you could pick up three of these for starters. As a power supply, this one would be good to start with, although we will be discussing more on power supplies later on in the blog.

RAMPS
Arduino Mega with RAMPS and stepper drivers attached.

The sample code that Zortrax provides is a short G-code that allows the robot arms to move (dance actually), but in my case, it works only when I hold the arms in a vertical position during the start. If I do not, the arms do not lift or will suddenly drop. It is not yet as in the movies.

Dancing Zortrax Robotic Arms by Trinity3D

Although I only see the axis 2 robotic arm in near vertical position in the video clip as well, I can not hold the position of my robotic arms when horizontal (or 45 degrees for that matter). Somehow, my robot lacks sufficient power to lift it’s arms from horizontal to vertical.

My robot arm cannot lift it’s own weight.

So, time to start tinkering. Do I need a larger power supply, bigger stepper motors, stronger stepper drivers, or another control system?

And thus, after one month, several blown up stepper motor driver boards, a blown up power supply and some locked up stepper motors, I think I might have found a solution:

Very strong drivers

And combined with a large 24V power supply, it looks like this:

Large stepper motor drivers with 24V power Supply
Large stepper motor drivers with 24V Power Supply

The drive system consists of three TB6600 4A 9-42V stepper motor driver controllers set to 8 microsteps and 3.5A current per drive controller. My stepper motors are rated for 1.5A current per winding, and with two windings in one stepper motor should not receive more than 3.0A in total. However, I found that 3.0A current is not sufficient to lift the robot arm from a horizontal position to a vertical position. At 3.5A current (and at 24V voltage) I can, and although the stepper motors do get very hot, it is working quite well for me. If you want to replicate the same, please do so at your own risk and be careful, 3.5A at 24V is already quite dangerous and you might be overloading the stepper motors. I found both driver controllers and switching power supply on Amazon:

As you can see from the picture above, I made a small frame to secure the three driver controllers to the power supply. I used 10×10 mm MakerBeams for this. I find them very easy to use, if you like to try them out, just check out their website at https://www.makerbeam.com/. To prove that my hard work has paid out, below a short video impression of my robot arm introducing itself to you.

Introducing my robot arm

My complete setup (on my way to messy work bench) now looks like below. You will note that I have replaced the gripper with a permanent marker. This to achieve my final goal: A robot that can draw.

Ready to Draw!

Drawing Robot

The good people of Zortrax have indeed designed and shared a fine robot arm system, using as much as possible of the shelf 3D-printing hardware and software. With a 12V power supply, the RAMPS board, the Marlin Firmware on the Arduino Mega combined with the Pronterface software on your computer it is indeed possible to activate the robot arm using G-codes. However, I found that this set-up has some limitation for me. For one, it is not strong enough to lift the arm from a horizontal position to a vertical position. And next to that, I find it very complicated to move the arm to a specific position using G-codes that actuate the stepper motors with an interface designed for 3-D printers and not so much for robotic arms.

I now solved the power issue, switching to 24V and large 3.5A drivers, and the arm can lift itself, but how do I move the gripper from one position to the next without having to fiddle around with G-codes driving and controlling each stepper motor individually. Wouldn’t it be nice if I could just tell the arm to move the gripper from one (X, Y, Z) position to the next? It’s time to freshen up my math skills. I figure I need two steps to transform a giving (X, Y, Z) position of the gripper to the numbers of steps for all three stepper motors.

To make live a bit easier, I will focus on the position of the wrist joint instead of to the non-controllable gripper. First thing I need to do is to transform the cartesian coordinate (X, Y, Z) of the wrist to spherical coordinates. The math is as follows:

From this formula, we now know the horizontal angle of the shoulder joint (θ), and also know the length from the centre axis of the shoulder joint to the centre axis of the wrist (r). What’s left is to calculate the vertical angle of the shoulder joint and the angle of the elbow joint. We can do this by making a triangle calculation given r and the fixed lengths of the upper arm and the lower arm. From https://www.calculator.net/triangle-calculator.html we can learn that the angles (A, B, and C) can be calculated using below formulas given the known lengths of sides a, b, and c.

For the vertical angle of the shoulder joint we need to combine both ø from the cartesian to spherical transformation and the angle calculated from the triangle calculation to obtain the angle with the horizontal plane. The function in C that can do this (with some minor error checking included) looks like below:

// global variables
double AA1_new;   // horizontal angle of R
double AA2_new;   // vertical angle of R
double AA3_new;   // upper arm angle relative to R
double AA4_new;   // lower arm angle relative to upper arm
double LL1 = 160; // upper arm length
double LL2 = 340; // lower arm length
/ min max angles
double AAO3max = 3.15000;  // angle lower arm
double AAO4max = 3.15000;  // angle upper arm
ddouble RR2_min = 115.0; // minimum possible radius
double RR2_max = LL1 + LL2; // maximum possible radius

boolean carth_to_spher(double X, double Y, double Z) {
   RR2_new = sqrt(X * X + Y * Y + Z * Z);
   AA1_new = atan(Y / X);
   AA2_new = asin(Z / R);
   // check if carth coordinate fits in spherical range of robot arm
   if (RR2_new > RR2_max) {
     Serial.println("Max R exceeded");
     return(false);
   } else if (RR2_new < RR2_min) {
     Serial.println("Min R exceeded");
     return(false);
   } else {
     AA3_new = PI - (AA2_new + acos((LL1 * LL1 + RR2_new * RR2_new - LL2 * LL2) / (2 * LL1 * RR2_new)));
     AA4_new = acos((LL2 * LL2 + LL1 * LL1 - RR2_new * RR2_new) / (2 * LL2 * LL1));
     if (AA3_new >= AAO3min && AA3_new <= AAO3max && AA4_new >= AAO4min && AA4_new <= AAO4max) {
       return(true);
     }
     else {
      Serial.println("Angles exceeded");
      return(false);
     }
   }
}

To calculate from angles to steps and then activate the stepper motors, you can use below function:

void move_to_carth(double X, double Y, double Z) {
   if (carth_to_spher(X, Y, Z)) {
     new_position_d0 = steps1 / 3.14 * (AA1_new - AAO1);  // absolute steps to new position shoulder
     new_position_d1 = steps2 / 3.14 * (AA3_new - AAO3);  // absolute steps to new position upper arm
     new_position_d2 = steps3 / 3.14 * (AA4_new - AAO4);  // absolute steps to new position lower arm
     new_position_l0 = (long) new_position_d0;
     new_position_l1 = (long) new_position_d1;
     new_position_l2 = (long) new_position_d2;
     new_position[0] = new_position_l0;
     new_position[1] = new_position_l1;
     new_position[2] = new_position_l2;
   // activate steppers to go to new_position[n] - current_position[n]
     steppers.moveTo(new_position);
     steppers.runSpeedToPosition(); // Blocks until all are in position  
   } else {
     Serial.println("Could not calculate position");
   }
}

My aim was to develop a robot arm that would be able to draw a picture of some sorts on a flat surface. You can find my code (combining above functions) to draw a circle within a rectangle in a vertical plane below:

// Include the AccelStepper library:
#include <AccelStepper.h>
#include <MultiStepper.h>

// Define stepper motor connections and motor interface type. Motor interface type must be set to 1 when using a driver:
#define dirPinLowerArm 2
#define stepPinLowerArm 3
#define dirPinUpperArm 4
#define stepPinUpperArm 5
#define dirPinBase 8
#define stepPinBase 9
#define motorInterfaceType 1

// coordinates of wrist relative to shoulder
double XX2;
double YY2;
double ZZ2;
double RR2;
double XX2_new;
double YY2_new;
double ZZ2_new;
double RR2_new;
double AA1_new;   // horizontal angle of R
double AA2_new;   // vertical angle of R
double AA3_new;   // upper arm angle relative to R
double AA4_new;   // lower arm angle relative to upper arm

double LL1 = 160; // upper arm length
double LL2 = 340; // lower arm length

// start angles at stepper 0 positions
double AAO1 = 0.0000;  // angle shoulder at start
double AAO3 = 0.0392;  // angle lower arm at start
double AAO4 = 0.6211;  // angle upper arm at start

// min max angles
double AAO3min = 0.0387;  // angle lower arm
double AAO4min = 0.5233;  // angle upper arm
double AAO3max = 3.15000;  // angle lower arm
double AAO4max = 3.15000;  // angle upper arm
double RR2_min = 115.0; // minimum possible radius
double RR2_max = LL1 + LL2; // maximum possible radius

int steps1 = 7000; // number of steps for 180 degrees rotation shoulder
int steps2 = 4600; // number of steps for 180 degrees rotation upper arm
int steps3 = 3600; // number of steps for 180 degrees rotation lower arm

double new_position_d0;
double new_position_d1;
double new_position_d2;
long new_position_l0;
long new_position_l1;
long new_position_l2;
long new_position[3];
double new_position_d[3];

// Create a new instance of the AccelStepper class:
AccelStepper stepperUpperArm = AccelStepper(motorInterfaceType, stepPinUpperArm, dirPinUpperArm);
AccelStepper stepperBase = AccelStepper(motorInterfaceType, stepPinBase, dirPinBase);
AccelStepper stepperLowerArm = AccelStepper(motorInterfaceType, stepPinLowerArm, dirPinLowerArm);
MultiStepper steppers;

void setup() {
   // put your setup code here, to run once:
   Serial.begin(9600);

  // Set the maximum speed and acceleration:
  stepperLowerArm.setMaxSpeed(400);
  stepperLowerArm.setAcceleration(25);
  stepperLowerArm.setPinsInverted(true);
  stepperUpperArm.setMaxSpeed(400);
  stepperUpperArm.setAcceleration(25);
  stepperBase.setMaxSpeed(400);
  stepperBase.setAcceleration(25);

  // add steppers to multistepper
  steppers.addStepper(stepperBase);
  steppers.addStepper(stepperUpperArm);
  steppers.addStepper(stepperLowerArm);
 
   // reset position of stepper motors
   new_position[0] = 0;
   new_position[1] = 0;
   new_position[2] = 0;
 
   // move to starting position
   XX2_new = 350;
   YY2_new = cos(0) * 50;
   ZZ2_new = 125 + sin(0) * 50;
   move_to_carth(XX2_new, YY2_new, ZZ2_new);
   delay(2000);

    // draw a circle with R = 50, in X-Z plane maintaining Y
   for (int i=0;i<721;i++) {
     XX2_new = 350;
     YY2_new = cos(PI * i / 360) * 50;
     ZZ2_new = 125 + sin(PI * i / 360) * 50;
     move_to_carth(XX2_new, YY2_new, ZZ2_new);
   }
   XX2_new = 350;
   YY2_new = 50;
   ZZ2_new = 75;
   move_to_carth(XX2_new, YY2_new, ZZ2_new);
   XX2_new = 350;
   YY2_new = -50;
   ZZ2_new = 75;
   move_to_carth(XX2_new, YY2_new, ZZ2_new);
   XX2_new = 350;
   YY2_new = -50;
   ZZ2_new = 175;
   move_to_carth(XX2_new, YY2_new, ZZ2_new);
   XX2_new = 350;
   YY2_new = 50;
   ZZ2_new = 175;
   move_to_carth(XX2_new, YY2_new, ZZ2_new);
   XX2_new = 350;
   YY2_new = 50;
   ZZ2_new = 75;
   move_to_carth(XX2_new, YY2_new, ZZ2_new);

   delay(5000);

   // move to origin
   new_position[0] = 0;
   new_position[1] = 0;
   new_position[2] = 0;
   steppers.moveTo(new_position);
   steppers.runSpeedToPosition(); // Blocks until all are in position

}

void loop() {
   // put your main code here, to run repeatedly:
   // no code in loop for now
}

void move_to_carth(double X, double Y, double Z) {
   if (carth_to_spher(X, Y, Z)) {
     new_position_d0 = steps1 / 3.14 * (AA1_new - AAO1);  // absolute steps to new position shoulder
     new_position_d1 = steps2 / 3.14 * (AA3_new - AAO3);  // absolute steps to new position upper arm
     new_position_d2 = steps3 / 3.14 * (AA4_new - AAO4);  // absolute steps to new position lower arm
     new_position_l0 = (long) new_position_d0;
     new_position_l1 = (long) new_position_d1;
     new_position_l2 = (long) new_position_d2;
     new_position[0] = new_position_l0;
     new_position[1] = new_position_l1;
     new_position[2] = new_position_l2;
   // activate steppers to go to new_position[n] - current_position[n]
     steppers.moveTo(new_position);
     steppers.runSpeedToPosition(); // Blocks until all are in position  
   } else {
     Serial.println("Could not calculate position");
   }
}

boolean carth_to_spher(double X, double Y, double Z) {
   RR2_new = sqrt(X * X + Y * Y + Z * Z);
   AA1_new = atan(Y / X);
   AA2_new = asin(Z / RR2_new);
   // check if carth coordinate fits in spherical range of robot arm
   if (RR2_new > RR2_max) {
     Serial.println("Max R exceeded");
     return(false);
   } else if (RR2_new < RR2_min) {
     Serial.println("Min R exceeded");
     return(false);
   } else {
     AA3_new = PI - (AA2_new + acos((LL1 * LL1 + RR2_new * RR2_new - LL2 * LL2) / (2 * LL1 * RR2_new)));
     AA4_new = acos((LL2 * LL2 + LL1 * LL1 - RR2_new * RR2_new) / (2 * LL2 * LL1));
     if (AA3_new >= AAO3min && AA3_new <= AAO3max && AA4_new >= AAO4min && AA4_new <= AAO4max) {
       return(true);
     }
     else {
      Serial.println("Angles exceeded");
      return(false);
     }
   }
}

And the result?

Robot’s first drawing…

Not bad for a first try, right? …well, could be improved a bit perhaps… Thanks for reading anyway, and please do let me know what you think of my experiment or have any questions on this subject.

Leave a Reply

Your email address will not be published. Required fields are marked *