// Braccio robot is raised up by 26 cm

#include <Braccio.h>
#include <Servo.h>

Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_rot;
Servo wrist_ver;
Servo gripper;

void setup() {  
  //Initialization functions and set up the initial position for Braccio
  //All the servo motors will be positioned in the "safety" position:
  //Base (M1):90 degrees
  //Shoulder (M2): 45 degrees
  //Elbow (M3): 180 degrees
  //Wrist vertical (M4): 180 degrees
  //Wrist rotation (M5): 90 degrees
  //gripper (M6): 10 degrees
  Braccio.begin();
}

void loop() {
  

 //Position neutre M5 à 0°C  
                      //(step delay  M1 , M2 , M3 , M4 , M5 , M6);
 Braccio.ServoMovement(20,          90, 0, 100, 90, 0, 10);
  
  //Wait 1 second
delay(9000);

 //Position neutre M5 à 90°C
                      //(step delay  M1 , M2 , M3 , M4 , M5 , M6);
 Braccio.ServoMovement(20,          90, 0, 100, 90, 90, 10);
  
  //Wait 1 second
 delay(9000);


 //Position neutre M5 à 180°C
                      //(step delay  M1 , M2 , M3 , M4 , M5 , M6);
 Braccio.ServoMovement(20,          90, 0, 100, 90, 180, 10);
  
  //Wait 1 second
delay(9000);



//Position neutre M5 à 0°C  
                      //(step delay  M1 , M2 , M3 , M4 , M5 , M6);
//  Braccio.ServoMovement(20,          90, 0, 100, 90, 0, 10);
  
  //Wait 1 second
//  delay(9000);

//Position entre neutre et haute M5 à 0°C  
                      //(step delay  M1 , M2 , M3 , M4 , M5 , M6);
//  Braccio.ServoMovement(20,          90, 0, 120, 70, 90, 10);
  
  //Wait 1 second
  //delay(9000);


}