// rotation_test for kinetic sand table 10/29/2024

#include <AccelStepper.h>   // accelStepper library make sure your install it in IDE
#include <MultiStepper.h>   // this is part of accelStepper library so no need to install it

    long positions[2]; // Array of desired stepper positions
  
    int Rpos=700;     //  holder value for rotation position
    int Lpos=700;     // holder value for linear position
    float fRpos;      // define floating rotation position variabl
    float fLpos;      // define floating linear position variable

    int Rspeed=400;  //  speed for rotation
    int Lspeed=400;  // speed for linear
    long OneRotation = 16000;     // steps for one rotation  microstep 8 (1600)  x 200 steps/rev * 200 teeth/20 teeth

//**************************************************************************************************************************
    // change LinearRange to find how far the center is from the end - experiment
    long LinearRange = 9900;    // set this for linear range from switch to center, depends on your track, find by trial (mine 14")
    //**********************************************************************************************************************

    float radTOstep=(OneRotation/(3.141592*2));  // convert one rotation in steps to radians

//  pin declaration
    const int dirPinL = 3;    // linear stepper direction
    const int stepPinL = 4;   // linear stepper clock

    const int dirPinR = 6;    //rotational stepper direction
    const int stepPinR = 5;   // rotational stepper clock

     const int LinEnd = 7;   //    pin for the end of track  optosensor  HIGH when unblocked

    const int beepers = 46;   // pin for beeper

  // Define a stepper type and the pins it will use
    #define motorInterfaceType 1    // type of driver for steppers in a accelstepper
    AccelStepper stepperR(motorInterfaceType, stepPinR, dirPinR);
    AccelStepper stepperL(motorInterfaceType, stepPinL, dirPinL);
    MultiStepper steppers;   // add steppers to multistepper  stepper[0] is rotation  stepper[1]  is linear



void setup() {

  Serial.begin(115200);          
          
// Configure each stepper
  stepperR.setMaxSpeed(Rspeed);   // set maximum speed for each stepper
  stepperL.setMaxSpeed(Lspeed);

 
  // Then give them to MultiStepper to manage
  steppers.addStepper(stepperR);
  steppers.addStepper(stepperL);
  
  stepperL.setCurrentPosition(0);   // set current position to 0 linear
  stepperR.setCurrentPosition(0);   // set current position to 0 rotation

  pinMode(beepers, OUTPUT);     // set up beeper pin and mode
  digitalWrite (beepers, LOW);  // make sure beeper pin is low so it doesn't beep on startup


  pinMode(LinEnd, INPUT);    // set up opto pin for input

    beeper(1000);   //  beep to say we are starting
    delay(1000);    // wait a second
      Serial.println("   set up done    "); // send message to serial that setup is complete
}

void loop() {

        Serial.println("   start move    ");  // send message to serial saying we are starting

// time to move

  while(digitalRead(LinEnd) == HIGH)  // find end with switch  HIGH not blocked
    {
      Serial. print ("running to end     ");
      Serial. println (digitalRead(LinEnd));   // print out status of opto, 1= not blocked
      stepperL.setSpeed(-600);  // move slowly towards the end
      stepperL.runSpeed();
    }

  while(digitalRead(LinEnd) == LOW)  // backtrack to get away from switch a bit and stop
    {
      Serial. print ("backing up towards to center   "); 
      Serial. println (digitalRead(LinEnd));   // print out status of opto, 1= not blocked
      stepperL.setSpeed(400);  // move backwards to the middle
      stepperL.runSpeed();
    }

// since multistepper uses position, it needs to know where the zero position is, we set that at the end of the track for now
stepperL.setCurrentPosition(0);   // zero at end 
stepperR.setCurrentPosition(0);   // aero the rotation too, not necessary though

  Serial. print ("heading to center   "); 
  stepperL.setSpeed(-600);  // move slowly towards the end
  positions[0] = 0;    //move back two rotations (from (-linear range) to (+linear range)
  positions[1] = LinearRange;                 // move to end
  steppers.moveTo(positions);
  steppers.runSpeedToPosition();    // Blocks until all are in position

beeper(1000);
Serial. println ("Should be at center   "); 
  while(1){}  // pause program here

}

void beeper(int duration)  //routine for making the beeper beep  for "duration" time in ms
{
digitalWrite (beepers, HIGH);   // turn on beeper
delay(duration);                // wait a time in ms 
digitalWrite (beepers, LOW);    // turn off beeper
}