Stepper.move(x) does not move correctly

[baschi777]

Hello everyone,

I am building a 7DoF robot with servos and steppers. I wrote an Arduino sketch to move the motors and a GUI on Processing to control them with sliders.

I can not make the steppers move as I want: sometimes they move, sometimes they dont, sometime they move more than they should or less than they should and I cant understand why.

From the processing GUI, arduino receives the input of the degrees to rotate from the current position (im sure this works as you can see in the first photo).
Then I calculate the corresponding steps (im using 1/16 microstepping) and I call the function "stepper.move(steps)" and, if distance to go is different not 0, i call "stepper.run()" in a while loop.

I don't understand why it does not move the exact number of steps I give as input to "stepper.move(steps)".
I also tried with "stepper.moveTo(steps)" but I encounter the same problem.

pls help as soon as possible
thank you in advance.

Here is the photo and the codes:

Arduino code:

#include <AccelStepper.h>
#include <Servo.h>

// Stepper NEMA17 controlled through CNC Shield V3 with DRV8825 drivers (1/16 microstepping)
AccelStepper stepper1(1, 2, 5);  // X module on shield
AccelStepper stepper2(1, 3, 6);  // Y module on shield
AccelStepper stepper3(1, 4, 7);  // Z module on shield
AccelStepper stepper4(1, 12, 13);  // A module on shield = Robot's Z axis

Servo needleServo;  // Servo360 to move Needle
Servo ServoX;       // Servo360 to move x axis
Servo ServoY;       // Servo360 to move y axis

int servospeedforward = 80;
int servospeedbackward = 100;
float servorevpersecond = 0.33;


void setup() {
  Serial.begin(115200);

  // enable pin to be be able to move steppers
  pinMode(8, OUTPUT);
  digitalWrite(8, LOW);
  
  // Config Stepper speed and acceleration
  stepper1.setMaxSpeed(200);
  stepper1.setAcceleration(100);
  stepper2.setMaxSpeed(200);
  stepper2.setAcceleration(100);
  stepper3.setMaxSpeed(200);
  stepper3.setAcceleration(100);
  stepper4.setMaxSpeed(4000);
  stepper4.setAcceleration(2000);
  
  needleServo.attach(A1);  // Attach Servo to pin A1 (Needle control)
  ServoX.attach(A2);  // Attach Servo to pin A2 (X axis control)
  ServoY.attach(A0);  // Attach Servo to pin A0 (Y axis control)
}

