Servo Malfunction with Robotic Arm

Other Hardware Motors, Mechanics, Power and CNC
1

Hello to anybody who sees this. I am currently building a SCARA arm and have almost completed the build. I have found the designs and work on howtomechatronics.com and thought they were great so I built it. Everything works except the servo motor powering the gripper. The issue is that when the Arduino Uno is connected to power, the gripper just constantly turns and doesn't stop ever. I have no clue as to how to fix it and I will link the code below.

/*
   Arduino based SCARA Robot 
   by Dejan, www.HowToMechatronics.com
   AccelStepper: http://www.airspayce.com/mikem/arduino/AccelStepper/index.html

*/
#include <AccelStepper.h>
#include <Servo.h>
#include <math.h>

#define limitSwitch1 11
#define limitSwitch2 10
#define limitSwitch3 9
#define limitSwitch4 A3

// Define the stepper motors and the pins the will use
AccelStepper stepper1(1, 2, 5); // (Type:driver, STEP, DIR)
AccelStepper stepper2(1, 3, 6);
AccelStepper stepper3(1, 4, 7);
AccelStepper stepper4(1, 12, 13);

Servo grippAerServo;  // create servo object to control a servo


double x = 10.0;
double y = 10.0;
double L1 = 228; // L1 = 228mm
double L2 = 136.5; // L2 = 136.5mm
double theta1, theta2, phi, z;

int stepper1Position, stepper2Position, stepper3Position, stepper4Position;

const float theta1AngleToSteps = 44.444444;
const float theta2AngleToSteps = 35.555555;
const float phiAngleToSteps = 10;
const float zDistanceToSteps = 100;

byte inputValue[5];
int k = 0;

String content = "";
int data[10];

int theta1Array[100];
int theta2Array[100];
int phiArray[100];
int zArray[100];
int gripperArray[100];
int positionsCounter = 0;

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

  pinMode(limitSwitch1, INPUT_PULLUP);
  pinMode(limitSwitch2, INPUT_PULLUP);
  pinMode(limitSwitch3, INPUT_PULLUP);
  pinMode(limitSwitch4, INPUT_PULLUP);

  // Stepper motors max speed
  stepper1.setMaxSpeed(4000);
  stepper1.setAcceleration(2000);
  stepper2.setMaxSpeed(4000);
  stepper2.setAcceleration(2000);
  stepper3.setMaxSpeed(4000);
  stepper3.setAcceleration(2000);
  stepper4.setMaxSpeed(4000);
  stepper4.setAcceleration(2000);

  gripperServo.attach(A0, 600, 2500);
  // initial servo value - open gripper
  data[6] = 180;
  gripperServo.write(data[6]);
  delay(1000);
  data[5] = 100;
  homing();
}

