#include “AccelStepper.h” // Include AccelStepper library
#include <Servo.h> // Include Servo library

// AccelStepper Setup for stepper 1 (bottom stepper) and stepper 2 (top stepper)
AccelStepper stepper1(AccelStepper::DRIVER, 4, 3);
AccelStepper stepper2(AccelStepper::DRIVER, 5, 6);

// Stepper Travel Variables
long Travel1, Travel2; // Used to store number of steps to take to get to new position for stepper 1 and stepper 2
int move_finished, move_finished2 = 1; // Used to check if move is completed
long initial_homing, initial_homing2 = -1; // Used to Home Stepper at startup
double joint_length = 4.6; // Bar-linkage length in inches
double dx, dy; // Stores distance to translate in x and y directions
double prev_t1; //Store previous angular position dtheta1 before dtheta1 is updated
double prev_t2; //Store previous angular position dtheta2 before dtheta1 is updated
double dtheta1 = M_PI; //angular position for stepper 1; by default set to dtheta1 = pi rad (starting position)
double dtheta2 = M_PI / 2; //angular position for stepper 2; by default set to dtheta2 = pi/2 rad (starting position)
int x_val, y_val; //analog readings from joy-stick for x/y axis.

int MS1 = 7; //Declare microstepping pins
int MS2 = 8;
int MS3 = 9;
int MS4 = 10;

// pushbutton pin
const int buttonPin = 2; //button pin reading input from servo button on controller
// servo pin
const int servoPin = 13;
//Reading button state of buttonPin
int buttonState;

Servo servo1;

//create a variable to store a counter and set it to 0
int counter = 0; //counter to keep track of position of servo

void setup() {

delay(5); // Wait for EasyDriver wake up

servo1.attach (servoPin); //Attach servo to pin 13

// Set up the pushbutton pins to be an input:
pinMode(buttonPin, INPUT_PULLUP);

// Set Max Speed and Acceleration of each Steppers at startup for homing
stepper1.setMaxSpeed(1000.0); // Set Max Speed of Stepper (Slower to get better accuracy)
stepper1.setAcceleration(1000.0); // Set Acceleration of Stepper
stepper1.setCurrentPosition(0);

stepper2.setMaxSpeed(1000.0); // Set Max Speed of Stepper (Slower to get better accuracy)
stepper2.setAcceleration(1000.0); // Set Acceleration of Stepper
stepper2.setCurrentPosition(0);

pinMode (MS1, OUTPUT); //Set microstepping pins as OUTPUT and HIGH
pinMode (MS2, OUTPUT);
pinMode (MS3, OUTPUT);
pinMode (MS4, OUTPUT);
digitalWrite (MS1, HIGH);
digitalWrite (MS2, HIGH);
digitalWrite (MS3, HIGH);
digitalWrite (MS4, HIGH);
}

void loop() {
#ifdef ServoYes //Run ServoCode if ServoYes is defined
buttonState = digitalRead(buttonPin); //read buttonpin either HIGH or LOW
if (buttonState == LOW) //if reading is LOW, increase the counter. When counter=0, keep servo at 10 degrees. When counter is 1 (button pressed again), turn servo to 40 degrees (push the shaft up)
{ // If button is pressed again (which means counter = 2), bring the servo back to default position of 10 degrees and set counter = 0.
counter++;
}
if (counter == 0) { //
servo1.write (10);
}
else if (counter == 1) {
servo1.write(40);
}
else if (counter == 2) {
servo1.write (10);
counter = 0;
}
#endif

x_val = map(analogRead(A2), 0, 1023, 1023, 0); // read joystick x-axis values; map x-values of joystick so they match the orientation of joy-stick controller
y_val = analogRead(A3); //read joystick y-axis values

if ((x_val > 500 && x_val < 520)) { //Ignore any unintentional minor movement of joystick in x-dir near the centre by setting dx = 0
dx == 0;
}
else {
dx = (x_val – 512) / 111.3; //When real movement is detected outside the 490<x<520 range, calculate the actual travel distance in x-direction.
} //111.3 is the conversion factor to change 1 point signal change to travel distance in inches. This was a quick approximation and can be improved by later groups.

if ((y_val > 500 && y_val < 520)) { //Ignore any unintentional minor movement of joystick near the centre by setting dy = 0
dy == 0;
}
else {
dy = (y_val – 512) / 111.3; //When real movement is detected outside the 490<x<520 range, calculate the actual travel distance in x-direction.
} //111.3 is the conversion factor to change 1 point signal change to travel distance in inches. This is an approximation and should be improved/recalculated, if needed, in the future.

if ((dx == 0) and (dy == 0)) { //If both dx and dy are calculated to be 0 (joystick at the centre), set angular positions of motor 1 and motor 2 to 0 and
dtheta1 = 0;
dtheta2 = 0;
}
else {
prev_t1 = dtheta1; //Otherwise use prev_t1/prev_t2 to store previous angular positions of motors.
prev_t2 = dtheta2;
dtheta2 = acos((square(joint_length + dx) + square(joint_length + dy) – 2 * square(joint_length)) / (2 * square(joint_length))); //Calculate new angular positions with inverse kinematics equations
dtheta1 = atan2(joint_length + dy, joint_length + dx) – 0.5 * dtheta2;
}
Travel1 = ceil (dtheta1 / 0.003926991); //Convert dtheta1/dtheta2 in to the number of steps. Each step is 1/1600 of a revolution = 2*pi/1600 = 0.003926991 (in rad).
Travel2 = ceil (dtheta2 / 0.003926991); //Store steps for motor 1 and motor 2, in Travel1 and Travel2, respectively.

if ( (x_val < 500) || (x_val > 520)) { // Check if values are in the operational range of x-direction potentiometer
move_finished = 0; // Set variable for checking move of the Stepper

if (Travel1 >= -400 || Travel1 <= 1000) { // Put boundaries on how far the absolute step position can be of motor 1, and if both conditions are satisfied, then move motor 1
stepper1.moveTo(Travel1); // Set new moveto position of Stepper
}
}

if ( (y_val < 500) || (y_val > 520)) { // Check if values are in the operational range of y-direction potentiometer
move_finished2 = 0; // Set variable for checking move of the Stepper

if (Travel2 >= -400 || Travel2 <= 1000) { // Put boundaries on how far the absolute step position can be of motor 2, and if both conditions are satisfied, then move motor 2
stepper2.moveTo(Travel2); // Set new moveto position of Stepper
}
}

if (Travel1 >= -400 && Travel1 <= 1000) {
// Check if the Stepper 1 has reached desired position
if ((stepper1.distanceToGo() != 0)) {
stepper1.run(); // Move Stepper 1 into position
}

// If move is completed display message on Serial Monitor
if ((move_finished == 0) && (stepper1.distanceToGo() == 0)) {
move_finished = 1; // Reset move variable for stepper 1
}
}

if (Travel2 >= -400 && Travel2 <= 1000) {
// Check if the Stepper 2 has reached desired position
if ((stepper2.distanceToGo() != 0)) {
stepper2.run(); // Move Stepper 2 into position
}

// If move is completed display message on Serial Monitor
if ((move_finished2 == 0) && (stepper2.distanceToGo() == 0)) {
move_finished2 = 1; // Reset move variable for stepper 2
}
}
}