Stepper motor oscillating unexpectedly when using Accelstepper

Projects Programming
1

Hello!
I am programming my robot to play me in chess.
I have everything working, except, whenever my robotic arm is moving to a piece, my j2Stepper motor starts oscillating for reasons unknown resulting in being at the wrong position due to losing step count. I have located the problem to be within the code which calculates the speed to move the motor. The motor never exceeds maxSpeed and works correctly whenever I have a preprogrammed constant speed. When calculating the speed with timeToPosition, the problem occurs. Each motor moves at a constant speed to the calulated position in steps.

Here is my code:

#include <AccelStepper.h>
#include <ESP32Servo.h>

float theta1;
float theta2;

float x;
float y;
float z;

float delta;

float theta3;
float psi = 180; //Desired gripper orientation. Offsetting for servo tolerance

const int wristPin = 2;
const int gripperPin = 1; //Was 38
const int gripPitchPin = 8; //Was non existent
const int numRows = 8;
const int numCols = 8;
const int depth = 3;

const float timeToTarget = 4;
unsigned long prevMillis = 0;

int serialNum = 0;

int j1Speed = 2000;
int j2Speed = 2000;
int baseSpeed = 2000;

float j1MaxDis = 0;
float j2MaxDis = 0;
float baseMaxDis = 0;

int reqRow = 0;
int reqCol = 0;

int gripperNum = 1;
int gripperCount = 0;

bool piece;
bool down;
bool up;
bool grab = true;

//Create 3D array to hold the chess board's positions. (8 rows, 8 columns, 3 values in each sub-array(x, y, z))
int chessBoard[numRows][numCols][depth] = 
{
  //The coordinates of each piece on a chess board (x, y, z)
  {{246, -300, 166}, {246, -300, 118}, {246, -300, 70}, {246, -300, 22}, {246, -300, -26}, {246, -300, -74}, {246, -300, -122}, {246, -300, -170}},/*8*/
  {{294, -300, 166}, {294, -300, 118}, {294, -300, 70}, {294, -300, 22}, {294, -300, -26}, {294, -300, -74}, {294, -300, -122}, {294, -300, -170}},/*7*/
  {{345, -300, 166}, {345, -300, 118}, {345, -300, 70}, {345, -300, 22}, {345, -300, -26}, {345, -300, -74}, {345, -300, -122}, {345, -300, -170}},/*6*/
  {{396, -300, 166}, {396, -300, 118}, {396, -300, 70}, {396, -300, 22}, {396, -300, -26}, {396, -300, -74}, {396, -300, -122}, {396, -300, -170}},/*5*/
  {{445, -300, 166}, {445, -300, 118}, {445, -300, 70}, {445, -300, 22}, {445, -300, -26}, {445, -300, -74}, {445, -300, -122}, {445, -300, -170}},/*4*/
  {{496, -300, 166}, {496, -300, 118}, {496, -300, 70}, {496, -300, 22}, {496, -300, -26}, {496, -312, -74}, {496, -300, -122}, {496, -300, -170}},/*3*/
  {{547, -300, 166}, {547, -300, 118}, {547, -300, 70}, {547, -300, 22}, {547, -300, -26}, {547, -314, -74}, {547, -300, -122}, {547, -300, -170}},/*2*/
  {{598, -312, 166}, {598, -300, 118}, {598, -300, 70}, {598, -300, 22}, {598, -300, -26}, {598, -316, -74}, {598, -300, -122}, {598, -300, -170}} /*1*/
  // a  b  c  d  e  f  g  h
  //Called as [row, col, position in array]
};

Servo wrist;  //Create servo object to control a servo
Servo gripper;  //Create servo object to control a servo
AccelStepper baseStepper(1, 5, 4);   // (Type:driver(1 is default driver), STEP, DIR)
AccelStepper j1Stepper_L(1, 7, 6);   // (Type:driver(1 is default driver), STEP, DIR)
AccelStepper j1Stepper_R(1, 17, 15);   // (Type:driver(1 is default driver), STEP, DIR)
AccelStepper j2Stepper(1, 10, 9);   // (Type:driver(1 is default driver), STEP, DIR)
 