void loop() {

  // extract received data
  if (Serial.available() > 0) {
    String data = Serial.readStringUntil('\n');
    int j1 = data.substring(0, data.indexOf(',')).toInt();
    data = data.substring(data.indexOf(',') + 1);
    int j2 = data.substring(0, data.indexOf(',')).toInt();
    data = data.substring(data.indexOf(',') + 1);
    int j3 = data.substring(0, data.indexOf(',')).toInt();
    data = data.substring(data.indexOf(',') + 1);
    int z = data.substring(0, data.indexOf(',')).toInt();
    data = data.substring(data.indexOf(',') + 1);
    int needle = data.substring(0, data.indexOf(',') + 1).toInt();
    data = data.substring(data.indexOf(',') + 1);
    int servoXVal = data.substring(0, data.indexOf(',')).toInt();
    data = data.substring(data.indexOf(',') + 1);
    int servoYVal = data.toInt();
    Serial.println("input received");
 

    // move joint 1
    Serial.println("j1 = ");
    Serial.println(j1);
    int j1steps = round(j1 / 0.1125);   // 1 step = 1.8°/16 = 0.1125°
    if (j1 > 1) {
      stepper1.move(-j1steps);
      while (stepper1.distanceToGo() != 0) {
        stepper1.run();
      }
    }
    

    // move joint 2
    Serial.println("j2 = ");
    Serial.println(j2);
    int j2steps = round(j2 / 0.1125);   // 1 step = 1.8°/16 = 0.1125°
    if (j2 > 1) {
      stepper2.move(-j2steps);
      while (stepper2.distanceToGo() != 0) {
        stepper2.run();
      }
    }

    // move joint 3
    Serial.println("j3 = ");
    Serial.println(j3);
    int j3steps = round(j3 / 0.1125);   // 1 step = 1.8°/16 = 0.1125°
    if (j3 > 1) {
      stepper3.move(-j3steps);
      while (stepper3.distanceToGo() != 0) {
        stepper3.run();
      }
    }

    // move Z axis
    Serial.println("z = ");
    Serial.println(z);
    float zsteps = z * ((200 * 16) / 2);    // 200*16 steps = full rev = 2mm
    if (z > 1) {
      stepper4.move(zsteps);
      while (stepper4.distanceToGo() != 0) {
        stepper4.run();
      }
    }
    
   
   
  // Servo [mm] to [ms] (of running) conversion
  float needle_ms = ((abs(needle) / 4) / servorevpersecond) * 1000;         // 4 = screw lead = 4mm per revolution
  float servoXVal_ms = ((abs(servoXVal) / 4) / servorevpersecond) * 1000; 
  float servoYVal_ms = ((abs(servoYVal) / 4) / servorevpersecond) * 1000; 


  // Move servo motors
    if (needle > 0) {
      needleServo.write(servospeedforward);    // run forward
      Serial.println("servo needle running forward");
      delay(needle_ms);
      Serial.println("servo needle stopped");
      needleServo.write(90);                    // stop
    }  
    if (needle < 0) {
      needleServo.write(servospeedbackward);    // run backward
      Serial.println("servo needle running backwards");
      delay(needle_ms);
      Serial.println("servo needle stopped");
      needleServo.write(90);                    // stop
    }

    if (servoXVal > 0) {
      ServoX.write(servospeedforward);    // run forward
      Serial.println("servo X running forward");
      delay(servoXVal_ms);
      Serial.println("servo X stopped");
      ServoX.write(90);                    // stop
    }  
    if (servoXVal < 0) {
      ServoX.write(servospeedbackward);    // run backward
      Serial.println("servo X running backward");
      delay(servoXVal_ms);
      Serial.println("servo X stopped");
      ServoX.write(90);                    // stop
    }

    if (servoYVal > 0) {
      ServoY.write(servospeedforward);    // run forward
      Serial.println("servo Y running forward");
      delay(servoYVal_ms);
      Serial.println("servo Y stopped");
      ServoY.write(90);                    // stop
    }  
    if (servoYVal < 0) {
      ServoY.write(servospeedbackward);    // run backward
      Serial.println("servo Y running backward");
      delay(servoYVal_ms);
      Serial.println("servo Y stopped");
      ServoY.write(90);                    // stop
    }

  }
}

If needed here is the Processing code:

import processing.serial.*;
import controlP5.*;

Serial myPort;
ControlP5 cp5;

int j1Slider = 0;
int j2Slider = 0;
int j3Slider = 0;
int zSlider = 0;
int needleValue = 0;
int xSlider = 0; 
int ySlider = 0; 


int slider1Previous = 0;
int slider2Previous = 0;
int slider3Previous = 0;
int sliderzPrevious = 0;
int needleValuePrevious = 0;
int sliderxPrevious = 0;
int slideryPrevious = 0;

float q1_offset = 100;  //[mm]
float q1, q4;           //[mm]      
float q0, q2, q3;       //[deg]
float a1 = 52.5;        //[mm]
float a2 = 80;
float a3 = 104;
float j1, j2, j3, x, y, z, n;
float rhoP;
float xP = 0;
float yP = 0;
float zP = 0;

String data;

void setup() {
  size(960, 800);
  myPort = new Serial(this, "COM4", 115200); 
  
  cp5 = new ControlP5(this);
  PFont pfont = createFont("Arial", 25, true);
  ControlFont font = new ControlFont(pfont, 22);

  // Joint1 control
  cp5.addSlider("j1Slider")
    .setPosition(110, 190)
    .setSize(270, 30)
    .setRange(-75, 75)       // [deg]
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("J1");

  // Joint2 control
  cp5.addSlider("j2Slider")
    .setPosition(110, 315)
    .setSize(270, 30)  
    .setRange(0, 60)      // [deg]
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("J2");

  // Joint3 control
  cp5.addSlider("j3Slider")
    .setPosition(110, 440)
    .setSize(270, 30)
    .setRange(-45, 45)      // [deg]
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("J3");

  // Z axis control
  cp5.addSlider("zSlider")
    .setPosition(110, 565)
    .setSize(270, 30)
    .setRange(0, 170)         // [mm]
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("Z");

  // Needle control
  cp5.addSlider("needleValue")
    .setPosition(110, 690)
    .setSize(270, 30)
    .setRange(0, 40)         // [mm]
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("Needle");
    
  // X axis control
  cp5.addSlider("xSlider")
    .setPosition(500, 565)
    .setSize(270, 30)
    .setRange(-50, 50)       // [mm]
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("X");
    
  // Y axis control
  cp5.addSlider("ySlider")
    .setPosition(500, 690)
    .setSize(270, 30)
    .setRange(-50, 50)       // [mm]
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("Y");
}

