//Name + Id#

#include <Servo.h>


Servo servo1, servo2;

#define LED1 2
#define LED2 A0
#define SLIDE_SWITCH 9
#define BUTTON1_LEFT 13
#define BUTTON1_RIGHT 12
#define BUTTON2_LEFT A4
#define BUTTON2_RIGHT A2
#define DOWN LOW
#define UP HIGH
int potpin = A5;

//Allows for changes to parameters
float step = 1.0;
int del = 20;
int s2conlow = 135;
int s2conhigh = 180;
int s1conlow = 0;
int s1conhigh = 45;


//Initializes variables
int val1 = 90;
int val2 = 90;
float pos1 = 28.0;
float pos2 = 158.0;
bool b1rprev = UP;
bool b1lprev = UP;
bool b2rprev = UP;
bool b2lprev = UP;


void setup() 
{
  // put your setup code here, to run once:

    servo1.attach(3);
    servo2.attach(5);
    pinMode(BUTTON1_LEFT, INPUT_PULLUP);
    pinMode(BUTTON1_RIGHT, INPUT_PULLUP);
    pinMode(BUTTON2_LEFT, INPUT_PULLUP);
    pinMode(BUTTON2_RIGHT, INPUT_PULLUP);
    pinMode(SLIDE_SWITCH,INPUT);
    servo2.write(val2);
    servo1.write(val1);
}

void loop() 
{
    // put your main code here, to run repeatedly:

  //Potentiometer
  if(digitalRead(SLIDE_SWITCH) == UP)
  {
    digitalWrite(LED1, HIGH);
    digitalWrite(LED2, HIGH);
    val1 = analogRead(potpin);
    val1 = map(val1, 0, 1023, 45, 90);
    val1 = constrain(val1, 45, 90);
    servo1.write(val1);
    val2 = analogRead(potpin);
    val2 = map(val2, 0, 1023, 180, 135);
    val2 = constrain(val2, 135, 180);
    servo2.write(val2);
  }

    //Buttons
  else
  {
    bool b1rcur = digitalRead(BUTTON1_RIGHT);
    bool b1lcur = digitalRead(BUTTON1_LEFT);
    bool b2rcur = digitalRead(BUTTON2_RIGHT);
    bool b2lcur = digitalRead(BUTTON2_LEFT);

    digitalWrite(LED1,LOW);
    digitalWrite(LED2,LOW);

    if (b1rcur == DOWN || b1lcur == DOWN)
    {
      digitalWrite(LED1, HIGH);
    }

    if(b2rcur == DOWN || b2lcur == DOWN)
    {
      digitalWrite(LED2, HIGH);
    }

    if (b1rcur == DOWN && b1rprev == UP)
    {
      if (pos1 < s1conhigh)
      {
        pos1 += step;
      }
    } 

    if (b1lcur == DOWN && b1lprev == UP) 
    {
      if (pos1 > s1conlow)
      {
        pos1 -= step;
      }
    }

    if (b2lcur == DOWN && b2lprev == UP) 
    {
      if (pos2 < s2conhigh)
      {
        pos2 += step;
      }
    }

    if (b2rcur == DOWN && b2rprev == UP) 
    {
      if (pos2 > s2conlow)
      {
        pos2 -= step;
      }
    }

    servo1.write(pos1);
    servo2.write(pos2);

    b1rprev = b1rcur;
    b1lprev = b1lcur;
    b2rprev = b2rcur;
    b2lprev = b2lcur;
  }
  delay(del);
}