Manually Specified Positions with Arduino
Standing (and lying down) are the first easy motions we can accomplish with the dog, since both of these motions can be accomplished with each leg being in the same position. If we know the standing position setpoints for each servo, we can simply command each servo to travel to the required position all at the same time.
With an Arduino, and a function moveServo() that takes a servo ID, position setpoint, and desired speed as arguments, the robot can be commanded to stand with the following code:
void stand(int speed){
// Move shoulders to flat-ish position
moveServo(1, 500, speed);
moveServo(4, 500, speed);
moveServo(7, 500, speed);
moveServo(10, 500, speed);
// Thighs
moveServo(2, 350, speed); //LHS
moveServo(11, 350, speed); //LHS
moveServo(5, 674, speed); //RHS
moveServo(8, 674, speed); //RHS
// Knees
moveServo(3, 233, speed); //LHS
moveServo(12, 233, speed); //LHS
moveServo(6, 791, speed); //RHS
moveServo(9, 791, speed); //RHS
}And the equivalent “down” function:
void down(int speed){
// Shoulders
moveServo(1, 500, speed);
moveServo(4, 500, speed);
moveServo(7, 500, speed);
moveServo(10, 500, speed);
// Thighs
moveServo(2, 512, speed);
moveServo(11, 512, speed);
moveServo(5, 512, speed);
moveServo(8, 512, speed);
// Knees
moveServo(3, 450, speed);
moveServo(12, 450, speed);
moveServo(6, 574, speed);
moveServo(9, 574, speed);
}
Moving Individual Servos
The moveServo() function uses the setPosition() member function from Pololu’s Arduino library for the XYZrobot Smart Servo A1-16. This setPosition() function takes two arguments, desired setpoint, which should be between 0-1023 (512 being position of 0 degrees), and playtime, which defines how long the movement should take in units of hundredth’s of a seconds. Note that the playtime argument is of type uint8_t, so the possible values are integers between 0-255.
In order to write the custom moveServo() function that takes a Servo ID as an argument, XYZrobotServo objects must be created for each servo so that they can be controlled individually. Then, the moveServo() function must be able to call the appropriate object’s setPosition() function.
In the below moveServo() definition, this is accomplished in a way written to demonstrate the use of C pointers. The code could should be refactored to use an array and to remove the use of pointers.
// Set up a servo object, specifying what serial port to use
// and what ID number to use.
XYZrobotServo servo1(servoSerial, 1);
XYZrobotServo servo2(servoSerial, 2);
XYZrobotServo servo3(servoSerial, 3);
XYZrobotServo servo4(servoSerial, 4);
XYZrobotServo servo5(servoSerial, 5);
XYZrobotServo servo6(servoSerial, 6);
XYZrobotServo servo7(servoSerial, 7);
XYZrobotServo servo8(servoSerial, 8);
XYZrobotServo servo9(servoSerial, 9);
XYZrobotServo servo10(servoSerial, 10);
XYZrobotServo servo11(servoSerial, 11);
XYZrobotServo servo12(servoSerial, 12);
void moveServo(int ServoID, int position, int speed){
switch (ServoID){
case 1:
curServo = &servo1;
break;
case 2:
curServo = &servo2;
break;
case 3:
curServo = &servo3;
break;
case 4:
curServo = &servo4;
break;
case 5:
curServo = &servo5;
break;
case 6:
curServo = &servo6;
break;
case 7:
curServo = &servo7;
break;
case 8:
curServo = &servo8;
break;
case 9:
curServo = &servo9;
break;
case 10:
curServo = &servo10;
break;
case 11:
curServo = &servo11;
break;
case 12:
curServo = &servo12;
break;
}
curServo->setPosition(position, 75);
}Combining It All Together
Use the following Arduino code (with the XYZservoRobot library installed) to interactively control the dog through the computer’s Serial Monitor.
// This sketch only writes data to the servos; it does not
// receive anything.
#include <XYZrobotServo.h>
// On boards with a hardware serial port available for use, use
// that port. For other boards, create a SoftwareSerial object
// using pin 10 to receive (RX) and pin 11 to transmit (TX).
#ifdef SERIAL_PORT_HARDWARE_OPEN
#define servoSerial SERIAL_PORT_HARDWARE_OPEN
#else
#include <SoftwareSerial.h>
SoftwareSerial servoSerial(10, 11);
#endif
// Set up a servo object, specifying what serial port to use and
// what ID number to use.
XYZrobotServo servo1(servoSerial, 1);
XYZrobotServo servo2(servoSerial, 2);
XYZrobotServo servo3(servoSerial, 3);
XYZrobotServo servo4(servoSerial, 4);
XYZrobotServo servo5(servoSerial, 5);
XYZrobotServo servo6(servoSerial, 6);
XYZrobotServo servo7(servoSerial, 7);
XYZrobotServo servo8(servoSerial, 8);
XYZrobotServo servo9(servoSerial, 9);
XYZrobotServo servo10(servoSerial, 10);
XYZrobotServo servo11(servoSerial, 11);
XYZrobotServo servo12(servoSerial, 12);
const uint8_t playtime = 75;
int Position, servoID, commandID;
XYZrobotServo *curServo;
void setup()
{
// Turn on the serial port and set its baud rate.
servoSerial.begin(9600);
Serial.begin(115200);
}
void printCommandList(){
Serial.println("\n\n\n1 - Control by ID\n2 - Straighten Shoulders\n3 - 90 deg. Knees\n4 - Hip Stand Pos\n5 - Stand\n6 - Down\n7 - Pushup");
}
void moveServo(int ServoID, int position, int speed){
switch (ServoID){
case 1:
curServo = &servo1;
break;
case 2:
curServo = &servo2;
break;
case 3:
curServo = &servo3;
break;
case 4:
curServo = &servo4;
break;
case 5:
curServo = &servo5;
break;
case 6:
curServo = &servo6;
break;
case 7:
curServo = &servo7;
break;
case 8:
curServo = &servo8;
break;
case 9:
curServo = &servo9;
break;
case 10:
curServo = &servo10;
break;
case 11:
curServo = &servo11;
break;
case 12:
curServo = &servo12;
break;
}
curServo->setPosition(position, 75);
}
void displayMoveToByID(){
int servoID, position, speed;
Serial.println("Enter Servo ID to change: ");
while (Serial.available() < 1){
servoID = Serial.parseInt();
}
delay(50);
Serial.read();
Serial.print("Enter position for Servo ");
Serial.print(servoID);
Serial.println(": ");
while (Serial.available() < 1){
position = Serial.parseInt();
}
delay(50);
Serial.read();
Serial.print("Moving to position ");
Serial.println(position);
moveServo(servoID, position, 75);
}
void down(int speed){
// Shoulders
moveServo(1, 500, speed);
moveServo(4, 500, speed);
moveServo(7, 500, speed);
moveServo(10, 500, speed);
// Thighs
moveServo(2, 512, speed);
moveServo(11, 512, speed);
moveServo(5, 512, speed);
moveServo(8, 512, speed);
// Knees
moveServo(3, 450, speed);
moveServo(12, 450, speed);
moveServo(6, 574, speed);
moveServo(9, 574, speed);
}
void stand(int speed){
// Shoulders
moveServo(1, 500, speed);
moveServo(4, 500, speed);
moveServo(7, 500, speed);
moveServo(10, 500, speed);
// Knees
moveServo(3, 233, speed);
moveServo(12, 233, speed);
moveServo(6, 791, speed);
moveServo(9, 791, speed);
// Thighs
moveServo(2, 350, speed);
moveServo(11, 350, speed);
moveServo(5, 674, speed);
moveServo(8, 674, speed);
}
void pushup(int numDances){
for(int i=0; i<numDances; i++){
stand(200);
delay(1000);
down(200);
delay(1000);
}
}
void loop()
{
printCommandList();
Serial.println("Enter command ID: ");
while (Serial.available() < 1){
commandID = Serial.parseInt();
}
delay(50);
Serial.read();
switch (commandID){
case 1:
displayMoveToByID();
break;
case 2:
Serial.println("Flatting Shoulders");
moveServo(1, 500, 100);
moveServo(4, 500, 100);
moveServo(7, 500, 100);
moveServo(10, 500, 100);
break;
case 3:
Serial.println("Moving knees");
moveServo(3, 233, 100);
moveServo(12, 233, 100);
moveServo(6, 791, 100);
moveServo(9, 791, 100);
break;
case 4:
Serial.println("Moving hips");
moveServo(2, 350, 100);
moveServo(11, 350, 100);
moveServo(5, 674, 100);
moveServo(8, 674, 100);
break;
case 5:
Serial.println("Standing (Quickly)");
stand(20);
break;
case 6:
Serial.println("Going Down");
down(20);
break;
case 7:
Serial.println("Doing 10 Pushups");
pushup(10);
break;
}
}