void setup() {
  //Init steppers (Half steps)
  baseStepper.setMaxSpeed(baseSpeed); //800 pulse/rev
  j1Stepper_L.setMaxSpeed(j1Speed); //400 pulse/rev
  j1Stepper_R.setMaxSpeed(j1Speed); //400 pulse/rev
  j2Stepper.setMaxSpeed(j2Speed); //400 pulse/rev

  wrist.attach(wristPin);
  gripper.attach(gripperPin);

  //Initialize Serial Monitors
  Serial.begin(115200);
  Serial1.begin(115200, SERIAL_8N1, 17, 18); //(TX, RX)
  
  moveGripper(0);
  //Start position
  goHome();
}

void loop() 
{
  unsigned long currentMillis = millis();
  
  if (Serial.available() > 0) //Serial1 becuase it's the second set of serial pins/Serial input code. Tells us which piece to move
  {
    //Read the data
    //serialNum = Serial1.read(); //Coords from chess engine
    serialNum = Serial.parseInt(); //Used for testing
    if (serialNum > 0)
    {
      reqRow = serialNum / 10;  //Divide by 10 to get the ones digit (integer division)
      if (reqRow == 8)
        reqRow = 0;
      reqCol = serialNum % 10;  //Use modulus operator to get the tens digit
      if (reqCol == 8)
        reqCol = 0;
      resetDis();
      moveToPiece(reqRow, reqCol);
    }
  }
  else if (atPiece())
  {
    resetDis();
    down = true;
    moveDown(90);
  }
  else if (down && atPosition())
  {
    resetDis();
    moveGripper(gripperNum);
    changeGripperNum();
    delay(1000);
    up = true;
    grab = false; //Dont update gripper while moving up
    moveUp(90);
    down = false;
  }
  else if (up && atPosition())
  {
    resetDis();
    goHome();
    grab = true;
    up = false;
  }


  /*if (Serial1.available() > 0) //Serial1 becuase it's the second set of serial pins/If at home position
  {
    // Read the data
    int serialNum = Serial1.read();
    if (serialNum > 0 && serialNum < 100)
    {
      reqRow = serialNum / 10;  //Divide by 10 to get the ones digit (integer division)
      reqCol = serialNum % 10;  //Use modulus operator to get the tens digit
      Serial.print(reqRow);
      Serial.print(" - ");
      Serial.println(reqCol);
      moveToPiece(reqRow, reqCol);
      //Serial.println(serialNum);
    }
  }*/
  /*Serial.print(x);
  Serial.print(" , ");
  Serial.print(y);
  Serial.print(" , ");
  Serial.print(z);
  Serial.print(" | base: ");
  Serial.print(baseStepper.distanceToGo());
  Serial.print(" J1: ");
  Serial.print(j1Stepper_R.distanceToGo());
  Serial.print(" J2: ");
  Serial.println(j2Stepper.distanceToGo());*/

  inverseKinematics(x, y, z); //x, y & z are the coordinates sent from the controller. ARM MUST BE COMPLETELY VERTICAL AT START TO WORK (home position, since all relative to y-axis)

  if (theta3 > 0 && grab)
  {
    //*.666667 because I had to scale the 0-180deg write value to 0-270. (Used 270deg servo). Also you need make sure gripper has access to full range of motion when mounting. (If max rotation 270deg, mount it in a way so it can rotate that)
    wrist.write(theta3 * .666667);
  }

  //If motors close, slow down (by 2)
  /*if (isClose(j1Stepper_L, 400)) //(Stepper, steps to be considered close)
    j1Speed /= 4;
  if (isClose(j2Stepper, 400)) //(Stepper, steps to be considered close)
    j2Speed /= 4;
  if (isClose(baseStepper, 400)) //(Stepper, steps to be considered close)
    baseSpeed /= 4;*/

  //Calc degrees to steps and move (assuming 400 steps/rev)
  //Tell the motors which way to get to theta if past 90deg. (Do the motors need to go left or right)
  if (theta1 > 90)
  {
    j1Stepper_L.moveTo((90 - theta1) * -37.037037); //Set target position (37.037037 steps/degree)
    j1Stepper_R.moveTo((90 - theta1) * 37.037037); //Set target position (37.037037 steps/degree)
    if (abs(j1Stepper_R.distanceToGo()) > j1MaxDis) //Find distance to target position relative to start of movement
      j1MaxDis = abs(j1Stepper_R.distanceToGo());
    j1Speed = j1MaxDis/timeToTarget;
    //if (isClose(j1Stepper_R, j1MaxDis*.1)) //Slow down if wihin 10% of target
    //  j1Speed /= 2;
    j1Stepper_L.setSpeed(j1Speed); //Neg. since it needs to move in the other direction
    j1Stepper_R.setSpeed(-j1Speed); //Neg. since it needs to move in the other direction
  }
  else
  {
    //90 - theta1 to make it relative to y-axis
    j1Stepper_L.moveTo((90 - theta1) * -37.037037); //Set target position (37.037037 steps/degree)
    j1Stepper_R.moveTo((90 - theta1) * 37.037037); //Set target position (37.037037 steps/degree)
    if (abs(j1Stepper_R.distanceToGo()) > j1MaxDis) //Find distance to target position relative to start of movement
      j1MaxDis = abs(j1Stepper_R.distanceToGo());
    j1Speed = j1MaxDis/timeToTarget;
    //if (isClose(j1Stepper_R, j1MaxDis*.1)) //Slow down if wihin 10% of target
    //  j1Speed /= 2;
    j1Stepper_L.setSpeed(-j1Speed); 
    j1Stepper_R.setSpeed(j1Speed); 
  }  

  j2Stepper.moveTo(theta2*137.335640); //Set target position (137.335640 steps/degree).
  if (abs(j2Stepper.distanceToGo()) > j2MaxDis) //Find distance to target position relative to start of movement
    j2MaxDis = abs(j2Stepper.distanceToGo());
  j2Speed = j2MaxDis/timeToTarget;
  //if (isClose(j2Stepper, j2MaxDis*.1)) //Slow down if wihin 10% of target
  //  j2Speed /= 2;
  j2Stepper.setSpeed(j2Speed); //800 steps/sec

  baseStepper.moveTo(delta*16.666667); //Set target position (16.666667 steps/degree).
  if (abs(baseStepper.distanceToGo()) > baseMaxDis) //Find distance to target position relative to start of movement
    baseMaxDis = abs(baseStepper.distanceToGo());
  baseSpeed = baseMaxDis/timeToTarget;
  //if (isClose(baseStepper, baseMaxDis*.1)) //Slow down if wihin 10% of target
  //  baseSpeed /= 2;
  baseStepper.setSpeed(baseSpeed); // steps/sec

  /*Serial.print(j2MaxDis);
  Serial.print(" Speed: ");
  Serial.print(j2Speed);
  Serial.print(" | ");
  Serial.print(j1MaxDis);
  Serial.print(" Speed: ");
  Serial.print(j1Speed);
  Serial.print(" | ");
  Serial.print(baseMaxDis);
  Serial.print(" Speed: ");
  Serial.print(baseSpeed);
  Serial.print("    ");
  Serial.print(j2Stepper.currentPosition());
  Serial.print("    ");
  Serial.print(j1Stepper_R.currentPosition());
  Serial.print("    ");
  Serial.println(baseStepper.currentPosition());*/

  j2Stepper.runSpeedToPosition();
  j1Stepper_L.runSpeedToPosition();
  j1Stepper_R.runSpeedToPosition();
  baseStepper.runSpeedToPosition();
}

