Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allow non zero z calibration #162

Draft
wants to merge 10 commits into
base: Maslow-Main
Choose a base branch
from
35 changes: 20 additions & 15 deletions FluidNC/src/Maslow/Maslow.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -398,13 +398,13 @@ bool Maslow_::takeSlackFunc() {
if(takeSlackState == 2){
if (take_measurement_avg_with_check(0, UP)) {

double offset = _beltEndExtension + _armLength;

double threshold = 12;

float diffTL = calibration_data[0][0] - measurementToXYPlane(computeTL(0, 0, 0), tlZ);
float diffTR = calibration_data[1][0] - measurementToXYPlane(computeTR(0, 0, 0), trZ);
float diffBL = calibration_data[2][0] - measurementToXYPlane(computeBL(0, 0, 0), blZ);
float diffBR = calibration_data[3][0] - measurementToXYPlane(computeBR(0, 0, 0), brZ);
float diffTL = calibration_data[0][0] - measurementToXYPlane(computeTL(0, 0, targetZ), tlZ);
float diffTR = calibration_data[1][0] - measurementToXYPlane(computeTR(0, 0, targetZ), trZ);
float diffBL = calibration_data[2][0] - measurementToXYPlane(computeBL(0, 0, targetZ), blZ);
float diffBR = calibration_data[3][0] - measurementToXYPlane(computeBR(0, 0, targetZ), brZ);
log_info("Center point deviation: TL: " << diffTL << " TR: " << diffTR << " BL: " << diffBL << " BR: " << diffBR);
if (abs(diffTL) > threshold || abs(diffTR) > threshold || abs(diffBL) > threshold || abs(diffBR) > threshold) {
log_error("Center point deviation over " << threshold << "mm, your coordinate system is not accurate, maybe try running calibration again?");
Expand Down Expand Up @@ -738,9 +738,10 @@ float Maslow_::computeTL(float x, float y, float z) {
//------------------------------------------------------

//Takes a raw measurement, projects it into the XY plane, then adds the belt end extension and arm length to get the actual distance.
float Maslow_::measurementToXYPlane(float measurement, float zHeight){
float Maslow_::measurementToXYPlane(float measurement, float zbase){

float lengthInXY = sqrt(measurement * measurement - zHeight * zHeight);
float z = zbase + targetZ;
float lengthInXY = sqrt(measurement * measurement - z * z);
return lengthInXY + _beltEndExtension + _armLength; //Add the belt end extension and arm length to get the actual distance
}

Expand Down Expand Up @@ -977,10 +978,10 @@ bool Maslow_::take_measurement_avg_with_check(int waypoint, int dir) {
if(waypoint == 0){
double threshold = 100;

float diffTL = calibration_data[0][0] - measurementToXYPlane(computeTL(0, 0, 0), tlZ);
float diffTR = calibration_data[1][0] - measurementToXYPlane(computeTR(0, 0, 0), trZ);
float diffBL = calibration_data[2][0] - measurementToXYPlane(computeBL(0, 0, 0), blZ);
float diffBR = calibration_data[3][0] - measurementToXYPlane(computeBR(0, 0, 0), brZ);
float diffTL = calibration_data[0][0] - measurementToXYPlane(computeTL(0, 0, targetZ), tlZ);
float diffTR = calibration_data[1][0] - measurementToXYPlane(computeTR(0, 0, targetZ), trZ);
float diffBL = calibration_data[2][0] - measurementToXYPlane(computeBL(0, 0, targetZ), blZ);
float diffBR = calibration_data[3][0] - measurementToXYPlane(computeBR(0, 0, targetZ), brZ);
log_info("Center point deviation: TL: " << diffTL << " TR: " << diffTR << " BL: " << diffBL << " BR: " << diffBR);

if (abs(diffTL) > threshold || abs(diffTR) > threshold || abs(diffBL) > threshold || abs(diffBR) > threshold) {
Expand Down Expand Up @@ -1405,10 +1406,12 @@ void Maslow_::runCalibration() {
}
stop();

//Save the z-axis 'stop' position
targetZ = 0;
setZStop();

//if the Z mechanical limit is not known, we can't run calibration
if (!zMechanicalLimitSet) {
log_error("Cannot run calibration until the Z position is known. Run it down to the stops and set Z stop");
sys.set_state(State::Idle);
return;
}
//if not all axis are homed, we can't run calibration, OR if the user hasnt entered width and height?
if (!allAxisExtended()) {
log_error("Cannot run calibration until all belts are extended fully");
Expand Down Expand Up @@ -1631,6 +1634,7 @@ void Maslow_::loadZPos() {
int zAxis = 2;
float* mpos = get_mpos();
mpos[zAxis] = targetZ;
zMechanicalLimitSet = true;
set_motor_steps_from_mpos(mpos);

log_info("Current z-axis position loaded as: " << targetZ);
Expand All @@ -1649,6 +1653,7 @@ void Maslow_::setZStop() {
int zAxis = 2;
float* mpos = get_mpos();
mpos[zAxis] = targetZ;
zMechanicalLimitSet = true;
set_motor_steps_from_mpos(mpos);

gc_sync_position();//This updates the Gcode engine with the new position from the stepping engine that we set with set_motor_steps
Expand Down
6 changes: 5 additions & 1 deletion FluidNC/src/Maslow/Maslow.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,9 @@
// Common Default strings - especially used by config
const std::string M = "Maslow";
// Non-volatile storage name
const char * nvs = "maslow";

//const char * nvs_t = "maslow";


struct TelemetryFileHeader {
unsigned int structureSize; // 4 bytes
Expand Down Expand Up @@ -69,6 +71,7 @@ struct TelemetryData {
bool extendedTR;
bool extendedBL;
bool extendedBR;
bool zMechanicalLimitSet;

bool extendingALL;
bool complyALL;
Expand Down Expand Up @@ -170,6 +173,7 @@ class Maslow_ {
bool extendedTR = false;
bool extendedBL = false;
bool extendedBR = false;
bool zMechanicalLimitSet = false; //This is separate from homing because this is the mechanical stop of the machine while Z home is where the bit is at Z=0

bool extendingALL = false;
bool complyALL = false;
Expand Down