From 63ea3b055439e08d54472d30dbdddbd99c5d1d8e Mon Sep 17 00:00:00 2001 From: Tony Date: Mon, 3 Mar 2014 19:39:39 +0000 Subject: [PATCH] Check for the Selected head (selected tool). This will allow for multi extruder printing support in future pull requests. It works fine for single extruder setups as well as the selected head is always E0. --- GCodes.cpp | 102 +++++++++++++++++++++++++++++++++++---------------- GCodes.h | 9 ++++- Heat.cpp | 22 +++++++++++ Heat.h | 1 + Move.cpp | 11 +++--- Platform.cpp | 30 +++++++-------- Platform.h | 39 +++++++++++++------- 7 files changed, 147 insertions(+), 67 deletions(-) diff --git a/GCodes.cpp b/GCodes.cpp index 1b744b9..a5173fd 100644 --- a/GCodes.cpp +++ b/GCodes.cpp @@ -75,7 +75,7 @@ void GCodes::Init() offSetSet = false; dwellWaiting = false; stackPointer = 0; - selectedHead = -1; + selectedHead = 0; //default to zero so parameter setting gcodes (eg M906) work for tools prior to tools being selected gFeedRate = platform->MaxFeedrate(Z_AXIS); // Typically the slowest zProbesSet = false; probeCount = 0; @@ -341,11 +341,15 @@ void GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, bool doingG92, bool applyL } } else { - if(gb->Seen(gCodeLetters[i])) + if(gb->Seen('E')&& ((i-AXES) == selectedHead)) //Only affect the selected extruder FIXME update to work with multiple concurrent extruders { float moveArg = gb->GetFValue()*distanceScale; if(drivesRelative || doingG92) + { moveBuffer[i] = moveArg; + if(doingG92) + lastPos[i - AXES] = moveBuffer[i]; + } else { float absE = moveArg; @@ -358,7 +362,7 @@ void GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, bool doingG92, bool applyL // Deal with feedrate - if(gb->Seen(gCodeLetters[DRIVES])) + if(gb->Seen(FEEDRATE_LETTER)) gFeedRate = gb->GetFValue()*distanceScale*0.016666667; // G Code feedrates are in mm/minute; we need mm/sec moveBuffer[DRIVES] = gFeedRate; // We always set it, as Move may have modified the last one. @@ -560,7 +564,7 @@ bool GCodes::OffsetAxes(GCodeBuffer* gb) } } - if(gb->Seen(gCodeLetters[DRIVES])) // Has the user specified a feedrate? + if(gb->Seen(FEEDRATE_LETTER)) // Has the user specified a feedrate? { moveToDo[DRIVES] = gb->GetFValue(); activeDrive[DRIVES] = true; @@ -881,14 +885,20 @@ bool GCodes::SetPrintZProbe(GCodeBuffer* gb, char* reply) // are updated at the end of each movement, so this won't tell you // where you are mid-movement. -// FIXME - needs to deal with multiple extruders +//Fixed to deal with multiple extruders char* GCodes::GetCurrentCoordinates() { float liveCoordinates[DRIVES+1]; reprap.GetMove()->LiveCoordinates(liveCoordinates); - snprintf(scratchString, STRING_LENGTH, "X:%f Y:%f Z:%f E:%f", liveCoordinates[X_AXIS], liveCoordinates[Y_AXIS], liveCoordinates[Z_AXIS], liveCoordinates[AXES]); + snprintf(scratchString, STRING_LENGTH, "X:%f Y:%f Z:%f ", liveCoordinates[X_AXIS], liveCoordinates[Y_AXIS], liveCoordinates[Z_AXIS]); + char eString[STRING_LENGTH]; + for(int i = AXES; i< DRIVES; i++) + { + snprintf(eString,STRING_LENGTH,"E%d:%f ",i-AXES,liveCoordinates[i]); + strncat(scratchString,eString,STRING_LENGTH); + } return scratchString; } @@ -1088,6 +1098,7 @@ bool GCodes::SetOffsets(GCodeBuffer *gb) if(gb->Seen('S')) reprap.GetHeat()->SetActiveTemperature(head, gb->GetFValue()); // FIXME - do X, Y and Z + platform->Message(HOST_MESSAGE, "Not Yet fully implemented: Use head offset in slicing software"); } return true; } @@ -1111,7 +1122,7 @@ bool GCodes::StandbyHeaters() return false; for(int8_t heater = 0; heater < HEATERS; heater++) reprap.GetHeat()->Standby(heater); - selectedHead = -1; + selectedHead = -1; //FIXME is this behaviour desirable? return true; } @@ -1453,11 +1464,20 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) case 92: // Set/report steps/mm for some axes seen = false; for(int8_t drive = 0; drive < DRIVES; drive++) - if(gb->Seen(gCodeLetters[drive])) - { - platform->SetDriveStepsPerUnit(drive, gb->GetFValue()); - seen = true; - } + { + //Do AXES first + if(gb->Seen(gCodeLetters[drive])&& driveSetDriveStepsPerUnit(drive, gb->GetFValue()); + seen = true; + } + //FIXME handle selectedHead=-1 case + else if(gb->Seen('E')&& ((drive-AXES) == selectedHead))//then do active extruder + { + platform->SetDriveStepsPerUnit(AXES+selectedHead, gb->GetFValue()); + seen=true; + } + } reprap.GetMove()->SetStepHypotenuse(); if(!seen) snprintf(reply, STRING_LENGTH, "Steps/mm: X: %d, Y: %d, Z: %d, E: %d", @@ -1478,14 +1498,16 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) case 104: // Deprecated if(gb->Seen('S')) { - reprap.GetHeat()->SetActiveTemperature(1, gb->GetFValue()); // 0 is the bed - reprap.GetHeat()->Activate(1); + //only sets the selected head (As set by T#) + reprap.GetHeat()->SetActiveTemperature(selectedHead+1, gb->GetFValue()); // 0 is the bed + reprap.GetHeat()->Activate(selectedHead+1); } break; case 105: // Deprecated... strncpy(reply, "T:", STRING_LENGTH); - for(int8_t heater = HEATERS - 1; heater > 0; heater--) + //FIXME - why did this decrement rather than increment through the heaters (odd behaviour) + for(int8_t heater = 1; heater < HEATERS; heater++) { strncat(reply, ftoa(0, reprap.GetHeat()->GetTemperature(heater), 1), STRING_LENGTH); strncat(reply, " ", STRING_LENGTH); @@ -1532,10 +1554,12 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) case 109: // Deprecated if(gb->Seen('S')) { - reprap.GetHeat()->SetActiveTemperature(1, gb->GetFValue()); // 0 is the bed - reprap.GetHeat()->Activate(1); + reprap.GetHeat()->SetActiveTemperature(selectedHead+1, gb->GetFValue()); // 0 is the bed + reprap.GetHeat()->Activate(selectedHead+1); } - /* no break */ + //check here rather than falling through to M116, we want to just wait for the extruder we specified (otherwise use M116 not M109) + result = reprap.GetHeat()->HeaterAtSetTemperature(selectedHead+1); + break; case 116: // Wait for everything, especially set temperatures if(!AllMovesAreFinishedAndMoveBufferIsLoaded()) return false; @@ -1580,25 +1604,33 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) case 201: // Set axis accelerations for(int8_t drive = 0; drive < DRIVES; drive++) { - float value; - if(gb->Seen(gCodeLetters[drive])) - { - value = gb->GetFValue(); - }else{ - value = -1; - } - platform->SetAcceleration(drive, value); + //Do AXES first + if(gb->Seen(gCodeLetters[drive])&& driveSetAcceleration(drive, gb->GetFValue()); + //then do active extruder + }else if(gb->Seen('E')&& ((drive-AXES) == selectedHead)){ + platform->SetAcceleration(AXES+selectedHead, gb->GetFValue()); //Set the E acceleration for the currently selected tool + }else{ + platform->SetAcceleration(drive, -1); + } } break; case 203: // Set maximum feedrates for(int8_t drive = 0; drive < DRIVES; drive++) { - if(gb->Seen(gCodeLetters[drive])) - { - float value = gb->GetFValue()*distanceScale*0.016666667; // G Code feedrates are in mm/minute; we need mm/sec; + //Do AXES first + if(gb->Seen(gCodeLetters[drive])&& driveGetFValue()*distanceScale*0.016666667; // G Code feedrates are in mm/minute; we need mm/sec; platform->SetMaxFeedrate(drive, value); - } + } + else if(gb->Seen('E')&& ((drive-AXES) == selectedHead))//then do active extruder + { + float value = gb->GetFValue()*distanceScale*0.016666667; // G Code feedrates are in mm/minute; we need mm/sec; + platform->SetMaxFeedrate(AXES+selectedHead, value); //Set the E Steps for the currently selected tool + } } break; @@ -1817,11 +1849,19 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb) case 906: // Set Motor currents for(uint8_t i = 0; i < DRIVES; i++) { - if(gb->Seen(gCodeLetters[i])) + //Do AXES first + if(gb->Seen(gCodeLetters[i])&& iGetFValue(); // mA platform->SetMotorCurrent(i, value); } + else //do for selected extruder + { + if(gb->Seen(gCodeLetters[i])){ + float value = gb->GetFValue(); // mA + platform->SetMotorCurrent(AXES+selectedHead, value); + } + } } break; diff --git a/GCodes.h b/GCodes.h index 7e79bb5..5a6ed49 100644 --- a/GCodes.h +++ b/GCodes.h @@ -25,7 +25,8 @@ Licence: GPL #define STACK 5 #define GCODE_LENGTH 100 // Maximum length of internally-generated G Code string -#define GCODE_LETTERS { 'X', 'Y', 'Z', 'E', 'F' } // The drives and feedrate in a GCode +#define GCODE_LETTERS { 'X', 'Y', 'Z', 'E', 'F' } // The drives and feedrate in a GCode //FIXME when working with multiple extruders GCODE_LETTERS[DRIVES] is out of scope +#define FEEDRATE_LETTER 'F'//FIX to work with multiple extruders without having to re-define GCODE_LETTERS array // Small class to hold an individual GCode and provide functions to allow it to be parsed @@ -79,6 +80,7 @@ class GCodes char* GetCurrentCoordinates(); // Get where we are as a string bool PrintingAFile() const; // Are we in the middle of printing a file? void Diagnostics(); // Send helpful information out + int8_t GetSelectedHead(); // return which tool is selected bool HaveIncomingData() const; // Is there something that we have to do? bool GetAxisIsHomed(uint8_t axis) const { return axisIsHomed[axis]; } // Is the axis at 0? @@ -231,4 +233,9 @@ inline bool GCodes::RunConfigurationGCodes() return !DoFileCannedCycles(platform->GetConfigFile()); } +inline int8_t GCodes::GetSelectedHead() +{ + return selectedHead; +} + #endif diff --git a/Heat.cpp b/Heat.cpp index 259133f..9549dec 100644 --- a/Heat.cpp +++ b/Heat.cpp @@ -88,6 +88,28 @@ bool Heat::AllHeatersAtSetTemperatures() } return true; } +//query an individual heater +bool Heat::HeaterAtSetTemperature(int8_t heater) +{ + float dt; + dt = GetTemperature(heater); + if(pids[heater]->Active()) + { + if(GetActiveTemperature(heater) < TEMPERATURE_LOW_SO_DONT_CARE) + dt = 0.0; + else + dt = fabs(dt - GetActiveTemperature(heater)); + } else + { + if(GetStandbyTemperature(heater) < TEMPERATURE_LOW_SO_DONT_CARE) + dt = 0.0; + else + dt = fabs(dt - GetStandbyTemperature(heater)); + } + if(dt > TEMPERATURE_CLOSE_ENOUGH) + return false; + return true; +} //****************************************************************************************************** diff --git a/Heat.h b/Heat.h index dd12e51..6d9bd7a 100644 --- a/Heat.h +++ b/Heat.h @@ -81,6 +81,7 @@ class Heat float GetTemperature(int8_t heater); // Get the temperature of a heater void ResetFault(int8_t heater); // Reset a heater fault - oly call this if you know what you are doing bool AllHeatersAtSetTemperatures(); // Is everything at temperature within tolerance? + bool HeaterAtSetTemperature(int8_t heater); // Is a specific heater at temperature within tolerance? void Diagnostics(); // Output useful information private: diff --git a/Move.cpp b/Move.cpp index 2cad55d..df7056e 100644 --- a/Move.cpp +++ b/Move.cpp @@ -196,7 +196,7 @@ void Move::Spin() if(movementType & xyMove) nextMove[DRIVES] = fmax(nextMove[DRIVES], platform->InstantDv(X_AXIS)); else if(movementType & eMove) - nextMove[DRIVES] = fmax(nextMove[DRIVES], platform->InstantDv(AXES)); + nextMove[DRIVES] = fmax(nextMove[DRIVES], platform->InstantDv((AXES+gCodes->GetSelectedHead()))); else nextMove[DRIVES] = fmax(nextMove[DRIVES], platform->InstantDv(Z_AXIS)); @@ -205,7 +205,7 @@ void Move::Spin() if(movementType & xyMove) nextMove[DRIVES] = fmin(nextMove[DRIVES], platform->MaxFeedrate(X_AXIS)); // Assumes X and Y are equal. FIXME? else if(movementType & eMove) - nextMove[DRIVES] = fmin(nextMove[DRIVES], platform->MaxFeedrate(AXES)); // Picks up the value for the first extruder. FIXME? + nextMove[DRIVES] = fmin(nextMove[DRIVES], platform->MaxFeedrate(AXES+gCodes->GetSelectedHead())); // Fixed else // Must be z nextMove[DRIVES] = fmin(nextMove[DRIVES], platform->MaxFeedrate(Z_AXIS)); @@ -273,7 +273,8 @@ bool Move::GetCurrentState(float m[]) if(i < AXES) m[i] = lastMove->MachineToEndPoint(i); else - m[i] = 0.0; + m[i] = 0.0; //FIXME This resets extruders to 0.0, even the inactive ones (is this behaviour desired?) + //m[i] = lastMove->MachineToEndPoint(i); //FIXME TEST alternative that does not reset extruders to 0 } if(currentFeedrate >= 0.0) m[DRIVES] = currentFeedrate; @@ -366,7 +367,7 @@ void Move::SetStepHypotenuse() // We don't want 0. If no axes/extruders are moving these should never be used. // But try to be safe. - stepDistances[0] = 1.0/platform->DriveStepsPerUnit(AXES); + stepDistances[0] = 1.0/platform->DriveStepsPerUnit(AXES); //FIXME this is not multi extruder safe (but we should never get here) extruderStepDistances[0] = stepDistances[0]; } @@ -518,7 +519,7 @@ void Move::DoLookAhead() else if (mt & zMove) c = platform->InstantDv(Z_AXIS); else - c = platform->InstantDv(AXES); // value for first extruder FIXME?? + c = platform->InstantDv((AXES+gCodes->GetSelectedHead())); // value for current extruder } n1->SetV(c); n1->SetProcessed(vCosineSet); diff --git a/Platform.cpp b/Platform.cpp index 89bdedb..9f94506 100644 --- a/Platform.cpp +++ b/Platform.cpp @@ -158,21 +158,21 @@ void Platform::Init() if(stepPins[i] >= 0) { - if(i > Z_AXIS) + if(i == E0_DRIVE || i == E3_DRIVE) //STEP_PINS {14, 25, 5, X2, 41, 39, X4, 49} pinModeNonDue(stepPins[i], OUTPUT); else pinMode(stepPins[i], OUTPUT); } if(directionPins[i] >= 0) { - if(i > Z_AXIS) + if(i == E0_DRIVE) //DIRECTION_PINS {15, 26, 4, X3, 35, 53, 51, 48} pinModeNonDue(directionPins[i], OUTPUT); else pinMode(directionPins[i], OUTPUT); } if(enablePins[i] >= 0) { - if(i >= Z_AXIS) + if(i == Z_AXIS || i==E0_DRIVE || i==E2_DRIVE) //ENABLE_PINS {29, 27, X1, X0, 37, X8, 50, 47} pinModeNonDue(enablePins[i], OUTPUT); else pinMode(enablePins[i], OUTPUT); @@ -180,8 +180,7 @@ void Platform::Init() Disable(i); driveEnabled[i] = false; } - - for(i = 0; i < AXES; i++) + for(i = 0; i < DRIVES; i++) { if(lowStopPins[i] >= 0) { @@ -195,21 +194,20 @@ void Platform::Init() } } - if(heatOnPins[0] >= 0) - pinMode(heatOnPins[0], OUTPUT); - thermistorInfRs[0] = ( thermistorInfRs[0]*exp(-thermistorBetas[0]/(25.0 - ABS_ZERO)) ); - - for(i = 1; i < HEATERS; i++) + for(i = 0; i < HEATERS; i++) { if(heatOnPins[i] >= 0) - pinModeNonDue(heatOnPins[i], OUTPUT); + if(i == E0_HEATER || i==E1_HEATER) //HEAT_ON_PINS {6, X5, X7, 7, 8, 9} + pinModeNonDue(heatOnPins[i], OUTPUT); + else + pinMode(heatOnPins[i], OUTPUT); thermistorInfRs[i] = ( thermistorInfRs[i]*exp(-thermistorBetas[i]/(25.0 - ABS_ZERO)) ); } if(coolingFanPin >= 0) { - pinMode(coolingFanPin, OUTPUT); - analogWrite(coolingFanPin, 0); + //pinModeNonDue(coolingFanPin, OUTPUT); //not required as analogwrite does this automatically + analogWriteNonDue(coolingFanPin, 255); //inverse logic for Duet v0.6 this turns it off } InitialiseInterrupts(); @@ -385,10 +383,10 @@ void Platform::SetHeater(int8_t heater, const float& power) byte p = (byte)(255.0*fmin(1.0, fmax(0.0, power))); if(HEAT_ON == 0) p = 255 - p; - if(heater == 0) - analogWrite(heatOnPins[heater], p); + if(heater == E0_HEATER || heater == E1_HEATER) //HEAT_ON_PINS {6, X5, X7, 7, 8, 9} + analogWriteNonDue(heatOnPins[heater], p); else - analogWriteNonDue(heatOnPins[heater], p); + analogWrite(heatOnPins[heater], p); } diff --git a/Platform.h b/Platform.h index a465873..3737f76 100644 --- a/Platform.h +++ b/Platform.h @@ -77,15 +77,15 @@ Licence: GPL // DRIVES -#define STEP_PINS {14, 25, 5, X2} -#define DIRECTION_PINS {15, 26, 4, X3} +#define STEP_PINS {14, 25, 5, X2} // Full array for Duet + Duex4 is {14, 25, 5, X2, 41, 39, X4, 49} +#define DIRECTION_PINS {15, 26, 4, X3} // Full array for Duet + Duex4 is {15, 26, 4, X3, 35, 53, 51, 48} #define FORWARDS true // What to send to go... #define BACKWARDS false // ...in each direction -#define ENABLE_PINS {29, 27, X1, X0} -#define ENABLE false // What to send to enable... +#define ENABLE_PINS {29, 27, X1, X0} // Full array for Duet + Duex4 is {29, 27, X1, X0, 37, X8, 50, 47} +#define ENABLE false // What to send to enable... #define DISABLE true // ...and disable a drive #define DISABLE_DRIVES {false, false, true, false} // Set true to disable a drive when it becomes idle -#define LOW_STOP_PINS {11, -1, 60, 31} +#define LOW_STOP_PINS {11, -1, 60, 31} // Full array endstop pins for Duet + Duex4 is {11, 28, 60, 31, 24, 46, 45, 44} #define HIGH_STOP_PINS {-1, 28, -1, -1} #define ENDSTOP_HIT 1 // when a stop == this it is hit #define POT_WIPES {1, 3, 2, 0} // Indices for motor current digipots (if any) @@ -110,10 +110,16 @@ Licence: GPL #define Y_AXIS 1 // The index of the Y axis #define Z_AXIS 2 // The index of the Z axis -// HEATERS - The bed is assumed to be the at index 0 +#define E0_DRIVE 3 //the index of the first Extruder drive +#define E1_DRIVE 4 //the index of the second Extruder drive +#define E2_DRIVE 5 //the index of the third Extruder drive +#define E3_DRIVE 6 //the index of the fourth Extruder drive +#define E4_DRIVE 7 //the index of the fifth Extruder drive -#define TEMP_SENSE_PINS {5, 4} // Analogue pin numbers -#define HEAT_ON_PINS {6, X5} // PWM pins +// HEATERS - The bed is assumed to be the first + +#define TEMP_SENSE_PINS {5, 4} // Analogue pin numbers (full array for Duet+Duex4 = {5, 4, 0, 7, 8, 9} ) +#define HEAT_ON_PINS {6, X5} // PWM pins (full array for Duet+Duex4 = {6, X5, X7, 7, 8, 9} ) // Bed thermistor: http://uk.farnell.com/epcos/b57863s103f040/sensor-miniature-ntc-10k/dp/1299930?Ntt=129-9930 // Hot end thermistor: http://www.digikey.co.uk/product-search/en?x=20&y=11&KeyWords=480-3137-ND @@ -131,12 +137,17 @@ Licence: GPL #define TEMP_INTERVAL 0.122 // secs - check and control temperatures this often #define STANDBY_TEMPERATURES {ABS_ZERO, ABS_ZERO} // We specify one for the bed, though it's not needed #define ACTIVE_TEMPERATURES {ABS_ZERO, ABS_ZERO} -#define COOLING_FAN_PIN 34 +#define COOLING_FAN_PIN X6 //pin D34 is PWM capable but not an Arduino PWM pin - use X6 instead #define HEAT_ON 0 // 0 for inverted heater (eg Duet v0.6) 1 for not (e.g. Duet v0.4) #define AD_RANGE 1023.0 //16383 // The A->D converter that measures temperatures gives an int this big as its max value -#define HOT_BED 0 // The index of the heated bed; set to -1 if there is no heated bed +#define HOT_BED 0 // The index of the heated bed; set to -1 if there is no heated bed +#define E0_HEATER 1 //the index of the first extruder heater +#define E1_HEATER 2 //the index of the first extruder heater +#define E2_HEATER 3 //the index of the first extruder heater +#define E3_HEATER 4 //the index of the first extruder heater +#define E4_HEATER 5 //the index of the first extruder heater /****************************************************************************************************/ @@ -726,7 +737,7 @@ inline void Platform::SetDirection(byte drive, bool direction) { if(directionPins[drive] < 0) return; - if(drive == AXES) + if(drive == E0_DRIVE) //DIRECTION_PINS {15, 26, 4, X3, 35, 53, 51, 48} digitalWriteNonDue(directionPins[drive], direction); else digitalWrite(directionPins[drive], direction); @@ -736,7 +747,7 @@ inline void Platform::Disable(byte drive) { if(enablePins[drive] < 0) return; - if(drive >= Z_AXIS) + if(drive == Z_AXIS || drive==E0_DRIVE || drive==E2_DRIVE) //ENABLE_PINS {29, 27, X1, X0, 37, X8, 50, 47} digitalWriteNonDue(enablePins[drive], DISABLE); else digitalWrite(enablePins[drive], DISABLE); @@ -749,13 +760,13 @@ inline void Platform::Step(byte drive) return; if(!driveEnabled[drive] && enablePins[drive] >= 0) { - if(drive >= Z_AXIS) + if(drive == Z_AXIS || drive==E0_DRIVE || drive==E2_DRIVE) //ENABLE_PINS {29, 27, X1, X0, 37, X8, 50, 47} digitalWriteNonDue(enablePins[drive], ENABLE); else digitalWrite(enablePins[drive], ENABLE); driveEnabled[drive] = true; } - if(drive == AXES) + if(drive == E0_DRIVE || drive == E3_DRIVE) //STEP_PINS {14, 25, 5, X2, 41, 39, X4, 49} { digitalWriteNonDue(stepPins[drive], 0); digitalWriteNonDue(stepPins[drive], 1);