/*
  robot-arm-basic
  Door J. Snijder
  6-4-2023
  Bestaande sketch voor 1 joystick_2 motor aangepast
  Gewijzigde pinouts en 2e joystick toegevoegd.

  6-4-2023 toevoegen gripper rotatie servo toegevoegd
  Gripper rotatiemotor keert na loslaten joystick terug naar beginpositie !
*/

//Standard libraries aanroepen
#include <Stepper.h>
#include <Servo.h>
#include <ezButton.h>


// Constanten, variabelen en pinverbindingen definiëren:
#define VRY_PIN      A3 // Arduino-ingangspen aangesloten op VRY-pen (joystick Y-as)
#define SERVO_Y_PIN  12  // Arduino-uitgangspen aangesloten op servomotor

#define SW_PIN   A5  // Arduino-pin aangesloten op SW-pin (switch-pin van de joystick module)

// Definiëren servo commando's/waarden
#define COMMAND_NO     0x00
#define COMMAND_LEFT   0x01 // Servo draait tegen de klok in
#define COMMAND_RIGHT  0x02 // Servo draait met de klok mee
//#define COMMAND_KLEM     0x04 // wordt nog niet gebruikt, nodig voor klem
//#define COMMAND_LOS   0x08 // wordt nog niet gebruikt, nodig voor klem

#define LEFT_THRESHOLD  400 //
#define RIGHT_THRESHOLD 800
//#define KLEM_THRESHOLD    400 // wordt nog niet gebruikt, nodig voor klem
//#define LOS_THRESHOLD  800 // wordt nog niet gebruikt, nodig voor klem

#define UPDATE_INTERVAL 50 // Snelheid waarmee de servo beweegt, default was 100

ezButton button(SW_PIN);
Servo yServo;  // maak een servo-object om servo 1 te besturen
// Servo xServo;  // maak een servo-object om servo 2 te besturen

int yValue = 0 ; // Om de waarde van de Y-as op te slaan
// int xValue = 0 ; // Om de waarde van de X-as op te slaan
int yAngle = 90; // de middenpositie van servo Y
// int yAngle = 90; // de middenpositie van servo X
int command = COMMAND_NO;

unsigned long lastUpdateTime = 0;



const int stepsPerRevolution = 64;  // verander dit zodat het past bij het aantal stappen per omwenteling voor de motor
// voor 28BYJ-48 motoren is dit 64 steps per omwenteling



// initialiseren van de stepper bibliotheek voor alle steppers:
Stepper small_stepper1(stepsPerRevolution, 8, 10, 9, 11);
Stepper small_stepper2(stepsPerRevolution, 0, 2, 1, 3);
Stepper small_stepper3(stepsPerRevolution, 4, 6, 5, 7);


void setup() {
  /*
    Serial.begin(9600) ;
  */
  yServo.attach(SERVO_Y_PIN);

  button.setDebounceTime(50); // set debounce time to 50 milliseconds

  // instellen van de snelheid van de motoren:
  small_stepper1.setSpeed(300);    // set first stepper speed
  small_stepper2.setSpeed(300);   // set second stepper speed
  small_stepper3.setSpeed(300);   // set third stepper speed
}


void loop() {
  // Servo button loop
  button.loop(); // MUST call the loop() function first

  // Motor aansturing
  int sensorReading = analogRead(A0); // Waarde uitlezen van joystick_1 X-as

  if (sensorReading < 490) {
    small_stepper1.step(1);  // step links
  }
  if (sensorReading > 540) {
    small_stepper1.step(-1);  // step rechts
  }

  int sensorReading2 = analogRead(A1); // Waarde uitlezen van joystick_1 Y-as

  if (sensorReading2 < 490) {
    small_stepper2.step(1);  // step vooruit
  }
  if (sensorReading2 > 540) {
    small_stepper2.step(-1);  // step achteruit
  }

  int sensorReading3 = analogRead(A2); // Waarde uitlezen van joystick_2 X-as

  if (sensorReading3 < 490) {
    small_stepper3.step(1);  // step omhoog
  }
  if (sensorReading3 > 540) {
    small_stepper3.step(-1);  // step omlaag
  }


  // Servo aansturing
  // button.loop(); // MUST call the loop() function first

  int yValue = analogRead(VRY_PIN); // servo read analog Y analog values
  yServo.write(yAngle); // rotate servo motor

  if (millis() - lastUpdateTime > UPDATE_INTERVAL) {
    lastUpdateTime = millis() ;

    // read analog X and Y analog values
    // yValue = analogRead(VRY_PIN); // *****

    // converts the analog value to commands
    // reset commands
    command = COMMAND_NO;

    // check up/down commands
    if (yValue < LEFT_THRESHOLD)
      command = command | COMMAND_LEFT;
    else if (yValue > RIGHT_THRESHOLD)
      command = command | COMMAND_RIGHT;


    if (command & COMMAND_LEFT) {
      yAngle--;
    }

    if (command & COMMAND_RIGHT) {
      yAngle++;
    }
  }

  if (button.isPressed()) {
    yAngle = 90; // the center position of servo
  }

  yServo.write(yAngle); // rotate servo motor

}