void inverseKinematics(float x, float y, float z) //x & y are desired position coordinates (mm)
{

  const float pi = 3.141593;
  float j1 = 255.68; //Length J1 in mm
  float j2 = 428.40; //Length J2 in mm

  //Z move
  delta = atan(z/x); //Angle base needs to move
  delta *= (180/pi); //Radians to degrees

  float x2 = sqrt(sq(z) + sq(x)) - x; //Difference added to x to stay alligned with x-coord as base moves

  if (z != 0)
    x += x2;

  theta2 = -acos((sq(x) + sq(y) -sq(j1) - sq(j2)) / (2 * j1 * j2)); //Calculate theta2 (in rads)

  theta1 = atan(y / x) + atan((j2 * sin(theta2)) / (j1 + j2 * cos(theta2))); //Calculate theta1 (in rads)

  //Radians to degrees
  theta2 *= (180/pi);
  theta1 *= (180/pi);
  
  //Adjust angles 
  if (theta2 < 0 && theta1 > 0) 
  {
    if (theta1 < 90)
    {
      theta1 += (180-(theta1 * 2)); //Mirror across y
      theta2 *= -1;
    }
    else if (theta1 > 90)
    {
      theta1 = theta1 - (2 * (theta1 - 90)); //Mirror across y
      theta2 *= -1;
    }
  }
  else if (theta1 < 0 && theta2 < 0)
  {
    theta1 *= -1;
    theta2 *= -1;
  }

  //Gripper orientation. 
  //Psi is desired orientation with respect to y-axis. (180 is straight down)
  theta3 = psi - theta2 - (90 - theta1); //(90 - theta1) so we can make it relative to y-axis. Did it here so it wouldn't be an abs(value)
  theta3 = 185 - theta3; //185 because that is the angle for gripper to be straight, & minus sign because down is negative for how I mounted the gripper

  /*Serial.print("(X, Y, Z): (");
  Serial.print(x);
  Serial.print(", ");
  Serial.print(y);
  Serial.print(", ");
  Serial.print(z);
  Serial.print(")    ");
  Serial.print("Theta 1: ");
  Serial.print(90 - theta1);
  Serial.print("    ");
  Serial.print("Theta 2: ");
  Serial.print(theta2);
  Serial.print("    ");
  Serial.print("Theta 3: ");
  Serial.print(theta3);  
  Serial.print("    ");
  Serial.print("Delta: ");
  Serial.print(delta);  
  Serial.print("    ");
  Serial.print("X2: ");
  Serial.println(x2);*/
  
}

