Skip to content

Commit

Permalink
Merge pull request #1078 from breiler/feature/tinyg
Browse files Browse the repository at this point in the history 
Added support for gcode states and work coordinates
  • Loading branch information
@breiler
breiler authored Jul 16, 2018
2 parents afde061 + 9395392 commit e159183
Show file tree
Hide file tree
Showing 5 changed files with 445 additions and 54 deletions.
40 changes: 22 additions & 18 deletions ugs-core/src/com/willwinder/universalgcodesender/TinyGController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ This file is part of Universal Gcode Sender (UGS).
import org.apache.commons.lang3.StringUtils;

import java.io.File;
import java.util.List;
import java.util.logging.Level;
import java.util.logging.Logger;

Expand Down Expand Up @@ -143,11 +144,6 @@ protected Boolean isIdleEvent() {
return getControlState() == COMM_IDLE || getControlState() == COMM_CHECK;
}

@Override
public void restoreParserModalState() {
// no op
}

@Override
protected void rawResponseHandler(String response) {
JsonObject jo;
Expand Down Expand Up @@ -190,7 +186,7 @@ protected void rawResponseHandler(String response) {
dispatchConsoleMessage(MessageType.INFO, response + "\n");
checkStreamFinished();
} else if (TinyGGcodeCommand.isOkErrorResponse(response)) {
if (jo.get("r").getAsJsonObject().has(TinyGUtils.FIELD_STATUS_RESULT)) {
if (jo.get("r").getAsJsonObject().has(TinyGUtils.FIELD_STATUS_REPORT)) {
updateControllerStatus(jo.get("r").getAsJsonObject());
checkStreamFinished();
} else if (rowsRemaining() > 0) {
Expand All @@ -216,8 +212,12 @@ private void updateControllerStatus(JsonObject jo) {
ControllerState previousState = controllerStatus.getState();
UGSEvent.ControlState previousControlState = getControlState(previousState);

// Update the internal state
List<String> gcodeList = TinyGUtils.convertStatusReportToGcode(jo);
gcodeList.forEach(gcode -> updateParserModalState(new GcodeCommand(gcode)));

// Notify our listeners about the new status
controllerStatus = TinyGUtils.updateControllerStatus(controllerStatus, jo);
controllerStatus = TinyGUtils.updateControllerStatus(controllerStatus, getCurrentGcodeState(), jo);
dispatchStatusString(controllerStatus);

// Notify state change to our listeners
Expand Down Expand Up @@ -245,6 +245,9 @@ private void sendInitCommands() {
// 0=off, 1=filtered, 2=verbose
comm.queueStringForComm("{sv:1}");

// Configure status reports
comm.queueStringForComm("{sr:{posx:t, posy:t, posz:t, mpox:t, mpoy:t, mpoz:t, plan:t, vel:t, unit:t, stat:t, dist:t, admo:t, frmo:t, coor:t}}");

// Request initial status report
comm.queueStringForComm("{sr:n}");

Expand All @@ -254,14 +257,23 @@ private void sendInitCommands() {
setStatusUpdateRate(getStatusUpdateRate());
}

@Override
public void updateParserModalState(GcodeCommand command) {
// Prevent internal TinyG commands to update the parser modal state
if(!command.getCommandString().startsWith("{")) {
super.updateParserModalState(command);
}
}

@Override
public void performHomingCycle() throws Exception {
sendCommandImmediately(new GcodeCommand("G28.2 Z0 X0 Y0"));
}

@Override
public void resetCoordinatesToZero() throws Exception {
throw new UnsupportedOperationException(NOT_SUPPORTED_YET);
String command = TinyGUtils.generateResetCoordinatesToZeroCommand(controllerStatus, getCurrentGcodeState());
sendCommandImmediately(new GcodeCommand(command));
}

@Override
Expand Down Expand Up @@ -290,15 +302,6 @@ public void viewParserState() throws Exception {
}
}

@Override
public void updateParserModalState(GcodeCommand command) {
if (command.getCommandString().startsWith("{\"gc")) {
String gcode = StringUtils.substringBetween(command.getCommandString(), "{\"gc\":\"", "\"");
GcodeCommand gcodeCommand = new GcodeCommand(gcode);
super.updateParserModalState(gcodeCommand);
}
}

@Override
public void softReset() throws Exception {
// TODO This doesn't work, it will disconnect from the host
Expand All @@ -312,7 +315,8 @@ public void softReset() throws Exception {

@Override
public void setWorkPosition(Axis axis, double position) throws Exception {
throw new UnsupportedOperationException(NOT_SUPPORTED_YET);
String command = TinyGUtils.generateSetWorkPositionCommand(controllerStatus, getCurrentGcodeState(), axis, position);
sendCommandImmediately(new GcodeCommand(command));
}

@Override
Expand Down
190 changes: 162 additions & 28 deletions ugs-core/src/com/willwinder/universalgcodesender/TinyGUtils.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,11 +20,19 @@ This file is part of Universal Gcode Sender (UGS).

import com.google.gson.JsonObject;
import com.google.gson.JsonParser;
import com.willwinder.universalgcodesender.gcode.GcodeState;
import com.willwinder.universalgcodesender.gcode.util.Code;
import com.willwinder.universalgcodesender.listeners.ControllerState;
import com.willwinder.universalgcodesender.listeners.ControllerStatus;
import com.willwinder.universalgcodesender.model.Axis;
import com.willwinder.universalgcodesender.model.Position;
import com.willwinder.universalgcodesender.model.UnitUtils;
import com.willwinder.universalgcodesender.model.WorkCoordinateSystem;
import org.apache.commons.lang3.StringUtils;
import org.apache.commons.lang3.math.NumberUtils;

import java.util.ArrayList;
import java.util.List;

/**
* Common utils for TinyG controllers
Expand All @@ -44,17 +52,23 @@ public class TinyGUtils {

public static final String COMMAND_STATUS_REPORT = "{sr:n}";
public static final String COMMAND_KILL_ALARM_LOCK = "{clear:n}";

public static final String FIELD_FIRMWARE_VERSION = "fv";
public static final String FIELD_RESPONSE = "r";

public static final String FIELD_STATUS_RESULT = "sr";
public static final String FIELD_STATUS_RESULT_UNIT = "unit";
public static final String FIELD_STATUS_RESULT_POSX = "posx";
public static final String FIELD_STATUS_REPORT_POSY = "posy";
public static final String FIELD_STATUS_REPORT_POSZ = "posz";
public static final String FIELD_STATUS_REPORT_VELOCITY = "vel";
public static final String FIELD_STATUS_REPORT_STATUS = "stat";
public static final String FIELD_STATUS_REPORT = "sr";
private static final String FIELD_FIRMWARE_VERSION = "fv";
private static final String FIELD_RESPONSE = "r";
private static final String FIELD_STATUS_REPORT_UNIT = "unit";
private static final String FIELD_STATUS_REPORT_POSX = "posx";
private static final String FIELD_STATUS_REPORT_POSY = "posy";
private static final String FIELD_STATUS_REPORT_POSZ = "posz";
private static final String FIELD_STATUS_REPORT_VELOCITY = "vel";
private static final String FIELD_STATUS_REPORT_COORD = "coor";
private static final String FIELD_STATUS_REPORT_PLANE = "plan";
private static final String FIELD_STATUS_REPORT_DISTANCE_MODE = "dist";
private static final String FIELD_STATUS_REPORT_ARC_DISTANCE_MODE = "admo";
private static final String FIELD_STATUS_REPORT_FEED_MODE = "frmo";
private static final String FIELD_STATUS_REPORT_STATUS = "stat";
private static final String FIELD_STATUS_REPORT_MPOX = "mpox";
private static final String FIELD_STATUS_REPORT_MPOY = "mpoy";
private static final String FIELD_STATUS_REPORT_MPOZ = "mpoz";

private static JsonParser parser = new JsonParser();

Expand Down Expand Up @@ -98,7 +112,7 @@ public static boolean isReadyResponse(JsonObject response) {
JsonObject jo = response.getAsJsonObject(FIELD_RESPONSE);
if (jo.has("msg")) {
String msg = jo.get("msg").getAsString();
return StringUtils.equals(msg,"SYSTEM READY");
return StringUtils.equals(msg, "SYSTEM READY");
}
}
return false;
Expand All @@ -115,36 +129,47 @@ public static boolean isStatusResponse(JsonObject response) {
* @param response the response string from the controller
* @return a new updated controller status
*/
public static ControllerStatus updateControllerStatus(final ControllerStatus lastControllerStatus, final JsonObject response) {
if (response.has(FIELD_STATUS_RESULT)) {
JsonObject statusResultObject = response.getAsJsonObject(FIELD_STATUS_RESULT);
public static ControllerStatus updateControllerStatus(final ControllerStatus lastControllerStatus, final GcodeState lastGcodeState, final JsonObject response) {
if (response.has(FIELD_STATUS_REPORT)) {
JsonObject statusResultObject = response.getAsJsonObject(FIELD_STATUS_REPORT);

UnitUtils.Units currentUnits = lastGcodeState.units == Code.G20 ? UnitUtils.Units.INCH : UnitUtils.Units.MM;

Position workCoord = lastControllerStatus.getWorkCoord().getPositionIn(currentUnits);
if (hasNumericField(statusResultObject, FIELD_STATUS_REPORT_POSX)) {
workCoord.setX(statusResultObject.get(FIELD_STATUS_REPORT_POSX).getAsDouble());
}

if (hasNumericField(statusResultObject, FIELD_STATUS_REPORT_POSY)) {
workCoord.setY(statusResultObject.get(FIELD_STATUS_REPORT_POSY).getAsDouble());
}

Position machineCoord = lastControllerStatus.getMachineCoord();
if (statusResultObject.has(FIELD_STATUS_RESULT_POSX)) {
machineCoord.setX(statusResultObject.get(FIELD_STATUS_RESULT_POSX).getAsDouble());
if (hasNumericField(statusResultObject, FIELD_STATUS_REPORT_POSZ)) {
workCoord.setZ(statusResultObject.get(FIELD_STATUS_REPORT_POSZ).getAsDouble());
}

if (statusResultObject.has(FIELD_STATUS_REPORT_POSY)) {
machineCoord.setY(statusResultObject.get(FIELD_STATUS_REPORT_POSY).getAsDouble());
// The machine coordinates are always in MM, make sure the position is using that unit before updating the values
Position machineCoord = lastControllerStatus.getMachineCoord().getPositionIn(UnitUtils.Units.MM);
if (hasNumericField(statusResultObject, FIELD_STATUS_REPORT_MPOX)) {
machineCoord.setX(statusResultObject.get(FIELD_STATUS_REPORT_MPOX).getAsDouble());
}

if (statusResultObject.has(FIELD_STATUS_REPORT_POSZ)) {
machineCoord.setZ(statusResultObject.get(FIELD_STATUS_REPORT_POSZ).getAsDouble());
if (hasNumericField(statusResultObject, FIELD_STATUS_REPORT_MPOY)) {
machineCoord.setY(statusResultObject.get(FIELD_STATUS_REPORT_MPOY).getAsDouble());
}

if (statusResultObject.has(FIELD_STATUS_RESULT_UNIT)) {
UnitUtils.Units units = statusResultObject.get(FIELD_STATUS_RESULT_UNIT).getAsInt() == 0 ? UnitUtils.Units.INCH : UnitUtils.Units.MM;
machineCoord = new Position(machineCoord.getX(), machineCoord.getY(), machineCoord.getZ(), units);
if (hasNumericField(statusResultObject, FIELD_STATUS_REPORT_MPOZ)) {
machineCoord.setZ(statusResultObject.get(FIELD_STATUS_REPORT_MPOZ).getAsDouble());
}

Double feedSpeed = lastControllerStatus.getFeedSpeed();
if (statusResultObject.has(FIELD_STATUS_REPORT_VELOCITY)) {
if (hasNumericField(statusResultObject, FIELD_STATUS_REPORT_VELOCITY)) {
feedSpeed = statusResultObject.get(FIELD_STATUS_REPORT_VELOCITY).getAsDouble();
}

ControllerState state = lastControllerStatus.getState();
String stateString = lastControllerStatus.getStateString();
if (statusResultObject.has(FIELD_STATUS_REPORT_STATUS)) {
if (hasNumericField(statusResultObject, FIELD_STATUS_REPORT_STATUS)) {
state = getState(statusResultObject.get(FIELD_STATUS_REPORT_STATUS).getAsInt());
stateString = getStateAsString(statusResultObject.get(FIELD_STATUS_REPORT_STATUS).getAsInt());
}
Expand All @@ -155,7 +180,7 @@ public static ControllerStatus updateControllerStatus(final ControllerStatus las
ControllerStatus.EnabledPins enabledPins = lastControllerStatus.getEnabledPins();
ControllerStatus.AccessoryStates accessoryStates = lastControllerStatus.getAccessoryStates();

return new ControllerStatus(stateString, state, machineCoord, machineCoord, feedSpeed, spindleSpeed, overrides, workCoordinateOffset, enabledPins, accessoryStates);
return new ControllerStatus(stateString, state, machineCoord.getPositionIn(currentUnits), workCoord.getPositionIn(currentUnits), feedSpeed, spindleSpeed, overrides, workCoordinateOffset, enabledPins, accessoryStates);
}

return lastControllerStatus;
Expand Down Expand Up @@ -206,4 +231,113 @@ private static String getStateAsString(int state) {
ControllerState controllerState = getState(state);
return controllerState.name();
}

/**
* Generates a command for resetting the coordinates for the current coordinate system to zero.
*
* @param controllerStatus the current controller status
* @param gcodeState the current gcode state
* @return a string with the command to reset the coordinate system to zero
*/
public static String generateResetCoordinatesToZeroCommand(ControllerStatus controllerStatus, GcodeState gcodeState) {
int offsetCode = WorkCoordinateSystem.fromGCode(gcodeState.offset).getPValue();
Position machineCoord = controllerStatus.getMachineCoord();
return "G10 L2 P" + offsetCode +
" X" + Utils.formatter.format(machineCoord.get(Axis.X)) +
" Y" + Utils.formatter.format(machineCoord.get(Axis.Y)) +
" Z" + Utils.formatter.format(machineCoord.get(Axis.Z));
}

/**
* Generates a command for setting the axis to a position in the current coordinate system
*
* @param controllerStatus the current controller status
* @param gcodeState the current gcode state
* @param axis the axis to set
* @param position the position to set
* @return a command for setting the position
*/
public static String generateSetWorkPositionCommand(ControllerStatus controllerStatus, GcodeState gcodeState, Axis axis, double position) {
int offsetCode = WorkCoordinateSystem.fromGCode(gcodeState.offset).getPValue();
Position machineCoord = controllerStatus.getMachineCoord();
double coordinate = -(position - machineCoord.get(axis));
return "G10 L2 P" + offsetCode + " " +
axis.name() + Utils.formatter.format(coordinate);
}

/**
* Updates the Gcode state from the response if it contains a status line
*
* @param response the response to parse the gcode state from
* @return a list of gcodes representing the state of the controllers
*/
public static List<String> convertStatusReportToGcode(JsonObject response) {
List<String> gcodeList = new ArrayList<>();
if (response.has(TinyGUtils.FIELD_STATUS_REPORT)) {
JsonObject statusResultObject = response.getAsJsonObject(TinyGUtils.FIELD_STATUS_REPORT);

if (hasNumericField(statusResultObject, TinyGUtils.FIELD_STATUS_REPORT_COORD)) {
int offsetCode = statusResultObject.get(TinyGUtils.FIELD_STATUS_REPORT_COORD).getAsInt();
gcodeList.add(WorkCoordinateSystem.fromPValue(offsetCode).getGcode().name());
}

if (hasNumericField(statusResultObject, TinyGUtils.FIELD_STATUS_REPORT_UNIT)) {
int units = statusResultObject.get(TinyGUtils.FIELD_STATUS_REPORT_UNIT).getAsInt();
// 0=inch, 1=mm
if (units == 0) {
gcodeList.add(Code.G20.toString());
} else {
gcodeList.add(Code.G21.toString());
}
}

if (hasNumericField(statusResultObject, TinyGUtils.FIELD_STATUS_REPORT_PLANE)) {
int plane = statusResultObject.get(TinyGUtils.FIELD_STATUS_REPORT_PLANE).getAsInt();
// 0=XY plane, 1=XZ plane, 2=YZ plane
if (plane == 0) {
gcodeList.add(Code.G17.toString());
} else if (plane == 1) {
gcodeList.add(Code.G18.toString());
} else if (plane == 2) {
gcodeList.add(Code.G19.toString());
}
}

if (hasNumericField(statusResultObject, TinyGUtils.FIELD_STATUS_REPORT_FEED_MODE)) {
int feedMode = statusResultObject.get(TinyGUtils.FIELD_STATUS_REPORT_FEED_MODE).getAsInt();
// 0=units-per-minute-mode, 1=inverse-time-mode
if (feedMode == 0) {
gcodeList.add(Code.G93.toString());
} else if (feedMode == 1) {
gcodeList.add(Code.G94.toString());
}
}

if (hasNumericField(statusResultObject, TinyGUtils.FIELD_STATUS_REPORT_DISTANCE_MODE)) {
int distance = statusResultObject.get(TinyGUtils.FIELD_STATUS_REPORT_DISTANCE_MODE).getAsInt();
// 0=absolute distance mode, 1=incremental distance mode
if (distance == 0) {
gcodeList.add(Code.G90.name());
} else if (distance == 1) {
gcodeList.add(Code.G91.name());
}
}

if (hasNumericField(statusResultObject, TinyGUtils.FIELD_STATUS_REPORT_ARC_DISTANCE_MODE)) {
int arcDistance = statusResultObject.get(TinyGUtils.FIELD_STATUS_REPORT_ARC_DISTANCE_MODE).getAsInt();
// 0=absolute distance mode, 1=incremental distance mode
if (arcDistance == 0) {
gcodeList.add(Code.G90_1.toString());
} else if (arcDistance == 1) {
gcodeList.add(Code.G91_1.toString());
}
}
}
return gcodeList;
}

private static boolean hasNumericField(JsonObject statusResultObject, String fieldName) {
return statusResultObject.has(fieldName) &&
NumberUtils.isNumber(statusResultObject.get(fieldName).getAsString());
}
}
Loading

0 comments on commit e159183

Please sign in to comment.