/* **************************************** *
 * Plotter Stepper Test                     *
 * **************************************** *
 * (C) 2016-10-01 Paolo Iocco               *
 * rev. 0.10                                *
 * **************************************** *
 * Circuit diagram
 * ---------------
 * ***************************************** */

#define LED 13						// LED pin on Arduino Uno
#define zeroX 15					// finecorsa X
#define zeroY 14					// finecorsa Y
#define ena 2						// enable
#define penZUp 83
#define penZDown 110
#define penServoPin 10
#define STEP MICROSTEP
#define STEPDELAY 6
#define STEP_FW 2000
#define STEP_BK 2000

#include <Servo.h>
#include <AFMotor.h>

// Connect a stepper motor with 48 steps per revolution (7.5 degree)/20 piccolino, 64 cinese
AF_Stepper motor_Y(48, 1); 		// Y direction - table
AF_Stepper motor_X(20, 2); 		// X direction - pen
Servo penServo;

void Home() {
	//go to home X
	while (digitalRead(zeroX) == HIGH) {
		motor_X.step(1, BACKWARD, STEP);
	}
	digitalWrite(LED,LOW);
	//go to home Y
	while (digitalRead(zeroY) == HIGH) {
		motor_Y.step(1, BACKWARD, STEP);
	}
	digitalWrite(LED,HIGH);
}
void setup() {
	pinMode(LED, OUTPUT);
	pinMode(zeroX,INPUT_PULLUP);
	pinMode(zeroY,INPUT_PULLUP);
	pinMode(ena,INPUT_PULLUP);
	Serial.begin(9600);         // set up Serial library at 9600 bps
	motor_X.setSpeed(500);  	// 500 rpm
	motor_Y.setSpeed(500);  	// 500 rpm

	Home();
  penServo.attach(penServoPin);
  penServo.write(penZDown);
  delay(100);
  //motor_X.step(1000, FORWARD, DOUBLE);
  //motor_Y.step(100, FORWARD, STEP);
	penServo.write(penZUp);
  delay(100);
}

void PenTest() {
	penServo.write(penZDown);
	delay(1000);
    penServo.write(penZUp);
}

void loop() {

	if (digitalRead(ena) == HIGH) {
		//PenTest();

		Serial.println(" ");
		Serial.println("Stepper test - step function");
		Serial.println("----------------------------");
/*
		//Home();
		Serial.println("Single coil steps");
		digitalWrite(LED,LOW);
		motor_X.step(STEP_FW, FORWARD, SINGLE);
		motor_X.step(STEP_BK, BACKWARD, SINGLE);
		//motor_Y.step(STEP_FW, FORWARD, SINGLE);
		//motor_Y.step(STEP_BK, BACKWARD, SINGLE);
		delay(2000);
    
		//Home();
		Serial.println("Double coil steps");
		digitalWrite(LED,LOW);
		motor_X.step(STEP_FW, FORWARD, DOUBLE);
		motor_X.step(STEP_BK, BACKWARD, DOUBLE);
		//motor_Y.step(STEP_FW, FORWARD, DOUBLE);
		//motor_Y.step(STEP_BK, BACKWARD, DOUBLE);
		delay(2000);

		//Home();
		Serial.println("Interleave coil steps");
		digitalWrite(LED,LOW);
		motor_X.step(STEP_FW, FORWARD, INTERLEAVE);
		motor_X.step(STEP_BK, BACKWARD, INTERLEAVE);
		//motor_Y.step(STEP_FW, FORWARD, INTERLEAVE);
		//motor_Y.step(STEP_BK, BACKWARD, INTERLEAVE);
		delay(2000);
/*
		//Home();
		Serial.println("Microstep coil steps");
		digitalWrite(LED,LOW);
		motor_X.step(STEP_FW, FORWARD, MICROSTEP);
		motor_X.step(STEP_BK, BACKWARD, MICROSTEP);
		//motor_Y.step(STEP_FW, FORWARD, MICROSTEP);
		//motor_Y.step(STEP_BK, BACKWARD, MICROSTEP);
		delay(2000);
*/
		int steps;
		Serial.println(" ");
		Serial.println("Stepper test - onestep function");
		Serial.println("-------------------------------");
/*
		//Home();
		Serial.println("Single coil onesteps");
		digitalWrite(LED,LOW);
		steps=STEP_FW;
		while (steps--){
			motor_X.onestep(FORWARD, SINGLE);
     delay(STEPDELAY);
		}
		steps=STEP_FW;
		while (steps--){
			motor_X.onestep(BACKWARD, SINGLE);
      delay(STEPDELAY);
		}
		/*steps=STEP_BK;
		while (steps--){
			motor_Y.onestep(FORWARD, SINGLE);
     delay(STEPDELAY);
		}
		steps=STEP_BK;
		while (steps--){
			motor_Y.onestep(BACKWARD, SINGLE);
      delay(STEPDELAY);
		}*/
		//delay(2000);

		//Home();
		/*Serial.println("Double coil onesteps");
		digitalWrite(LED,LOW);
		steps=STEP_FW;
		while (steps--){
			motor_X.onestep(FORWARD, DOUBLE);
      delay(STEPDELAY);
		}
		steps=STEP_FW;
		while (steps--){
			motor_X.onestep(BACKWARD, DOUBLE);
      delay(STEPDELAY);
		}
		/*steps=STEP_BK;
		while (steps--){
			motor_Y.onestep(FORWARD, DOUBLE);
      delay(STEPDELAY);
		}
		steps=STEP_BK;
		while (steps--){
			motor_Y.onestep(BACKWARD, DOUBLE);
      delay(STEPDELAY);
		}*/
		//delay(2000);
/*
		//Home();
		Serial.println("Interleave coil onesteps");
		digitalWrite(LED,LOW);
		steps=STEP_FW;
		while (steps--){
			motor_X.onestep(FORWARD, INTERLEAVE);
      delay(STEPDELAY);
		}
		steps=STEP_FW;
		while (steps--){
			motor_X.onestep(BACKWARD, INTERLEAVE);
      delay(STEPDELAY);
		}
		/*steps=STEP_BK;
		while (steps--){
			motor_Y.onestep(FORWARD, INTERLEAVE);
      delay(STEPDELAY);
		}
		steps=STEP_BK;
		while (steps--){
			motor_Y.onestep(BACKWARD, INTERLEAVE);
      delay(STEPDELAY);
		}*/
		delay(2000);

/*
		//Home();
		Serial.println("Microstep coil onesteps");
		digitalWrite(LED,LOW);
		steps=STEP_FW;
		while (steps--){
			motor_X.onestep(FORWARD, MICROSTEP);
      delay(STEPDELAY);
		}
		steps=STEP_FW;
		while (steps--){
			motor_X.onestep(BACKWARD, MICROSTEP);
      delay(STEPDELAY);
		}*/
    penServo.write(penZDown);
		steps=STEP_FW;
		while (steps--){
			motor_Y.onestep(FORWARD, MICROSTEP);
      delay(STEPDELAY);
		}
    penServo.write(penZUp);
		steps=STEP_BK;
		while (steps--){
			motor_Y.onestep(BACKWARD, MICROSTEP);
      delay(STEPDELAY);
		}
		//delay(2000);

	}
}