#include <Servo.h>

Servo myservo;

int Open = 1800;
int Closed = 2400;

int rackmovetime = 2000;

int timeforballs = 4000;

// DC Motor connections


// NEMA 17 SETUP STUFF
// Define pin connections & motor's steps per revolution
const int dirPin = 4;
const int stepPin = 5;
const int stepsPerRevolution = 200;
int StepperSpeed = 2500;


int firstcolumn = 1;
int loopcount = 0;

///////////////////////////////////////////////////////////////////////

void setup()
{

// Set all the DC motor control pins to outputs
  Serial.begin(9600);
  myservo.attach(3);
  myservo.write(90);

// Declare Nema17 pins as Outputs

  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
}

///////////////////////////////////////////////////////////////////////

void loop(){
  
if (firstcolumn == 1){
// initially put servo in "closed" position
myservo.write(140);


unsigned long startTime = millis();
unsigned long endTime;
endTime = startTime;

//run the rack for "rackmovetime"
while ((endTime - startTime) <= 2000) { 
// do this loop for up to 2000mS or 2 seconds
  RackCW();
endTime = millis();
}

//timeforballs to move
delay(timeforballs);


// First, 4 balls drop to keep machine launching (i<5)
for (int i=0; i<5; i++) stepperopen();
}

// After first column drops:
else {

stepperopen();

unsigned long startTime = millis();
unsigned long endTime;
endTime = startTime;

//run the rack for "rackmovetime"
while ((endTime - startTime) <= 2000) { 
// do this loop for up to 2000mS or 2 seconds
  RackCW();
endTime = millis();
}

delay(rackmovetime);


//timeforballs to move
delay(timeforballs);

for (int i=0; i<6; i++) stepperopen();
}

firstcolumn++;

}



//void loop(){
//
//RackCW();
//}

///////////////////////////////////////////////////////////////////////


void RackCW() {
// Set motor direction clockwise
  digitalWrite(dirPin, LOW);
  
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(StepperSpeed);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(StepperSpeed);
}



void stepperopen(){

myservo.write(140);
delay(Closed);
myservo.write(96.5);
delay(Open);
}





//void speedControl() {
//  // Turn on motors
//  digitalWrite(in1, LOW);
//  digitalWrite(in2, HIGH);
//  
//  // Accelerate from zero to maximum speed
//  for (int i = 0; i < 256; i++) {
//    analogWrite(enA, i);
//    delay(20);
//  }
//  
//  // Decelerate from maximum speed to zero
//  for (int i = 255; i >= 0; --i) {
//    analogWrite(enA, i);
//    delay(20);
//  }
//  
//  // Now turn off motors
//  digitalWrite(in1, LOW);
//  digitalWrite(in2, LOW);
//}