 // degree step sizes. Motors with finer step sizes are available but driving them in 
 // quickly using sub-micron steps is a limiting factor. If you want to get up and running 
 // in the shortest time, 0.9 degree motors are suggested. 
-float fullStep[numAxes]={0.9,0.9,0.9}; //In degrees
+float fullStep[maxAxes]={0.9,0.9,0.9,0.9}; //In degrees
 
 // disableWhenStationary
 // bool to tell the system whether or not a motor should be disabled when the stage isn't moving.
 // Disabling will reduce noise but can cause the motors to move to the nearest full step when the
 // power is switched off. Perhaps it makes make sense to do this in X and Y but not Z. 
-bool disableWhenStationary[numAxes]={0,0,0};
-
-
-//------------------------------------------------------------------------------------------------
+bool disableWhenStationary[maxAxes]={0,0,0,0};
+
+
+
+
+//------------------------------------------------------------------------------------------------
+// * Motor control DIO lines
+// Alter the following based on how your system is wired. 
+
+// stepOut
+// One pulse at these pins moves the motor by one step (or one micro-step)
+byte stepOut[maxAxes]={22,24,26,0}; //Set these to the step out pins (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[maxAxes]={23,25,27,0}; //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[maxAxes]={28,29,30,0}; //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[numAxes]={&stepperX, &stepperY, &stepperZ};
+
+
+
+
+//------------------------------------------------------------------------------------------------
+// * Outputs
+// The following are pin definitions of controller outputs. These signal information to the user
+
+byte beepPin=9; //Set this to 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};
+
+
+
+
+
+//------------------------------------------------------------------------------------------------
+// PS3 Controller speed settings
+// The following settings define how the PS3 controller will control the stage. If you don't have a
+// PS3 controller, you can ignore the following 
+
 // * Speed modes (hat-stick)
 //
 // There will be four (coarse to fine) speeds which may be selected via the two shoulder buttons 

// 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. 




// Acceleration in X, Y, and Z
unsigned long DPadAccel[numAxes]={1.0E4, 1.0E4, 1.0E4};

// * moveTo speeds
//
// The moveToTarget() function is executed when the user travels to a right-button set point
// or responds to a serial command. It does this using the settings described below. The minimum
// step size and motor RPM are reported to the serial terminal on bootup.
float moveToStepSize=1.0/2.0;
unsigned int moveToSpeed[maxAxes]={1600,1600,1200,1600}; 
unsigned int moveToAccel[maxAxes]={1.0E4,1.0E4,1.0E4,1.0E4};
//------------------------------------------------------------------------------------------------





//---------------------------------UNLIKELY TO NEED TO CHANGE THESE-------------------------------
// The following settings can be changed, but you're unlikely to need to do so. 
byte coarseFine=2; //boot up in a fine mode
short hatStickThresh=40; //Ensure we don't get motion for stick values smaller than the following threshold

// curve
// Curve is the non-linear mapping value to convert analog stick position to speed. The idea
// is to make the conversion from analog stick value to motor pulse rate logarithmic. 
// 0 is linear. See the fscale function for details. 
float curve[4]={-7,-6,-5,-4};
//------------------------------------------------------------------------------------------------



//---------------------------------NON USER GLOBALS-----------------------------------------------
//The following variables aren't user settings. They are counters, etc 

long values[maxAxes]; // array holding values for all the received fields from the seria1 port

// 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][maxAxes] = {{0,0,0,0},
                                          {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};


// SPEENMAT 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];


// stagePosition
// The current stage position in microns. 
float stagePosition[maxAxes]={0,0,0,0}; 


// stepperPreviousPos
