/*
 Controlling a servo position using a potentiometer (variable resistor)
 by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

 modified on 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Knob
*/

#include <Servo.h>

Servo myservo;  // create servo object to control a servo

const int RedButtonPin = 12; 
const int GreenButtonPin = 11; 
int move_out = 12;  
int move_in = 11;
unsigned long time_value  = 0;
unsigned long moveStartTime = 0;
int potpin = 0;  // analog pin used to connect the potentiometer
int val;    // variable to read the value from the analog pin
int RedButtonState;            // the current reading from the input pin
int lastRedButtonState = LOW;  // the previous reading from the input pin
int GreenButtonState;            // the current reading from the input pin
int lastGreenButtonState = LOW;  // the previous reading from the input pin
unsigned long RedlastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long GreenlastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 100;    // the debounce time; increase if the output flickers
byte startAngle = 0;
byte stopAngle = 180;
byte currentAngle = 180;


void setup() {
  Serial.begin(115200); // Any baud rate should work
  Serial.println("Hello Arduino\n");  
  pinMode(2, INPUT_PULLUP); 
  pinMode(3, INPUT_PULLUP); 
  pinMode(4, INPUT_PULLUP); 
  pinMode(5, INPUT_PULLUP); 
  pinMode(6, INPUT_PULLUP); 
  pinMode(7, INPUT_PULLUP); 
  pinMode(8, INPUT_PULLUP); 
  pinMode(9, INPUT_PULLUP); 
  pinMode(11, INPUT_PULLUP); 
  pinMode(12, INPUT_PULLUP); 
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
  myservo.attach(10);  // attaches the servo on pin 9 to the servo object
  currentAngle = 180;
  myservo.write(currentAngle);  // sets the servo position according to the scaled 
  time_value = ((digitalRead(2)<<7) |(digitalRead(3)<<6) |(digitalRead(4)<<5) |(digitalRead(5)<<4) |(digitalRead(6)<<3) |(digitalRead(7)<<2) |(digitalRead(8)<<1) |(digitalRead(9)<<0))*100;
  delay(2000);
  digitalWrite(LED_BUILTIN, HIGH);
}

void loop() {
  int RedReading = digitalRead(RedButtonPin);
  if (RedReading != lastRedButtonState) {
    RedlastDebounceTime = millis();
  }
  if ((millis() - RedlastDebounceTime) > debounceDelay) {
    if (RedReading != RedButtonState) {
      RedButtonState = RedReading;
      if (RedButtonState == HIGH) {
 startAngle = 180;
 stopAngle = 0;
 moveStartTime = millis();
Serial.println("release_jaw");
////        release_jaw();
      }
    }
  }
  lastRedButtonState = RedReading;
  if (RedReading == 0) {
  int GreenReading = digitalRead(GreenButtonPin);
  lastGreenButtonState = GreenReading;
  if (GreenReading != lastGreenButtonState) {
    GreenlastDebounceTime = millis();
  }
  if ((millis() - GreenlastDebounceTime) > debounceDelay) {
    if (GreenReading != GreenButtonState) {
      GreenButtonState = GreenReading;
      if (GreenButtonState == HIGH) {
 startAngle = 0;
 stopAngle = 180;
 moveStartTime = millis();
  Serial.println("grab_jaw");
//        grab_jaw();
      }
    }
  }
  lastGreenButtonState = GreenReading;
  }
  time_value = ((digitalRead(2)<<7) |(digitalRead(3)<<6) |(digitalRead(4)<<5) |(digitalRead(5)<<4) |(digitalRead(6)<<3) |(digitalRead(7)<<2) |(digitalRead(8)<<1) |(digitalRead(9)<<0)) * 100;
  unsigned long move_progress = millis() - moveStartTime;
  if (move_progress <= (time_value + moveStartTime))
  {
    currentAngle = map(move_progress, 0, (time_value + moveStartTime), startAngle, stopAngle);
  }
  myservo.write(currentAngle);                  // sets the servo position according to the scaled value
  Serial.print(currentAngle, DEC);
  Serial.print(" ");
  Serial.println(time_value, DEC);
delay(100);
}