From 6c22f3060081de215d1e4754cd9aa3b6b0a8b8eb Mon Sep 17 00:00:00 2001 From: Adrian Bowyer Date: Tue, 12 Nov 2013 16:47:41 +0000 Subject: [PATCH] Axis angle compensation code added. Briefly tested in XY plane. Seems to work. More testing is probably needed... --- GCodes.cpp | 49 ++++++++++++++++++++++++++-------------------- Move.cpp | 22 +++++++++++++++++---- Move.h | 22 ++++++++++++++++++++- RepRapFirmware.cpp | 21 +++++++++++++------- 4 files changed, 81 insertions(+), 33 deletions(-) diff --git a/GCodes.cpp b/GCodes.cpp index d46f7b6..d883e40 100644 --- a/GCodes.cpp +++ b/GCodes.cpp @@ -797,15 +797,15 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) case 82: if(drivesRelative) - for(uint8_t i = AXES; i < DRIVES; i++) - lastPos[i - AXES] = 0.0; + for(int8_t extruder = AXES; extruder < DRIVES; extruder++) + lastPos[extruder - AXES] = 0.0; drivesRelative = false; break; case 83: if(!drivesRelative) - for(uint8_t i = AXES; i < DRIVES; i++) - lastPos[i - AXES] = 0.0; + for(int8_t extruder = AXES; extruder < DRIVES; extruder++) + lastPos[extruder - AXES] = 0.0; drivesRelative = true; break; @@ -814,23 +814,23 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) break; case 92: // Set steps/mm for each axis - for(uint8_t i = 0; i < DRIVES; i++) + for(int8_t drive = 0; drive < DRIVES; drive++) { - if(gb->Seen(gCodeLetters[i])) + if(gb->Seen(gCodeLetters[drive])) { value = gb->GetFValue(); }else{ value = -1; } - platform->SetDriveStepsPerUnit(i, value); + platform->SetDriveStepsPerUnit(drive, value); } break; case 105: // Deprecated... strncpy(reply, "T:", STRING_LENGTH); - for(int8_t i = HEATERS - 1; i > 0; i--) + for(int8_t heater = HEATERS - 1; heater > 0; heater--) { - strncat(reply, ftoa(0, reprap.GetHeat()->GetTemperature(i), 1), STRING_LENGTH); + strncat(reply, ftoa(0, reprap.GetHeat()->GetTemperature(heater), 1), STRING_LENGTH); strncat(reply, " ", STRING_LENGTH); } strncat(reply, "B:", STRING_LENGTH); @@ -906,25 +906,25 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) break; case 201: // Set axis accelerations - for(uint8_t i = 0; i < DRIVES; i++) + for(int8_t drive = 0; drive < DRIVES; drive++) { - if(gb->Seen(gCodeLetters[i])) + if(gb->Seen(gCodeLetters[drive])) { value = gb->GetFValue(); }else{ value = -1; } - platform->SetAcceleration(i, value); + platform->SetAcceleration(drive, value); } break; case 203: // Set maximum feedrates - for(uint8_t i = 0; i < DRIVES; i++) + for(int8_t drive = 0; drive < DRIVES; drive++) { - if(gb->Seen(gCodeLetters[i])) + if(gb->Seen(gCodeLetters[drive])) { value = gb->GetFValue()*distanceScale*0.016666667; // G Code feedrates are in mm/minute; we need mm/sec; - platform->SetMaxFeedrate(i, value); + platform->SetMaxFeedrate(drive, value); } } break; @@ -933,23 +933,23 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) break; case 208: // Set maximum axis lengths - for(uint8_t i = 0; i < AXES; i++) + for(int8_t axis = 0; axis < AXES; axis++) { - if(gb->Seen(gCodeLetters[i])) + if(gb->Seen(gCodeLetters[axis])) { value = gb->GetFValue()*distanceScale; - platform->SetAxisLength(i, value); + platform->SetAxisLength(axis, value); } } break; case 210: // Set homing feedrates - for(uint8_t i = 0; i < AXES; i++) + for(int8_t axis = 0; axis < AXES; axis++) { - if(gb->Seen(gCodeLetters[i])) + if(gb->Seen(gCodeLetters[axis])) { value = gb->GetFValue()*distanceScale*0.016666667; - platform->SetHomeFeedRate(i, value); + platform->SetHomeFeedRate(axis, value); } } break; @@ -982,6 +982,13 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) break; case 504: // Axis compensation + if(gb->Seen('S')) + { + value = gb->GetFValue(); + for(int8_t axis = 0; axis < AXES; axis++) + if(gb->Seen(gCodeLetters[axis])) + reprap.GetMove()->SetAxisCompensation(axis, gb->GetFValue()/value); + } break; case 906: // Set Motor currents diff --git a/Move.cpp b/Move.cpp index 0ae4540..2aaf34f 100644 --- a/Move.cpp +++ b/Move.cpp @@ -144,6 +144,9 @@ void Move::Init() currentFeedrate = -1.0; SetIdentityTransform(); + tanXY = 0.0; + tanYZ = 0.0; + tanXZ = 0.0; lastZHit = 0.0; zProbing = false; @@ -566,6 +569,13 @@ LookAhead* Move::LookAheadRingGet() return result; } +// Note that we don't set the tan values to 0 here. This means that the bed probe +// values will be a fraction of a millimeter out in X and Y, which, as the bed should +// be nearly flat (and the probe doesn't coincide with the nozzle anyway), won't matter. +// But it means that the tan values can be set for the machine +// at the start in the configuration file and be retained, without having to know and reset +// them after every Z probe of the bed. + void Move::SetIdentityTransform() { aX = 0.0; @@ -573,14 +583,18 @@ void Move::SetIdentityTransform() aC = 0.0; } -void Move::Transform(float move[]) +void Move::Transform(float xyzPoint[]) { - move[2] = move[2] + aX*move[0] + aY*move[1] + aC; + xyzPoint[X_AXIS] = xyzPoint[X_AXIS] + tanXY*xyzPoint[Y_AXIS] + tanXZ*xyzPoint[Z_AXIS]; + xyzPoint[Y_AXIS] = xyzPoint[Y_AXIS] + tanYZ*xyzPoint[Z_AXIS]; + xyzPoint[Z_AXIS] = xyzPoint[Z_AXIS] + aX*xyzPoint[X_AXIS] + aY*xyzPoint[Y_AXIS] + aC; } -void Move::InverseTransform(float move[]) +void Move::InverseTransform(float xyzPoint[]) { - move[2] = move[2] - (aX*move[0] + aY*move[1] + aC); + xyzPoint[Z_AXIS] = xyzPoint[Z_AXIS] - (aX*xyzPoint[X_AXIS] + aY*xyzPoint[Y_AXIS] + aC); + xyzPoint[Y_AXIS] = xyzPoint[Y_AXIS] - tanYZ*xyzPoint[Z_AXIS]; + xyzPoint[X_AXIS] = xyzPoint[X_AXIS] - (tanXY*xyzPoint[Y_AXIS] + tanXZ*xyzPoint[Z_AXIS]); } void Move::SetProbedBedPlane() diff --git a/Move.h b/Move.h index 4e3729b..8f94369 100644 --- a/Move.h +++ b/Move.h @@ -162,6 +162,7 @@ class Move void SetZProbing(bool probing); void SetProbedBedPlane(); float GetLastProbedZ(); + void SetAxisCompensation(int8_t axis, float tangent); void SetIdentityTransform(); void Transform(float move[]); void InverseTransform(float move[]); @@ -212,7 +213,8 @@ class Move float stepDistances[(1< dx, dy, dz <- msb float extruderStepDistances[(1<<(DRIVES-AXES))]; // NB - limits us to 5 extruders long nextMachineEndPoints[DRIVES+1]; - float aX, aY, aC; + float aX, aY, aC; // Bed plane explicit equation z' = z + aX*x + aY*y + aC + float tanXY, tanYZ, tanXZ; // 90 degrees + angle gives angle between axes float lastZHit; bool zProbing; }; @@ -404,6 +406,24 @@ inline float Move::GetLastProbedZ() return lastZHit; } +inline void Move::SetAxisCompensation(int8_t axis, float tangent) +{ + switch(axis) + { + case X_AXIS: + tanXY = tangent; + break; + case Y_AXIS: + tanYZ = tangent; + break; + case Z_AXIS: + tanXZ = tangent; + break; + default: + platform->Message(HOST_MESSAGE, "SetAxisCompensation: dud axis.\n"); + } +} + inline void Move::HitLowStop(int8_t drive, LookAhead* la, DDA* hitDDA) { float hitPoint = 0.0; diff --git a/RepRapFirmware.cpp b/RepRapFirmware.cpp index 6054d79..910e8f9 100644 --- a/RepRapFirmware.cpp +++ b/RepRapFirmware.cpp @@ -215,7 +215,7 @@ void RepRap::Diagnostics() webserver->Diagnostics(); } -// Disable interrupts, turn off the heaters, disable the motors, and +// Turn off the heaters, disable the motors, and // deactivate the Heat and Move classes. Leave everything else // working. @@ -223,18 +223,25 @@ void RepRap::EmergencyStop() { int8_t i; - platform->DisableInterrupts(); + //platform->DisableInterrupts(); + heat->Exit(); for(i = 0; i < HEATERS; i++) platform->SetHeater(i, 0.0); - heat->Exit(); - for(i = 0; i < DRIVES; i++) + // We do this twice, to avoid an interrupt switching + // a drive back on. move->Exit() should prevent + // interrupts doing this. + + for(int8_t i = 0; i < 2; i++) { - platform->SetMotorCurrent(i, 0.0); - platform->Disable(i); + move->Exit(); + for(i = 0; i < DRIVES; i++) + { + platform->SetMotorCurrent(i, 0.0); + platform->Disable(i); + } } - move->Exit(); }