//If a stepper motor is close to target position, return true
bool isClose(AccelStepper s1, int threshold) //"s1" corresponds with the stepper motor declared in the function call, threshold is steps to be considered close
{
  if (abs(s1.distanceToGo()) < threshold)
  {
    return true;
  }
  return false;
}
bool atPiece()
{
  if (j1Stepper_R.distanceToGo() == 0 && j1Stepper_L.distanceToGo() == 0 && j2Stepper.distanceToGo() == 0 && baseStepper.distanceToGo() == 0 && piece)
  {
    return true;
  }
  return false;
}
bool atPosition()
{
  if (j1Stepper_R.distanceToGo() == 0 && j1Stepper_L.distanceToGo() == 0 && j2Stepper.distanceToGo() == 0 && baseStepper.distanceToGo() == 0)
  {
    return true;
  }
  return false;
}

//Move functions
void goHome() //Move to center above the board
{
  x = 430;
  y = -350;
  z = 0;
  piece = false;
}
void moveToPiece(int row, int col) //row, col, array size
{
  x = chessBoard[row][col][0];
  y = chessBoard[row][col][1];
  z = chessBoard[row][col][2];

  piece = true;
}
void moveDown(float yAmount) 
{
  y += yAmount; //Remember, down is positive

  piece = false;
}
void moveUp(float yAmount)
{
  y -= yAmount; //Remember, down is positive 
  
  piece = false;
}

//Open or close gripper
void moveGripper(int val) //1 to close, 0 to open
{
  if (val == 0)
    gripper.write(110);
  else
    gripper.write(145);
}
void changeGripperNum()
{
  if (gripperNum == 1)
    gripperNum = 0;
  else
    gripperNum = 1;
}

void resetDis()
{
  j1MaxDis = 0;
  j2MaxDis = 0;
  baseMaxDis = 0;
}

Any help is appreciated.
Thank you

2

Does oscillating mean the stepper-motor does not turn into the progammed direction continiously but instead the axle of the motor is only moving less than
1 degree clockwise then 1 degree counterclockwise
1 degree clockwise then 1 degree counterclockwise
1 degree clockwise then 1 degree counterclockwise
??
If this happends this means the stepper-motor has too much load for the current.
Of course you can't increase the coil-current above the rated maximum of your stepper-motor
If the current is below long endurance current you could increase the current

