3143- Arm Project

#include <Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
int d1 = 70;

//3, 5, 6, 9, 10, 11

void setup() {
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(9);
  servo5.attach(10);
  servo6.attach(11);
  Serial.begin(9600);
}

void loop() {


  // begin Pos 1
  /////////////////////////////////////////
  jawOpen();
  servo1.write(0);
  servo2.write(90);
  servo3.write(90);
  servo4.write(0);
  servo5.write(90);
  delay(5000);
  /////////////////////////////////////////


  // go to A
  /////////////////////////////////////////
  servo1.write(0);//END
  servo2.write(95);
  delay(d1);
  servo2.write(100);
  delay(d1);
  servo2.write(105);
  delay(d1);
  servo2.write(110);
  delay(d1);
  servo2.write(115);
  delay(d1);
  servo2.write(120);
  delay(d1);
  servo2.write(125);
  delay(d1);
  servo2.write(130);
  delay(d1);
  servo6.write(0); //  <-------
  servo2.write(135);
  delay(d1);
  servo2.write(140);
  delay(d1);
  servo2.write(145);
  delay(d1);
  servo2.write(150);
  delay(d1);
  servo2.write(155);
  delay(d1);
  servo2.write(160);
  delay(d1);
  servo2.write(165);
  delay(d1);
  servo2.write(170);//END
  servo3.write(80);//END
  servo4.write(0);// END
  /////////////////////////////////////////


  // hold bottle
  /////////////////////////////////////////
  delay(1000);
  jawClose();
  delay(2000);
  /////////////////////////////////////////


  // go up while holding
  /////////////////////////////////////////
  servo2.write(165);
  delay(d1);
  servo2.write(160);
  delay(d1);
  servo2.write(155);
  delay(d1);
  servo2.write(150);
  delay(d1);
  servo2.write(145);
  delay(d1);
  servo2.write(140);
  delay(d1);
  servo2.write(135);
  delay(d1);
  servo2.write(130);
  delay(d1);
  servo2.write(125);
  delay(d1);
  servo2.write(120);
  delay(d1);
  servo2.write(115);
  delay(d1);
  servo2.write(110);
  delay(d1);
  servo2.write(105);
  delay(d1);
  servo2.write(100);//END
  delay(100);
  /////////////////////////////////////////


  // go to B and hold
  /////////////////////////////////////////
  delay(1000);
  servo1.write(90);
  delay(1000);
  servo2.write(100);
  delay(d1);
  servo2.write(105);
  delay(d1);
  servo2.write(110);
  delay(d1);
  servo2.write(115);
  delay(d1);
  servo2.write(120);
  delay(d1);
  servo2.write(125);
  delay(d1);
  servo2.write(130);
  delay(d1);
  servo2.write(135);
  delay(d1);
  servo2.write(140);
  delay(d1);
  servo2.write(145);
  delay(d1);
  servo2.write(150);
  delay(d1);
  servo2.write(155);
  delay(d1);
  servo2.write(160);
  delay(d1);
  servo2.write(165);
  delay(d1);
  servo2.write(170);//end
  delay(d1);
  /////////////////////////////////////////


  // release bottle
  /////////////////////////////////////////
  delay(1000);
  jawOpen();
  delay(1000);
  /////////////////////////////////////////


  // go to Pos 1
  /////////////////////////////////////////
  servo2.write(90);
  servo3.write(90);
  servo4.write(0);
  servo5.write(90);
  delay(5000);
  servo1.write(0);
  /////////////////////////////////////////
}

void jawOpen() {
  servo6.write(5);
}
void jawClose() {
  servo6.write(80);
}