#include <Servo.h>
Servo servothumb; // Define thumb servo
Servo servoindex; // Define index servo
Servo servomajeure;
Servo servoringfinger;
Servo servopinky;
Servo servowrist;
Servo servobiceps;
Servo servoshoulder;
Servo servoomoplat;

void setup() {
servothumb.attach(2); // Set thumb servo to digital pin 2
servoindex.attach(3); // Set index servo to digital pin 3
servomajeure.attach(4);
servoringfinger.attach(5);
servopinky.attach(6);
servowrist.attach(7);
servobiceps.attach(8);
servoshoulder.attach(9);
servoomoplat.attach(10);
}

void loop() { // Loop through motion tests
allopen(); // Example: allopen
delay(4000); // Wait 4000 milliseconds (4 seconds)
handclose();
delay(4000);
allopen();
delay(2000);
shoulderalone();
delay(1000);
}

// Motion routines for handopen, handclose, victory...
void allopen() {
servothumb.write(0);
servoindex.write(0);
servomajeure.write(0);
servoringfinger.write(0);
servopinky.write(0);
servowrist.write(0);
servobiceps.write(0); //Never more then (90 degree)
servoshoulder.write(110); //Never more then (130 degree)
servoomoplat.write(0);
}

void handclose() {
servothumb.write(180);
servoindex.write(180);
servomajeure.write(180);
servoringfinger.write(180);
servopinky.write(180);
servowrist.write(180);
servobiceps.write(85); //Never more then (90 degree)
servoshoulder.write(90); //Never more then (130 degree)
servoomoplat.write(180);
}

void shoulderalone() {
servothumb.write(0);
servoindex.write(0);
servomajeure.write(0);
servoringfinger.write(0);
servopinky.write(0);
servowrist.write(90);
servobiceps.write(85); //Never more then (90 degree)
servoshoulder.write(130); //Never more then (130 degree)
servoomoplat.write(0);
}