void loop() {

  if (Serial.available()) {
    content = Serial.readString(); // Read the incomding data from Processing
    // Extract the data from the string and put into separate integer variables (data[] array)
    for (int i = 0; i < 10; i++) {
      int index = content.indexOf(","); // locate the first ","
      data[i] = atol(content.substring(0, index).c_str()); //Extract the number from start to the ","
      content = content.substring(index + 1); //Remove the number from the string
    }
    /*
     data[0] - SAVE button status
     data[1] - RUN button status
     data[2] - Joint 1 angle
     data[3] - Joint 2 angle
     data[4] - Joint 3 angle
     data[5] - Z position
     data[6] - Gripper value
     data[7] - Speed value
     data[8] - Acceleration value
    */
    // If SAVE button is pressed, store the data into the appropriate arrays
    if (data[0] == 1) {
      theta1Array[positionsCounter] = data[2] * theta1AngleToSteps; //store the values in steps = angles * angleToSteps variable
      theta2Array[positionsCounter] = data[3] * theta2AngleToSteps;
      phiArray[positionsCounter] = data[4] * phiAngleToSteps;
      zArray[positionsCounter] = data[5] * zDistanceToSteps;
      gripperArray[positionsCounter] = data[6];
      positionsCounter++;
    }
    // clear data
    if (data[0] == 2) {
      // Clear the array data to 0
      memset(theta1Array, 0, sizeof(theta1Array));
      memset(theta2Array, 0, sizeof(theta2Array));
      memset(phiArray, 0, sizeof(phiArray));
      memset(zArray, 0, sizeof(zArray));
      memset(gripperArray, 0, sizeof(gripperArray));
      positionsCounter = 0;
    }
  }
  // If RUN button is pressed
  while (data[1] == 1) {
    stepper1.setSpeed(data[7]);
    stepper2.setSpeed(data[7]);
    stepper3.setSpeed(data[7]);
    stepper4.setSpeed(data[7]);
    stepper1.setAcceleration(data[8]);
    stepper2.setAcceleration(data[8]);
    stepper3.setAcceleration(data[8]);
    stepper4.setAcceleration(data[8]);

    // execute the stored steps
    for (int i = 0; i <= positionsCounter - 1; i++) {
      if (data[1] == 0) {
        break;
      }
      stepper1.moveTo(theta1Array[i]);
      stepper2.moveTo(theta2Array[i]);
      stepper3.moveTo(phiArray[i]);
      stepper4.moveTo(zArray[i]);
      while (stepper1.currentPosition() != theta1Array[i] || stepper2.currentPosition() != theta2Array[i] || stepper3.currentPosition() != phiArray[i] || stepper4.currentPosition() != zArray[i]) {
        stepper1.run();
        stepper2.run();
        stepper3.run();
        stepper4.run();
      }
      if (i == 0) {
        gripperServo.write(gripperArray[i]);
      }
      else if (gripperArray[i] != gripperArray[i - 1]) {
        gripperServo.write(gripperArray[i]);
        delay(800); // wait 0.8s for the servo to grab or drop - the servo is slow
      }

      //check for change in speed and acceleration or program stop
      if (Serial.available()) {
        content = Serial.readString(); // Read the incomding data from Processing
        // Extract the data from the string and put into separate integer variables (data[] array)
        for (int i = 0; i < 10; i++) {
          int index = content.indexOf(","); // locate the first ","
          data[i] = atol(content.substring(0, index).c_str()); //Extract the number from start to the ","
          content = content.substring(index + 1); //Remove the number from the string
        }

        if (data[1] == 0) {
          break;
        }
        // change speed and acceleration while running the program
        stepper1.setSpeed(data[7]);
        stepper2.setSpeed(data[7]);
        stepper3.setSpeed(data[7]);
        stepper4.setSpeed(data[7]);
        stepper1.setAcceleration(data[8]);
        stepper2.setAcceleration(data[8]);
        stepper3.setAcceleration(data[8]);
        stepper4.setAcceleration(data[8]);
      }
    }
    /*
      // execute the stored steps in reverse
      for (int i = positionsCounter - 2; i >= 0; i--) {
      if (data[1] == 0) {
        break;
      }
      stepper1.moveTo(theta1Array[i]);
      stepper2.moveTo(theta2Array[i]);
      stepper3.moveTo(phiArray[i]);
      stepper4.moveTo(zArray[i]);
      while (stepper1.currentPosition() != theta1Array[i] || stepper2.currentPosition() != theta2Array[i] || stepper3.currentPosition() != phiArray[i] || stepper4.currentPosition() != zArray[i]) {
        stepper1.run();
        stepper2.run();
        stepper3.run();
        stepper4.run();
      }
      gripperServo.write(gripperArray[i]);

      if (Serial.available()) {
        content = Serial.readString(); // Read the incomding data from Processing
        // Extract the data from the string and put into separate integer variables (data[] array)
        for (int i = 0; i < 10; i++) {
          int index = content.indexOf(","); // locate the first ","
          data[i] = atol(content.substring(0, index).c_str()); //Extract the number from start to the ","
          content = content.substring(index + 1); //Remove the number from the string
        }
        if (data[1] == 0) {
          break;
        }
      }
      }
    */
  }

  stepper1Position = data[2] * theta1AngleToSteps;
  stepper2Position = data[3] * theta2AngleToSteps;
  stepper3Position = data[4] * phiAngleToSteps;
  stepper4Position = data[5] * zDistanceToSteps;

  stepper1.setSpeed(data[7]);
  stepper2.setSpeed(data[7]);
  stepper3.setSpeed(data[7]);
  stepper4.setSpeed(data[7]);

  stepper1.setAcceleration(data[8]);
  stepper2.setAcceleration(data[8]);
  stepper3.setAcceleration(data[8]);
  stepper4.setAcceleration(data[8]);

  stepper1.moveTo(stepper1Position);
  stepper2.moveTo(stepper2Position);
  stepper3.moveTo(stepper3Position);
  stepper4.moveTo(stepper4Position);

  while (stepper1.currentPosition() != stepper1Position || stepper2.currentPosition() != stepper2Position || stepper3.currentPosition() != stepper3Position || stepper4.currentPosition() != stepper4Position) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
    stepper4.run();
  }
  delay(100);
  gripperServo.write(data[6]);
  delay(300);
}

