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

The sketch inizializes the position of the two stepper motors, move the laser 5 cm (10000 steps) away from the x micro switch, move the laser 2.5 cm (5000) away from the z micro switch, and then perform a scan for finding the focal length.

Scan for finding the focal length:
- at a given z position, move one step backward along x, wait, move one step backward along x, wait, and so on (enough times to go across two strips);
- go back to the initial position along x; 
- move one step forward along z;
- repeat from the beginning (enough times to be sure to find the focal length, but not that many to touch the silicon sensor);
- again and again.
In the current configuration, scan of 1 mm along x and 1 mm along z.

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;

// Scan properties.
// Total number of steps we need along x.
const int totStepsX = 200;
// Total number of steps we need along y.
const int totStepsZ = 200;
int stepsX = 0;
int stepsZ = 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.");
    
  Serial.println("Moving the laser 5 cm (10000 steps) away from the x micro switch...");
  
  myMotorX->step(10000,BACKWARD,DOUBLE);
  
  Serial.println("Laser moved 5 cm (10000 steps) away from the x micro switch.");

  Serial.println("Moving the laser 2.5 cm (5000 steps) away from the z micro switch...");
  
  myMotorZ->step(5000,FORWARD,DOUBLE);
  
  Serial.println("Laser moved 2.5 cm (5000 steps) away from the z micro switch.");

  myMotorX->setSpeed(5);
  myMotorZ->setSpeed(5);
  
  myMotorX->release();
  myMotorZ->release();
}
  
void loop() {
// Scan for finding the focal length.
  if(stepsZ < totStepsZ) {
    Serial.print("Data acquisition at z = ");
    Serial.println(stepsZ);
    if (stepsX < totStepsX) {
      myMotorX->step(1,BACKWARD,DOUBLE);
      myMotorX->release();
      stepsX = stepsX + 1;
    }
    else if (stepsX == totStepsX) {
      myMotorX->step(totStepsX,FORWARD,DOUBLE);
      myMotorX->release();
      stepsX = 0;
      myMotorZ->step(1,FORWARD,DOUBLE);
      myMotorZ->release();
      stepsZ = stepsZ + 1;
    }
  }
  else {
    Serial.println("Scan for finding the focal length completed.");
  }
}