/*
 *
 * LINEAR ACTUATOR & FLIPPER PROGRAM CONTROL!


further modified 19 May 2016, Thayer ENGS21 proj, Wes Kendrick.
Using grove shield,


modified even more for the linear actuator 5/25/2016

CONFIG!!!
servo wired to D7
switch to D5

//potentiometer is wired into A0. Note that the cable, as it is currently configured, is screwed up, need jumper cables
//brown == ground; red = power, orange == signal.


*/

#include <Servo.h>
Servo myservo;  // create servo object to control a servo

//CONSTANTS FOR EZ CONFIGURATION!!!

const int DI_button_pin=8;
const int servo = 7;

//put the linear actuator motor control cable in the grove slot labled D2. Don't switch up the orientation
const int purple = 2;
const int grey = 3;



bool go;
bool reached;
int flipperTriggered;

int counter;

void setup() {
  myservo.attach(7);  // attaches the servo on pin 7 to the servo object

  pinMode(DI_button_pin,INPUT);
  digitalWrite(DI_button_pin,HIGH); ///////that super strange line that fixes stuff

  pinMode(purple,OUTPUT);
  pinMode(grey,OUTPUT);
  Serial.begin(9600);
  myservo.write(160);
  digitalWrite(purple,LOW);
  digitalWrite(grey,LOW);
  go = false;
  reached = false;
  counter = 0;
  flipperTriggered = 0;;
}

void loop() {

  int buttonValue=digitalRead(DI_button_pin);
  //Serial.println(buttonValue);


  if(buttonValue==LOW){
    go = true;
    reached = false;
    flipperTriggered = 0;

  }

  int potentiometerValue = analogRead(A0);

  if(go){

    counter++;

    if(counter > 500 && reached == false){
      reached = true;
      flipperTriggered = 1;
    }

    if(potentiometerValue <= 1020 && reached == false){
      digitalWrite(purple,HIGH);
    }
    if(potentiometerValue >= 1000){
      reached = true;
      flipperTriggered = 1;
    }
    if(reached && potentiometerValue >= 100){
      digitalWrite(purple,HIGH);
      digitalWrite(grey,HIGH);
    }
    if(potentiometerValue <=200 && reached == true){
      go = false;
      digitalWrite(purple,LOW);
      digitalWrite(grey,LOW);
      counter = 0;

    }
  }

  Serial.println(myservo.read());

     // flipper control!

  if(flipperTriggered == 1){
    myservo.write(0);
    if(myservo.read() < 5){
      flipperTriggered = 2;
    }
  }

  if(flipperTriggered == 2){
    myservo.write(160);
  }

  delay(1);
}