/* 
Author: F. Lionetto
Created on: June 4th, 2014

The sketch inizializes the position of the two stepper motors.

Coordinate system:
- x axis across the strips, forward direction is toward the window, backward direction is toward the door (stepper motor 1);
- y axis along the strips;
- z axis toward the strips, forward direction is down, backward direction is up (stepper motor 2).

The x micro switch is connected to pin 22.
The z micro switch is connected to pin 23.

Distances in microns.
*/

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"

// Stepper motor properties.
const int stepsPerRevolution = 200;
const int singleStep = 5;

// Create the motor shield object with the default I2C address.
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 

// Connect a stepper motor with 200 steps per revolution (1.8 degree) to motor port #1 (M1 and M2).
Adafruit_StepperMotor *myMotorX = AFMS.getStepper(stepsPerRevolution, 1);
Adafruit_StepperMotor *myMotorZ = AFMS.getStepper(stepsPerRevolution, 2);

// Micro switch properties.
const int switchXPin = 22;
const int switchZPin = 23;
int switchXState = 0;
int switchZState = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("This is a test!");

  AFMS.begin();  
  
  myMotorX->setSpeed(50);  
  myMotorZ->setSpeed(50);
  
  pinMode(switchXPin,INPUT);
  pinMode(switchZPin,INPUT);
  
  Serial.println("Inizializing position along x...");

  while (switchXState == LOW) {
    switchXState = digitalRead(switchXPin);
    if (switchXState == LOW) {
    // The stepper motor is not at the origin of the coordinate system.
    myMotorX->step(1,FORWARD,DOUBLE);
    }
  }
  
  Serial.println("Position along x successfully initialized.");
  
  Serial.println("Inizializing position along z...");

  while (switchZState == LOW) {
    switchZState = digitalRead(switchZPin);
    if (switchZState == LOW) {
    // The stepper motor is not at the origin of the coordinate system.
    myMotorZ->step(1,BACKWARD,DOUBLE);
    }
  }
  
  Serial.println("Position along z successfully initialized.");
}
  
void loop() {
}