/*Example sketch to control a stepper motor 
  with DRV8825 stepper motor driver, AccelStepper library 
  and Arduino: number of steps or revolutions. 
  More info: https://www.makerguides.com */

#include "AccelStepper.h"
#include "Servo.h" //******servo*****

//******servo*****
Servo myServo;

int MAX_DISTANCE=140; //** maximal distance of the actuator, mm
int INIT_POS=0;// ** Initial position of the actuator, mm
int FIN_POS=50;// ** Final position of the actuator, m
int ROT_STEP=25; //Number of steps per rotation (200/8=25), as 8 positions are there

int delayMS = 1500;
int d = 10;
//****************


// Define stepper motor connections and motor interface type. Motor interface type must be set to 1 when using a driver:
#define dirPin 2
#define stepPin 3
#define motorInterfaceType 1

// Create a new instance of the AccelStepper class:
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);

//****SERVO*****
//Functions to define the movement of the servomotor im percents and mm's
void SetStrokePerc(float strokePercentage)
{
  if ( strokePercentage >= 1.0 && strokePercentage <= 99.0 )
  {
    int usec = 1000 + strokePercentage * ( 2000 - 1000 ) / 100.0 ;
    myServo.writeMicroseconds( usec );
  }
}

void SetStrokeMM(int strokeReq,int strokeMax)
{
  SetStrokePerc( ((float)strokeReq) / strokeMax );
}
//**********


void setup() {
  // Set the maximum speed in steps per second:
  stepper.setMaxSpeed(1000);

  //******servo*****
  myServo.attach(6);  // attaches the servo on pin 6 to the servo object
  //****************
}

void loop() {
  // Set the current position of ROTATION MOTOR to 0:
  stepper.setCurrentPosition(0);
  int stp_i=0;

  if(digitalRead(8) == HIGH){
    //******servo***** MOVING FORWARD
    int ii = 0;
    for (ii = 1; ii < 99; ii += d )
    {
      Serial.println(ii);
      //SetStrokeMM(j,MAX_DISTANCE); //Moving from 0 to Fin mm
      SetStrokePerc(ii);
      delay(delayMS);
    }
    delay(1000);   // THE TRACER PROPULSED
    //RETURN
    //******servo***** MOVING BACKWARD
    for ( ii = 1; ii < 99; ii -= d )
    {
      //SetStrokeMM(ii,MAX_DISTANCE); //Moving from 0 to Fin mm
      SetStrokePerc(ii);
      delay(delayMS);
    }

   
    if (stp_i<=200){
      while (stp_i<=stp_i+ROT_STEP) {
      // rotating by 25 degrees
        while(stepper.currentPosition() != stp_i)
        {
          stepper.setSpeed(200);
          stepper.runSpeed();
        }

      }
    
      delay(100);
      stp_i=stp_i+ROT_STEP; //Renew step position
    }

  }
}