// test a stepper motor with a Pololu A4988 driver board, onboard led will flash at each step
// this version uses delay() to manage timing

#include <Servo.h>

byte directionPin = 8;
byte stepPin = 9;
int numberOfSteps = 200;
byte ledPin = 13;
int pulseWidthMicros = 20;  // microsecondo
int millisbetweenSteps = 10; // milliseconds - or try 100 for slower steps
int statostart;


Servo arm; // Create a "Servo" object called "arm"
float pos = 0.0; // Variable where the arm's position will be stored (in degrees)
float step = 1.0; // Variable used for the arm's position step

void setup() { 
  Serial.begin(9600);
  Serial.println("Starting StepperTest");
  digitalWrite(ledPin, LOW);
  pinMode(directionPin, OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(2, INPUT);

  pinMode(A1, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode
  pinMode(A2, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode

  arm.attach(2); // Attache the arm to the pin 2
  arm.write(pos); // Initialize the arm's position to 0 (leftmost)
}

void loop() { 
    controlled_stepper();
    controlled_servo();
}

void controlled_stepper(){
  statostart=digitalRead(2);
   if(digitalRead(2) == HIGH){
    digitalWrite(directionPin, HIGH);
    for(int n = 0; n < numberOfSteps; n++) {
     digitalWrite(stepPin, HIGH);
     delayMicroseconds(pulseWidthMicros); 
     digitalWrite(stepPin, LOW);
     delay(millisbetweenSteps);
     digitalWrite(ledPin, !digitalRead(ledPin));
   }
    delay(1000);
    digitalWrite(directionPin, LOW);
    for(int n = 0; n < numberOfSteps; n++) {
    digitalWrite(stepPin, HIGH);
    // delayMicroseconds(pulseWidthMicros); // probably not needed
    digitalWrite(stepPin, LOW);
    delay(millisbetweenSteps);
    digitalWrite(ledPin, !digitalRead(ledPin));
  }}
}

void controlled_servo(){
 if (!digitalRead(A1)) // Check for the yellow button input
  {
    if (pos>0) // Check that the position won't go lower than 0°
    {
      arm.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }

  if (!digitalRead(A2)) // Check for the blue button input
  {
    if (pos<180) // Check that the position won't go higher than 180°
    {
      arm.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }
}