// https://docs.arduino.cc/retired/getting-started-guides/Braccio
// https://content.arduino.cc/assets/Braccio%20Quick%20Start%20Guide.pdf

#include <Servo.h>
#include <Braccio.h>
// https://github.com/arduino-libraries/Braccio
#include <BraccioRobot.h>
// https://github.com/stefangs/arduino-library-braccio-robot/tree/master

// Servo objects
Servo base;      // = 11; // M1     90     0 to 180
Servo shoulder;  // = 10; // M2     45     15 to 165
Servo elbow;     // =  9; // M3    180     0 to 180
Servo wrist_ver; // =  6; // M4    180     0 to 180
Servo wrist_rot; // =  5; // M5     90     0 to 180
Servo gripper;   // =  3; // M6     10     10 to 73 (open to closed) 

// SR04
long duration;
long distance;
const int trigPin   = 7;
const int echoPin   = 8;

const int buzzer    = 12; // blue LED
const int ledPin    = 13; // red LED

void setup() {
  Braccio.begin();
  Serial.begin(9600);
  pinMode(buzzer, OUTPUT);
  pinMode(ledPin, OUTPUT);
}

void loop() {
  Serial.println("braccioSafety");
  braccioSafety();
  delay(2000);
  Serial.println("braccioTest");
  braccioTest();
  delay(2000);
  Serial.println("bracciUpright");
  braccioUpright();
  delay(2000);
  Serial.println("braccioTakeTheSponge");
  braccioTakeTheSponge();
  delay(2000);
  // Serial.println("braccioRobot");
  // braccioRobot();
  // delay(2000);
}

void braccioTest() {
  //                  (delay M1, M2, M3, M4, M5, M6)
  Braccio.ServoMovement( 20, 15,  0, 180, 180,  0, 73);
  delay(1000);
  Braccio.ServoMovement( 20, 165, 180,  0,  0, 180, 10);
  delay(1000);
  braccioUpright();
}

void braccioSafety() {
  //                  (delay M1, M2, M3, M4, M5, M6)
  Braccio.ServoMovement( 20, 90, 45, 180, 180, 90, 10);
}

void braccioUpright() {
  //                  (delay M1, M2, M3, M4, M5, M6)
  Braccio.ServoMovement( 20, 90, 90, 90, 90, 90, 73);
}

void braccioTakeTheSponge() {
  //                  (delay M1, M2, M3, M4, M5, M6)
  Braccio.ServoMovement( 20,  0, 45, 180, 180, 90, 10);
  delay(1000);
  Braccio.ServoMovement( 20,  0, 90, 180, 180, 90, 10);
  delay(1000);
  Braccio.ServoMovement( 10,  0, 90, 180, 180, 90, 60);
  delay(1000);
  Braccio.ServoMovement( 20,  0, 45, 180, 45,  0, 60);
  delay(1000);
  Braccio.ServoMovement( 20, 180, 45, 180, 45,  0, 60);
  delay(1000);
  Braccio.ServoMovement( 20,  0, 90, 180, 180, 90, 60);
  delay(1000);
  Braccio.ServoMovement( 20,  0, 90, 180, 180, 90, 10);
}

void braccioRobot() {
  digitalWrite(ledPin, LOW);
  BraccioRobot.init();
  delay(250);

  Position p1, dn, up, p2;

  //    (M1, M2, M3, M4, M5, M6)
  p1.set(90, 90, 45, 45, 90, 20);
  dn.set(90, 90, 22, 22, 90, 70);
  up.set(90, 90, 90, 90, 90, 73);
  p2.set(90, 90, 10, 10, 90, 20);

  // Move to position with specified speed between 20-200 degrees per second
  BraccioRobot.moveToPosition(p1, 100);

  //                                (M1, M2, M3, M4, M5, M6)
  BraccioRobot.moveToPosition(dn.set(90, 90, 10, 10, 90, 20), 100);
  delay(2000);
  //                                (M1, M2, M3, M4, M5, M6)
  BraccioRobot.moveToPosition(p1.set(90, 90, 10, 10, 90, 70), 100);
  delay(2000);
  BraccioRobot.moveToPosition(up, 50);
  delay(2000);
  //                                (M1, M2, M3, M4, M5, M6)
  BraccioRobot.moveToPosition(p1.set(90, 90, 10, 10, 90, 70), 25);
  BraccioRobot.moveToPosition(p2, 50);
  digitalWrite(ledPin, HIGH);
}