// https://wokwi.com/projects/362524913933364225
// https://forum.arduino.cc/t/vex-motor-code-help-need/1088689

# include <Servo.h>
const byte upLimit_Pin   = 2;
const byte DownLimit_Pin = 3;
const byte StopBtn_Pin   = 4;
const byte servo_Pin     = 7;

const byte upLED_Pin     = 8;
const byte downLED_Pin   = 9;
Servo aServo;

boolean printed = false;

void setup(){
  Serial.begin(115200);
  Serial.println("\nhello world.\n");

  pinMode(upLimit_Pin  , INPUT_PULLUP);
  pinMode(DownLimit_Pin, INPUT_PULLUP);
  pinMode(StopBtn_Pin,   INPUT_PULLUP);

  pinMode(upLED_Pin,   OUTPUT);
  pinMode(downLED_Pin, OUTPUT);

  aServo.attach(servo_Pin);
  aServo.write(90);  
}

int servoState = 1;
int lastServoState = 1;
void loop() {
  if (!digitalRead(upLimit_Pin)) {
    servoState = -1;
    //Serial.println("               gonna go DOWN now! ");
    }
  

  if (!digitalRead(DownLimit_Pin)) {
    servoState = 1;
    //Serial.println("               gonna UP now. ");
  }

  if (!digitalRead(StopBtn_Pin)) {
    servoState = 0;
    //Serial.println("               gonna stop now. ");
  }
  printStateOnChange();
}


void printStateOnChange() {
  // check if variable "servoState" has changed 
  // its value since last call of this function
  if (lastServoState != servoState) {
    // if it has rEALLY changed its value 
    lastServoState = servoState; // update lastServoState

    if (servoState == -1) {
      Serial.println("going DOWN now.");    
    }

    if (servoState == 1) {
      Serial.println("going UP now!");    
    }

    if (servoState == 0) {
      Serial.println("stopped.");    
    }
  }

// servo and limit LEDs
  static unsigned long lastTwitch;
  if (millis() - lastTwitch < 50) return;
  lastTwitch = millis();

  static int position = 90;
  position -= servoState << 2;
  position = constrain(position, 0, 180);
  if (position == 0) position = 5;
  if (position == 180) position = 175;

  digitalWrite(upLED_Pin, position < 25);
  digitalWrite(downLED_Pin, position > 155);
  
  aServo.write(position);
}


//