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

#include <AccelStepper.h>
#include <MultiStepper.h>

    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=2000;  //  speed for rotation
    int Lspeed=600;  // speed for linear
    long OneRotation = 16000;     // steps for one rotation  microstep 8 (1600)  x 200 steps/rev * 200 teeth/20 teeth
    long LinearRange = -14000;    // 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 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);
  stepperL.setMaxSpeed(Lspeed);

  stepperR.setAcceleration(20);
  stepperL.setAcceleration(20);
 
  // 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
  // put your setup code here, to run once:

  pinMode(beepers, OUTPUT);
  digitalWrite (beepers, LOW);

    beeper(1000);
    delay(1000);
      Serial.println("   set up done    "); 
}

void loop() {

        Serial.println("   start move    "); 
// time to move


  positions[0] = OneRotation;   //rotation   spin once
  positions[1] = 0;             // move to end
  steppers.moveTo(positions);
  steppers.runSpeedToPosition(); // Blocks until all are in position
    delay(1000);

  positions[0] = (-OneRotation);    //move back two rotations (from (-linear range) to (+linear range)
  positions[1] = 0;                 // move to end
  steppers.moveTo(positions);
  steppers.runSpeedToPosition();    // Blocks until all are in position
    delay(1000);

}

void beeper(int duration)  //routine for making the beeper beep  for "duration" time in ms
{
digitalWrite (beepers, HIGH);
delay(duration);
digitalWrite (beepers, LOW);
Serial.println(duration);
}