void serialFlush() {
  while (Serial.available() > 0) {  //while there are characters in the serial buffer, because Serial.available is >0
    Serial.read();         // get one character
  }
}

void homing() {
  // Homing Stepper4
  while (digitalRead(limitSwitch4) != 1) {
    stepper4.setSpeed(1500);
    stepper4.runSpeed();
    stepper4.setCurrentPosition(17000); // When limit switch pressed set position to 0 steps
  }
  delay(20);
  stepper4.moveTo(10000);
  while (stepper4.currentPosition() != 10000) {
    stepper4.run();
  }

  // Homing Stepper3
  while (digitalRead(limitSwitch3) != 1) {
    stepper3.setSpeed(-1100);
    stepper3.runSpeed();
    stepper3.setCurrentPosition(-1662); // When limit switch pressed set position to 0 steps
  }
  delay(20);

  stepper3.moveTo(0);
  while (stepper3.currentPosition() != 0) {
    stepper3.run();
  }

  // Homing Stepper2
  while (digitalRead(limitSwitch2) != 1) {
    stepper2.setSpeed(-1300);
    stepper2.runSpeed();
    stepper2.setCurrentPosition(-5420); // When limit switch pressed set position to -5440 steps
  }
  delay(20);

  stepper2.moveTo(0);
  while (stepper2.currentPosition() != 0) {
    stepper2.run();
  }

  // Homing Stepper1
  while (digitalRead(limitSwitch1) != 1) {
    stepper1.setSpeed(-1200);
    stepper1.runSpeed();
    stepper1.setCurrentPosition(-3955); // When limit switch pressed set position to 0 steps
  }
  delay(20);
  stepper1.moveTo(0);
  while (stepper1.currentPosition() != 0) {
    stepper1.run();
  }
}
2

That sounds like you have a "continuous" servo. That is a servo thst has had its internal feedback mechanism disconnected and end stops removed. The servo becomes a gear motor whose speed and direction are controlled by a sevo signal. It is no longer a servo, its position can't be controlled.

If that is the case your only recourse is to replace the continuous servo with one that has not been modified.

3

Please post a link to the datasheet of that unit.

4

You could write a small simple sketch that only tests the servo to confirm if it a software or hardware problem.

This will open and close the gripper if the servo is a regular servo.

#include <Servo.h>

Servo gripperServo;

void setup()
{
   Serial.begin(115200);
   gripperServo.write(90);
   gripperServo.attach(A0, 600, 2500);
}

void loop()
{
    gripperServo.write(30);
    delay(2000);
    gripperServo.write(150);
    delay(2000);
}
5

Thank you groundFungus for the quick response. I used your code with the servo and it worked fine. The servo continuously rotated back and forth, "opening" and "closing" which lead me to believe that there is an issue with the code. For further info, the servo is being controlled by a GUI with a slider to make it open and close to a value between 0 and 100. The issue now is that there is probably an issue with the code telling the servo to just continuously turn and there isn't a value being read to tell it to stop, I just don't know where that would be in the code. Any help is greatly appreciated.

6

