/*
################################################################################
# By DrCrowller                                                                #
#                                                                              #
# Feel free to copy, modify and do whatever you want with this project         #
# Be aware that the use of many variables may consume more memory than wanted  #
#                                                                              #
# Created : 7 Apr. 2022                                                        #
################################################################################
*/

#include <Servo.h>

Servo arm; // Create a "Servo" object called "arm"
Servo arm1;
Servo arm2;
Servo arm3;
Servo arm4;
Servo arm5;
Servo arm6;
Servo arm7;
float pos = 0.0; // Variable where the arm's position will be stored (in degrees)
float step = 1.0; // Variable used for the arm's position step

void setup()
{
  pinMode(A1, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode
  pinMode(A2, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode
  pinMode(A3, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode
  pinMode(A4, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode
  pinMode(A5, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode
  pinMode(A0, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode

  arm.attach(2); // Attache the arm to the pin 2
  arm.write(pos); // Initialize the arm's position to 0 (leftmost)
  arm1.attach(3); // Attache the arm to the pin 2
  arm1.write(pos); // Initialize the arm's position to 0 (leftmost)
  arm2.attach(4); // Attache the arm to the pin 2
  arm2.write(pos); // Initialize the arm's position to 0 (leftmost)
  arm3.attach(5); // Attache the arm to the pin 2
  arm3.write(pos); // Initialize the arm's position to 0 (leftmost)
  arm4.attach(6); // Attache the arm to the pin 2
  arm4.write(pos); // Initialize the arm's position to 0 (leftmost)
  arm5.attach(9); // Attache the arm to the pin 2
  arm5.write(pos); // Initialize the arm's position to 0 (leftmost)
  arm6.attach(10); // Attache the arm to the pin 2
  arm6.write(pos); // Initialize the arm's position to 0 (leftmost)
  arm7.attach(11); // Attache the arm to the pin 2
  arm7.write(pos); // Initialize the arm's position to 0 (leftmost)
}

void loop()
{

  if (!digitalRead(A1)) // Check for the yellow button input
  {
    if (pos>0) // Check that the position won't go lower than 0°
    {
      arm.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm1.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }
  if (!digitalRead(A3)) // Check for the yellow button input
  {
    if (pos>0) // Check that the position won't go lower than 0°
    {
      arm2.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm3.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }
  if (!digitalRead(A5)) // Check for the yellow button input
  {
    if (pos>0) // Check that the position won't go lower than 0°
    {
      arm4.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm5.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm6.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm7.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }

  if (!digitalRead(A2)) // Check for the blue button input
  {
    if (pos<180) // Check that the position won't go higher than 180°
    {
      arm.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm1.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }
if (!digitalRead(A4)) // Check for the blue button input
  {
    if (pos<180) // Check that the position won't go higher than 180°
    {
      arm2.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm3.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }
  if (!digitalRead(A0)) // Check for the blue button input
  {
    if (pos<180) // Check that the position won't go higher than 180°
    {
      arm4.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm5.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm6.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
      arm7.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }
}