/* 
Author: F. Lionetto
Created on: June 23rd, 2014

The sketch inizializes the position of the two stepper motors, move the laser away from the x and z micro switches, and then waits for commands sent through the USB port.

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.

The stepper motors have two different speeds, one for non-precise and one for precise positioning, respectively.

ASCII printable characters:
+ ---> 43;
0 ---> 48;
1 ---> 49;
2 ---> 50;
3 ---> 51.

The only allowed commands (checked before and after being sent through the USB port) are:
+10 ---> move one step forward along x;
+20 ---> move one step backward along x
+30 ---> move at the opposite edge of the silicon sensor (along x);
+40 ---> move back;
+01 ---> move one step forward along z;
+02 ---> move one step backward along z;
+03 ---> do not do anything;
+04 ---> do not do anything.

Distances in microns.

For Hans410 sensor:
- the laser is initially on the strips corresponding to Beetle channels 216/220;
- +30 brings the laser on the strips corresponding to the Beetle channels 16/20;
- +40 brings the laser on the strips corresponding to the Beetle channels 216/220.
*/

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

// Stepper motor properties.
const int stepsPerRevolution = 200;
const int singleStep = 5;
const int speedFast = 50;
const int speedSlow = 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;

// Sensor properties.
char sensor[] = "Hans410";

// Scan properties.
// Maximum distance from the x micro switch (!!!)
// const int maxStepsX = 16000;
// Maximum distance from the z micro switch (!!!)
// const int maxStepsZ = 5400;

// Hans410 sensor.
// Move the laser 3.60 cm (7200 steps) away from the x micro switch
const int stepsFromXSwitch = 7200;
// Move the laser 1.80 cm (3600 steps) away from the z micro switch
const int stepsFromZSwitch = 3600;

// Hans320 sensor.
// Move the laser 5.40 cm (10800 steps) away from the x micro switch
// const int stepsFromXSwitch = 10800;
// Move the laser 1.80 cm (3600 steps) away from the z micro switch
// const int stepsFromZSwitch = 3600;

// ATLAS sensor.
// Move the laser 5.30 cm (10600 steps) away from the x micro switch
// const int stepsFromXSwitch = 10600;
// Move the laser 0.85 cm (1700 steps) away from the z micro switch
// const int stepsFromZSwitch = 1700;

// 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;
// Byte read from the USB port.
byte byteUSB;
// 1 if the byte read from the USB port is +.
boolean commandReceived = false;
// 1 if the byte read from the USB port is the first after +.
boolean xCommandReceived = false;
// 1 if the byte read from the USB port is the second after +.
boolean zCommandReceived = false;
int moveX = 0;
int moveZ = 0;

void stepAction(int moveX, int moveZ) {
  if (moveX == 1) {
    myMotorX->step(1,FORWARD,DOUBLE);
    Serial.println("Moved forward along x.");
  }
  else if (moveX == 2) {
    myMotorX->step(1,BACKWARD,DOUBLE);
    Serial.println("Moved backward along x.");
  }
  else if (moveX == 3) {
   myMotorX->step(1900,BACKWARD,DOUBLE);
   Serial.println("Moved at the opposite edge of the silicon sensor (along x)."); 
  }
  else if (moveX == 4) {
   myMotorX->step(1900,FORWARD,DOUBLE);
   Serial.println("Moved back.");
   }
  if (moveZ == 1) {
    myMotorZ->step(1,FORWARD,DOUBLE);
    Serial.println("Moved forward along z.");
  }
  else if (moveZ == 2) {
    myMotorZ->step(1,BACKWARD,DOUBLE);
    Serial.println("Moved backward along z.");
  }
  else if (moveZ == 3) {
    Serial.println("Nothing done.");
  }
  else if (moveZ == 4) {
    Serial.println("Nothing done.");
  }
}

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

  AFMS.begin();  
  
  myMotorX->setSpeed(speedFast);  
  myMotorZ->setSpeed(speedFast);
  
  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.");
    
  myMotorX->step(stepsFromXSwitch,BACKWARD,DOUBLE);
  myMotorZ->step(stepsFromZSwitch,FORWARD,DOUBLE);
    
  Serial.println("Laser moved away from the x and z micro switches.");
    
  myMotorX->setSpeed(speedSlow);
  myMotorZ->setSpeed(speedSlow);
  
  // myMotorX->release();
  // myMotorZ->release();
}
  
void loop() {
  if (Serial.available()) {
   byteUSB = Serial.read();
   Serial.println(byteUSB);
   if ((byteUSB != 43) && (byteUSB !=48) && (byteUSB != 49) && (byteUSB != 50) && (byteUSB != 51) && (byteUSB != 52)) {
     Serial.println("Invalid command received.");
   }
   else if ((commandReceived == false) && (byteUSB == 43)) {
     commandReceived = true;
     Serial.println("Valid command received.");
     xCommandReceived = false;
     zCommandReceived = false;
     moveX = 0;
     moveZ = 0;
   }
   else if ((commandReceived == true) && (xCommandReceived == false)) {
     moveX = byteUSB - 48;
     Serial.println(moveX);
     xCommandReceived = true;
     Serial.println("Command for x direction received.");
   }
   else if ((commandReceived == true) && (zCommandReceived == false)) {
     moveZ = byteUSB - 48;
     Serial.println(moveZ);
     zCommandReceived = true;
     Serial.println("Command for z direction received.");
   }
  }
  else if ((commandReceived == true) && (xCommandReceived == true) && (zCommandReceived == true)) {
   stepAction(moveX,moveZ);
   commandReceived = false;
  } 
}