Yes, sorry about the vagueness of the "gripper"
The servo is a MG996R Digital High Torque servo, but I cannot figure out how to link the datasheet, so I'll link the site that I found it on.
(https://www.digikey.com/en/htmldatasheets/production/5014637/0/0/1/mg996r.html)

7

It worked! The link gave the information I wanted. Never mind, You found a mishap in the code. All is well.

8

Thank you for your help. My only thing now is to find where in the code the issue is and then it will probably work. Thanks again, Railroader

9

should be..

memset(theta1Array, 0, sizeof(theta1Array)*sizeof(theta1Array[0]);

for all the memsets..

~q

10

@qubits-us has one observation. Hopefully it pulls the plug.
Else, using serial monitor and Serial.println in various ways is a universal help. Print a key variable, print millis()....... Don't be surprised if You need several attempts to catch the fault. Good luck!

11

Thank you for the response. I replaced the memsets like you said, going from

      memset(theta1Array, 0, sizeof(theta1Array));
      memset(theta2Array, 0, sizeof(theta2Array));
      memset(phiArray, 0, sizeof(phiArray));
      memset(zArray, 0, sizeof(zArray));
      memset(gripperArray, 0, sizeof(gripperArray));

to

      memset(theta1Array, 0, sizeof(theta1Array)*sizeof(theta1Array[0]));
      memset(theta2Array, 0, sizeof(theta2Array)*sizeof(theta2Array[0]));
      memset(phiArray, 0, sizeof(phiArray)*sizeof(phiArray[0]));
      memset(zArray, 0, sizeof(zArray)*sizeof(zArray[0]));
      memset(gripperArray, 0, sizeof(gripperArray)*sizeof(gripperArray[0]));

and unfortunatly it is still continously rotating. I am still extremely new to arduino, but I can't help to wonder if the line of code that says, gripperServo.attach(A0, 600, 2500); if the numbers 600 and 2500 should be different. I thought that 60 set the minnimum 2500 set the maximum value that the servo can turn, so shouldn't the values be between 0 and 180? I am probably wrong, seeing how the servo is able to rotate a full 360 degrees.

12

now i think you're on to something..
digging into servo.cpp i see this..

uint8_t Servo::attach(int pin, int min, int max)
{
  if(this->servoIndex < MAX_SERVOS ) {
    pinMode( pin, OUTPUT) ;                                   // set servo pin to output
    servos[this->servoIndex].Pin.nbr = pin;
    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 us
    this->max  = (MAX_PULSE_WIDTH - max)/4;

pay close attention to the todo comment.. :slight_smile:
~q

13

They probably set the pulse width limits in microseconds. Generally, servos are set with either degrees, or uS.

14

Thank you for the reply. I will have to do some digging to understand some components of the code, but this is sure to point me in the right direction on what to look for. I will get back after I understand what the todo comment is saying and hopefully it will help with the issue. :grin:

15

Thank you for the clarification. This should allow me to better understand what I need to do to fix the issue.

16

you're welcome.. not sure if that's the issue though..
todo is about the min should be less than 128, which it is..
and i see you had demo code that worked to test..
so problem lies some where else..
beak the code into smaller sections, some serial prints to see what's going on..

the memset corrections were needed..
sizeof returns number of elements in the array not size in bytes..
ints are either 2 bytes or 4 depending on architecture..

good luck.. ~q

17

The issue is, most coders will just use integers for position(and most libraries are written that way), resulting in one-degree steps, but many servos respond better to much finer resolution changes. Specifying in uS allows that.

18

Okay. Thank you for the help qubits-us. I will make sure to section out the code and figure out what is missing. I'll probably post the problem answer when I find out what the issue is.

19

sorry, couldn't help more..
got allot of while and for loops..
sounds like you might be stuck in one..
add some serial.println("im here") to see if you can tack flow..
breaking the loop up into a few functions might make debugging easier..

20

I have a question groundFungus. If the servo was a normal servo that rotated between 0 and 180 degrees, should the code theoretically work? If not, should a continuous servo work? They are MG996R servos and say that they are normal 180 degrees but they rotate 360+.