void StepperControl::loadSettings() { // Load motor settings axisX.loadPinNumbers(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN, E_STEP_PIN, E_DIR_PIN, E_ENABLE_PIN); axisY.loadPinNumbers(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN, 0, 0, 0); axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN, 0, 0, 0); axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE; axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE; axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE; loadMotorSettings(); // Load encoder settings loadEncoderSettings(); encoderX.loadMdlEncoderId(_MDL_X1); encoderY.loadMdlEncoderId(_MDL_Y); encoderZ.loadMdlEncoderId(_MDL_Z); encoderX.loadPinNumbers(X_ENCDR_A, X_ENCDR_B, X_ENCDR_A_Q, X_ENCDR_B_Q); encoderY.loadPinNumbers(Y_ENCDR_A, Y_ENCDR_B, Y_ENCDR_A_Q, Y_ENCDR_B_Q); encoderZ.loadPinNumbers(Z_ENCDR_A, Z_ENCDR_B, Z_ENCDR_A_Q, Z_ENCDR_B_Q); encoderX.loadSettings(motorConsEncoderType[0], motorConsEncoderScaling[0], motorConsEncoderInvert[0]); encoderY.loadSettings(motorConsEncoderType[1], motorConsEncoderScaling[1], motorConsEncoderInvert[1]); encoderZ.loadSettings(motorConsEncoderType[2], motorConsEncoderScaling[2], motorConsEncoderInvert[2]); }
StepperControl::StepperControl() { axisX = StepperControlAxis(); axisY = StepperControlAxis(); axisZ = StepperControlAxis(); axisX.label = 'X'; axisY.label = 'Y'; axisZ.label = 'Z'; axisX.loadPinNumbers(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN); axisY.loadPinNumbers(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN); axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN); loadMotorSettings(); }
/** * xDest - destination X in steps * yDest - destination Y in steps * zDest - destination Z in steps * maxStepsPerSecond - maximum number of steps per second * maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second */ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd, bool xHome, bool yHome, bool zHome) { unsigned long currentMillis = 0; unsigned long timeStart = millis(); int incomingByte = 0; int error = 0; // load motor settings loadMotorSettings(); // if a speed is given in the command, use that instead of the config speed if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) { speedMax[0] = xMaxSpd; } if (yMaxSpd > 0 && yMaxSpd < speedMax[1]) { speedMax[1] = yMaxSpd; } if (zMaxSpd > 0 && zMaxSpd < speedMax[2]) { speedMax[2] = zMaxSpd; } axisX.setMaxSpeed(speedMax[0]); axisY.setMaxSpeed(speedMax[1]); axisZ.setMaxSpeed(speedMax[2]); // Load coordinates into axis class long sourcePoint[3] = {0,0,0}; sourcePoint[0] = CurrentState::getInstance()->getX(); sourcePoint[1] = CurrentState::getInstance()->getY(); sourcePoint[2] = CurrentState::getInstance()->getZ(); long currentPoint[3] = {0,0,0}; currentPoint[0] = CurrentState::getInstance()->getX(); currentPoint[1] = CurrentState::getInstance()->getY(); currentPoint[2] = CurrentState::getInstance()->getZ(); long destinationPoint[3]= {0,0,0}; destinationPoint[0] = xDest; destinationPoint[1] = yDest; destinationPoint[2] = zDest; // Load coordinates into motor control axisX.loadCoordinates(currentPoint[0],destinationPoint[0],xHome); axisY.loadCoordinates(currentPoint[1],destinationPoint[1],yHome); axisZ.loadCoordinates(currentPoint[2],destinationPoint[2],zHome); // Prepare for movement storeEndStops(); reportEndStops(); axisX.setDirectionAxis(); axisY.setDirectionAxis(); axisZ.setDirectionAxis(); enableMotors(); // Start movement axisActive[0] = true; axisActive[1] = true; axisActive[2] = true; axisX.checkMovement(); axisY.checkMovement(); axisZ.checkMovement(); Timer1.start(); // Let the interrupt handle all the movements while (axisActive[0] || axisActive[1] || axisActive[2]) { delay(1); axisActive[0] = axisX.isAxisActive(); axisActive[1] = axisY.isAxisActive(); axisActive[2] = axisZ.isAxisActive(); storeEndStops(); // Check timeouts if (axisActive[0] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) { error = 1; } if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) { error = 1; } if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) { error = 1; } // Check if there is an emergency stop command if (Serial.available() > 0) { incomingByte = Serial.read(); if (incomingByte == 69 || incomingByte == 101) { error = 1; } } if (error == 1) { Serial.print("R99 error\n"); Timer1.stop(); axisActive[0] = false; axisActive[1] = false; axisActive[2] = false; } // Periodically send message still active currentMillis++; if (currentMillis % 2500 == 0) { Serial.print("R04\n"); } } Serial.print("R99 stopped\n"); Timer1.stop(); disableMotors(); currentPoint[0] = axisX.currentPoint(); currentPoint[1] = axisY.currentPoint(); currentPoint[2] = axisZ.currentPoint(); CurrentState::getInstance()->setX(currentPoint[0]); CurrentState::getInstance()->setY(currentPoint[1]); CurrentState::getInstance()->setZ(currentPoint[2]); storeEndStops(); reportEndStops(); reportPosition(); return error; }
int StepperControl::calibrateAxis(int axis) { // Load motor settings loadMotorSettings(); //unsigned long timeStart = millis(); bool movementDone = false; int paramValueInt = 0; int stepsCount = 0; int incomingByte = 0; int error = 0; bool invertEndStops = false; int parEndInv; int parNbrStp; // Prepare for movement storeEndStops(); reportEndStops(); // Select the right axis StepperControlAxis calibAxis; switch (axis) { case 0: calibAxis = axisX; parEndInv = MOVEMENT_INVERT_ENDPOINTS_X; parNbrStp = MOVEMENT_AXIS_NR_STEPS_X; invertEndStops = endStInv[0]; break; case 1: calibAxis = axisY; parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y;; parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y; invertEndStops = endStInv[0]; break; case 2: calibAxis = axisZ; parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z; parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z; invertEndStops = endStInv[0]; break; default: Serial.print("R99 Calibration error: invalid axis selected\n"); return 1; } // Preliminary checks if (calibAxis.endStopMin() || calibAxis.endStopMax()) { Serial.print("R99 Calibration error: end stop active before start\n"); return 1; } Serial.print("R99"); Serial.print(" axis "); Serial.print(calibAxis.label); Serial.print(" calibration start"); Serial.print("\n"); // Move towards home calibAxis.enableMotor(); calibAxis.setDirectionHome(); stepsCount = 0; movementDone = false; while (!movementDone && error == 0) { // Check if there is an emergency stop command if (Serial.available() > 0) { incomingByte = Serial.read(); if (incomingByte == 69 || incomingByte == 101) { movementDone = true; error = 1; } } // Move until the end stop for home position is reached if ((!calibAxis.endStopMin() && !calibAxis.endStopMax()) && !movementDone) { calibAxis.setMotorStep(); delayMicroseconds(1000000 / speedMin[axis] /2); stepsCount++; if (stepsCount % (speedMin[axis] * 3) == 0) { // Periodically send message still active Serial.print("R04\n"); } calibAxis.resetMotorStep(); delayMicroseconds(1000000 / speedMin[axis] /2); } else { movementDone = true; // If end stop for home is active, set the position to zero if (calibAxis.endStopMax()) { invertEndStops = true; } } } Serial.print("R99"); Serial.print(" axis "); Serial.print(calibAxis.label); Serial.print(" at first end stop"); Serial.print("\n"); // Report back the end stop setting if (error == 0) { if (invertEndStops) { paramValueInt = 1; } else { paramValueInt = 0; } Serial.print("R23"); Serial.print(" "); Serial.print("P"); Serial.print(parEndInv); Serial.print(" "); Serial.print("V"); Serial.print(paramValueInt); Serial.print("\n"); } // Store the status of the system storeEndStops(); reportEndStops(); // Move into the other direction now, and measure the number of steps stepsCount = 0; movementDone = false; calibAxis.setDirectionAway(); while (!movementDone && error == 0) { // Check if there is an emergency stop command if (Serial.available() > 0) { incomingByte = Serial.read(); if (incomingByte == 69 || incomingByte == 101) { movementDone = true; error = 1; } } // Move until the end stop at the other side of the axis is reached if (((!invertEndStops && !calibAxis.endStopMax()) || (invertEndStops && !calibAxis.endStopMin())) && !movementDone) { calibAxis.setMotorStep(); stepsCount++; delayMicroseconds(1000000 / speedMin[axis] /2); if (stepsCount % (speedMin[axis] * 3) == 0) { // Periodically send message still active Serial.print("R04\n"); } calibAxis.resetMotorStep(); delayMicroseconds(1000000 / speedMin[axis] /2); } else { movementDone = true; } } Serial.print("R99"); Serial.print(" axis "); Serial.print(calibAxis.label); Serial.print(" at second end stop"); Serial.print("\n"); // Report back the end stop setting if (error == 0) { Serial.print("R23"); Serial.print(" "); Serial.print("P"); Serial.print(parNbrStp); Serial.print(" "); Serial.print("V"); Serial.print(stepsCount); Serial.print("\n"); } calibAxis.disableMotor(); storeEndStops(); reportEndStops(); switch (axis) { case 0: CurrentState::getInstance()->setX(stepsCount); break; case 1: CurrentState::getInstance()->setY(stepsCount); break; case 2: CurrentState::getInstance()->setZ(stepsCount); break; } reportPosition(); return error; }
int StepperControl::calibrateAxis(int axis) { // Load motor and encoder settings loadMotorSettings(); loadEncoderSettings(); //unsigned long timeStart = millis(); bool movementDone = false; int paramValueInt = 0; int stepsCount = 0; int incomingByte = 0; int error = 0; bool invertEndStops = false; int parEndInv; int parNbrStp; float *missedSteps; int *missedStepsMax; long *lastPosition; float *encoderStepDecay; bool *encoderEnabled; int *axisStatus; long *axisStepsPerMm; // Prepare for movement storeEndStops(); reportEndStops(); // Select the right axis StepperControlAxis *calibAxis; StepperControlEncoder *calibEncoder; switch (axis) { case 0: calibAxis = &axisX; calibEncoder = &encoderX; parEndInv = MOVEMENT_INVERT_ENDPOINTS_X; parNbrStp = MOVEMENT_AXIS_NR_STEPS_X; invertEndStops = endStInv[0]; missedSteps = &motorConsMissedSteps[0]; missedStepsMax = &motorConsMissedStepsMax[0]; lastPosition = &motorLastPosition[0]; encoderStepDecay = &motorConsMissedStepsDecay[0]; encoderEnabled = &motorConsEncoderEnabled[0]; axisStatus = &axisSubStep[0]; axisStepsPerMm = &stepsPerMm[0]; break; case 1: calibAxis = &axisY; calibEncoder = &encoderY; parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y; parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y; invertEndStops = endStInv[1]; missedSteps = &motorConsMissedSteps[1]; missedStepsMax = &motorConsMissedStepsMax[1]; lastPosition = &motorLastPosition[1]; encoderStepDecay = &motorConsMissedStepsDecay[1]; encoderEnabled = &motorConsEncoderEnabled[1]; axisStatus = &axisSubStep[1]; axisStepsPerMm = &stepsPerMm[1]; break; case 2: calibAxis = &axisZ; calibEncoder = &encoderZ; parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z; parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z; invertEndStops = endStInv[2]; missedSteps = &motorConsMissedSteps[2]; missedStepsMax = &motorConsMissedStepsMax[2]; lastPosition = &motorLastPosition[2]; encoderStepDecay = &motorConsMissedStepsDecay[2]; encoderEnabled = &motorConsEncoderEnabled[2]; axisStatus = &axisSubStep[2]; axisStepsPerMm = &stepsPerMm[2]; break; default: Serial.print("R99 Calibration error: invalid axis selected\r\n"); error = 1; CurrentState::getInstance()->setLastError(error); return error; } // Preliminary checks if (calibAxis->endStopMin() || calibAxis->endStopMax()) { Serial.print("R99 Calibration error: end stop active before start\r\n"); error = 1; CurrentState::getInstance()->setLastError(error); return error; } Serial.print("R99"); Serial.print(" axis "); Serial.print(calibAxis->channelLabel); Serial.print(" move to start for calibration"); Serial.print("\r\n"); *axisStatus = COMM_REPORT_MOVE_STATUS_START_MOTOR; reportStatus(calibAxis, axisStatus[0]); // Move towards home calibAxis->enableMotor(); calibAxis->setDirectionHome(); calibAxis->setCurrentPosition(calibEncoder->currentPosition()); stepsCount = 0; *missedSteps = 0; movementDone = false; motorConsMissedSteps[0] = 0; motorConsMissedSteps[1] = 0; motorConsMissedSteps[2] = 0; *axisStatus = COMM_REPORT_MOVE_STATUS_CRAWLING; reportStatus(calibAxis, axisStatus[0]); reportCalib(calibAxis, COMM_REPORT_CALIBRATE_STATUS_TO_HOME); while (!movementDone && error == 0) { #if defined(FARMDUINO_V14) checkEncoders(); #endif checkAxisVsEncoder(calibAxis, calibEncoder, &motorConsMissedSteps[axis], &motorLastPosition[axis], &motorConsEncoderLastPosition[axis], &motorConsEncoderUseForPos[axis], &motorConsMissedStepsDecay[axis], &motorConsEncoderEnabled[axis]); // Check if there is an emergency stop command if (Serial.available() > 0) { incomingByte = Serial.read(); if (incomingByte == 69 || incomingByte == 101) { Serial.print("R99 emergency stop\r\n"); movementDone = true; CurrentState::getInstance()->setEmergencyStop(); Serial.print(COMM_REPORT_EMERGENCY_STOP); CurrentState::getInstance()->printQAndNewLine(); error = 1; } } // Move until the end stop for home position is reached, either by end stop or motor skipping if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax)) { calibAxis->setStepAxis(); delayMicroseconds(100000 / speedHome[axis] / 2); stepsCount++; if (stepsCount % (speedHome[axis] * 3) == 0) { // Periodically send message still active Serial.print(COMM_REPORT_CMD_BUSY); CurrentState::getInstance()->printQAndNewLine(); } if (debugMessages) { if (stepsCount % (speedHome[axis] / 6) == 0 /*|| *missedSteps > 3*/) { Serial.print("R99"); Serial.print(" step count "); Serial.print(stepsCount); Serial.print(" missed steps "); Serial.print(*missedSteps); Serial.print(" max steps "); Serial.print(*missedStepsMax); Serial.print(" cur pos mtr "); Serial.print(calibAxis->currentPosition()); Serial.print(" cur pos enc "); Serial.print(calibEncoder->currentPosition()); Serial.print("\r\n"); } } calibAxis->resetMotorStep(); delayMicroseconds(100000 / speedHome[axis] / 2); } else { movementDone = true; Serial.print("R99 movement done\r\n"); // If end stop for home is active, set the position to zero if (calibAxis->endStopMax()) { invertEndStops = true; } } } reportCalib(calibAxis, COMM_REPORT_CALIBRATE_STATUS_TO_END); Serial.print("R99"); Serial.print(" axis "); Serial.print(calibAxis->channelLabel); Serial.print(" at starting point"); Serial.print("\r\n"); // Report back the end stop setting if (error == 0) { if (invertEndStops) { paramValueInt = 1; } else { paramValueInt = 0; } Serial.print("R23"); Serial.print(" "); Serial.print("P"); Serial.print(parEndInv); Serial.print(" "); Serial.print("V"); Serial.print(paramValueInt); //Serial.print("\r\n"); CurrentState::getInstance()->printQAndNewLine(); } // Store the status of the system storeEndStops(); reportEndStops(); // Move into the other direction now, and measure the number of steps Serial.print("R99"); Serial.print(" axis "); Serial.print(calibAxis->channelLabel); Serial.print(" calibrating length"); Serial.print("\r\n"); stepsCount = 0; movementDone = false; *missedSteps = 0; calibAxis->setDirectionAway(); calibAxis->setCurrentPosition(calibEncoder->currentPosition()); motorConsMissedSteps[0] = 0; motorConsMissedSteps[1] = 0; motorConsMissedSteps[2] = 0; long encoderStartPoint = calibEncoder->currentPosition(); long encoderEndPoint = calibEncoder->currentPosition(); while (!movementDone && error == 0) { #if defined(FARMDUINO_V14) checkEncoders(); #endif checkAxisVsEncoder(calibAxis, calibEncoder, &motorConsMissedSteps[axis], &motorLastPosition[axis], &motorConsEncoderLastPosition[axis], &motorConsEncoderUseForPos[axis], &motorConsMissedStepsDecay[axis], &motorConsEncoderEnabled[axis]); // Check if there is an emergency stop command if (Serial.available() > 0) { incomingByte = Serial.read(); if (incomingByte == 69 || incomingByte == 101) { Serial.print("R99 emergency stop\r\n"); movementDone = true; CurrentState::getInstance()->setEmergencyStop(); Serial.print(COMM_REPORT_EMERGENCY_STOP); CurrentState::getInstance()->printQAndNewLine(); error = 1; } } // Ignore the missed steps at startup time if (stepsCount < 10) { *missedSteps = 0; } // Move until the end stop at the other side of the axis is reached if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax)) { calibAxis->setStepAxis(); stepsCount++; //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); delayMicroseconds(100000 / speedHome[axis] / 2); if (stepsCount % (speedHome[axis] * 3) == 0) { // Periodically send message still active Serial.print(COMM_REPORT_CMD_BUSY); //Serial.print("\r\n"); CurrentState::getInstance()->printQAndNewLine(); Serial.print("R99"); Serial.print(" step count: "); Serial.print(stepsCount); Serial.print("\r\n"); } calibAxis->resetMotorStep(); delayMicroseconds(100000 / speedHome[axis] / 2); } else { Serial.print("R99 movement done\r\n"); movementDone = true; } } Serial.print("R99"); Serial.print(" axis "); Serial.print(calibAxis->channelLabel); Serial.print(" at end point"); Serial.print("\r\n"); encoderEndPoint = calibEncoder->currentPosition(); // if the encoder is enabled, use the encoder data instead of the step count if (encoderEnabled) { stepsCount = abs(encoderEndPoint - encoderStartPoint); } // Report back the end stop setting if (error == 0) { Serial.print("R23"); Serial.print(" "); Serial.print("P"); Serial.print(parNbrStp); Serial.print(" "); Serial.print("V"); Serial.print((float)stepsCount); CurrentState::getInstance()->printQAndNewLine(); } *axisStatus = COMM_REPORT_MOVE_STATUS_STOP_MOTOR; reportStatus(calibAxis, axisStatus[0]); calibAxis->disableMotor(); storeEndStops(); reportEndStops(); switch (axis) { case 0: CurrentState::getInstance()->setX(stepsCount); break; case 1: CurrentState::getInstance()->setY(stepsCount); break; case 2: CurrentState::getInstance()->setZ(stepsCount); break; } reportPosition(); *axisStatus = COMM_REPORT_MOVE_STATUS_IDLE; reportStatus(calibAxis, axisStatus[0]); reportCalib(calibAxis, COMM_REPORT_CALIBRATE_STATUS_IDLE); CurrentState::getInstance()->setLastError(error); return error; }