void draw() { 
  background(#F2F2F2);
  textSize(26);
  fill(33);
  text("Venipuncture Robot Control", 260, 60); 
  textSize(22);
  text("Joint 1", 35, 250);
  text("Joint 2", 35, 375);
  text("Joint 3", 35, 500);
  text("Axis Z", 35, 625);
  text("Needle", 35, 750);
  text("Axis X", 35, 875);
  text("Axis Y", 35, 1000);

  fill(33);
  textSize(32);
  text("X: " + nf(xP, 0, 2), 500, 290);
  text("Y: " + nf(yP, 0, 2), 650, 290);
  text("Z: " + nf(zP, 0, 2), 800, 290);

  updateData();
  
  if (slider1Previous != j1Slider || slider2Previous != j2Slider || slider3Previous != j3Slider || sliderzPrevious != zSlider || needleValuePrevious != needleValue || sliderxPrevious != xSlider || slideryPrevious != ySlider) {
    j1 = round(cp5.getController("j1Slider").getValue());
    j2 = round(cp5.getController("j2Slider").getValue());
    j3 = round(cp5.getController("j3Slider").getValue());
    z = round(cp5.getController("zSlider").getValue());
    n = round(cp5.getController("needleValue").getValue());
    x = round(cp5.getController("xSlider").getValue());
    y = round(cp5.getController("ySlider").getValue());
        
    forwardKinematics();
    myPort.write(data);            // Send data to Arduino
  }

  slider1Previous = j1Slider;
  slider2Previous = j2Slider;
  slider3Previous = j3Slider;
  sliderzPrevious = zSlider;
  needleValuePrevious = needleValue;
  sliderxPrevious = xSlider;
  slideryPrevious = ySlider;
}

void forwardKinematics() {
  q0 = j1;
  if (q0 < 0) {
    q0 = q0 + 360;
  }
  q1 = z;
  q2 = j2 - 45;
  if (q2 < 0) {
    q2 = q2 + 360;
  }
  q3 = 180 - j3 + j2;
  q4 = needleValue;
  rhoP = a2*cos(q2) + (a3+q4)*cos(q3);
  zP = q1_offset + q1 - a1 + a2*sin(q2) + (a3+q4)*sin(q3);
  xP = rhoP*cos(q0) + x;
  yP = rhoP*sin(q0) + y;
  
}

void updateData() {
  data = String.format("%d,%d,%d,%d,%d,%d,%d\n", (j1Slider-slider1Previous), (j2Slider-slider2Previous), (j3Slider-slider3Previous), (zSlider-sliderzPrevious), (needleValue-needleValuePrevious), (xSlider-sliderxPrevious), (ySlider-slideryPrevious));
}

void serialEvent (Serial myPort) {
  
  String inString = myPort.readStringUntil('\n');
  if (inString != null) {
    print(inString);
  }
}

[xfpd]

Usual problems are wiring and insufficient power. Show your wiring diagram and power source.

[baschi777]

Im Sorry but I dont think that is the problem since for certain step values (most of them actually) the motors move 100% of the time.
I believe I have a problem with the step value I give as input to stepper.move(stepvalue)

Anyway here are some photos of the wiring. I am using a 12V 10A Power supply and I am quite sure It Is enough.

[xfpd]

@baschi777

dont think that is the problem

Ok. Thanks for the missing pieces.

[baschi777]

I found where the problem is but I cant understand why this is happening.
Its a pure programming problem.

As you can see the problem is the calculation of zsteps. Sometimes the signs of z and zsteps are not the same.

I thought It was an overflow problem but the float type has much more digits available to handle the small values of z I give.
Basically that formula is doing: (z * 1600).

What is happening??

[van_der_decken]

int z = data.substring(0, data.indexOf(',')).toInt();

float zsteps = z * ((200 * 16) / 2);    // 200*16 steps = full rev = 2mm

Since z is in an int, the calculation above is done with integer math, and it's only when the result is assigned to zsteps that it is converted to a float.

On an Uno, an int is 16 bits, with a range of -32768 to 32767

Ignoring overflow, the value of z * ((200 * 16) / 2) when z is 31 is 49600. This is outside of the range of a signed 16 bit integer and gets interpreted as -15936 (49600 - 65536).

Force the calculation to use float math to avoid this.

float zsteps = z * ((200.0 * 16) / 2);    // 200*16 steps = full rev = 2mm

[baschi777]

Thank you so much!
This was the solution!

I knew It was something to do with integer max values but I didnt know I had to force float math before assigning float type.