Here's the servo handler, probably has no bearing on the problem, but you never know...
void xyzServos() {
if (zSteps > 0 || (xSteps + y1Steps) < 500) tMult = 2000; // Change acceleration profile to suit Z(B)
else tMult = 1000; // stepper motor which stalls below 3ms
max1 = max(xSteps, y1Steps);
max2 = max(y1Steps, zSteps);
totalSteps = max(max1, max2);
mx = (double)xSteps/(double)totalSteps;
my1 = (double)y1Steps/(double)totalSteps;
my2 = (double)y2Steps/(double)totalSteps;
mz = (double)zSteps/(double)totalSteps;
x = mx;
y1 = my1;
y2 = my2;
z = mz;
laserOn = false;
if (cut > 0 || laserTime > 0) {
IOX.write(0x0200, OUTPORT); // Turn lamp & fan ON
lampState = 0x0200;
}
if (cut == 0 && laserTime > 0) digitalWrite(lpower, HIGH);
for (int i = 0; i < totalSteps; i++) {
x += mx;
if (x >= 1 && mx != 0 && xStop == false){ // X-stepper control
x -= 1.0;
if (xDir == true) {
if (xStepIdx == 0) xStepIdx = 4;
xStepIdx--;
}
else {
xStepIdx++;
if (xStepIdx > 3) xStepIdx = 0;
}
if(twoWire[xStepIdx] & 1<<1){
digitalWrite(xctrl1,HIGH);
}
else {
digitalWrite(xctrl1,LOW);
}
if(twoWire[xStepIdx] & 1<<0){
digitalWrite(xctrl2,HIGH);
}
else {
digitalWrite(xctrl2,LOW);
}
}
y1 += my1;
if (y1 >= 1 && my1 != 0 && yStop == false){ // Y1(South)-stepper control
y1 -= 1.0;
if (yDir == true) {
if (y1StepIdx == 0) y1StepIdx = 4;
y1StepIdx--;
}
else {
y1StepIdx++;
if (y1StepIdx > 3) y1StepIdx = 0;
}
if(twoWire[y1StepIdx] & 1<<1){
digitalWrite(yctrl1,HIGH);
}
else {
digitalWrite(yctrl1,LOW);
}
if(twoWire[y1StepIdx] & 1<<0){
digitalWrite(yctrl2,HIGH);
}
else {
digitalWrite(yctrl2,LOW);
}
}
y2 += my2;
if (y2 >= 1 && my2 != 0 && yStop == false){ // Y2(North)-stepper control
y2 -= 1.0;
if (yDir == true) {
if (y2StepIdx == 0) y2StepIdx = 4;
y2StepIdx--;
}
else {
y2StepIdx++;
if (y2StepIdx > 3) y2StepIdx = 0;
}
if(twoWire[y2StepIdx] & 1<<1){
digitalWrite(y2ctrl1,HIGH);
}
else {
digitalWrite(y2ctrl1,LOW);
}
if(twoWire[y2StepIdx] & 1<<0){
digitalWrite(y2ctrl2,HIGH);
}
else {
digitalWrite(y2ctrl2,LOW);
}
}
z += mz;
if (z >= 1 && mz != 0){ // Z(B)-stepper control
z -= 1.0;
if (zDir == true) {
if (zStepIdx == 0) zStepIdx = 4;
zStepIdx--;
}
else {
zStepIdx++;
if (zStepIdx > 3) zStepIdx = 0;
}
IOX.write(fullWaveB[zStepIdx] + lampState, OUTPORT);
}
if (cut > 0) { // Put lasing code here..
bitIndex = (i - (offset1 + space)) % (space + cut + space + gap - 1); // - 1 makes interval agree with vb HorizSteps value!
if (bitIndex == 0 && i < (totalSteps - offset2)) laserOn = true;
if (bitIndex == cut) laserOn = false;
if (laserOn == true) {
digitalWrite(lpower, HIGH); //rem-out while testing
delay(laserTime);
}
else {
digitalWrite(lpower, LOW);
delay(3); // fastest stable transit to next cutting point
}
}
else {
if (i < totalSteps / 2)
iTimeIdx = i;
else
iTimeIdx = totalSteps - i;
if (iTimeIdx > 180) iTimeIdx = 180;
MsDelay = (tMult*(3-(sin((270+iTimeIdx)*PI/180)))) - 1000;
if (laserTime == 0) delayMicroseconds(MsDelay);
else delay(laserTime);
}
if (digitalRead(xsensors) == HIGH && i > 25) xStop = true;
if (digitalRead(ysensors) == HIGH && i > 25) yStop = true;
if (xStop == true || yStop == true) digitalWrite(lpower, LOW); // cut laser power if any limit reached
if (xStop == true && yStop == true) i = totalSteps; // bomb-out of loop if both limits reached
}
digitalWrite(lpower, LOW);
//IOX.write(0x0000, OUTPORT); // Turn lamp & fan OFF
}