 #include <Wire.h> //Seems to be needed on some systems
 
 
+
+//------------------------------------------------------------------------------------------------
+// Some global definitions we want to set up before moving on 
+//
+// Define pins for LCD display (http://learn.adafruit.com/character-lcds/wiring-a-character-lcd)
+LiquidCrystal lcd(7, 6, 5, 4, 3, 2 );
+const byte maxAxes=4; //None of our stages will ever have more than 4 axes. 
+HardwareSerial* SerialComms;  //pointer to stage comms serial object
+
+
+
+
 //------------------------------------------------------------------------------------------------
 // * Enable/disable major OpenStage functions 
 //
+bool doGamePad=1; //Set to 1 to enable PS3 DualShock as an input device
+bool doLCD=1;     //Set to 1 to enable LCD character display
+
+
+
+//------------------------------------------------------------------------------------------------
+// * Serial communications
+//
+// You have three options for serial comms: 
+// 1) Control stage via PC serial port and receive optional debug information via USB (virtual 
+//    serial port). The latter is available by the verbose option in some of the functions. 
+// 2) Control stage via USB (virtual serial) but receive no debug info. 
+// 3) Disable serial comms for stage control. 
+//
+// These three possibilities are controlled by altering the following two variables:
 bool doSerialInterface=1; //Set to 1 to communicate with the stage via a PC serial port. 
-bool controlViaUSB=0; //Optional bool to control via USB. See setup() for serial port IDs.
-bool doGamePad=1; //Set to 1 to enable PS3 DualShock as an input device
-bool doLCD=1; //Set to 1 to enable LCD character display
+bool controlViaUSB=0;     //Set to 1 to control via USB. See setup() for serial port IDs.
+#define HARDWARE_SERIAL_PORT Serial1 //If using PC serial port, this is the ID of the MEGA's hardware serial line 
+
+
+
 
 
 

// will be updated whenever pollPS3 is called. Note that stepperPreviousPos relates to stepper 
// motor counts (which are integers) not the actual stage position (which a float and measured 
// in microns).
long stepperPreviousPos[3]={0,0,0}; 



// thisStep
// How far the last step has pushed the stage in microns. This is used to update the stage postion.
float thisStep[numAxes][4];


// SPEEDMAT
// This matrix stores the motor speed for each hat-stick value at each speed mode. These numbers
// are calculated once in the setup function and never change during execution. Pre-calculating 
// them is a good way of improving performance during hat-stick motions. 
float SPEEDMAT[128][4];

// hatStickThresh
//Ensure we don't get motion for stick values smaller than the following threshold
short hatStickThresh=40; 






//------------------------------------------------------------------------------------------------
// * DualShock controller variables
//
// The inputs to the PlayStation controller modify the following global variables which
// are used to move the stage. These are not user settings. 

// buttonStageLocations
// The buttons will be used to store 4 different stage locations. We store these in a 2-D
// array, where the first dimension is button and the second is axis:
float buttonStageLocations[4][numAxes] = {{0,0,0},
                                          {0,0,0},
                                          {0,0,0},
                                          {0,0,0}};

// locationStored
// zero indicates that no position was stored. As positions are stored we update these to 1. 
// This is important, as it minimises the possibility of the user double-clicking a button 
// unbound to a location as it may make the stage perform a large motion to back to zero.
bool locationStored[4]={0,0,0,0};



//------------------------------------------------------------------------------------------------
// * Motor control DIO lines
//

// stepOut
// One pulse at these pins moves the motor by one step (or one micro-step)
byte stepOut[numAxes]={22,24,26}; //Ordered X, Y, and Z

// stepDir
// These pins tell the Big Easy Driver to which they connect which direction to rotate the motor
byte stepDir[numAxes]={23,25,27}; //Ordered X, Y, and Z

// enable
// If these pins are low, the motor is enabled. If high it's disabled. Disabling might decrease 
// electrical noise but will lead to errors in absolute positioning. 
byte enable[numAxes]={28,29,30}; //Ordered X, Y, and Z

// The microstep pins.
// These pins define the microstep size. The MS pins on all axes are wired together.
byte MS1=45;
byte MS2=47;
byte MS3=49;



//------------------------------------------------------------------------------------------------
// * AccelStepper 
// Set up three AccelStepper instances for managing the three axes of motion. There are are a 
// couple of other ways to issue steps and keep track of the stage position. 
// 1. Software pulse generation and software stage position counters
// 2. Hardware timer pulse generation and hardware counter for step pulses. 
// AccelStepper does a good job and makes the code more compact so we've gone with it.
AccelStepper stepperX(1, stepOut[0], stepDir[0]); 
AccelStepper stepperY(1, stepOut[1], stepDir[1]); 
AccelStepper stepperZ(1, stepOut[2], stepDir[2]); 

//Make an array of pointers to the AccelStepper instances to make loops possible
AccelStepper *mySteppers[3]={&stepperX, &stepperY, &stepperZ};



//------------------------------------------------------------------------------------------------
// * Outputs
// The following are pin definitions of controller outputs. These signal information to the user

//Misc
byte beepPin=9; //This is the pin to which the Piezo buzzer is connected 

// stageLEDs
// LEDs will light when the stage moves or an axis is reset, 
// *NOTE: Updating the stage LEDs for hat-stick motions requires fast digital writes as motor speed
//        is set in a closed-loop based on hat-stick position. So we set the stage LEDs using 
//        direct port writes. The code expects the LEDS to be on the first 3 pins of port C of the
//        Mega. You can use a different port (or even different pins on that port), but then you will
//        have to change the code in pollPS3. Direct port writes on these pins are only implemented 
//        for the closed-loop hat-stick control code. Elsewhere we use conventional Arduino 
//        digitalWrites, as these are adequate. 
//
// PC0: 37 (X axis)
// PC1: 36 (Y axis)
// PC2: 35 (Z axis)
// PC3: 34 (a good idea to reserve this for a 4th axis, such as a PIFOC)
byte stageLEDs[4]={37,36,35,35};



// Define pins for LCD display (http://learn.adafruit.com/character-lcds/wiring-a-character-lcd)
LiquidCrystal lcd(7, 6, 5, 4, 3, 2 );


//------------------------------------------------------------------------------------------------
// * Serial communiciation via serial shifter (Sparkfun) to allow the PC to interface with the
// the stage. We use the serial shifter rather than the virtual serial port because this makes
// debugging easier. 
long values[numAxes]; // array holding values for all the received fields from the seria1 port
                     // see serialMove(). For some reason this must be a global. 




// The microstep sizes for each speed mode.
float stepSize[4]={1/16.0, 1/8.0, 1/4.0, 1/2.0}; //Defined in fractions of a full step

// * Speed mode (D-pad)
//
// The D-pad will be used for making fixed-distance motions of a high speed. The size of the motions
