#include <Arduino.h>
#include "BasicStepperDriver.h"
#include <Servo.h>

// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step
#define MOTOR_STEPS 200
#define RPM 100

// Since microstepping is set externally, make sure this matches the selected mode
// If it doesn't, the motor will move at a different RPM than chosen
// 1=full step, 2=half step etc.
#define MICROSTEPS 1

// All the wires needed for full functionality
#define DIR_EXTRUDER 2
#define STEP_EXTRUDER 3

#define DIR_GRIPPERTWIST 4
#define STEP_GRIPPERTWIST 5

#define SERVO_CLIPPER_PIN 6
#define SERVO_TWISTFIX_PIN 7

#define STEPS_PER_ROTATION 1 * MOTOR_STEPS * MICROSTEPS
#define STEPS_PER_ROTATION_GRIPPERTWIST 1 * MOTOR_STEPS * MICROSTEPS

BasicStepperDriver extruder(MOTOR_STEPS, DIR_EXTRUDER, STEP_EXTRUDER);
BasicStepperDriver grippertwist(MOTOR_STEPS, DIR_GRIPPERTWIST, STEP_GRIPPERTWIST);

Servo clipper;
Servo twistfix;

int analogPin = A0;
float timeVal = 0;

void setup() {
  extruder.begin(RPM, MICROSTEPS);
  grippertwist.begin(RPM, MICROSTEPS);

  clipper.attach(SERVO_CLIPPER_PIN);
  twistfix.attach(SERVO_TWISTFIX_PIN);

  delay(1000); // Add a delay to stabilize the motors before starting the sequence
 
void loop() {
  //Extruder - 5 full rotations
  for (int i = 0; i < 28; i++) {
    extruder.move(-STEPS_PER_ROTATION);
  }
  delay(500);

  //Clipper - 90 degrees clockwise and back to 0 degrees
  clipper.write(100);
  delay(500);
  clipper.write(0);
  delay(1000);

  //Grippertwist - 5 full rotations
  //gripper closes
  for (int i = 0; i < 5; i++) {
    grippertwist.move(-STEPS_PER_ROTATION_GRIPPERTWIST);
  }
  delay(1000);

  //Twistfix - back to 0 degrees - release
  twistfix.write(0);
  delay(1000);
  // 

  //twisting
  for (int i = 0; i < 6; i++) {
    grippertwist.move(STEPS_PER_ROTATION);
  }
  delay(1000);

  //Twistfix - back to 90 degrees - close
  twistfix.write(90);
  delay(1000);

  //Gripper opens
  for (int i = 0; i < 5; i++) {
    grippertwist.move(STEPS_PER_ROTATION_GRIPPERTWIST);
  }
  delay(1000);

}