#include <Servo.h> // servo library

// https://docs.arduino.cc/retired/getting-started-guides/Braccio/

// insert start and stop angles for robot arm
int angleMIN[]   = {  0,  15, 180, 180,   0, 73};
int angleStart[] = { 90,  45, 180, 180,  90, 10};
int angleMAX[]   = {180, 165,   0,   0, 180, 10};

const int servoPin[] = {8, 7, 6, 5, 4, 3}; // PWM pins
const int servoNum = sizeof(servoPin) / sizeof(servoPin[0]); // number of servos/PWM pins
Servo servo[servoNum]; // declare number of Servo objects/instances
int servoRnd, servoAngle;

void setup() {
  Serial.begin(115200); // for data display
  randomSeed(analogRead(A0)); // for more-random random
  for (int i = 0; i < servoNum; i++) {
    servo[i].attach(servoPin[i]); // attach servos to pins
    servo[i].write(0);
  }
  delay(500);
}

void loop() {
  range(); // show start, minimum, maximum ranges
  // randomoes();
  // dominoes();
}

void range() {
  for (int j = 0; j < 4; j++) {
    for (int i = 0; i < servoNum; i++) {
      switch (j) {
        case 0: servo[i].write(angleStart[i]);  break;
        case 1: servo[i].write(angleMAX[i]); break;
        case 2: servo[i].write(angleStart[i]); break;
        case 3: servo[i].write(angleMIN[i]); break;
        default: break;
      }
    }
    switch (j) {
      case 0: Serial.print("Start "); break;
      case 1: Serial.print("MIN "); break;
      case 2: Serial.print("Start "); break;
      case 3: Serial.println("MAX"); break;
      default: break;
    }
    delay(1);
    delay(1000);
  }
}

void randomoes() {
  servoRnd = (random(6)); // pick a servo pin
  servoAngle = (random(180)); // pick an angle
  servo[servoRnd].write(servoAngle); // put random servo at random angle

  Serial.print("Servo ");
  for (int i = 0; i < servoNum; i++) {
    if (i == servoRnd)
      Serial.print(servoRnd); // display the servo
    else Serial.print("."); // format the display
  }

  Serial.print(" Angle ");
  if (servoAngle < 100) Serial.print(" "); // pad two digit values with a space
  if (servoAngle < 10) Serial.print(" "); // pad one digit values with a space
  Serial.print(servoAngle); Serial.println("\xC2\xB0"); // degree symbol... for simulation
  delay(250); // slow loop to four per second
}

void dominoes() {
  for (int i = 0; i < 6; i++) {
    for (int j = 0; j < 180; j++) {
      servo[i].write(j);
      delay(1);
    }
  }
  delay(100);
  for (int i = 0; i < 6; i++) {
    for (int j = 0; j < 180; j++) {
      servo[5 - i].write(180 - j);
      delay(1);
    }
  }
  delay(100);
}