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

# include <Servo.h>

Servo aServo;

//void xprintf(char *) {}
void xprintf(char *) {}

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

  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);

  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);

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

int servoState = 1;

void loop()
{
  if (!digitalRead(2)) {
    servoState = -1;
    xprintf("               gonna go DOWN now! ");
  }

  if (!digitalRead(3)) {
    servoState = 1;
    xprintf("               gonna UP now. ");
  }

  if (!digitalRead(4)) {
    servoState = 0;
    xprintf("               gonna stop now. ");
  }

  if (servoState == -1) {
    xprintf("going DOWN now.\n");    
  }

  if (servoState == 1) {
    xprintf("going UP now!\n");    
  }

  if (servoState == 0) {
    xprintf("stopped.\n");    
  }


// 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(8, position < 25);
  digitalWrite(9, position > 155);
  
  aServo.write(position);
}


//