1167 lines
37 KiB
C#
1167 lines
37 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using MatterHackers.MatterControl.SlicerConfiguration;
|
|
using MatterHackers.VectorMath;
|
|
|
|
//Protocol Documentation found at: https://github.com/makerbot/s3g/blob/master/doc/s3gProtocol.md
|
|
|
|
namespace MatterHackers.MatterControl.Plugins.X3GDriver
|
|
{
|
|
public class X3GWriter
|
|
{
|
|
private PrinterConfig printer;
|
|
private X3GPrinterDetails printerDetails;
|
|
|
|
private Queue<byte[]> overFlowPackets;
|
|
|
|
private bool relativePos = false;
|
|
private int feedrate;
|
|
//private int activeExtruderIndex;
|
|
|
|
public static int lineNumber;
|
|
|
|
public X3GWriter()
|
|
{
|
|
overFlowPackets = new Queue<byte[]>();
|
|
printerDetails = new X3GPrinterDetails();
|
|
feedrate = 3200;
|
|
printerDetails.activeExtruderIndex = 0;
|
|
lineNumber = 0;
|
|
}
|
|
|
|
public X3GWriter(X3GPrinterDetails printerInfo, PrinterConfig printer)
|
|
{
|
|
this.printer = printer;
|
|
printerDetails = printerInfo;
|
|
overFlowPackets = new Queue<byte[]>();
|
|
feedrate = 3200;
|
|
printerDetails.activeExtruderIndex = 0;
|
|
lineNumber = 0;
|
|
}
|
|
|
|
public byte[] translate(string writemessage, out bool sendToPrinter)
|
|
{
|
|
byte[] convertedMessage = new byte[] { 0 };
|
|
List<string> commands;
|
|
X3GPacketFactory binaryPacket;
|
|
char commandType = writemessage[0];
|
|
sendToPrinter = true;
|
|
|
|
if (commandType == 'N') //Strips leading line number and post command checksum
|
|
{
|
|
|
|
lineNumber = (int)getParameterValue(writemessage, 'N');
|
|
int start = writemessage.IndexOf(' ', 0) + 1;
|
|
int checksumIndex = writemessage.IndexOf('*');
|
|
if (checksumIndex > 0)
|
|
{
|
|
writemessage = writemessage.Substring(start, checksumIndex - start);
|
|
}
|
|
else
|
|
{
|
|
writemessage = writemessage.Substring(start);
|
|
}
|
|
writemessage += "\r\n";
|
|
commandType = writemessage[0];
|
|
}
|
|
|
|
commands = parseGcode(writemessage); //gcode is parsed into a list of strings each corresponding to a parameter (example: G1X10Y38.5 => G1,X10,Y38.5)
|
|
|
|
//Convert Connect message to X3G
|
|
switch (commandType)
|
|
{
|
|
case 'M':
|
|
|
|
int commandVal = (int)getParameterValue(commands, 'M');
|
|
switch (commandVal)
|
|
{
|
|
case 73://Set Build Perc M73
|
|
binaryPacket = new X3GPacketFactory(150);
|
|
binaryPacket.addByte((byte)getParameterValue(commands, 'P'));
|
|
binaryPacket.addByte(0);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 82://set extruder to absolute move M82
|
|
sendToPrinter = false;
|
|
printerDetails.extruderRelativePos = false;
|
|
break;
|
|
case 83://set extruder to relative move M83
|
|
sendToPrinter = false;
|
|
printerDetails.extruderRelativePos = true;
|
|
break;
|
|
case 84://Stop idle hold (release motors) M84
|
|
binaryPacket = new X3GPacketFactory(137);
|
|
binaryPacket.addByte(31);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 92://set axis steps per unit M92
|
|
sendToPrinter = false;
|
|
updateStepsPerMm(commands);
|
|
|
|
break;
|
|
case 114://Get Current Position M114
|
|
binaryPacket = new X3GPacketFactory(21);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 115://connecting M115
|
|
binaryPacket = new X3GPacketFactory(0x00);
|
|
binaryPacket.add16bits(0x28);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
|
|
case 105://get temperature M105
|
|
binaryPacket = new X3GPacketFactory(0x0A);
|
|
binaryPacket.addByte(0x00);
|
|
binaryPacket.addByte(0x02);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
printerDetails.requiredTemperatureResponseCount = 1;
|
|
|
|
if (printer.Settings.GetValue<bool>(SettingsKey.has_heated_bed))//if it has a bed get the bed temp
|
|
{
|
|
binaryPacket = new X3GPacketFactory(10);
|
|
binaryPacket.addByte(0);
|
|
binaryPacket.addByte(30);
|
|
printerDetails.requiredTemperatureResponseCount++;
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
}
|
|
|
|
if (printer.Settings.GetValue<int>(SettingsKey.extruder_count) > 1)
|
|
{
|
|
binaryPacket = new X3GPacketFactory(10);
|
|
binaryPacket.addByte(1);
|
|
binaryPacket.addByte(2);
|
|
printerDetails.requiredTemperatureResponseCount++;
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
}
|
|
printerDetails.teperatureResponseCount = 0;
|
|
break;
|
|
case 104://set extruder temperature M104
|
|
int temp = (int)getParameterValue(commands, 'S');
|
|
byte extruder = (byte)getParameterValue(commands, 'T');
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(extruder);
|
|
binaryPacket.addByte(0x03);
|
|
binaryPacket.addByte(0x02);
|
|
binaryPacket.add16bits(temp);
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
|
|
binaryPacket = new X3GPacketFactory(136);//turns on cooling fan
|
|
binaryPacket.addByte(extruder);
|
|
binaryPacket.addByte(12);
|
|
binaryPacket.addByte(1);
|
|
binaryPacket.addByte(1);
|
|
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
|
|
printerDetails.targetTempForMakerbotStyleCommands = temp;
|
|
break;
|
|
case 109://set extruder temperature and wait M109
|
|
temp = (int)getParameterValue(commands, 'S');
|
|
extruder = (byte)getParameterValue(commands, 'T');
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(extruder);
|
|
binaryPacket.addByte(0x03);
|
|
binaryPacket.addByte(0x02);
|
|
binaryPacket.add16bits(temp);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(extruder);
|
|
binaryPacket.addByte(12);
|
|
binaryPacket.addByte(1);
|
|
binaryPacket.addByte(1);
|
|
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
|
|
binaryPacket = new X3GPacketFactory(135);
|
|
binaryPacket.addByte(0x00);
|
|
binaryPacket.add16bits(100);//delay between query packets in ms
|
|
binaryPacket.add16bits(1200);//timeout before continuing w/o tool ready in seconds
|
|
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
|
|
printerDetails.targetExtruderTemps[extruder] = temp;
|
|
printerDetails.heatingLockout = true;
|
|
|
|
break;
|
|
case 106://Fan On M106
|
|
int zeroCheck = (int)getParameterValue(commands, 'S');
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(0x00);
|
|
binaryPacket.addByte(13);
|
|
binaryPacket.addByte(1);
|
|
|
|
if (zeroCheck > 0)
|
|
{
|
|
binaryPacket.addByte(1);//If the value is not zero enable motor
|
|
}
|
|
else
|
|
{
|
|
binaryPacket.addByte(0);//If value is zero disable motor
|
|
}
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 107://Fan off M107
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(0x00);
|
|
binaryPacket.addByte(13);
|
|
binaryPacket.addByte(1);
|
|
binaryPacket.addByte(0);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 110://set current line number M110
|
|
lineNumber = (int)getParameterValue(commands, 'N');
|
|
sendToPrinter = false;
|
|
break;
|
|
case 117://Set Display message M117
|
|
binaryPacket = new X3GPacketFactory(149);
|
|
binaryPacket.addByte(4);
|
|
binaryPacket.addByte(0);
|
|
binaryPacket.addByte(0);
|
|
binaryPacket.addByte(20); //20 second timeout on message
|
|
for (int i = 1; i < commands.Count; i++)
|
|
{
|
|
byte b = Convert.ToByte(commands.ElementAt(i)[0]);
|
|
binaryPacket.addByte(b);
|
|
}
|
|
binaryPacket.addByte(0);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
|
|
break;
|
|
case 127://Disable extra output(fan) Makerbot M127
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(0x00);
|
|
binaryPacket.addByte(13);
|
|
binaryPacket.addByte(1);
|
|
binaryPacket.addByte(0);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 132://load axis offset of current home pos Makerbot M132
|
|
|
|
binaryPacket = new X3GPacketFactory(144);
|
|
binaryPacket.addByte(31);
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
|
|
break;
|
|
case 133://wait for toolhead to heat to target temp Makerbot M133
|
|
temp = printerDetails.targetTempForMakerbotStyleCommands;
|
|
extruder = (byte)getParameterValue(commands, 'T');
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(extruder);
|
|
binaryPacket.addByte(0x03);
|
|
binaryPacket.addByte(0x02);
|
|
binaryPacket.add16bits(temp);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(extruder);
|
|
binaryPacket.addByte(12);
|
|
binaryPacket.addByte(1);
|
|
binaryPacket.addByte(1);
|
|
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
|
|
binaryPacket = new X3GPacketFactory(135);
|
|
binaryPacket.addByte(0x00);
|
|
binaryPacket.add16bits(100);//delay between query packets in ms
|
|
binaryPacket.add16bits(1200);//timeout before continuing w/o tool ready in seconds
|
|
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
|
|
printerDetails.targetExtruderTemps[extruder] = temp;
|
|
printerDetails.heatingLockout = true;
|
|
break;
|
|
case 134://wait for build platform temp Makerbot M134
|
|
if (printer.Settings.GetValue<bool>(SettingsKey.has_heated_bed))
|
|
{
|
|
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(0);
|
|
binaryPacket.addByte(31);
|
|
binaryPacket.addByte(2);
|
|
binaryPacket.add16bits(printerDetails.targetBedTemp);
|
|
}
|
|
else
|
|
{
|
|
sendToPrinter = false;
|
|
}
|
|
|
|
break;
|
|
case 135://change toolhead Makerbot M135
|
|
printerDetails.activeExtruderIndex = (byte)getParameterValue(commands, 'T');
|
|
//Swaps active&inactive toolheads
|
|
float switchPositionHolder = printerDetails.activeExtruderPosition;
|
|
printerDetails.activeExtruderPosition = printerDetails.inactiveExtruderPosition;
|
|
printerDetails.inactiveExtruderPosition = switchPositionHolder;
|
|
//sends toolchange command to printer
|
|
binaryPacket = new X3GPacketFactory(134);
|
|
binaryPacket.addByte(printerDetails.activeExtruderIndex);
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 140://Set Bed temp M140
|
|
if (printer.Settings.GetValue<bool>(SettingsKey.has_heated_bed))
|
|
{
|
|
int temperature = (int)getParameterValue(commands, 'S');
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(0);
|
|
binaryPacket.addByte(31);
|
|
binaryPacket.addByte(2);
|
|
binaryPacket.add16bits(temperature);
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
printerDetails.targetBedTemp = temperature;
|
|
}
|
|
else
|
|
{
|
|
sendToPrinter = false;
|
|
}
|
|
|
|
break;
|
|
case 190://Wait for bed to reach target temp M190
|
|
if (printer.Settings.GetValue<bool>(SettingsKey.has_heated_bed))
|
|
{
|
|
int temperature = (int)getParameterValue(commands, 'S');
|
|
binaryPacket = new X3GPacketFactory(136);
|
|
binaryPacket.addByte(0);
|
|
binaryPacket.addByte(31);
|
|
binaryPacket.addByte(2);
|
|
binaryPacket.add16bits(temperature);
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
printerDetails.targetBedTemp = temperature;
|
|
|
|
binaryPacket = new X3GPacketFactory(141);
|
|
binaryPacket.addByte(0);
|
|
binaryPacket.add16bits(100);
|
|
binaryPacket.add16bits(1200);
|
|
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
printerDetails.heatingLockout = true;
|
|
}
|
|
else
|
|
{
|
|
sendToPrinter = false;
|
|
}
|
|
|
|
break;
|
|
case 206://Positional offset for bed M206
|
|
sendToPrinter = false;
|
|
updateBedOffset(commands);
|
|
|
|
break;
|
|
//The following are fake gcode commands to do features that are not included in gCode or are needed for printer initialization
|
|
case 1200: //Build Start Notification M1200
|
|
binaryPacket = new X3GPacketFactory(153);
|
|
binaryPacket.add32bits(0);
|
|
for (int i = 1; i < commands.Count; i++)
|
|
{
|
|
byte b = Convert.ToByte(commands.ElementAt(i)[0]);
|
|
binaryPacket.addByte(b);
|
|
}
|
|
//binaryPacket.addByte(77);
|
|
//binaryPacket.addByte(67);
|
|
binaryPacket.addByte(0);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 1201: //Build End Notification M1201
|
|
binaryPacket = new X3GPacketFactory(154);
|
|
binaryPacket.addByte(0);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
printerDetails.heatingLockout = false;
|
|
printerDetails.targetBedTemp = 0;
|
|
printerDetails.targetExtruderTemps[0] = 0;
|
|
printerDetails.targetExtruderTemps[1] = 0;
|
|
break;
|
|
case 1202: //dtr hi-low (reset) M1202
|
|
binaryPacket = new X3GPacketFactory(3);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 1203: //toolhead offset M1203
|
|
sendToPrinter = false;
|
|
printerDetails.extruderOffset = new Vector2(getParameterValue(commands, 'X'), getParameterValue(commands, 'Y'));
|
|
break;
|
|
default:
|
|
sendToPrinter = false;
|
|
convertedMessage = new byte[] { 0 };
|
|
break;
|
|
}
|
|
|
|
break;
|
|
|
|
case 'G':
|
|
int commandValue = getCommandValue(commands[0]);
|
|
|
|
switch (commandValue)
|
|
{
|
|
case 0:
|
|
case 1://Move G0 Xnnn Ynnn Znnn Ennn Fnnn Snnn G0/G1
|
|
|
|
if (FeedrateOnly(writemessage))
|
|
{
|
|
sendToPrinter = false;
|
|
updateFeedRate((int)getParameterValue(commands, 'F'));
|
|
}
|
|
else
|
|
{
|
|
if (!relativePos)
|
|
{
|
|
binaryPacket = new X3GPacketFactory(155);//Host command code
|
|
updateTargetPostition(commands);
|
|
updateFeedRate((int)getParameterValue(commands, 'F'));
|
|
binaryPacket.add32bits((long)((printerDetails.targetMovePosition.X) * printerDetails.stepsPerMm.X));
|
|
binaryPacket.add32bits((long)((printerDetails.targetMovePosition.Y) * printerDetails.stepsPerMm.Y));
|
|
binaryPacket.add32bits((long)((printerDetails.targetMovePosition.Z) * printerDetails.stepsPerMm.Z));
|
|
if (printerDetails.activeExtruderIndex == 0)//checks which extruder is active and
|
|
{
|
|
binaryPacket.add32bits((long)((printerDetails.targetExtruderPosition) * printerDetails.extruderStepsPerMm));//First extruder
|
|
binaryPacket.add32bits((long)(printerDetails.inactiveExtruderPosition * printerDetails.extruderStepsPerMm)); //second extruder
|
|
}
|
|
else
|
|
{
|
|
binaryPacket.add32bits((long)(printerDetails.inactiveExtruderPosition * printerDetails.extruderStepsPerMm));
|
|
binaryPacket.add32bits((long)((printerDetails.targetExtruderPosition) * printerDetails.extruderStepsPerMm));//second extruder
|
|
}
|
|
|
|
binaryPacket.add32bits((long)(feedrate * (printerDetails.stepsPerMm.X / 60)));//feedrate in steps/second
|
|
binaryPacket.addByte(getRelativeMovementAxes(commands));//specifies which axes should make a relative move:none (0)
|
|
float move = CalculateMoveInMM(commands);
|
|
binaryPacket.addFloat(move);//this calculates time(needs length of the move target - current and get magnitude) (expected in mm)
|
|
binaryPacket.add16bits((feedrate / 60) * 64);//feedrate(mm/s) mult by 64 used with above float to calc time
|
|
|
|
//Update of the position is now down when an OK is returned from the printer
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
}
|
|
else
|
|
{
|
|
binaryPacket = new X3GPacketFactory(155);
|
|
updateTargetPostition(commands);
|
|
updateFeedRate((int)getParameterValue(commands, 'F'));
|
|
binaryPacket.add32bits((long)((printerDetails.targetMovePosition.X) * printerDetails.stepsPerMm.X));
|
|
binaryPacket.add32bits((long)((printerDetails.targetMovePosition.Y) * printerDetails.stepsPerMm.Y));
|
|
binaryPacket.add32bits((long)((printerDetails.targetMovePosition.Z) * printerDetails.stepsPerMm.Z));
|
|
if (printerDetails.activeExtruderIndex == 0)
|
|
{
|
|
binaryPacket.add32bits((long)(printerDetails.targetExtruderPosition * printerDetails.extruderStepsPerMm));
|
|
binaryPacket.add32bits(0);
|
|
}
|
|
else
|
|
{
|
|
binaryPacket.add32bits(0);
|
|
binaryPacket.add32bits((long)(printerDetails.targetExtruderPosition * printerDetails.extruderStepsPerMm));
|
|
}
|
|
|
|
|
|
binaryPacket.add32bits((long)(feedrate * (printerDetails.stepsPerMm.X) / 60));
|
|
binaryPacket.addByte(getRelativeMovementAxes(commands));//specifies which axes should make a relative move: all(31)
|
|
float move = CalculateMoveInMM(commands);
|
|
binaryPacket.addFloat(move);
|
|
binaryPacket.add16bits((feedrate / 60) * 64);
|
|
|
|
printerDetails.targetMovePosition.X = printerDetails.currentPosition.X + printerDetails.targetMovePosition.X;
|
|
printerDetails.targetMovePosition.Y = printerDetails.currentPosition.Y + printerDetails.targetMovePosition.Y;
|
|
printerDetails.targetMovePosition.Z = printerDetails.currentPosition.Z + printerDetails.targetMovePosition.Z;
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
}
|
|
}
|
|
|
|
|
|
break;
|
|
case 2:
|
|
case 3://Controlled Arc Move G2 Xnnn Ynnn Innn Jnnn Ennn Fnnn (clockwise arc) G3 (counter-Clockwise)
|
|
break;
|
|
case 28://Move to Origin (home) G28 (Flags)X Y Z
|
|
byte axesBitfeild = 0;
|
|
if (commandHasNoParameters(commands))//If there are no parameters home all axes
|
|
{
|
|
binaryPacket = new X3GPacketFactory(132);
|
|
binaryPacket.addByte(3);
|
|
binaryPacket.add32bits((long)printerDetails.homingFeedRate.X);
|
|
binaryPacket.add16bits(45);//Time out in seconds
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
printerDetails.targetMovePosition.X = printerDetails.positionalOffset.X;
|
|
printerDetails.targetMovePosition.Y = printerDetails.positionalOffset.Y;
|
|
printerDetails.targetMovePosition.Z = printerDetails.positionalOffset.Z;
|
|
axesBitfeild = 7;
|
|
}
|
|
else//Otherwise check for which axes should be homed
|
|
{
|
|
if (checkCommandForFlag(writemessage, 'X'))
|
|
{
|
|
axesBitfeild = 1;
|
|
printerDetails.targetMovePosition.X = printerDetails.positionalOffset.X;
|
|
}
|
|
if (checkCommandForFlag(writemessage, 'Y'))
|
|
{
|
|
axesBitfeild += 2;
|
|
printerDetails.targetMovePosition.Y = printerDetails.positionalOffset.Y;
|
|
}
|
|
binaryPacket = new X3GPacketFactory(132);
|
|
binaryPacket.addByte(axesBitfeild);
|
|
binaryPacket.add32bits((long)printerDetails.homingFeedRate.X);
|
|
binaryPacket.add16bits(45);//Time out
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
if (checkCommandForFlag(writemessage, 'Z'))
|
|
{
|
|
axesBitfeild += 4;
|
|
printerDetails.targetMovePosition.Z = printerDetails.positionalOffset.Z;
|
|
}
|
|
}
|
|
|
|
if (axesBitfeild > 3 || axesBitfeild == 0)//handles Z homing
|
|
{
|
|
binaryPacket = new X3GPacketFactory(131); //This Will Home Z if it is specified
|
|
binaryPacket.addByte(0x04);
|
|
binaryPacket.add32bits((long)printerDetails.homingFeedRate.Z);
|
|
binaryPacket.add16bits(45);
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
}
|
|
|
|
printerDetails.currentPosition.X = printerDetails.targetMovePosition.X;
|
|
printerDetails.currentPosition.Y = printerDetails.targetMovePosition.Y;
|
|
printerDetails.currentPosition.Z = printerDetails.targetMovePosition.Z;
|
|
printerDetails.activeExtruderPosition = 0;
|
|
printerDetails.inactiveExtruderPosition = 0;
|
|
printerDetails.targetExtruderPosition = 0;
|
|
|
|
//Set position of homed position the inverse of the printer offset (this will turn 0,0 for the printer to be the same as what MC expects)
|
|
binaryPacket = new X3GPacketFactory(140);
|
|
binaryPacket.add32bits((long)(printerDetails.targetMovePosition.X * printerDetails.stepsPerMm.X));
|
|
binaryPacket.add32bits((long)(printerDetails.targetMovePosition.Y * printerDetails.stepsPerMm.Y));
|
|
binaryPacket.add32bits((long)(printerDetails.targetMovePosition.Z * printerDetails.stepsPerMm.Z));
|
|
binaryPacket.add32bits(0);
|
|
binaryPacket.add32bits(0);
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
|
|
break;
|
|
case 29://Detailed Z-Probe G29
|
|
break;
|
|
case 30://Single Z-Probe G30
|
|
break;
|
|
case 4://Dwell G4 Pnnn or Snnn (P = milliseconds S = seconds)
|
|
binaryPacket = new X3GPacketFactory(0x85);
|
|
long i = (long)getParameterValue(commands, 'P');
|
|
if (i == 0)
|
|
{
|
|
i = (long)(getParameterValue(commands, 'S') * 1000);
|
|
}
|
|
printerDetails.dwellTime = i;
|
|
binaryPacket.add32bits(i);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 10://Retract G10
|
|
break;
|
|
case 11://UnRetract G11
|
|
break;
|
|
case 20://set units to inches G20
|
|
break;
|
|
case 21://set units to Millimeters G21
|
|
sendToPrinter = false;
|
|
break;
|
|
case 90://Set to Absolute Positioning G90
|
|
relativePos = false;
|
|
printerDetails.extruderRelativePos = false;
|
|
sendToPrinter = false;
|
|
break;
|
|
case 91://set to Relative Positioning G91
|
|
relativePos = true;
|
|
printerDetails.extruderRelativePos = true;
|
|
sendToPrinter = false;
|
|
break;
|
|
case 92://Set Position G92 Xnnn Ynnn Znnn Ennn
|
|
binaryPacket = new X3GPacketFactory(0x8C);
|
|
updateTargetPostition(commands);
|
|
binaryPacket.add32bits((long)(printerDetails.targetMovePosition.X * printerDetails.stepsPerMm.X));
|
|
binaryPacket.add32bits((long)(printerDetails.targetMovePosition.Y * printerDetails.stepsPerMm.Y));
|
|
binaryPacket.add32bits((long)(printerDetails.targetMovePosition.Z * printerDetails.stepsPerMm.Z));
|
|
if (printerDetails.activeExtruderIndex == 0)
|
|
{
|
|
binaryPacket.add32bits((long)(printerDetails.targetExtruderPosition * printerDetails.extruderStepsPerMm));
|
|
binaryPacket.add32bits((long)(printerDetails.inactiveExtruderPosition * printerDetails.extruderStepsPerMm));
|
|
}
|
|
else
|
|
{
|
|
binaryPacket.add32bits((long)(printerDetails.inactiveExtruderPosition * printerDetails.extruderStepsPerMm));
|
|
binaryPacket.add32bits((long)(printerDetails.targetExtruderPosition * printerDetails.extruderStepsPerMm));
|
|
}
|
|
|
|
printerDetails.currentPosition = new Vector3(printerDetails.targetMovePosition); //sets the current position to the targeted move position
|
|
printerDetails.activeExtruderPosition = printerDetails.targetExtruderPosition;
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 130://Set digital Potentiometer G130 Xnn Ynn Znn Ann Bnn
|
|
if (checkCommandForFlag(commands, 'X'))
|
|
{
|
|
binaryPacket = new X3GPacketFactory(145);
|
|
binaryPacket.addByte(0);
|
|
binaryPacket.addByte((byte)getParameterValue(commands, 'X'));
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
}
|
|
if (checkCommandForFlag(commands, 'Y'))
|
|
{
|
|
binaryPacket = new X3GPacketFactory(145);
|
|
binaryPacket.addByte(1);
|
|
binaryPacket.addByte((byte)getParameterValue(commands, 'Y'));
|
|
if (convertedMessage != null)
|
|
{
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
}
|
|
else
|
|
{
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
}
|
|
}
|
|
if (checkCommandForFlag(commands, 'Z'))
|
|
{
|
|
binaryPacket = new X3GPacketFactory(145);
|
|
binaryPacket.addByte(2);
|
|
binaryPacket.addByte((byte)getParameterValue(commands, 'Z'));
|
|
if (convertedMessage != null)
|
|
{
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
}
|
|
else
|
|
{
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
}
|
|
}
|
|
if (checkCommandForFlag(commands, 'A'))
|
|
{
|
|
binaryPacket = new X3GPacketFactory(145);
|
|
binaryPacket.addByte(3);
|
|
binaryPacket.addByte((byte)getParameterValue(commands, 'A'));
|
|
if (convertedMessage != null)
|
|
{
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
}
|
|
else
|
|
{
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
}
|
|
}
|
|
if (checkCommandForFlag(commands, 'B'))
|
|
{
|
|
binaryPacket = new X3GPacketFactory(145);
|
|
binaryPacket.addByte(4);
|
|
binaryPacket.addByte((byte)getParameterValue(commands, 'B'));
|
|
if (convertedMessage != null)
|
|
{
|
|
overFlowPackets.Enqueue(binaryPacket.getX3GPacket());
|
|
}
|
|
else
|
|
{
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
}
|
|
}
|
|
break;
|
|
case 161://Home axis to minimum G161 Z Fnnn
|
|
double targetFeedrate = getParameterValue(commands, 'F');
|
|
if (targetFeedrate < printerDetails.homingFeedRate.Z)
|
|
{
|
|
targetFeedrate = printerDetails.homingFeedRate.Z;
|
|
}
|
|
binaryPacket = new X3GPacketFactory(131);
|
|
binaryPacket.addByte(0x04);
|
|
binaryPacket.add32bits((long)targetFeedrate);
|
|
binaryPacket.add16bits(45);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
//Set positional details
|
|
printerDetails.targetMovePosition.Z = printerDetails.positionalOffset.Z;
|
|
printerDetails.currentPosition.Z = printerDetails.targetMovePosition.Z;
|
|
|
|
break;
|
|
case 162://Home axis to maximum G162 X Y Fnnn
|
|
targetFeedrate = getParameterValue(commands, 'F');
|
|
if (targetFeedrate < printerDetails.homingFeedRate.X)
|
|
{
|
|
targetFeedrate = printerDetails.homingFeedRate.X;
|
|
}
|
|
binaryPacket = new X3GPacketFactory(132);
|
|
binaryPacket.addByte(0x03);
|
|
binaryPacket.add32bits((long)targetFeedrate);
|
|
binaryPacket.add16bits(45);
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
//Set positional details
|
|
printerDetails.targetMovePosition.X = printerDetails.positionalOffset.X;
|
|
printerDetails.targetMovePosition.Y = printerDetails.positionalOffset.Y;
|
|
printerDetails.currentPosition.X = printerDetails.targetMovePosition.X;
|
|
printerDetails.currentPosition.Y = printerDetails.targetMovePosition.Y;
|
|
|
|
break;
|
|
default:
|
|
sendToPrinter = false;
|
|
convertedMessage = new byte[] { 0 };
|
|
break;
|
|
}
|
|
|
|
break;
|
|
|
|
case 'T'://Change toolhead
|
|
printerDetails.activeExtruderIndex = (byte)getParameterValue(commands, 'T');
|
|
//Swaps active&inactive toolheads
|
|
float positionHolder = printerDetails.activeExtruderPosition;
|
|
printerDetails.activeExtruderPosition = printerDetails.inactiveExtruderPosition;
|
|
printerDetails.inactiveExtruderPosition = positionHolder;
|
|
//sends toolchange command to printer
|
|
binaryPacket = new X3GPacketFactory(134);
|
|
binaryPacket.addByte(printerDetails.activeExtruderIndex);
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
case 'X'://Used to test binary commands typed in via terminal(ex. Home x&y: X132 B03 L500 I30)
|
|
writemessage = writemessage.Substring(1);
|
|
int arraySize = commands.Count;
|
|
|
|
binaryPacket = new X3GPacketFactory((byte)getParameterValue(commands, 'X'));
|
|
|
|
for (int i = 1; i < arraySize; i++)
|
|
{
|
|
if (commands.ElementAt(i) != null && commands.ElementAt(i) != "")
|
|
{
|
|
char c = commands[i][0];
|
|
commands[i] = commands[i].Substring(1);
|
|
switch (c)
|
|
{
|
|
case 'b':
|
|
case 'B':
|
|
binaryPacket.addByte(Byte.Parse(commands[i]));
|
|
break;
|
|
case 'i':
|
|
case 'I':
|
|
binaryPacket.add16bits(int.Parse(commands[i]));
|
|
break;
|
|
case 'l':
|
|
case 'L':
|
|
binaryPacket.add32bits(long.Parse(commands[i]));
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
convertedMessage = binaryPacket.getX3GPacket();
|
|
break;
|
|
default:
|
|
convertedMessage = new byte[] { 0 };
|
|
sendToPrinter = false;
|
|
break;
|
|
}//End Switch
|
|
|
|
|
|
return convertedMessage;
|
|
}
|
|
|
|
private byte getRelativeMovementAxes(List<string> commands)
|
|
{
|
|
byte axes = 0;//defaults all to absolute
|
|
if (relativePos)
|
|
{
|
|
axes = 31;//sets all movements axes to be relative
|
|
}
|
|
else if (printerDetails.extruderRelativePos)
|
|
{
|
|
axes = 24;//sets both extruders to be relative moves
|
|
}
|
|
|
|
return axes;
|
|
}
|
|
|
|
private void updateBedOffset(List<string> commands)
|
|
{
|
|
float offSet;
|
|
if (checkCommandForFlag(commands, 'X'))
|
|
{
|
|
offSet = getParameterValue(commands, 'X');
|
|
printerDetails.positionalOffset.X = offSet;
|
|
}
|
|
|
|
if (checkCommandForFlag(commands, 'Y'))
|
|
{
|
|
offSet = getParameterValue(commands, 'Y');
|
|
printerDetails.positionalOffset.Y = offSet;
|
|
|
|
}
|
|
|
|
if (checkCommandForFlag(commands, 'Z'))
|
|
{
|
|
offSet = getParameterValue(commands, 'Z');
|
|
printerDetails.positionalOffset.Z = offSet;
|
|
|
|
}
|
|
}
|
|
|
|
private void updateStepsPerMm(List<string> commands)
|
|
{
|
|
float steps = getParameterValue(commands, 'X');
|
|
if (steps != 0)
|
|
{
|
|
printerDetails.stepsPerMm.X = steps;
|
|
}
|
|
steps = getParameterValue(commands, 'Y');
|
|
if (steps != 0)
|
|
{
|
|
printerDetails.stepsPerMm.Y = steps;
|
|
}
|
|
steps = getParameterValue(commands, 'Z');
|
|
if (steps != 0)
|
|
{
|
|
printerDetails.stepsPerMm.Z = steps;
|
|
}
|
|
steps = getParameterValue(commands, 'E');
|
|
if (steps != 0)
|
|
{
|
|
printerDetails.extruderStepsPerMm = (long)steps;
|
|
}
|
|
}
|
|
|
|
public void updateCurrentPosition()
|
|
{
|
|
printerDetails.currentPosition = new Vector3(printerDetails.targetMovePosition); //sets the current position to the targeted move position
|
|
printerDetails.activeExtruderPosition = printerDetails.targetExtruderPosition;
|
|
}
|
|
|
|
public Queue<byte[]> GetAndClearOverflowPackets()
|
|
{
|
|
Queue<byte[]> temp = new Queue<byte[]>(overFlowPackets);
|
|
overFlowPackets.Clear();
|
|
|
|
return temp;
|
|
}
|
|
|
|
private bool FeedrateOnly(string writemessage)
|
|
{
|
|
bool feedRateOnlyCheck = checkCommandForFlag(writemessage, 'F');
|
|
|
|
if (feedRateOnlyCheck)
|
|
{
|
|
feedRateOnlyCheck = !(checkCommandForFlag(writemessage, 'X') || checkCommandForFlag(writemessage, 'Y') || checkCommandForFlag(writemessage, 'Z') || checkCommandForFlag(writemessage, 'E'));
|
|
}
|
|
|
|
return feedRateOnlyCheck;
|
|
}
|
|
|
|
private List<string> parseGcode(string writemessage) //Because of different valid input I am now breaking the gcode command into a list of strings with each letter and values in their own string
|
|
{
|
|
List<string> gCodeList = new List<string>();
|
|
StringBuilder singleParameterValuePair = new StringBuilder();
|
|
foreach (char c in writemessage)
|
|
{
|
|
if (Char.IsDigit(c))
|
|
{
|
|
singleParameterValuePair.Append(c);
|
|
}
|
|
else
|
|
{
|
|
if ((Char.IsLetter(c) && singleParameterValuePair.Length == 0) || Char.IsPunctuation(c))
|
|
{
|
|
singleParameterValuePair.Append(c);
|
|
}
|
|
else
|
|
{
|
|
if (singleParameterValuePair.Length != 0)
|
|
{
|
|
gCodeList.Add(singleParameterValuePair.ToString());
|
|
singleParameterValuePair.Clear();
|
|
if (Char.IsLetter(c))
|
|
{
|
|
singleParameterValuePair.Append(c);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}//End else
|
|
}
|
|
|
|
return gCodeList;
|
|
}
|
|
|
|
private bool moveIsExtrudeOnly(List<string> commands)
|
|
{
|
|
int numParamsNotExtrude = commands.Count((x => x[0] != 'E' && x[0] != 'F'));
|
|
|
|
return numParamsNotExtrude <= 1;//The G command will count as 1 so if there are more then there are other params
|
|
}
|
|
|
|
private float CalculateMoveInMM(List<string> commands)
|
|
{
|
|
float move;
|
|
if (moveIsExtrudeOnly(commands))
|
|
{
|
|
if (relativePos)
|
|
{
|
|
move = printerDetails.targetExtruderPosition;
|
|
}
|
|
else
|
|
{
|
|
move = (printerDetails.targetExtruderPosition - printerDetails.activeExtruderPosition);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (relativePos)
|
|
{
|
|
move = (float)printerDetails.targetMovePosition.Length;
|
|
}
|
|
else
|
|
{
|
|
move = (float)(printerDetails.targetMovePosition - printerDetails.currentPosition).Length;
|
|
}
|
|
}
|
|
|
|
return move;
|
|
}
|
|
|
|
private bool commandHasNoParameters(string writemessage)
|
|
{
|
|
char[] whitespace = { ' ', '\r', '\n' };
|
|
int firstWhitspace = writemessage.IndexOfAny(whitespace);
|
|
return (firstWhitspace + 2) >= writemessage.Length;
|
|
}
|
|
|
|
private bool commandHasNoParameters(List<string> commands)
|
|
{
|
|
return commands.Count <= 1;
|
|
}
|
|
|
|
private void updateFeedRate(int newFeedrate)
|
|
{
|
|
if (newFeedrate != 0)
|
|
{
|
|
feedrate = newFeedrate;
|
|
}
|
|
}
|
|
|
|
private long CalculateDDA()
|
|
{
|
|
return ((60000000 / (long)(printerDetails.stepsPerMm.X * feedrate)));
|
|
}
|
|
|
|
private void updateTargetPostition(List<string> commands)
|
|
{
|
|
float temp;
|
|
if (checkCommandForFlag(commands, 'X'))
|
|
{
|
|
temp = getParameterValue(commands, 'X');
|
|
if (!relativePos)
|
|
{
|
|
if (printerDetails.activeExtruderIndex == 0)
|
|
{
|
|
printerDetails.targetMovePosition.X = temp;
|
|
}
|
|
else
|
|
{
|
|
printerDetails.targetMovePosition.X = (temp - printerDetails.extruderOffset.X);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
printerDetails.targetMovePosition.X = temp;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
if (relativePos)
|
|
{
|
|
printerDetails.targetMovePosition.X = 0;
|
|
}
|
|
else
|
|
{
|
|
printerDetails.targetMovePosition.X = printerDetails.currentPosition.X;
|
|
}
|
|
}//End X position
|
|
if (checkCommandForFlag(commands, 'Y'))
|
|
{
|
|
temp = getParameterValue(commands, 'Y');
|
|
if (!relativePos)
|
|
{
|
|
if (printerDetails.activeExtruderIndex == 0)
|
|
{
|
|
printerDetails.targetMovePosition.Y = temp;
|
|
}
|
|
else
|
|
{
|
|
printerDetails.targetMovePosition.Y = (temp - printerDetails.extruderOffset.Y);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
printerDetails.targetMovePosition.Y = temp;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
if (relativePos)
|
|
{
|
|
printerDetails.targetMovePosition.Y = 0;
|
|
}
|
|
else
|
|
{
|
|
printerDetails.targetMovePosition.Y = printerDetails.currentPosition.Y;
|
|
}
|
|
}//End Y position
|
|
|
|
if (checkCommandForFlag(commands, 'Z'))
|
|
{
|
|
temp = getParameterValue(commands, 'Z');
|
|
printerDetails.targetMovePosition.Z = temp;
|
|
}
|
|
else
|
|
{
|
|
if (relativePos)
|
|
{
|
|
printerDetails.targetMovePosition.Z = 0;
|
|
}
|
|
else
|
|
{
|
|
printerDetails.targetMovePosition.Z = printerDetails.currentPosition.Z;
|
|
}
|
|
}//end Zposition
|
|
if (checkCommandForFlag(commands, 'E'))
|
|
{
|
|
temp = getParameterValue(commands, 'E');
|
|
printerDetails.targetExtruderPosition = (temp * -1); //Extruder is inverted on makerbot. not sure why.
|
|
}
|
|
else
|
|
{
|
|
if (printerDetails.extruderRelativePos)
|
|
{
|
|
printerDetails.targetExtruderPosition = 0;
|
|
}
|
|
else
|
|
{
|
|
printerDetails.targetExtruderPosition = printerDetails.activeExtruderPosition;
|
|
}
|
|
}//End Extruder Pos
|
|
|
|
}
|
|
|
|
private bool checkCommandForFlag(string command, char p)
|
|
{
|
|
return command.Contains(p);
|
|
}
|
|
|
|
private bool checkCommandForFlag(List<string> commands, char p)
|
|
{
|
|
return commands.Exists(x => x != "" && x[0] == p);
|
|
}
|
|
|
|
private int getCommandValue(string command)
|
|
{
|
|
string intValueString = command.Substring(1);
|
|
return int.Parse(intValueString);
|
|
}
|
|
|
|
private float getParameterValue(string command, char targetParam)
|
|
{
|
|
float i = 0;
|
|
int index = command.IndexOf(targetParam);
|
|
|
|
if (index != -1)
|
|
{
|
|
char[] whitespace = { ' ', '\n', '\t' };
|
|
string str = command.Substring(index + 1, command.IndexOfAny(whitespace, index) - index);
|
|
i = float.Parse(str);
|
|
}
|
|
|
|
return i;
|
|
}
|
|
|
|
private float getParameterValue(List<string> commandList, char targetParam)
|
|
{
|
|
float paramValue = 0;
|
|
|
|
string targetCommand = commandList.Find(x => x != "" && x[0] == targetParam);
|
|
|
|
if (targetCommand != null && targetCommand != "")
|
|
{
|
|
targetCommand = targetCommand.Substring(1);
|
|
paramValue = float.Parse(targetCommand);
|
|
}
|
|
|
|
return paramValue;
|
|
}
|
|
|
|
private class X3GPacketFactory
|
|
{
|
|
private const int MAX_PACKET_SIZE = 256;
|
|
|
|
private byte[] packetOutline;
|
|
|
|
private int index;
|
|
private X3GCrc crc;
|
|
|
|
public X3GPacketFactory(byte printerCommand)
|
|
{
|
|
packetOutline = new byte[MAX_PACKET_SIZE];
|
|
index = 2;
|
|
crc = new X3GCrc();
|
|
addByte(printerCommand);
|
|
}
|
|
|
|
public void addByte(byte command)
|
|
{
|
|
packetOutline[index] = command;
|
|
index++;
|
|
crc.update(command);
|
|
}
|
|
|
|
public void add16bits(int command)
|
|
{
|
|
addByte((byte)(command & 0xff));
|
|
addByte((byte)((command >> 8) & 0xff));
|
|
}
|
|
|
|
public void add32bits(long command)
|
|
{
|
|
add16bits((int)command & 0xffff);
|
|
add16bits((int)(command >> 16) & 0xffff);
|
|
}
|
|
|
|
public void addFloat(float command)
|
|
{
|
|
//Convert float into bits:
|
|
//Bit 31 = Sign
|
|
//Bits 30-23 = Exponent (8bits)
|
|
//Bits 22-0 = Mantissa (23bits)
|
|
byte[] bits = BitConverter.GetBytes(command);
|
|
add32bits((long)BitConverter.ToInt32(bits, 0));
|
|
}
|
|
|
|
public byte[] getX3GPacket()
|
|
{
|
|
byte packetLength = (byte)(index + 1);
|
|
|
|
byte[] packetArray = new byte[packetLength];
|
|
packetArray[0] = 0xD5;
|
|
packetArray[1] = (byte)(packetLength - (byte)3); //Length does not count packet header or crc
|
|
|
|
for (int i = 2; i < packetLength - 1; i++)
|
|
{
|
|
packetArray[i] = packetOutline[i];
|
|
}
|
|
|
|
packetArray[packetLength - 1] = crc.getCrc();
|
|
|
|
return packetArray;
|
|
}
|
|
|
|
public void clear()
|
|
{
|
|
index = 0;
|
|
packetOutline = new byte[MAX_PACKET_SIZE];
|
|
crc.clear();
|
|
}
|
|
}
|
|
}
|
|
}
|