I want to be able to control multiple servos to specific angles with specific speeds at specific times (all at the same time) in my project, but I have run into a programming problem when defining the speed. Just writing:
loop()
{
for (int a = 0; a <= 180; a++)
{
delay(50);
setServo(0, a); // Makes the servo connected to port 0 go to angle "a" as fast as it can.
}
}
works, it moves from 0° to 180° relatively smoothly at a slow speed. However, this code only works with one servo. This made me write the program to calculate every single servo's new "step", command every servo to go its new step and then to wait 50 ms before doing it again. This worked a couple of days ago, but now that I've added the "Main()" function with its "hasElapsed()" (so that I can have time between commands to the servos), the servos always go at one certain speed. Even if adjust "velocity" to 1 to make them turn 1° every 50, 100 or 1000 ms, they still go as fast as they can to their target angle.
All I need help with is to be able to control the velocity which the servos turn at.
I've attached the project in this thread, I didn't know if it was too large for posting in code.
If I'm to vague with my problem or if I haven't explained a certain part of my code, please forgive me.
Thankful for all the help I can get, since this is my first Arduino project!
UKHeliBob:
There is also a VarSpeedServo library available
I've seen that before, but I've only used:
pwm.setPWM(s, 0, pulseWidth(newAngle)); // Rotate servo to the new angle(the next step.
and all of the other code that this guy used. I never use:
#include <VarSpeedServo.h>
VarSpeedServo myservo; // create servo object to control a servo
void setup() {
myservo.attach(9); // attaches the servo on pin 9 to the servo object
}
How can I change my code so that it works with pulseWidth? Or how do I make the board work with code from VarSpeedServo?
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define MIN_PULSE_WIDTH 650
#define MAX_PULSE_WIDTH 2350
#define DEFAULT_PULSE_WIDTH 1500
#define FREQUENCY 50
unsigned long currentMillis;
unsigned long previousMillis = 0;
unsigned long waitMillis = 0;
const int interval = 50; // The time between steps of servos
int currentAngle[16]; // This stores the properties of a maximum of 16 servos
int velocity[16]; // (what my servo shield supports).
int targetAngle[16];
int lastTime;
int timeNow;
int c = 1; // Which part of the sequence to run.
bool updateServo[16];
bool allClear = true;
void setup()
{
pinMode(LED_BUILTIN, OUTPUT);
pwm.begin();
pwm.setPWMFreq(FREQUENCY);
digitalWrite(LED_BUILTIN, HIGH);
delay(4000);
digitalWrite(LED_BUILTIN, LOW);
for (int s = 0; s < 16; s++) updateServo[s] = false;
setServo(0, 0); // All servos used in the project must know where to start.
setServo(4, 0);
delay(2000);
for (int a = 0; a <= 180; a++)
{
delay(50);
setServo(0, a);
}
delay(2000);
}
void Main()
{
if (allClear)
{
int velocity1 = 1;
int velocity2 = 2;
if (c == 1 && hasElapsed(1000))
{
commandServo(0, 45, velocity1);
commandServo(4, 135, velocity2);
c++;
}
else if (c == 2 && hasElapsed(2000))
{
commandServo(0, 90, velocity1);
commandServo(4, 90, velocity2);
c++;
}
else if (c == 3 && hasElapsed(3000))
{
commandServo(0, 180, velocity1);
commandServo(4, 0, velocity2);
c++;
}
else if (c == 4 && hasElapsed(4000))
{
commandServo(0, 0, velocity1);
commandServo(4, 0, velocity2);
c++;
}
// else
// {
// digitalWrite(LED_BUILTIN, HIGH);
// allClear = false;
// }
}
}
void loop()
{
currentMillis = millis();
timeNow = millis();
Main();
if (currentMillis - previousMillis >= interval)
{
previousMillis = currentMillis;
for (int s = 0; s < 16; s++)
{
if (updateServo[s] == true) // Checks to see if servo number s needs to be updated.
{
int newAngle; // The next incremented angle to rotate to (not target).
int rotation; // Which way (up or down) the servo should rotate.
// if (velocity[s] >= 100) setServo(s, targetAngle[s]); // Makes sure that max 100% is allowed.
// else
// {
if (targetAngle[s] > currentAngle[s]) rotation = 1; // Sets the rotation direction depending
else rotation = -1; // on whether the target angle is over or below current.
while (currentAngle[s] != targetAngle[s]) // Keep going as long as it hasn't reached its target.
{
newAngle = currentAngle[s] + rotation * (velocity[s]); // Assigns the position of the next step.
if ((rotation == 1 && newAngle > targetAngle[s]) // If the next step exceeds target, rotate to target instead.
|| (rotation == -1 && newAngle < targetAngle[s]))
newAngle = targetAngle[s];
pwm.setPWM(s, 0, pulseWidth(newAngle)); // Rotate servo to the new angle(the next step.
currentAngle[s] = newAngle; // Update the current angle of rotated servo.
if (currentAngle[s] == targetAngle[s])
{
updateServo[s] == false; // Stop updating and rotating this servo if it has reached its target.
break;
}
}
}
}
}
}
void setServo(int s, int angle) // s is which servo it moves (0 to 15).
{
pwm.setPWM(s, 0, pulseWidth(angle));
currentAngle[s] = angle; // The servo's current angle is updated.
}
void commandServo(int s, int angle, int v)
{
targetAngle[s] = angle;
velocity[s] = v;
updateServo[s] = true;
}
bool hasElapsed(int period)
{
if (timeNow >= lastTime + period)
{
lastTime = timeNow;
return true;
}
else
{
return false;
}
}
int pulseWidth(int angle)
{
int pulse_wide, analog_value;
pulse_wide = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
return analog_value;
}
How can I change my code so that it works with pulseWidth?
I am not familiar with that library but don't move the servos from their current position to their target position all in one command. Move them a little every now and again using millis() for timing. Use a boolean variable to stop them moving when the target position is reached
For one servo, something like this
start of loop()
get current time from millis()
if current time - previous move time >= wait period and the moving variable is true
move a little
save the time of the move
change the target position a little
if at the target position
set the boolean to false
end if
end if
end of loop()
You can extend this to multiple servos by using arrays for the values and an array of servos
UKHeliBob:
I am not familiar with that library but don't move the servos from their current position to their target position all in one command. Move them a little every now and again using millis() for timing. Use a boolean variable to stop them moving when the target position is reached
For one servo, something like this
start of loop()
get current time from millis()
if current time - previous move time >= wait period and the moving variable is true
move a little
save the time of the move
change the target position a little
if at the target position
set the boolean to false
end if
end if
end of loop()
You can extend this to multiple servos by using arrays for the values and an array of servos
That's exactly what I've done, however the velocity doesn't work.
updateServo[s] == false; // Stop updating and rotating this servo if it has reached its target.
I really doubt that that is the correct operator to use in that statement.
Main();
Are we supposed to be able to guess what this function does? How would you like to program the Arduino is digitalWrite() were M1284() instead? If analogWrite() was M34278()?
updateServo[s] == false; // Stop updating and rotating this servo if it has reached its target.
I really doubt that that is the correct operator to use in that statement.
Thanks, didn't notice!
PaulS:
Main();
Are we supposed to be able to guess what this function does? How would you like to program the Arduino is digitalWrite() were M1284() instead? If analogWrite() was M34278()?
Main() commands the servos and stores the entire sequence of the sketch. When I get the velocity working, this is what I'll edit to program my station.
PaulS:
Fat-fingered something, and that was posted before I was done.
You appear to have several functions that move the servos, and call them all from loop(). Which function is not doing what you expect?
When it calculates the newAngle to rotate towards (the next step) it adds the "velocity" onto its former position. However it does not matter what that is set at. It can be 1, 10 or 100. It always rotates as fast as it can:
newAngle = currentAngle[s] + rotation * (velocity[s]); // Assigns the position of the next step.
When it calculates the newAngle to rotate towards (the next step) it adds the "velocity" onto its former position. However it does not matter what that is set at. It can be 1, 10 or 100. It always rotates as fast as it can:
How long does the servo rotate at any given speed? As near as I can tell, it is for whatever length of time it takes the while loop to iterate, which isn't very long. At the end of the while loop, the servo is going balls-to-the-wall. Which probably isn't what you want.
PaulS:
How long does the servo rotate at any given speed? As near as I can tell, it is for whatever length of time it takes the while loop to iterate, which isn't very long. At the end of the while loop, the servo is going balls-to-the-wall. Which probably isn't what you want.
Are you asking about the time it takes for the servo to go to its target angle? - A split second, which isn't what I want if I, for an example, have the "velocity" at 1.
If it moves 1° every 0,050s, it should rotate 20° after 1 s or 180° after 9s, which it definitely does not.
EDIT:
Is this the while loop you were referring to?
while (currentAngle[s] != targetAngle[s]) // Keep going as long as it hasn't reached its target.
{
// the rest of the loop
}
Are you asking about the time it takes for the servo to go to its target angle?
No, I'm asking how long you let the servo run at speed 1, then at speed 2, then at speed 3, etc. When velocity is 1, that is.
The answer is that you let the servo run at that speed only as long as it takes the while loop to complete and iterate again. Which is, of course, only a few hundred nano-seconds.
PaulS:
No, I'm asking how long you let the servo run at speed 1, then at speed 2, then at speed 3, etc. When velocity is 1, that is.
The answer is that you let the servo run at that speed only as long as it takes the while loop to complete and iterate again. Which is, of course, only a few hundred nano-seconds.
Yes.
Changed the "while" into an "if" and it now works! I think I once changed it into a while function when I for some reason wanted it out of the "loop()" function. Either way, it's fixed now. Thank you very much! I'll return if any other problem appears!