I'm trying to control a robotic arm using some MG996R servos, a PCA 9685 motor driver, and an Arduino Uno. It works great, except for the fact that right now the servos always move at full speed, which is too fast and destabilizes the arm (even though I have it bolted to my desk) and often times causes it to overshoot from its destination.
I'm trying to control the speed of the servos using the code similar to the one below. Basically I just define the number of times I want to update the servos per second (FREQUENCY), and then keep incrementing (or decrementing) the servo angles in a loop with a small delay -
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
const int MIN_PULSE_WIDTH = 650;
const int MAX_PULSE_WIDTH = 2350;
// The time in milliseconds that a servo should take to rotate from one angle to another
const float SERVO_ROTATION_DELAY = 1500;
// The frequency of updates for the PCA 9685 driver
const float FREQUENCY = 50;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
float servoAngle = 0;
void setup() {
Serial.begin(9600);
pwm.begin();
pwm.setPWMFreq(FREQUENCY);
setArmPosition(0);
}
void loop() {
if (Serial.available()) {
String move = Serial.readString().toInt();
else {
Serial.print("Moving to "); Serial.println(move);
setArmPosition(move);
}
}
}
void setArmPosition(float angle) {
// Number of times to update the servo position, given an update frequency of FREQUENCY
float numUpdates = (SERVO_ROTATION_DELAY / 1000.0) * FREQUENCY;
float delta = (angle - servoAngle) / numUpdates;
for (int i = 0; i < numUpdates; i++) {
servoAngle += delta;
int pulseWidth = map(servoAngle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
pulseWidth = int(float(pulseWidth) / 1000000 * FREQUENCY * 4096);
pwm.setPWM(0, 0, pulseWidth);
delayMicroseconds((SERVO_ROTATION_DELAY * 1000) / numUpdates);
}
}
However, the servos don't move smoothly and jump in discrete steps. I've tried increasing the number of updates and decreasing the delay in between updates with no success. What can I do in this case to make the movement of the servos smoother?