if the current is already at the rated maximum you have to either reduce the load or use a stepper-motor wih more torque (greater length of the stepper-motor with same NEMA-size

best regards Stefan

3

It does not. It just seems to move around pretty violently. Ive tried it with no load so I can confirm thats not an issue. Everythings geared down pretty well too. I think the issue is somewhere with the changing of speeds between desired coordinates. With a constant, preprogrammed speed it works. When I have the program calculate the speed so they all get there the same time the issue arrises

4

Then you should reduce the stepper acceleration. That's the most useful part of the AccelStepper library.

And you should be sure whether the jitter arises from weak PSU, too high acceleration, or too high torque.

5

Unfortunately he doesn't use acceleration at all.
@devised : You should change your Accelstepper code to use acceleration. Using steppers without acceleration/deceleration often leeds to such problems.

Which Arduino board are you using?

6

As creating step-pulses is time-critical one way to analyse this is to store values of variables in an array and to printout the values after having finished driving the stepper. To reduce the number of values that must be stored you could use if-conditions that store a value if

  • changed direction
  • difference to last stored value is higher than ....
  • counting up each time the calculated speed is different from the one before
  • differs more than ....
    If the cause is program-logic you could reduce the general speed.

best regards Stefan

7

I will experiment with it. I'm using a ESP32-S3

8

So ... whenever you use the servos, the stepper motor starts oscillating.

  • The servo library you're using doesn't appear to be compatible ESP32-S3 because the S3 is limited to 14-bit pwm and I believe the library defaults to 16-bit pwm.

  • Try using the ESP32-ESP32S2-AnalogWrite library.

Your updated code ...

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

float theta1;
float theta2;

float x;
float y;
float z;

float delta;

float theta3;
float psi = 180; //Desired gripper orientation. Offsetting for servo tolerance

const int wristPin = 2;
const int gripperPin = 1; //Was 38
const int gripPitchPin = 8; //Was non existent
const int numRows = 8;
const int numCols = 8;
const int depth = 3;

const float timeToTarget = 4;
unsigned long prevMillis = 0;

int serialNum = 0;

int j1Speed = 2000;
int j2Speed = 2000;
int baseSpeed = 2000;

float j1MaxDis = 0;
float j2MaxDis = 0;
float baseMaxDis = 0;

int reqRow = 0;
int reqCol = 0;

int gripperNum = 1;
int gripperCount = 0;

bool piece;
bool down;
bool up;
bool grab = true;

//Create 3D array to hold the chess board's positions. (8 rows, 8 columns, 3 values in each sub-array(x, y, z))
int chessBoard[numRows][numCols][depth] =
{
  //The coordinates of each piece on a chess board (x, y, z)
  {{246, -300, 166}, {246, -300, 118}, {246, -300, 70}, {246, -300, 22}, {246, -300, -26}, {246, -300, -74}, {246, -300, -122}, {246, -300, -170}},/*8*/
  {{294, -300, 166}, {294, -300, 118}, {294, -300, 70}, {294, -300, 22}, {294, -300, -26}, {294, -300, -74}, {294, -300, -122}, {294, -300, -170}},/*7*/
  {{345, -300, 166}, {345, -300, 118}, {345, -300, 70}, {345, -300, 22}, {345, -300, -26}, {345, -300, -74}, {345, -300, -122}, {345, -300, -170}},/*6*/
  {{396, -300, 166}, {396, -300, 118}, {396, -300, 70}, {396, -300, 22}, {396, -300, -26}, {396, -300, -74}, {396, -300, -122}, {396, -300, -170}},/*5*/
  {{445, -300, 166}, {445, -300, 118}, {445, -300, 70}, {445, -300, 22}, {445, -300, -26}, {445, -300, -74}, {445, -300, -122}, {445, -300, -170}},/*4*/
  {{496, -300, 166}, {496, -300, 118}, {496, -300, 70}, {496, -300, 22}, {496, -300, -26}, {496, -312, -74}, {496, -300, -122}, {496, -300, -170}},/*3*/
  {{547, -300, 166}, {547, -300, 118}, {547, -300, 70}, {547, -300, 22}, {547, -300, -26}, {547, -314, -74}, {547, -300, -122}, {547, -300, -170}},/*2*/
  {{598, -312, 166}, {598, -300, 118}, {598, -300, 70}, {598, -300, 22}, {598, -300, -26}, {598, -316, -74}, {598, -300, -122}, {598, -300, -170}} /*1*/
  // a  b  c  d  e  f  g  h
  //Called as [row, col, position in array]
};

Servo servo;  //Create servo object to control the servos
AccelStepper baseStepper(1, 5, 4);   // (Type:driver(1 is default driver), STEP, DIR)
AccelStepper j1Stepper_L(1, 7, 6);   // (Type:driver(1 is default driver), STEP, DIR)
AccelStepper j1Stepper_R(1, 17, 15);   // (Type:driver(1 is default driver), STEP, DIR)
AccelStepper j2Stepper(1, 10, 9);   // (Type:driver(1 is default driver), STEP, DIR)

void setup() {
  //Init steppers (Half steps)
  baseStepper.setMaxSpeed(baseSpeed); //800 pulse/rev
  j1Stepper_L.setMaxSpeed(j1Speed); //400 pulse/rev
  j1Stepper_R.setMaxSpeed(j1Speed); //400 pulse/rev
  j2Stepper.setMaxSpeed(j2Speed); //400 pulse/rev

  servo.attach(wristPin);
  servo.attach(gripperPin);

  //Initialize Serial Monitors
  Serial.begin(115200);
  Serial1.begin(115200, SERIAL_8N1, 17, 18); //(TX, RX)

  moveGripper(0);
  //Start position
  goHome();
}

void loop()
{
  unsigned long currentMillis = millis();

  if (Serial.available() > 0) //Serial1 becuase it's the second set of serial pins/Serial input code. Tells us which piece to move
  {
    //Read the data
    //serialNum = Serial1.read(); //Coords from chess engine
    serialNum = Serial.parseInt(); //Used for testing
    if (serialNum > 0)
    {
      reqRow = serialNum / 10;  //Divide by 10 to get the ones digit (integer division)
      if (reqRow == 8)
        reqRow = 0;
      reqCol = serialNum % 10;  //Use modulus operator to get the tens digit
      if (reqCol == 8)
        reqCol = 0;
      resetDis();
      moveToPiece(reqRow, reqCol);
    }
  }
  else if (atPiece())
  {
    resetDis();
    down = true;
    moveDown(90);
  }
  else if (down && atPosition())
  {
    resetDis();
    moveGripper(gripperNum);
    changeGripperNum();
    delay(1000);
    up = true;
    grab = false; //Dont update gripper while moving up
    moveUp(90);
    down = false;
  }
  else if (up && atPosition())
  {
    resetDis();
    goHome();
    grab = true;
    up = false;
  }


  /*if (Serial1.available() > 0) //Serial1 becuase it's the second set of serial pins/If at home position
    {
    // Read the data
    int serialNum = Serial1.read();
    if (serialNum > 0 && serialNum < 100)
    {
      reqRow = serialNum / 10;  //Divide by 10 to get the ones digit (integer division)
      reqCol = serialNum % 10;  //Use modulus operator to get the tens digit
      Serial.print(reqRow);
      Serial.print(" - ");
      Serial.println(reqCol);
      moveToPiece(reqRow, reqCol);
      //Serial.println(serialNum);
    }
    }*/
  /*Serial.print(x);
    Serial.print(" , ");
    Serial.print(y);
    Serial.print(" , ");
    Serial.print(z);
    Serial.print(" | base: ");
    Serial.print(baseStepper.distanceToGo());
    Serial.print(" J1: ");
    Serial.print(j1Stepper_R.distanceToGo());
    Serial.print(" J2: ");
    Serial.println(j2Stepper.distanceToGo());*/

  inverseKinematics(x, y, z); //x, y & z are the coordinates sent from the controller. ARM MUST BE COMPLETELY VERTICAL AT START TO WORK (home position, since all relative to y-axis)

  if (theta3 > 0 && grab)
  {
    //*.666667 because I had to scale the 0-180deg write value to 0-270. (Used 270deg servo). Also you need make sure gripper has access to full range of motion when mounting. (If max rotation 270deg, mount it in a way so it can rotate that)
    servo.write(wristPin, theta3 * .666667);
  }

  //If motors close, slow down (by 2)
  /*if (isClose(j1Stepper_L, 400)) //(Stepper, steps to be considered close)
    j1Speed /= 4;
    if (isClose(j2Stepper, 400)) //(Stepper, steps to be considered close)
    j2Speed /= 4;
    if (isClose(baseStepper, 400)) //(Stepper, steps to be considered close)
    baseSpeed /= 4;*/

  //Calc degrees to steps and move (assuming 400 steps/rev)
  //Tell the motors which way to get to theta if past 90deg. (Do the motors need to go left or right)
  if (theta1 > 90)
  {
    j1Stepper_L.moveTo((90 - theta1) * -37.037037); //Set target position (37.037037 steps/degree)
    j1Stepper_R.moveTo((90 - theta1) * 37.037037); //Set target position (37.037037 steps/degree)
    if (abs(j1Stepper_R.distanceToGo()) > j1MaxDis) //Find distance to target position relative to start of movement
      j1MaxDis = abs(j1Stepper_R.distanceToGo());
    j1Speed = j1MaxDis / timeToTarget;
    //if (isClose(j1Stepper_R, j1MaxDis*.1)) //Slow down if wihin 10% of target
    //  j1Speed /= 2;
    j1Stepper_L.setSpeed(j1Speed); //Neg. since it needs to move in the other direction
    j1Stepper_R.setSpeed(-j1Speed); //Neg. since it needs to move in the other direction
  }
  else
  {
    //90 - theta1 to make it relative to y-axis
    j1Stepper_L.moveTo((90 - theta1) * -37.037037); //Set target position (37.037037 steps/degree)
    j1Stepper_R.moveTo((90 - theta1) * 37.037037); //Set target position (37.037037 steps/degree)
    if (abs(j1Stepper_R.distanceToGo()) > j1MaxDis) //Find distance to target position relative to start of movement
      j1MaxDis = abs(j1Stepper_R.distanceToGo());
    j1Speed = j1MaxDis / timeToTarget;
    //if (isClose(j1Stepper_R, j1MaxDis*.1)) //Slow down if wihin 10% of target
    //  j1Speed /= 2;
    j1Stepper_L.setSpeed(-j1Speed);
    j1Stepper_R.setSpeed(j1Speed);
  }

  j2Stepper.moveTo(theta2 * 137.335640); //Set target position (137.335640 steps/degree).
  if (abs(j2Stepper.distanceToGo()) > j2MaxDis) //Find distance to target position relative to start of movement
    j2MaxDis = abs(j2Stepper.distanceToGo());
  j2Speed = j2MaxDis / timeToTarget;
  //if (isClose(j2Stepper, j2MaxDis*.1)) //Slow down if wihin 10% of target
  //  j2Speed /= 2;
  j2Stepper.setSpeed(j2Speed); //800 steps/sec

  baseStepper.moveTo(delta * 16.666667); //Set target position (16.666667 steps/degree).
  if (abs(baseStepper.distanceToGo()) > baseMaxDis) //Find distance to target position relative to start of movement
    baseMaxDis = abs(baseStepper.distanceToGo());
  baseSpeed = baseMaxDis / timeToTarget;
  //if (isClose(baseStepper, baseMaxDis*.1)) //Slow down if wihin 10% of target
  //  baseSpeed /= 2;
  baseStepper.setSpeed(baseSpeed); // steps/sec

  /*Serial.print(j2MaxDis);
    Serial.print(" Speed: ");
    Serial.print(j2Speed);
    Serial.print(" | ");
    Serial.print(j1MaxDis);
    Serial.print(" Speed: ");
    Serial.print(j1Speed);
    Serial.print(" | ");
    Serial.print(baseMaxDis);
    Serial.print(" Speed: ");
    Serial.print(baseSpeed);
    Serial.print("    ");
    Serial.print(j2Stepper.currentPosition());
    Serial.print("    ");
    Serial.print(j1Stepper_R.currentPosition());
    Serial.print("    ");
    Serial.println(baseStepper.currentPosition());*/

  j2Stepper.runSpeedToPosition();
  j1Stepper_L.runSpeedToPosition();
  j1Stepper_R.runSpeedToPosition();
  baseStepper.runSpeedToPosition();
}

void inverseKinematics(float x, float y, float z) //x & y are desired position coordinates (mm)
{

  const float pi = 3.141593;
  float j1 = 255.68; //Length J1 in mm
  float j2 = 428.40; //Length J2 in mm

  //Z move
  delta = atan(z / x); //Angle base needs to move
  delta *= (180 / pi); //Radians to degrees

  float x2 = sqrt(sq(z) + sq(x)) - x; //Difference added to x to stay alligned with x-coord as base moves

  if (z != 0)
    x += x2;

  theta2 = -acos((sq(x) + sq(y) - sq(j1) - sq(j2)) / (2 * j1 * j2)); //Calculate theta2 (in rads)

  theta1 = atan(y / x) + atan((j2 * sin(theta2)) / (j1 + j2 * cos(theta2))); //Calculate theta1 (in rads)

  //Radians to degrees
  theta2 *= (180 / pi);
  theta1 *= (180 / pi);

  //Adjust angles
  if (theta2 < 0 && theta1 > 0)
  {
    if (theta1 < 90)
    {
      theta1 += (180 - (theta1 * 2)); //Mirror across y
      theta2 *= -1;
    }
    else if (theta1 > 90)
    {
      theta1 = theta1 - (2 * (theta1 - 90)); //Mirror across y
      theta2 *= -1;
    }
  }
  else if (theta1 < 0 && theta2 < 0)
  {
    theta1 *= -1;
    theta2 *= -1;
  }

  //Gripper orientation.
  //Psi is desired orientation with respect to y-axis. (180 is straight down)
  theta3 = psi - theta2 - (90 - theta1); //(90 - theta1) so we can make it relative to y-axis. Did it here so it wouldn't be an abs(value)
  theta3 = 185 - theta3; //185 because that is the angle for gripper to be straight, & minus sign because down is negative for how I mounted the gripper

  /*Serial.print("(X, Y, Z): (");
    Serial.print(x);
    Serial.print(", ");
    Serial.print(y);
    Serial.print(", ");
    Serial.print(z);
    Serial.print(")    ");
    Serial.print("Theta 1: ");
    Serial.print(90 - theta1);
    Serial.print("    ");
    Serial.print("Theta 2: ");
    Serial.print(theta2);
    Serial.print("    ");
    Serial.print("Theta 3: ");
    Serial.print(theta3);
    Serial.print("    ");
    Serial.print("Delta: ");
    Serial.print(delta);
    Serial.print("    ");
    Serial.print("X2: ");
    Serial.println(x2);*/

}

//If a stepper motor is close to target position, return true
bool isClose(AccelStepper s1, int threshold) //"s1" corresponds with the stepper motor declared in the function call, threshold is steps to be considered close
{
  if (abs(s1.distanceToGo()) < threshold)
  {
    return true;
  }
  return false;
}
bool atPiece()
{
  if (j1Stepper_R.distanceToGo() == 0 && j1Stepper_L.distanceToGo() == 0 && j2Stepper.distanceToGo() == 0 && baseStepper.distanceToGo() == 0 && piece)
  {
    return true;
  }
  return false;
}
bool atPosition()
{
  if (j1Stepper_R.distanceToGo() == 0 && j1Stepper_L.distanceToGo() == 0 && j2Stepper.distanceToGo() == 0 && baseStepper.distanceToGo() == 0)
  {
    return true;
  }
  return false;
}

//Move functions
void goHome() //Move to center above the board
{
  x = 430;
  y = -350;
  z = 0;
  piece = false;
}
void moveToPiece(int row, int col) //row, col, array size
{
  x = chessBoard[row][col][0];
  y = chessBoard[row][col][1];
  z = chessBoard[row][col][2];

  piece = true;
}
void moveDown(float yAmount)
{
  y += yAmount; //Remember, down is positive

  piece = false;
}
void moveUp(float yAmount)
{
  y -= yAmount; //Remember, down is positive

  piece = false;
}

//Open or close gripper
void moveGripper(int val) //1 to close, 0 to open
{
  if (val == 0)
    servo.write(gripperPin, 110);
  else
    servo.write(gripperPin, 145);
}
void changeGripperNum()
{
  if (gripperNum == 1)
    gripperNum = 0;
  else
    gripperNum = 1;
}

void resetDis()
{
  j1MaxDis = 0;
  j2MaxDis = 0;
  baseMaxDis = 0;
}
9

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.