// Libraries
#include <Servo.h>
 

//Servo
Servo myservo;

int pos = 0;

//Joystick Control
#define Joy_switch 13  

int UD = 0; //js

int LR = 0; //js

int IUP=A0;

int ILR=A1;

int MID = 10; // 10 mid point delta arduino, use 4 for attiny

int LRMID = 0;

int UPMID = 0;

// Connections to A4988 (Stepper Driver)
const int dirPin1 = 2;  // Direction
const int stepPin1 = 4; // Step

const int dirPin2 = 7;
const int stepPin2 = 8;


 
// Motor steps per rotation (Stepper)
const int STEPS_PER_REV = 200;
 
void setup() {

//Stepper Setup
  
  pinMode(stepPin1,OUTPUT); 
  pinMode(dirPin1,OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);

//Servo Setup  

  myservo.attach(9);

  LRMID = analogRead(ILR);

  UPMID = analogRead(IUP);

   pinMode(Joy_switch, INPUT_PULLUP);

   //pinMode(MID, INPUT);

}
void loop() {

//Read Joystick

UD = analogRead(IUP);

LR = analogRead(ILR);

// UP-DOWN

if(UD < UPMID - MID){
  // Set motor direction clockwise (HIGH)
  
 digitalWrite(dirPin1,HIGH); 
  
  // Spin motor quarter rotation slowly
  for(int x = 0; x < (STEPS_PER_REV); x++) {
    digitalWrite(stepPin1,HIGH); 
    delayMicroseconds(2000); 
    digitalWrite(stepPin1,LOW); 
    delayMicroseconds(2000); 
  }
}

  if(UD > UPMID + MID){
    // Set motor direction counterclockwise (LOW)
    digitalWrite(dirPin1, LOW);

    // Spin motor quarter rotation slowly
     for(int x = 0; x < (STEPS_PER_REV); x++) {
    digitalWrite(stepPin1,HIGH); 
    delayMicroseconds(2000); 
    digitalWrite(stepPin1,LOW); 
    delayMicroseconds(2000); 
  }
  }
  
  // Pause for one second
 //delay(1000); 

// LEFT-RIGHT
 
  if(LR < LRMID - MID){
  // Set motor direction counterclockwise (LOW)
  digitalWrite(dirPin2,LOW);
 
  
  // Spin motor quarter rotations quickly
  for(int x = 0; x < (STEPS_PER_REV / 4); x++) {
    digitalWrite(stepPin2,HIGH);
    delayMicroseconds(2000); //changed to high was (1000)
    digitalWrite(stepPin2,LOW);
    delayMicroseconds(2000); //changed to high was (1000)
  }
  }

  if(LR > LRMID + MID){
  // Set motor direction counterclockwise (HIGH)
  digitalWrite(dirPin2,HIGH);


  // Spin motor quarter rotations quickly
  for(int x = 0; x < (STEPS_PER_REV / 4); x++) {
    digitalWrite(stepPin2,HIGH);
    delayMicroseconds(2000); //changed to high was (1000)
    digitalWrite(stepPin2,LOW);
    delayMicroseconds(2000); //changed to high was (1000)
  }
  }
  
  // Pause for one second
  //delay(1000);

    //Joystick Switch
   if (!digitalRead(Joy_switch)) {
    

  // Servo sweep 0 - 180 and back
  
  for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
}

}