/**
 * Logiciel Exofinger, solution gant
 * HumanKit Lab
 * 22/10/20 
 * RPG
 */

#include <Servo.h>

/* Tempo for trajectory loop */
#define TEMPO 10
#define SERVO_PIN 3
/* Pin buttons digital input */
#define INPUT_BUTTON_0_PIN 5
#define INPUT_BUTTON_1_PIN 8
/* Initial position */
#define INIT_POS 0

Servo servoThumb;
int last_inc, last_button0;
int inc;
int position;

void setup()
{
  /* activate pull-up internal resistor */
  pinMode(INPUT_BUTTON_0_PIN, INPUT_PULLUP);
  servoThumb.attach(SERVO_PIN);
  last_inc = inc = -1;
  position = INIT_POS;
  servoThumb.write(position);
  Serial.begin(115200);
}

void loop()
{
  int button0 = digitalRead(INPUT_BUTTON_0_PIN);

  if (button0 == 1)
  {
    if (last_button0 == 0)
    {
      inc = -last_inc;
      last_inc = inc;
    }
  }

  position = position + inc;
  if (position > 180)
    position = 180;
  if (position < 0)
    position = 0;

  servoThumb.write(position);
  delay(TEMPO);

  last_button0 = button0;
}