I'd try refactoring the code.
//Defines motor pin locations. Order is x, y, z, stir.
int motorPins[] = {9, 11, 5, 3};
boolean motorPinStates[] = {false, false, false};
int motorDirPins[] = {8, 10, 4, 2};
//Defines speeds and max speeds in microseconds/step. Order is x, y, z, stir.
int maxSpeeds[] = {100, 100, 40};
//Initializes the direction variable, which holds which direction is positive. Sending the value corresponding to the desired dimension to the direction pin will yield positive motion. Order is x, y, z. We don't give two craps about the stir direction...yet.
boolean posDir[] = {false, false, false};
//Initializes sensor variable, which holds the limit switch sensor values. Order is -x, +x, -y, +y, -z, +z.
int sensors[6];
//Initializes sensor ranges. A number between these two indicates an untouched sensor. Order is same as above.
int sensorMins[6];
int sensorMaxes[6];
//Initializes position variables with units in steps. The negative sensors are defined as zero. Order is x, y, z.
long pos[3];
long posMax[] = {0, 0, 0};
//Initializes movement variables. Order is x, y, z, stir.
long moveTime = 0;
long steps[] = {0, 0, 0, 0};
long stepsTaken[] = {0, 0, 0, 0};
//Initializes state variable(s) and error variable(s).
char state = 'I'; //I for initialization
boolean switchError = true;
boolean motorError = false;
//Counts arrays. Dang it c, just add a method.
#define count(arr) (sizeof(arr)/sizeof(arr[0]))
void setup() {
//Initializes serial monitor.
Serial.begin(9600);
//Initializes motor pins.
for (int i = 0; i <= count(motorPins); i++) {
pinMode(motorPins[i], OUTPUT);
digitalWrite(motorPins[i], LOW);
}
//Initializes motor direction pins.
for (int i = 0; i <= count(motorDirPins); i++) {
pinMode(motorDirPins[i], OUTPUT);
digitalWrite(motorDirPins[i], LOW);
}
//Initializes sensor pins.
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(A4, INPUT);
pinMode(A5, INPUT);
state = 'S'; //S for sensor calibration.
calibrateSensors();
//Calibrates motors if nothing's wrong with the switches.
if (!switchError) {
state = 'R'; //R for range calibration.
calibrateRange(2, 5);
//Todo: add method that moves the z-dir motor up if its last movement was in the negative direction.
calibrateRange(1, 5);
}
}
void loop() {
//Iterates a bunch of times, each time checking whether to take a step.
for (long i = 0; i <= moveTime; i++) {
for (int j = 0; j <= count(steps); j++) {
if (stepsTaken[j]*moveTime <= steps[j]*i) {
doStep(j);
stepsTaken[j]++;
}
}
checkSensors();
delayMicroseconds(1);
}
}
//Calibrates limit switch ranges. Switches must be untouched while doing this. Also able to set a machine error if data is too broad.
void calibrateSensors() {
updateSensors();
//Initializes sensorMins and sensorMaxes to sensors.
for (int i = 0; i < count(sensors); i++) {
sensorMins[i] = sensors[i];
sensorMaxes[i] = sensors[i];
}
//Takes 100 readings and sets the range to the observed range.
for (int i = 1; i <= 100; i++) {
updateSensors();
for (int j = 0; j < count(sensors); j++) {
//Checks if value is less than smallest observed value or larger than largest observed value. If so, adjusts min/max value.
if (sensorMins[j] > sensors[j]) {
sensorMins[j] = sensors[j];
} else if (sensorMaxes[j] < sensors[j]) {
sensorMaxes[j] = sensors[j];
}
}
delay(10);
}
double minVar = 0;
double maxVar = 0;
double minMean = 0;
double maxMean = 0;
//Calculates means
for (int i = 0; i < count(sensorMins); i++) {
minMean += sensorMins[i]/count(sensorMins);
maxMean += sensorMaxes[i]/count(sensorMaxes);
}
//Calculates variances
for (int i = 1; i < count(sensorMins); i++) {
minVar += pow(sensorMins[i]-minMean, 2)/count(sensorMins);
maxVar += pow(sensorMaxes[i]-maxMean, 2)/count(sensorMaxes);
}
//Checks if the variances are over a hard coded limit.
if (minVar > 100 || maxVar > 100 || minMean < 500 || maxMean > 1023) {
switchError = true;
Serial.println("Switch Error");
} else {
switchError = false;
}
}
//Sets the range of where the sliders can move. acc is how many steps you want it to be accurate to. dim is the dimension you want to move it in. 0, 1, 2 --> x, y, z
void calibrateRange(int dim, int acc) {
//Loops through twice: once in the negative direction and once in the positive.
for (int i = 0; i <= 1; i++) {
//Sets position.
pos[dim] = 0;
//Sets direction depending on how many sensors have been contacted by the slider.
digitalWrite(motorDirPins[dim], !(posDir[dim]^(i == 1)));
//Moves until a sensor is more triggered than a feminist.
updateSensors();
while ((sensors[2*dim] > sensorMins[2*dim]-10 && sensors[2*dim] < sensorMaxes[2*dim]+10 && sensors[2*dim+1] > sensorMins[2*dim+1]-10 && sensors[2*dim+1] < sensorMaxes[2*dim+1]+10 && abs(pos[dim]) < 3*maxSpeeds[dim]*pow(10, 5)) || (abs(pos[dim]) < maxSpeeds[dim]*pow(10, 1) && i == 1)) {
//Loops through to do less processes per step.
for (int j = 1; j <= acc; j++) {
doStep(dim);
delayMicroseconds(maxSpeeds[dim]);
}
//Updates sensors and adds to the position.
updateSensors();
pos[dim] += acc;
}
}
posMax[dim] = pos[dim]-acc;
//Sets the posDir variable. Plugging in posDir to the direction pin makes the motor move the head in the positive direction for that motor's axis.
//First statement flips the posDir if the negative sensor is triggered. Otherwise checks for an error.
if (sensors[2*dim] > sensorMins[2*dim]-10 || sensors[2*dim] < sensorMaxes[2*dim]+10) {
posDir[dim] = !posDir[dim];
pos[dim] = 0;
} else if (sensors[2*dim+1] > sensorMins[2*dim+1]-10 || sensors[2*dim+1] < sensorMaxes[2*dim+1]+10) {
posDir[dim] = posDir[dim];
} else {
motorError = true;
Serial.println("Motor error");
}
}
//Updates the sensors variable. C doesn't return arrays lol.
void updateSensors() {
sensors[0] = analogRead(A3);
sensors[1] = analogRead(A2);
sensors[2] = analogRead(A0);
sensors[3] = analogRead(A1);
sensors[4] = analogRead(A5);
sensors[5] = analogRead(A4);
}
//Checks to make sure none of the sensors is triggered. If one is, it sets the position to 0 or the calibration determined max.
boolean checkSensors() {
updateSensors();
for (int i = 0; i <= count(sensors); i++) {
if (sensors[i] < sensorMins[i]-10 || sensors[i] > sensorMaxes[i]+10) {
if (i%2) {
pos[(i-1)/2] = posMax[(i-1)/2];
return true;
} else {
pos[i/2] = 0;
return true;
}
}
}
return false;
}
//Sets the variables such that the head actually moves. fpos is the final position vector measured in steps, dt is the change in time measured in microseconds, and stirSpeed is the speed of the stirrer in microseconds/step.
boolean setPath(long fpos[3], long dt, int stirSpeed, boolean stirDir) {
moveTime = dt;
for (int i = 0; i < count(fpos); i++) {
if (fpos[i] < 0 && fpos[i] > posMax[i]) {
return false;
}
steps[i] = abs(pos[i]-fpos[i]);
stepsTaken[i] = 0;
digitalWrite(motorDirPins[i], !(posDir[i]^(pos[i] < fpos[i])));
if (moveTime < maxSpeeds[i]*steps[i]) {
moveTime = maxSpeeds[i]*steps[i];
}
}
steps[count(steps)-1] = moveTime/stirSpeed;
stepsTaken[count(stepsTaken)-1] = 0;
digitalWrite(motorDirPins[count(motorDirPins)-1], stirDir);
return true;
}
boolean setPath(long fpos[3], long dt, int stirSpeed) {
return setPath(fpos, dt, stirSpeed, true);
}
boolean setPath(long fpos[3], long dt, boolean stirDir) {
return setPath(fpos, dt, maxSpeeds[count(maxSpeeds)-1], stirDir);
}
boolean setPath(long fpos[3], long dt) {
return setPath(fpos, dt, true);
}
boolean setPath(long dt, int stirSpeed, boolean stirDir) {
long fpos[] = {0, 0, 0};
return setPath(fpos, dt, stirSpeed, stirDir);
}
boolean setPath(long dt, int stirSpeed) {
return setPath(dt, stirSpeed, true);
}
boolean setPath(long dt, boolean stirDir) {
return setPath(dt, maxSpeeds[count(maxSpeeds)-1], stirDir);
}
boolean setPath(long dt) {
return setPath(dt, maxSpeeds[count(maxSpeeds)-1], true);
}
//Makes the motor at some dimension take one step. Does not include time delay, so this must be included elsewhere.
void doStep(int dim) {
motorPinStates[dim] = !motorPinStates[dim];
digitalWrite(motorPins[dim], motorPinStates[dim]);
}