how to control several servo motors with arduino
The class
#include <Arduino.h>
#include <Servo.h>
class AsyncServo
{
Servo servo;
int pos;
int increment;
int incrementBy = 1;
int updateInterval = 10;
unsigned long lastUpdate;
bool done = false;
public:
AsyncServo(int interval =10, int i = 1)
{
updateInterval = interval;
increment = i;
incrementBy = i;
}
// Attach pin to servo
void Attach(int pin)
{
servo.attach(pin);
}
// Detach from servo
void Detach()
{
servo.detach();
}
// Rotate servo to "Home" ( pos = 0);
void Home()
{
if (pos >= 180)
{
done = false;
increment = -incrementBy;
}
}
// Rotate servo to "End" ( pos = 180 );
void End()
{
if (pos <= 0)
{
done = false;
increment = incrementBy;
}
}
// just set servo to some position between 0 and 180;
void setPosition(uint8_t p) {
done = true;
// pos = p; ????
servo.write(pos);
}
// check if servo done its movement
bool Done()
{
return done;
}
// check if servo is at home position: pos = 0;
bool AtHome()
{
return pos <= 0;
}
// asynchroniuosly rotate motor
void Run()
{
if (done)
return;
if ((millis() - lastUpdate) > updateInterval)
{
lastUpdate = millis();
pos += increment;
servo.write(pos);
Serial.println(pos);
if ((pos >= 180) || (pos <= 0))
{
done = true;
return;
}
}
}
};
class usage on arduino UNO / mega ( for others - check for PWM pins )
AsyncServo s1(100, 10); // slow movement
AsyncServo s2(10, 10); // fast movement
void setup()
{
// put your setup code here, to run once:
Serial.begin(9600);
s.Attach(9);
s.Attach(10);
s1.Home();
s2.Home();
}
void loop()
{
s1.Run(); // servo 1 step
s2.Run(); // servo 2 step
if (s1.Done()) // if servo motor stopped
{
if (s1.AtHome()) //check if its position is Home ( 0 )
{
s1.End(); // tell servo motor to rotate to max ( 180 )
}
else
{
s1.Home(); // tell servo motor to rate back to home ( 0 );
}
}
if (s2.Done()) // if servo motor stopped
{
if (s2.AtHome()) //check if its position is Home ( 0 )
{
s2.End(); // tell servo motor to rotate to max ( 180 )
}
else
{
s1.Home(); // tell servo motor to rate back to home ( 0 );
}
}
}