void doDrillCycle(int gCode, FloatPoint &fp)
{
unsigned int dwell = 0;
float delta = 0.;
bool slowRetract=false;
bool fullRetract=true;
float oldZ = sharedMachineModel.localPosition.z;
fp.z=oldZ;
bool error = false;
switch(gCode)
{
case 85:
slowRetract=true;
// fall through
case 81:
// Check for error conditions
if( !(gc.seen[GCODE_X] || gc.seen[GCODE_Y]) // No X or Y given
//|| !(gc.seen[GCODE_Z]) // Z must be given
|| (gc.seen[GCODE_L] && gc.L<=0) // Loops given but not positive
|| gc.seen[GCODE_A] || gc.seen[GCODE_B] // Movements on rotational axes
|| sharedMachineModel.getCutterRadiusCompensation()!=0 // cutter radius compensation is active
)
error = true;
break;
case 89:
slowRetract=true;
// fall through
case 82:
if(!gc.seen[GCODE_P])
gc.P = sharedMachineModel.stickyP;
// Check for error conditions
if( !(gc.seen[GCODE_X] || gc.seen[GCODE_Y]) // No X or Y given
//|| !(gc.seen[GCODE_Z]) // Z must be given
//|| !(gc.seen[GCODE_P]) // P must be given
|| gc.P<0 // and >= 0
|| (gc.seen[GCODE_L] && gc.L<=0) // Loops given but not positive
|| gc.seen[GCODE_A] || gc.seen[GCODE_B] // Movements on rotational axes
|| sharedMachineModel.getCutterRadiusCompensation()!=0 // cutter radius compensation is active
)
error = true;
dwell = gc.P;
sharedMachineModel.stickyP = gc.P;
break;
case 73:
fullRetract=false;
// fall through
case 83:
if(!gc.seen[GCODE_Q])
gc.Q = sharedMachineModel.stickyQ;
// Check for error conditions
if( !(gc.seen[GCODE_X] || gc.seen[GCODE_Y]) // No X or Y given
//|| !(gc.seen[GCODE_Z]) // Z must be given
//|| !(gc.seen[GCODE_Q]) // Q must be given
|| gc.Q<=0. // and > 0
|| (gc.seen[GCODE_L] && gc.L<=0) // Loops given but not positive
|| gc.seen[GCODE_A] || gc.seen[GCODE_B] // Movements on rotational axes
|| sharedMachineModel.getCutterRadiusCompensation()!=0 // cutter radius compensation is active
)
error = true;
delta = gc.Q;
sharedMachineModel.stickyQ = gc.Q;
break;
}
if(error)
{
if(SendDebug & DEBUG_ERRORS)
sprintf(talkToHost.string(), "Dud G code: G81 with invalid or missing parameters");
talkToHost.setResend(gc.LastLineNrRecieved+1);
}
else
{
int loops = 1;
if(gc.seen[GCODE_L])
loops = gc.L;
if(gc.seen[GCODE_R])
sharedMachineModel.setRetractHeight(gc.R);
if(fp.z<sharedMachineModel.getRetractHeight())
{
FloatPoint move = sharedMachineModel.localPosition;
move.z = sharedMachineModel.getRetractHeight();
rapidMove(move);
fp.z = sharedMachineModel.getRetractHeight();
}
while(loops-->0)
{
rapidMove(fp);
if(fp.z!=sharedMachineModel.getRetractHeight())
{
fp.z = sharedMachineModel.getRetractHeight();
rapidMove(fp);
}
if(delta>0.)
{
float z = fp.z;
while(z>gc.Z)
{
z -= delta;
if(z<gc.Z)
z=gc.Z;
fp.z=z;
sharedMachineModel.qMove(fp);
if(z>gc.Z)
{
if(fullRetract)
fp.z=sharedMachineModel.getRetractHeight();
else
fp.z=z+sharedMachineModel.getClearanceIncrement();
rapidMove(fp);
fp.z=z-.5;
rapidMove(fp);
}
}
}
else
{
fp.z = gc.Z;
sharedMachineModel.qMove(fp);
}
if(dwell>0)
{
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
unsigned long endTime = millis() + (int)(dwell + 0.5);
while(millis() < endTime)
sharedMachineModel.manage(true);
}
if(loops==0 && sharedMachineModel.getRetractMode())
fp.z = oldZ;
else
fp.z = sharedMachineModel.getRetractHeight();
if(slowRetract)
sharedMachineModel.qMove(fp);
else
rapidMove(fp);
if(loops>0 && !sharedMachineModel.getAbsMode())
{
if (gc.seen[GCODE_X])
fp.x += gc.X;
if (gc.seen[GCODE_Y])
fp.y += gc.Y;
}
}
}
}
void execute_commands(char instruction[])
{
bool axisSelected;
// Do we have lineNr and checksums in this gcode?
if((bool)(gc.seen[GCODE_CHECKSUM]) | (bool)(gc.seen[GCODE_N]))
{
// Check that if recieved a L code, we also got a C code. If not, one of them has been lost, and we have to reset queue
if( (bool)(gc.seen[GCODE_CHECKSUM]) != (bool)(gc.seen[GCODE_N]) )
{
if(SendDebug & DEBUG_ERRORS)
{
if(gc.seen[GCODE_CHECKSUM])
sprintf(talkToHost.string(), "Serial Error: checksum without line number. Checksum: %d, line received: %s", gc.Checksum, instruction);
else
sprintf(talkToHost.string(), "Serial Error: line number without checksum. Linenumber: %ld, line received: %s", gc.N, instruction);
}
talkToHost.setResend(gc.LastLineNrRecieved+1);
return;
}
// Check checksum of this string. Flush buffers and re-request line of error is found
if(gc.seen[GCODE_CHECKSUM]) // if we recieved a line nr, we know we also recieved a Checksum, so check it
{
// Calc checksum.
byte checksum = 0;
byte count=0;
while(instruction[count] != '*')
checksum = checksum^instruction[count++];
// Check checksum.
if(gc.Checksum != (int)checksum)
{
if(SendDebug & DEBUG_ERRORS)
sprintf(talkToHost.string(), "Serial Error: checksum mismatch. Remote (%d) not equal to local (%d), line received: %s",
gc.Checksum, (int)checksum, instruction);
talkToHost.setResend(gc.LastLineNrRecieved+1);
return;
}
// Check that this lineNr is LastLineNrRecieved+1. If not, flush
if(!( (bool)(gc.seen[GCODE_M]) && gc.M == 110)) // unless this is a reset-lineNr command
if(gc.N != gc.LastLineNrRecieved+1)
{
if(SendDebug & DEBUG_ERRORS)
sprintf(talkToHost.string(), "Serial Error: Linenumber (%ld) is not last + 1 (%ld), line received: %s", gc.N, gc.LastLineNrRecieved+1, instruction);
talkToHost.setResend(gc.LastLineNrRecieved+1);
return;
}
//If we reach this point, communication is a succes, update our "last good line nr" and continue
gc.LastLineNrRecieved = gc.N;
}
}
/* if no command was seen, but parameters were, then use the last G code as
* the current command
*/
if ((!(gc.seen[GCODE_G] | gc.seen[GCODE_M] | gc.seen[GCODE_T])) && (seenAnything() && (last_gcode_g >= 0)))
{
/* yes - so use the previous command with the new parameters */
gc.G[0] = last_gcode_g;
gc.GIndex=1;
gc.seen[GCODE_G]=true;
}
// Deal with emergency stop as No 1 priority
if ((gc.seen[GCODE_M]) && (gc.M == 112))
sharedMachineModel.shutdown();
//did we get a gcode?
if (gc.seen[GCODE_G])
{
// Handle all GCodes in this line
for(int gIndex=0; gIndex<gc.GIndex; gIndex++)
{
last_gcode_g = gc.G[gIndex]; /* remember this for future instructions */
unsigned long endTime; // For Dwell
// Process the buffered move commands first
bool gCodeHandled=false;
switch (gc.G[gIndex])
{
////////////////////////
// Buffered commands
////////////////////////
case 0: //Rapid move
fetchCartesianParameters();
rapidMove(fp);
break;
case 1: // Controlled move;
fetchCartesianParameters();
sharedMachineModel.qMove(fp);
break;
case 2: // G2, Clockwise arc
case 3: // G3, Counterclockwise arc
fetchCartesianParameters();
if(gc.seen[GCODE_R])
{
//drawRadius(tempX, tempY, rVal, (gc.G[gIndex]==2));
if(SendDebug & DEBUG_ERRORS)
sprintf(talkToHost.string(), "Dud G code: G%d with R param not yet implemented", gc.G[gIndex]);
talkToHost.setResend(gc.LastLineNrRecieved+1);
}
else if(gc.seen[GCODE_I] || gc.seen[GCODE_J])
{
drawArc(fp.x+gc.I, fp.y+gc.J, fp.x, fp.y, (gc.G[gIndex]==2));
}
else
{
if(SendDebug & DEBUG_ERRORS)
sprintf(talkToHost.string(), "Dud G code: G%d without I or J params", gc.G[gIndex]);
talkToHost.setResend(gc.LastLineNrRecieved+1);
}
break;
case 28: //go home. If we send coordinates (regardless of their value) only zero those axes
fetchCartesianParameters();
axisSelected = false;
if(gc.seen[GCODE_Z])
{
sharedMachineModel.zeroZ();
axisSelected = true;
}
if(gc.seen[GCODE_X])
{
sharedMachineModel.zeroX();
axisSelected = true;
}
if(gc.seen[GCODE_Y])
{
sharedMachineModel.zeroY();
axisSelected = true;
}
if(!axisSelected)
{
sharedMachineModel.zeroZ();
sharedMachineModel.zeroX();
sharedMachineModel.zeroY();
sharedMachineModel.absolutePositionValid=true;
}
sharedMachineModel.localPosition.f = SLOW_FEEDRATE; // Most sensible feedrate to leave it in
break;
////////////////////////
// Non-Buffered commands
////////////////////////
case 4: //Dwell
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
endTime = millis() + (int)(gc.P + 0.5);
while(millis() < endTime)
sharedMachineModel.manage(true);
break;
case 20: //Inches for Units
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
sharedMachineModel.setUnits(false);
break;
case 21: //mm for Units
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
sharedMachineModel.setUnits(true);
break;
case 54:
case 55:
case 56:
case 57:
case 58:
case 59: // Switch to Workin Coordinate System
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
if(!sharedMachineModel.switchToWCS(gc.G[gIndex]-54))
{
if(SendDebug & DEBUG_ERRORS)
sprintf(talkToHost.string(), "Dud G code: G%d not possible, probably machine not homed", gc.G[gIndex]);
talkToHost.setResend(gc.LastLineNrRecieved+1);
}
break;
case 73: // Peck drilling cycle for milling - high-speed
case 81: // Drill Cycle
case 82: // Drill Cycle with dwell
case 83: // Drill Cycle peck drilling
case 85: // Drill Cycle, slow retract
case 89: // Drill Cycle with dwell and slow reredract
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
fetchCartesianParameters();
doDrillCycle(gc.G[gIndex], fp);
break;
case 90: //Absolute Positioning
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
sharedMachineModel.setAbsMode(true);
break;
case 91: //Incremental Positioning
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
sharedMachineModel.setAbsMode(false);
break;
case 92: //Set position as fp
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
fetchCartesianParameters();
sharedMachineModel.setLocalZero(fp);
break;
case 98: // Return to initial Z level in canned cycle
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
sharedMachineModel.setRetractMode(true);
break;
case 99: // Return to R level in canned cycle
sharedMachineModel.waitFor_qEmpty(); // Non-buffered G command. Wait till the buffer q is empty first
sharedMachineModel.setRetractMode(false);
break;
default:
if(SendDebug & DEBUG_ERRORS)
sprintf(talkToHost.string(), "Dud G code: G%d", gc.G[gIndex]);
talkToHost.setResend(gc.LastLineNrRecieved+1);
} // switch
} // for
} // Gcode
// Get feedrate if supplied and queue is empty
if ( gc.seen[GCODE_F] && sharedMachineModel.qEmpty())
sharedMachineModel.localPosition.f=MIN(gc.F, FAST_XY_FEEDRATE);
//find us an m code.
if (gc.seen[GCODE_M])
{
// Wait till the q is empty first
sharedMachineModel.waitFor_qEmpty();
switch (gc.M)
{
case 0:
sharedMachineModel.shutdown();
break;
case 1:
//todo: optional stop
break;
case 2:
//todo: program end
break;
case 6:
// Tool change
if(gc.seen[GCODE_T])
sharedMachineModel.manualToolChange(gc.T);
else
sharedMachineModel.manualToolChange(-1);
break;
//custom code for temperature control
// case 104:
// if (gc.seen[GCODE_S])
// {
// ex[extruder_in_use]->setTemperature((int)gc.S);
// }
// break;
//custom code for temperature reading
// case 105:
// talkToHost.setETemp(ex[extruder_in_use]->getTemperature());
// talkToHost.setBTemp(heatedBed.getTemperature());
// break;
//
// //turn fan on
// case 106:
// ex[extruder_in_use]->setCooler(255);
// break;
//
// //turn fan off
// case 107:
// ex[extruder_in_use]->setCooler(0);
// break;
//
//
// // Set the temperature and wait for it to get there
// case 109:
// ex[extruder_in_use]->setTemperature((int)gc.S);
// ex[extruder_in_use]->waitForTemperature();
// break;
// Starting a new print, reset the gc.LastLineNrRecieved counter
case 110:
if (gc.seen[GCODE_N])
gc.LastLineNrRecieved = gc.N;
break;
case 111:
SendDebug = gc.S;
break;
case 112: // STOP! (priority commnand)
sharedMachineModel.shutdown();
break;
//custom code for returning current coordinates
case 114:
talkToHost.setCoords(sharedMachineModel.localPosition);
break;
//Reserved for returning machine capabilities in keyword:value pairs
//custom code for returning Firmware Version and Capabilities
case 115:
talkToHost.capabilities();
break;
// // TODO: make this work properly
// case 116:
// ex[extruder_in_use]->waitForTemperature();
// break;
//custom code for returning zero-hit coordinates
// case 117:
// talkToHost.setCoords(zeroHit);
// break;
// The valve (real, or virtual...) is now the way to control any extruder (such as
// a pressurised paste extruder) that cannot move using E codes.
// // Open the valve
// case 126:
// ex[extruder_in_use]->valveSet(true, (int)(gc.P + 0.5));
// break;
//
// // Close the valve
// case 127:
// ex[extruder_in_use]->valveSet(false, (int)(gc.P + 0.5));
// break;
// case 140:
// if (gc.seen[GCODE_S])
// {
// heatedBed.setTemperature((int)gc.S);
// }
// break;
case 141: //TODO: set chamber temperature
break;
case 142: //TODO: set holding pressure
break;
// Pleasant Mill priority commands
// These commands are executed, even if the machine isn't in "armed for data" mode
case 900: // Machine-Identification
sprintf(talkToHost.string(), "Pleasant Mill[%s]", FW_VERSION);
break;
case 901: // Get Device Name
sprintf(talkToHost.string(), EEPROM_ReadString(EEPROM_ADR_DEVICENAME, strBuffer));
break;
case 902: // Set Device Name: The string argument is found in the commen
if(strlen(strArgBuffer)>0)
{
strBuffer[16]=0x0;
EEPROM_WriteString(EEPROM_ADR_DEVICENAME, strArgBuffer);
}
else
{
if(SendDebug & DEBUG_ERRORS)
sprintf(talkToHost.string(), "Error: M902 not possible, bad device name argument: %s", strArgBuffer);
talkToHost.setResend(gc.LastLineNrRecieved+1);
}
break;
default:
if(SendDebug & DEBUG_ERRORS)
sprintf(talkToHost.string(), "Dud M code: M%d", gc.M);
talkToHost.setResend(gc.LastLineNrRecieved+1);
}
}
// Tool (i.e. extruder) change?
if (gc.seen[GCODE_T])
{
sharedMachineModel.waitFor_qEmpty();
// newExtruder(gc.T);
}
}
void GCode::home(){
/// Move the tool to its home position
rapidMove(homePoint,homeZ);
}
void GCode::writeClosedLoop(QPFWPVector closedLoop){
int pos=0;
///the end point of an arc
QPointFWithParent endPoint(0,0);
/// a point in a line entinty weiither the start or end (becomle the start in a arc one)
QPointFWithParent point(0,0);
qDebug()<<"Writing the G-code of a loop composed of "<<closedLoop.size()<<"entities";
/// we first go to the lead point if any
while (pos < closedLoop.size()) {
//qDebug()<<pos;
point=closedLoop.at(pos);
if (point.parentType==QPointFWithParent::Arc) {
endPoint=closedLoop.at(pos+1);
/// go to the first point at rapid move
if (pos==0) {
rapidMove(point,0);
}
else{
feedRateMove(point,0);
}
ArcCut(endPoint,0, partTrans.map(endPoint.center)-partTrans.map(point),0,endPoint.cWise);
///@note Have to skip the other point definig the arc
pos=pos+2;
//qDebug()<<"dealing with an arc rad="<<endPoint.parentRadius<<"center="<<endPoint.centerX<<endPoint.centerY;
/// @todo Implment operator = in QPointFWIthParent & QPointF::operator= ( const QPointF & point )
point.setX(endPoint.x());
point.setY(endPoint.y());
}
else if (point.parentType==QPointFWithParent::Circle) {
qDebug()<<"Writing g code of circle at"<<pos;
circleCenter=point;
radius=point.parentRadius;
if (plasmaMode) {
/// @todo Get the stored leadTouch
attackPoint=point.leadTouch;//;point+QPointF(radius,0);
feedRateMove(attackPoint);
// now that we are on the circle cut it
ArcCut(attackPoint,0, partTrans.map(circleCenter)-partTrans.map(attackPoint),0);
}
else {
attackPoint=point+QPointF(radius,0);
//feedRateMove(point.x()+radius,point.y()+radius);
rapidMove( circleCenter,0);
//ArcCut(attackPoint.x(),attackPoint.y(),0,circleCenter.x()-attackPoint.x(),circleCenter.y()-attackPoint.y(),0);
ArcCut(circleCenter,0,circleCenter-attackPoint,0);
}
pos++;
}
else {
//qDebug()<<"line at"<<pos;
/// go to the first point at rapid move
if (pos==0) {
rapidMove( point,0);
}
else{
feedRateMove(point,0);
}
pos++;
}
/// @bug isn't this granted by feedRate and arccut?
setLastPos(point);
//qDebug()<<"last point: ("<<lastX<<" , " << lastY<<")" ;
}
//qDebug()<<"loop written";
}
void GCode::writeClosedLoop(QPFWPVector closedLoop){
int pos=0;
///the end point of an arc
QPointFWithParent endPoint(0,0);
/// a point in a line entinty weiither the start or end (becomle the start in a arc one)
QPointFWithParent point(0,0);
///the center of a circle
QPointFWithParent circle(0,0);
///FIXME: a problem with qlist dimension size pos+2 to solve
qDebug()<<"Writing the G-code of a loop composed of "<<closedLoop.size()<<"entities";
/// we first go to the lead point if any
while (pos < closedLoop.size()) {
//qDebug()<<pos;
point=closedLoop.at(pos);
if (point.parentType==QPointFWithParent::Arc) {
endPoint=closedLoop.at(pos+1);
/// go to the first point at rapid move
if (pos==0) {
rapidMove(point,0);
}
else{
feedRateMove(point,0);
}
ArcCut(endPoint,0, (endPoint.center-point),0,endPoint.cWise);
///@note Have to skip the other point definig the arc
pos=pos+2;
//qDebug()<<"dealing with an arc rad="<<endPoint.parentRadius<<"center="<<endPoint.centerX<<endPoint.centerY;
/// @todo Implment operator = in QPointFWIthParent & QPointF::operator= ( const QPointF & point )
point.setX(endPoint.x());
point.setY(endPoint.y());
}
else if (point.parentType==QPointFWithParent::Circle) {
qDebug()<<"Writing g code of circle at"<<pos;
circleCenter=point;
radius=point.parentRadius;
///We touch the circle
/** @note:
G1 X0.0 Y1.0 F20.0 ----go to X1.0, Y0.0 at a feed rate of 20
G2 X1.0 Y0.0 I0.0 J-1.0 ----go in an arc from X0.0, Y1.0 to X1.0 Y0.0, with the center of the arc at X0.0, Y0.0
G2 for a clockwise Arc , G3 for an anticlockWise one
*/
/// go to the center with G0
///rapidMove( circleCenter.x(),circleCenter.y(),0);
///start cutting till being onthe circle
if (plasmaMode) { /// cncMode=1 if we are cutting plamsa (should be the default mode)
///feedRateMove( attackPoint.x(),attackPoint.y(),0);
///@todo make this homogene with genrated lead @fixme should get the point from lead
attackPoint=point+QPointF(radius,0);
feedRateMove(attackPoint);
// now that we are on the circle cut it
ArcCut(attackPoint,0,circleCenter-attackPoint,0);
}
else {
attackPoint=point+QPointF(radius,0);
//feedRateMove(point.x()+radius,point.y()+radius);
rapidMove( circleCenter,0);
//ArcCut(attackPoint.x(),attackPoint.y(),0,circleCenter.x()-attackPoint.x(),circleCenter.y()-attackPoint.y(),0);
ArcCut(circleCenter,0,circleCenter-attackPoint,0);
}
pos++;
}
else {
//qDebug()<<"line at"<<pos;
/// go to the first point at rapid move
if (pos==0) {
rapidMove( point,0);
}
else{
feedRateMove(point,0);
}
pos++;
}
/// @fixme isn't this granted by feedRate and arccut?
setLastPos(point);
//qDebug()<<"last point: ("<<lastX<<" , " << lastY<<")" ;
}
//qDebug()<<"loop written";
}
