This is code we'll use to elevate the motor to a static pose, then wiggle both motors back and forth

wiggle_wiggle_wiggle

// this code elevates the robotic arm then makes it wiggle back and forth

#include <Servo.h>

Servo ShoulderServo; // create servo object to control a servo
Servo ElbowServo; // create servo object to control a servo

int shoulderPos; // variable to store the servo position
int elbowPos; // variable to store the servo position

void setup()
{
  ShoulderServo.attach(5); // attaches the servo on pin indicated to the servo object
  ElbowServo.attach(3); // attaches the servo on pin indicated to the servo object

  strike_pose();
}


void loop()
{
  // wiggle the shoulder
  for(shoulderPos = 70; shoulderPos < 90; shoulderPos += 1) {  // rotate in one direction
    ShoulderServo.write(shoulderPos); // tell servo to go to position in variable 'pos'
    delay(15); // waits 15ms for the servo to reach the position
  }
  for(shoulderPos = 90; shoulderPos > 70; shoulderPos -= 1) {  // rotate in the other direction
    ShoulderServo.write(shoulderPos); // tell servo to go to position in variable 'pos'
    delay(15); // waits 15ms for the servo to reach the position
  }

  //wiggle the elbow 
  for(elbowPos = 130; elbowPos < 150; elbowPos += 1) {  // rotate in one direction
    ElbowServo.write(elbowPos); // tell servo to go to position in variable 'pos'
    delay(15); // waits 15ms for the servo to reach the position
  }
  for(elbowPos = 150; elbowPos > 130; elbowPos -= 1) {  // rotate in the other direction
    ElbowServo.write(elbowPos); // tell servo to go to position in variable 'pos'
    delay(15); // waits 15ms for the servo to reach the position
  }
} 

void strike_pose()
{
  shoulderPos = 80; // strike a pose position
  ShoulderServo.write(shoulderPos); // tell servo to go to position in variable 'pos'
  delay(15); // waits 15ms for the servo to reach the position

  elbowPos = 140; // strike a pose position
  ElbowServo.write(elbowPos); // tell servo to go to position in variable 'pos'
  delay(15); // waits 15ms for the servo to reach the position
}