/*Motor Control Sketch for SeeSaw Vessel Cradle Control using AccelStepper Library

  Created by James Goddings

*/

#include "AccelStepper.h";
//Library created by Mike McCauley at http://www.airspayce.com/mikem/arduino/AccelStepper
//Used for programming stepper motors using Arduino microprocessors

//Setup Control Variables


AccelStepper stepperX(1, 6, 7); //AccelStepper Setup
//1=Easy Driver Interface
//Arduino Pin 6 Connected to PUL- Pin => Pulse width controls speed of motor
//Arduino Pin 7 Connected to DIR- Pin => Controls motor direction

//Define Limit Switch Pins
const int limit_switch_l = 2;//Left Limit Switch Connected to Pin 2
const int limit_switch_r = 3;//Left Limit Switch Connected to Pin 3

//Define Variables
//long TravelX;//Used to Store the Value X Entered in the Serial Monitor
//int move_finished;//Used to CHeck if Move is Completed
long initial_homing = -1; //Used to Home Stepper at Startup
long find_max = 1; //Used to Find Max Limit at Startup
long max_position;//Used to Set the Maximum Position of the SeeSaw
int variable_speed;//Used to Vary Speed of SeeSaw Using Potentiometer


void setup() {
  //Start the Serial Monitor at 9600 Baud Rate
  Serial.begin(115200);

  //Pull Up Resistor Debouncing on Each Switch
  pinMode(limit_switch_l, INPUT_PULLUP);
  pinMode(limit_switch_r, INPUT_PULLUP);

  //Wait for Easy Driver to Wake
  delay(5);

  //Start the Homing Procedure ofthe Stepper Motor on Startup
  stepperX.setMaxSpeed(100.0);//Set MaxSpeed of Stepper Low for Homing
  stepperX.setAcceleration(500.0);//Set Acceleration of Stepper Low for Homing

  Serial.print("Vessel is finding home position");

  stepperX.moveTo(-10000);//Set the Stepper Motor to Move CCW
  while (digitalRead(limit_switch_l)) { //Make the Stepper Move CCW Until the Left Limit Switch is Activated
    stepperX.run();//Start Moving the Stepper Motor
  }
  stepperX.stop();
  stepperX.move(200);
  while (!digitalRead(limit_switch_l)) { //Make the Stepper Move CW Until the Left Limit Switch is Deactivated
    stepperX.run();//Start Moving the Stepper Motor
  }
  stepperX.stop();
  stepperX.setCurrentPosition(0); // this is the new reference point
  Serial.println("Homing Complete");
  Serial.println("");
  Serial.println("Vessel now Seeking Max Position");

  //Start the Max Seeking Procedure ofthe Stepper Motor

  stepperX.moveTo(10000);//more than possible max_position

  while (digitalRead(limit_switch_r)) { //Make the Stepper Move CW Until the Right Limit Switch is Activated
    stepperX.run();//Start Moving the Stepper Motor
  }
  stepperX.stop();
  stepperX.move(-200);//Set the Stepper Motor to Move CCW
  while (!digitalRead(limit_switch_r)) { //Make the Stepper Move CCW Until the Right Limit Switch is Deactivated
    stepperX.run();//Start Moving the Stepper Motor
  }
  stepperX.stop();
  max_position = stepperX.currentPosition();
  Serial.print("Max Seeking Complete, "); Serial.println(max_position);
  Serial.println("");
  Serial.println("Vessel Will Now Start Cycling");
  delay(5000);

}

void loop() {
  variable_speed = map(analogRead(A1), 0, 1023, 300, 3000); //Set Potentiometer Controlled Speed Variable to Map on to Potentiomater Position
  long left_position = 2; //Add a new left position variable just before left limit switch
  long right_position = max_position - 2; //Add a new right position variable just before left limit switch

  stepperX.setMaxSpeed(variable_speed);//Set Max Speed High for SeeSaw
  stepperX.setAcceleration(300.0);//Set Acceleration High for SeeSaw
  //stepperX.setSpeed(variable_speed);
  stepperX.runToNewPosition(left_position);
  delay(50);
  stepperX.runToNewPosition(right_position);

  Serial.println("Running Test");

}