Merge branch 'duet' of https://github.com/reprappro/RepRapFirmware into duet
Conflicts: GCodes.h
This commit is contained in:
commit
4fa348306a
9 changed files with 304 additions and 111 deletions
|
@ -24,8 +24,8 @@ Licence: GPL
|
|||
#define CONFIGURATION_H
|
||||
|
||||
#define NAME "RepRapFirmware"
|
||||
#define VERSION "0.64"
|
||||
#define DATE "2014-04-11"
|
||||
#define VERSION "0.65"
|
||||
#define DATE "2014-04-15"
|
||||
#define LAST_AUTHOR "reprappro"
|
||||
|
||||
// Other firmware that we might switch to be compatible with.
|
||||
|
|
264
GCodes.cpp
264
GCodes.cpp
|
@ -75,16 +75,17 @@ 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;
|
||||
cannedCycleMoveCount = 0;
|
||||
cannedCycleMoveQueued = false;
|
||||
limitAxes = true;
|
||||
axisIsHomed[X_AXIS] = axisIsHomed[Y_AXIS] = axisIsHomed[Z_AXIS] = false;
|
||||
active = true;
|
||||
longWait = platform->Time();
|
||||
dwellTime = longWait;
|
||||
axisIsHomed[X_AXIS] = axisIsHomed[Y_AXIS] = axisIsHomed[Z_AXIS] = false;
|
||||
}
|
||||
|
||||
void GCodes::DoFilePrint(GCodeBuffer* gb)
|
||||
|
@ -341,24 +342,89 @@ void GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, bool doingG92, bool applyL
|
|||
}
|
||||
} else
|
||||
{
|
||||
if(gb->Seen(gCodeLetters[i]))
|
||||
/*Fixed to work with multiple concurrent extruder drives:
|
||||
* Default or M160 S1 (set use only one extruder drive)
|
||||
* "G1 En.n" adds the float n.n to the move buffer for the selected head
|
||||
* There is no change in behaviour for one extruder drive setups, or multiple extruder
|
||||
* setups where only one drive is used at any one time.
|
||||
*
|
||||
* M160 Sn (set to use "n" extruder drives) eg
|
||||
* "M160 S3"
|
||||
* "G1 En.n:m.m:o.o" adds the floats to the move buffer in the following way:
|
||||
* moveBuffer[AXES+selectedHead) = n.n
|
||||
* moveBuffer[AXES+selectedHead+1) = m.m
|
||||
* moveBuffer[AXES+selectedHead+2) = o.o
|
||||
* so if selectedHead=0 move buffer ends up looking like this for a 5 extruder drive setup:
|
||||
* {x.x, y.y, z.z, n.n, m.m, o.o, 0.0,0.0, f.f}
|
||||
* where x,y,z are the axes and f is the feedrate.
|
||||
* If selected head > 0 then there is the possibility that more drives can be set than
|
||||
* exist, in that case the last values are discarded e.g:
|
||||
* "T3"
|
||||
* "M160 S3"
|
||||
* "G1 En.n:m.m:o.o"
|
||||
* would leave the move buffer on a 4 extruder drive setup looking like this:
|
||||
* {x.x, y.y, z.z, 0.0, 0.0, 0.0, n.n,m.m, f.f}
|
||||
*/
|
||||
if(gb->Seen('E')&& ((i-AXES) == selectedHead))
|
||||
{
|
||||
float moveArg = gb->GetFValue()*distanceScale;
|
||||
if(drivesRelative || doingG92)
|
||||
moveBuffer[i] = moveArg;
|
||||
else
|
||||
{
|
||||
float absE = moveArg;
|
||||
moveBuffer[i] = absE - lastPos[i - AXES];
|
||||
lastPos[i - AXES] = absE;
|
||||
}
|
||||
//the number of mixing drives set (by M160)
|
||||
int numDrives = platform->GetMixingDrives();
|
||||
const char* extruderString = gb->GetString();
|
||||
float eArg[numDrives];
|
||||
uint8_t sp = 0; //string pointer
|
||||
uint8_t fp = 0; //float pointer for the start of each floating point number in turn
|
||||
uint8_t hp = 0; //index of the head currently referred to for eArg
|
||||
while(extruderString[sp])
|
||||
{
|
||||
//first check to confirm we have not got to the feedrate setting part of the string
|
||||
if(extruderString[sp] == 'F')
|
||||
{
|
||||
break;
|
||||
}
|
||||
if(extruderString[sp] == ':')
|
||||
{
|
||||
eArg[hp] = (atoff(&extruderString[fp]))*distanceScale;
|
||||
hp++;
|
||||
if(hp >= numDrives)
|
||||
{
|
||||
platform->Message(HOST_MESSAGE, "More mixing extruder drives required in G1 string than set with M160: ");
|
||||
platform->Message(HOST_MESSAGE, gb->Buffer());
|
||||
platform->Message(HOST_MESSAGE, "\n");
|
||||
return;
|
||||
}
|
||||
sp++;
|
||||
fp = sp;
|
||||
}
|
||||
else
|
||||
sp++;
|
||||
}
|
||||
//capture the last drive step amount in the string (or the only one in the case of only one extruder)
|
||||
eArg[hp] = (atoff(&extruderString[fp]))*distanceScale;
|
||||
|
||||
//set the move buffer for each extruder drive
|
||||
for(int j=0;j<numDrives;j++)
|
||||
{
|
||||
if(drivesRelative || doingG92)
|
||||
{
|
||||
|
||||
moveBuffer[i+j] = eArg[j]; //Absolute
|
||||
if(doingG92)
|
||||
lastPos[i+j - AXES] = moveBuffer[i+j];
|
||||
}
|
||||
else
|
||||
{
|
||||
float absE = eArg[j];
|
||||
moveBuffer[i+j] = absE - lastPos[i+j - AXES];
|
||||
lastPos[i+j - AXES] = absE;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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.
|
||||
|
@ -376,20 +442,22 @@ bool GCodes::SetUpMove(GCodeBuffer *gb)
|
|||
if(moveAvailable)
|
||||
return false;
|
||||
|
||||
// Load the last position; If Move can't accept more, return false
|
||||
// Load the last position into moveBuffer; If Move can't accept more, return false
|
||||
|
||||
if(!reprap.GetMove()->GetCurrentState(moveBuffer))
|
||||
return false;
|
||||
|
||||
//check to see if the move is a 'homing' move that endstops are checked on.
|
||||
checkEndStops = false;
|
||||
if(gb->Seen('S'))
|
||||
{
|
||||
if(gb->GetIValue() == 1)
|
||||
checkEndStops = true;
|
||||
}
|
||||
|
||||
LoadMoveBufferFromGCode(gb, false, !checkEndStops);
|
||||
|
||||
//loads the movebuffer with either the absolute movement required or the
|
||||
//relative movement required
|
||||
LoadMoveBufferFromGCode(gb, false, !checkEndStops && limitAxes);
|
||||
//There is a new move in the move buffer
|
||||
moveAvailable = true;
|
||||
return true;
|
||||
}
|
||||
|
@ -560,7 +628,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;
|
||||
|
@ -588,7 +656,7 @@ bool GCodes::OffsetAxes(GCodeBuffer* gb)
|
|||
// booleans homeX, homeY and homeZ.
|
||||
// Returns true if completed, false if needs to be called again.
|
||||
// 'reply' is only written if there is an error.
|
||||
// 'error' is false on entry, gets changed to true iff there is an error.
|
||||
// 'error' is false on entry, gets changed to true if there is an error.
|
||||
bool GCodes::DoHome(char* reply, bool& error)
|
||||
//pre(reply.upb == STRING_LENGTH)
|
||||
{
|
||||
|
@ -634,6 +702,7 @@ bool GCodes::DoHome(char* reply, bool& error)
|
|||
|
||||
if(homeZ)
|
||||
{
|
||||
//FIXME this check should be optional
|
||||
if (!(axisIsHomed[X_AXIS] && axisIsHomed[Y_AXIS]))
|
||||
{
|
||||
// We can only home Z if X and Y have already been homed. Possibly this should only be if we are using an IR probe.
|
||||
|
@ -881,14 +950,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;
|
||||
}
|
||||
|
||||
|
@ -1113,7 +1188,7 @@ bool GCodes::StandbyHeaters()
|
|||
return false;
|
||||
for(int8_t heater = 0; heater < HEATERS; heater++)
|
||||
reprap.GetHeat()->Standby(heater);
|
||||
selectedHead = -1;
|
||||
selectedHead = -1; //FIXME check this does not mess up setters (eg M906) when they are used after this command is called
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -1455,16 +1530,34 @@ 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])&& drive<AXES)
|
||||
{
|
||||
platform->SetDriveStepsPerUnit(drive, gb->GetFValue());
|
||||
seen = true;
|
||||
}
|
||||
//FIXME handle selectedHead=-1 case
|
||||
//Just sets the currently selected head.
|
||||
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",
|
||||
if(!seen){
|
||||
snprintf(reply, STRING_LENGTH, "Steps/mm: X: %d, Y: %d, Z: %d, E: ",
|
||||
(int)platform->DriveStepsPerUnit(X_AXIS), (int)platform->DriveStepsPerUnit(Y_AXIS),
|
||||
(int)platform->DriveStepsPerUnit(Z_AXIS), (int)platform->DriveStepsPerUnit(AXES)); // FIXME - needs to do multiple extruders
|
||||
(int)platform->DriveStepsPerUnit(Z_AXIS));
|
||||
// Fixed to do multiple extruders.
|
||||
char * scratchString;
|
||||
for(int8_t drive = AXES; drive < DRIVES; drive++)
|
||||
{
|
||||
strncat(reply, ftoa(0,platform->DriveStepsPerUnit(drive),2), STRING_LENGTH);
|
||||
strncat(reply, ":", STRING_LENGTH);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
|
@ -1480,14 +1573,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);
|
||||
|
@ -1535,16 +1630,23 @@ 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;
|
||||
result = reprap.GetHeat()->AllHeatersAtSetTemperatures();
|
||||
break;
|
||||
|
||||
//TODO M119
|
||||
case 119:
|
||||
platform->Message(HOST_MESSAGE, "M119 - endstop status not yet implemented\n");
|
||||
break;
|
||||
|
||||
case 120:
|
||||
result = Push();
|
||||
break;
|
||||
|
@ -1580,28 +1682,54 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb)
|
|||
platform->Message(HOST_MESSAGE, "M141 - heated chamber not yet implemented\n");
|
||||
break;
|
||||
|
||||
case 160: //number of mixing filament drives
|
||||
if(gb->Seen('S'))
|
||||
{
|
||||
int iValue=gb->GetIValue();
|
||||
platform->SetMixingDrives(iValue);
|
||||
}
|
||||
break;
|
||||
case 190: // Deprecated...
|
||||
if(gb->Seen('S'))
|
||||
{
|
||||
float value=gb->GetFValue();
|
||||
reprap.GetHeat()->SetActiveTemperature(0, value);
|
||||
reprap.GetHeat()->SetStandbyTemperature(0, value); // FIXME have to set both?not sure as the bed should always be selected
|
||||
reprap.GetHeat()->Activate(0);
|
||||
}
|
||||
result = reprap.GetHeat()->HeaterAtSetTemperature(0);
|
||||
break;
|
||||
|
||||
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])&& drive<AXES)
|
||||
{
|
||||
platform->SetAcceleration(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])&& drive<AXES)
|
||||
{
|
||||
float value = gb->GetFValue()*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;
|
||||
|
||||
|
@ -1796,35 +1924,31 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb)
|
|||
}
|
||||
break;
|
||||
|
||||
// case 876: // TEMPORARY - this will go away...
|
||||
// if(gb->Seen('P'))
|
||||
// {
|
||||
// iValue = gb->GetIValue();
|
||||
// if(iValue != 1)
|
||||
// platform->SetHeatOn(0);
|
||||
// else
|
||||
// platform->SetHeatOn(1);
|
||||
// }
|
||||
// break;
|
||||
case 563: // Define tool
|
||||
|
||||
case 900:
|
||||
result = DoFileCannedCycles("homex.g");
|
||||
break;
|
||||
|
||||
case 901:
|
||||
result = DoFileCannedCycles("homey.g");
|
||||
case 564: // Think outside the box?
|
||||
if(gb->Seen('S'))
|
||||
limitAxes = (gb->GetIValue() != 0);
|
||||
break;
|
||||
|
||||
|
||||
|
||||
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])&& i<AXES)
|
||||
{
|
||||
float value = gb->GetFValue(); // 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;
|
||||
|
||||
|
@ -2034,7 +2158,7 @@ float GCodeBuffer::GetFValue()
|
|||
|
||||
const void GCodeBuffer::GetFloatArray(float a[], int& length)
|
||||
{
|
||||
length = -1;
|
||||
length = 0;
|
||||
if(readPointer < 0)
|
||||
{
|
||||
platform->Message(HOST_MESSAGE, "GCodes: Attempt to read a GCode float array before a search.\n");
|
||||
|
@ -2045,15 +2169,14 @@ const void GCodeBuffer::GetFloatArray(float a[], int& length)
|
|||
bool inList = true;
|
||||
while(inList)
|
||||
{
|
||||
length++;
|
||||
a[length] = (float)strtod(&gcodeBuffer[readPointer + 1], 0);
|
||||
length++;
|
||||
readPointer++;
|
||||
while(gcodeBuffer[readPointer] && (gcodeBuffer[readPointer] != ' ') && (gcodeBuffer[readPointer] != LIST_SEPARATOR))
|
||||
readPointer++;
|
||||
if(gcodeBuffer[readPointer] != LIST_SEPARATOR)
|
||||
inList = false;
|
||||
}
|
||||
length++;
|
||||
readPointer = -1;
|
||||
}
|
||||
|
||||
|
@ -2061,7 +2184,7 @@ const void GCodeBuffer::GetFloatArray(float a[], int& length)
|
|||
|
||||
const void GCodeBuffer::GetLongArray(long l[], int& length)
|
||||
{
|
||||
length = -1;
|
||||
length = 0;
|
||||
if(readPointer < 0)
|
||||
{
|
||||
platform->Message(HOST_MESSAGE, "GCodes: Attempt to read a GCode long array before a search.\n");
|
||||
|
@ -2072,15 +2195,14 @@ const void GCodeBuffer::GetLongArray(long l[], int& length)
|
|||
bool inList = true;
|
||||
while(inList)
|
||||
{
|
||||
length++;
|
||||
l[length] = strtol(&gcodeBuffer[readPointer + 1], 0, 0);
|
||||
length++;
|
||||
readPointer++;
|
||||
while(gcodeBuffer[readPointer] && (gcodeBuffer[readPointer] != ' ') && (gcodeBuffer[readPointer] != LIST_SEPARATOR))
|
||||
readPointer++;
|
||||
if(gcodeBuffer[readPointer] != LIST_SEPARATOR)
|
||||
inList = false;
|
||||
}
|
||||
length++;
|
||||
readPointer = -1;
|
||||
}
|
||||
|
||||
|
|
9
GCodes.h
9
GCodes.h
|
@ -25,6 +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 //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
|
||||
|
||||
class GCodeBuffer
|
||||
|
@ -79,6 +81,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?
|
||||
|
||||
|
@ -163,6 +166,7 @@ class GCodes
|
|||
bool cannedCycleMoveQueued; // True if a canned cycle move has been set
|
||||
bool zProbesSet; // True if all Z probing is done and we can set the bed equation
|
||||
float longWait; // Timer for things that happen occasionally (seconds)
|
||||
bool limitAxes; // Don't think outside the box.
|
||||
bool axisIsHomed[3]; // These record which of the axes have been homed
|
||||
};
|
||||
|
||||
|
@ -231,4 +235,9 @@ inline bool GCodes::RunConfigurationGCodes()
|
|||
return !DoFileCannedCycles(platform->GetConfigFile());
|
||||
}
|
||||
|
||||
inline int8_t GCodes::GetSelectedHead()
|
||||
{
|
||||
return selectedHead;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
22
Heat.cpp
22
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;
|
||||
}
|
||||
|
||||
//******************************************************************************************************
|
||||
|
||||
|
|
1
Heat.h
1
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:
|
||||
|
|
15
Move.cpp
15
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);
|
||||
|
@ -569,7 +570,7 @@ void Move::Interrupt()
|
|||
dda = NULL;
|
||||
}
|
||||
|
||||
|
||||
// creates a new lookahead object adds it to the lookahead ring, returns false if its full
|
||||
bool Move::LookAheadRingAdd(long ep[], float feedRate, float vv, bool ce, int8_t mt)
|
||||
{
|
||||
if(LookAheadRingFull())
|
||||
|
@ -1205,11 +1206,13 @@ float LookAhead::Cosine()
|
|||
return cosine;
|
||||
}
|
||||
|
||||
//Returns units (mm) from steps for a particular drive
|
||||
float LookAhead::MachineToEndPoint(int8_t drive, long coord)
|
||||
{
|
||||
return ((float)coord)/reprap.GetPlatform()->DriveStepsPerUnit(drive);
|
||||
}
|
||||
|
||||
//Returns steps from units (mm) for a particular drive
|
||||
long LookAhead::EndPointToMachine(int8_t drive, float coord)
|
||||
{
|
||||
return (long)roundf(coord*reprap.GetPlatform()->DriveStepsPerUnit(drive));
|
||||
|
|
37
Platform.cpp
37
Platform.cpp
|
@ -112,6 +112,7 @@ void Platform::Init()
|
|||
potWipes = POT_WIPES;
|
||||
senseResistor = SENSE_RESISTOR;
|
||||
maxStepperDigipotVoltage = MAX_STEPPER_DIGIPOT_VOLTAGE;
|
||||
numMixingDrives = NUM_MIXING_DRIVES;
|
||||
|
||||
// Z PROBE
|
||||
|
||||
|
@ -152,27 +153,31 @@ void Platform::Init()
|
|||
webDir = WEB_DIR;
|
||||
gcodeDir = GCODE_DIR;
|
||||
tempDir = TEMP_DIR;
|
||||
|
||||
/*
|
||||
FIXME Nasty having to specify individually if a pin is arduino or not.
|
||||
requires a unified variant file. If implemented this would be much better
|
||||
to allow for different hardware in the future
|
||||
*/
|
||||
for(i = 0; i < DRIVES; i++)
|
||||
{
|
||||
|
||||
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 +185,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,22 +199,21 @@ 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)) );
|
||||
tempSum[i] = 0;
|
||||
}
|
||||
|
||||
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();
|
||||
|
@ -386,10 +389,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);
|
||||
}
|
||||
|
||||
|
||||
|
|
63
Platform.h
63
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
|
||||
// Indices for motor current digipots (if any)
|
||||
|
@ -103,6 +103,7 @@ Licence: GPL
|
|||
#define ACCELERATIONS {800.0, 800.0, 10.0, 250.0} // mm/sec^2
|
||||
#define DRIVE_STEPS_PER_UNIT {87.4890, 87.4890, 4000.0, 420.0}
|
||||
#define INSTANT_DVS {15.0, 15.0, 0.2, 2.0} // (mm/sec)
|
||||
#define NUM_MIXING_DRIVES 1; //number of mixing drives
|
||||
|
||||
// AXES
|
||||
|
||||
|
@ -114,10 +115,16 @@ Licence: GPL
|
|||
#define Y_AXIS 1 // The index of the Y axis
|
||||
#define Z_AXIS 2 // The index of the Z axis
|
||||
|
||||
#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
|
||||
|
||||
// HEATERS - The bed is assumed to be the at index 0
|
||||
|
||||
#define TEMP_SENSE_PINS {5, 4} // Analogue pin numbers
|
||||
#define HEAT_ON_PINS {6, X5} // PWM pins
|
||||
#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
|
||||
|
@ -135,7 +142,7 @@ 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
|
||||
|
@ -145,7 +152,12 @@ Licence: GPL
|
|||
|
||||
#define POLL_TIME 0.006 // Poll the A to D converters this often (seconds)
|
||||
|
||||
#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
|
||||
|
||||
/****************************************************************************************************/
|
||||
|
||||
|
@ -493,6 +505,9 @@ class Platform
|
|||
void SetZProbe(int iZ);
|
||||
void SetZProbeType(int iZ);
|
||||
int GetZProbeType() const;
|
||||
//Mixing support
|
||||
void SetMixingDrives(int);
|
||||
int GetMixingDrives();
|
||||
|
||||
// Heat and temperature
|
||||
|
||||
|
@ -558,6 +573,7 @@ class Platform
|
|||
int zProbeADValue;
|
||||
float zProbeStopHeight;
|
||||
bool zProbeEnable;
|
||||
int8_t numMixingDrives;
|
||||
|
||||
// AXES
|
||||
|
||||
|
@ -731,7 +747,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);
|
||||
|
@ -741,7 +757,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);
|
||||
|
@ -754,13 +770,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);
|
||||
|
@ -891,8 +907,20 @@ inline int Platform::GetZProbeType() const
|
|||
return zProbeType;
|
||||
}
|
||||
|
||||
inline void Platform::SetMixingDrives(int num_drives)
|
||||
{
|
||||
if(num_drives>(DRIVES-AXES))
|
||||
{
|
||||
Message(HOST_MESSAGE, "More mixing extruder drives set with M160 than exist in firmware configuration\n");
|
||||
return;
|
||||
}
|
||||
numMixingDrives = num_drives;
|
||||
}
|
||||
|
||||
|
||||
inline int Platform::GetMixingDrives()
|
||||
{
|
||||
return numMixingDrives;
|
||||
}
|
||||
|
||||
//********************************************************************************************************
|
||||
|
||||
|
@ -959,11 +987,16 @@ inline float Platform::DMix(int8_t heater) const
|
|||
return dMix[heater];
|
||||
}
|
||||
|
||||
//Changed to be compatible with existing gcode norms
|
||||
// M106 S0 = fully off M106 S255 = fully on
|
||||
inline void Platform::CoolingFan(float speed)
|
||||
{
|
||||
//byte p = (byte)(255.0*fmin(1.0, fmax(0.0, speed))); //this reverts to 0= off, 1 = on if uncommented
|
||||
byte p = (byte)speed;
|
||||
p = 255 - p; //duet v0.6
|
||||
if(coolingFanPin < 0)
|
||||
return;
|
||||
analogWrite(coolingFanPin, (uint8_t)(speed*255.0));
|
||||
analogWriteNonDue(coolingFanPin, p);
|
||||
}
|
||||
|
||||
//inline void Platform::SetHeatOn(int8_t ho)
|
||||
|
|
Binary file not shown.
Reference in a new issue