// track test 11/12/2024
 

// Libraries to include
#include <AccelStepper.h>       // stepper motor control
#include <MultiStepper.h>       // included in AccelStepper, no need to install, but must include this line

// variable declarations

  long positions[2]; // Array of desired stepper positions

  float fRpos;                      // floating variable Rotational position not used in this situation but we will later
  float fLpos=5000;                 // floating variable Linear position you can play with this

  float btime=0;                    // variables used just for fun data
  float etime=0;
  float ttime=0;

//  pin declarations const int
const int dirPinL = 3;    // to stepper driver direction: linear stepper
const int stepPinL = 4;   //to stepper driver clock
const int stepPinR=5;
const int dirPinR=6;

const int LinEnd = 7;     // opto switch input

// Define a stepper type and the pins it will use
  #define motorInterfaceType 1    // type of driver for steppers in accelstepper  type 1 uses STEP and DIRECTION 
  AccelStepper stepperR(motorInterfaceType, stepPinR, dirPinR);  // not used or connected in this sketch
  AccelStepper stepperL(motorInterfaceType, stepPinL, dirPinL);
  MultiStepper steppers;   // define Multistepper as "steppers" and hand control over to multistepper

void setup() 
{
  Serial.begin(115200);   // start serial monitor, make sure your serial monitor is set to the same speed
  // Configure each stepper for maximum speed
  stepperR.setMaxSpeed(2000);     // again doesn't matter since we aren't using this to test arm
  stepperL.setMaxSpeed(2000);
  // add stepper motors to multistepper  steppers[0] is rotation  steppers[1]  is linear
  steppers.addStepper(stepperR);
  steppers.addStepper(stepperL);
  stepperR.setCurrentPosition(0);       // zero out positions, good habit to get into
  stepperL.setCurrentPosition(0);

  pinMode(LinEnd, INPUT);
  Serial.println (digitalRead(LinEnd)); // just to make sure it is working
}
 
void loop()   //  MAIN LOOP
{
      Serial.println("going to end");
      delay(500);           // just cuz
      btime=millis();       // beginning time of movement
      
      while(digitalRead(LinEnd)==HIGH)    // while opto switch not blocked run to end
      {
          stepperL.setSpeed(800);
          stepperL.runSpeed();
      }

      stepperL.setCurrentPosition(0);     // set the end of track to be linear position = 0

      Serial.println("going to middle");
      positions[0]=0;                     // set rotation position to fRpos even though we aren't using rotation
      positions[1]=-fLpos;                // set linear position to Flpos - negative is towards center since + is moving to end and  the end is position=0
      steppers.moveTo(positions);         // give coordinates to multipstepper
      steppers.runSpeedToPosition();      // move to those coordinates
      Serial.println(stepperL.currentPosition());   // just to double check that it moved like it should


etime=millis();                         // end of movement time
ttime=etime-btime;                      // total time of movement
Serial.print("total time:  ");
Serial.print(ttime/1000);               // print time in seconds
Serial.println("seconds");
Serial.print("speed in steps/ms:  ");
Serial.println((2*fLpos)/ttime);        // just for fun see the speed in steps per ms
}