/*
*
* Goko
* Copyright (C) 2013 PsyKo
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
package org.goko.controller.tinyg.controller;
import java.math.BigDecimal;
import java.util.List;
import org.apache.commons.collections.CollectionUtils;
import org.apache.commons.lang3.ObjectUtils;
import org.apache.commons.lang3.StringUtils;
import org.goko.controller.tinyg.commons.AbstractTinyGCommunicator;
import org.goko.controller.tinyg.commons.ITinyGStatus;
import org.goko.controller.tinyg.controller.configuration.TinyGConfiguration;
import org.goko.controller.tinyg.controller.configuration.TinyGConfigurationValue;
import org.goko.core.common.GkUtils;
import org.goko.core.common.exception.GkException;
import org.goko.core.common.exception.GkFunctionalException;
import org.goko.core.common.measure.quantity.Angle;
import org.goko.core.common.measure.quantity.AngleUnit;
import org.goko.core.common.measure.quantity.Length;
import org.goko.core.common.measure.quantity.Speed;
import org.goko.core.connection.serial.ISerialConnection;
import org.goko.core.connection.serial.SerialParameter;
import org.goko.core.controller.bean.MachineState;
import org.goko.core.gcode.element.ICoordinateSystem;
import org.goko.core.gcode.rs274ngcv3.context.CoordinateSystem;
import org.goko.core.gcode.rs274ngcv3.context.CoordinateSystemFactory;
import org.goko.core.gcode.rs274ngcv3.context.EnumDistanceMode;
import org.goko.core.gcode.rs274ngcv3.context.EnumMotionMode;
import org.goko.core.gcode.rs274ngcv3.context.EnumPlane;
import org.goko.core.gcode.rs274ngcv3.context.EnumUnit;
import org.goko.core.gcode.rs274ngcv3.context.GCodeContext;
import org.goko.core.log.GkLog;
import org.goko.core.math.Tuple6b;
import com.eclipsesource.json.JsonArray;
import com.eclipsesource.json.JsonObject;
import com.eclipsesource.json.JsonValue;
/**
* @author Psyko
* @date 8 janv. 2017
*/
public class TinyGCommunicator extends AbstractTinyGCommunicator<TinyGConfiguration, TinyGControllerService> {
/** LOG */
private static final GkLog LOG = GkLog.getLogger(TinyGCommunicator.class);
/**
* Constructor
* @param tinyg the tinyG service
*/
public TinyGCommunicator() {
setEndLineCharacters('\n');
}
/** (inheritDoc)
* @see org.goko.controller.tinyg.commons.AbstractTinyGCommunicator#onConnected()
*/
@Override
protected void onConnected() throws GkException {
getIncomingBuffer().clear();
getConnectionService().addInputDataListener(this);
// Force strict JSon mode
forceJsonMode();
requestStatusReport();
requestQueueReport();
requestConfigurationUpdate();
requestCoordinateSystemUpdate();
}
/** (inheritDoc)
* @see org.goko.controller.tinyg.commons.AbstractTinyGCommunicator#onDisconnected()
*/
@Override
protected void onDisconnected() throws GkException {
getConnectionService().removeInputDataListener(this);
getControllerService().setState(MachineState.UNDEFINED);
getControllerService().resetConfiguration();
getIncomingBuffer().clear();
}
/**
* Verify the response using the footer
* @param jsonFooter the parsed response
* @throws GkException GkException
*/
protected ITinyGStatus getResponseFooter(JsonValue jsonFooter) throws GkException {
JsonArray footerArray = jsonFooter.asArray();
int statusCodeIntValue = footerArray.get(TinyGv097.FOOTER_STATUS_CODE_INDEX).asInt();
ITinyGStatus status = TinyGStatusCode.findEnum(statusCodeIntValue);
return status;
}
/** (inheritDoc)
* @see org.goko.controller.tinyg.commons.AbstractTinyGCommunicator#handleResponse(com.eclipsesource.json.JsonObject, org.goko.controller.tinyg.commons.ITinyGStatus)
*/
@Override
protected void handleResponse(JsonObject responseEnvelope, ITinyGStatus status) throws GkException {
for(String name : responseEnvelope.names()){
if(StringUtils.equals(name, TinyGv097.GCODE_COMMAND)){
handleGCodeResponse(responseEnvelope.get(TinyGv097.GCODE_COMMAND), status);
}else if(StringUtils.equals(name, TinyGv097.STATUS_REPORT)){
handleStatusReport((JsonObject)responseEnvelope.get(TinyGv097.STATUS_REPORT));
}else if(StringUtils.equals(name, TinyGv097.QUEUE_REPORT)){
handleQueueReport(responseEnvelope.get(TinyGv097.QUEUE_REPORT));
}else if(StringUtils.equals(name, TinyGv097.LINE_REPORT)){
// LOG.info("Skipping line report "+String.valueOf(responseEnvelope.get(name)));
}else if(StringUtils.equals(name, TinyGv097.PROBE_REPORT)){
handleProbeReport(responseEnvelope.get(TinyGv097.PROBE_REPORT));
}else if(StringUtils.equals(name, TinyGv097.MESSAGE_REPORT)){
handleMessage(responseEnvelope.get(TinyGv097.MESSAGE_REPORT));
}else if(StringUtils.defaultString(name).matches("(g|G)5(4|5|6|7|8|9)")){
handleCoordinateSystemOffsetReport(name, responseEnvelope.get(name));
}else{
handleConfigurationModification(responseEnvelope);
}
}
}
private void handleCoordinateSystemOffsetReport(String offsetName, JsonValue jsonOffset) throws GkException{
CoordinateSystem cs = new CoordinateSystemFactory().get(StringUtils.upperCase(offsetName));
JsonObject offsetObj = (JsonObject) jsonOffset;
JsonValue xOffset = offsetObj.get("x");
JsonValue yOffset = offsetObj.get("y");
JsonValue zOffset = offsetObj.get("z");
JsonValue aOffset = offsetObj.get("a");
Tuple6b offset = new Tuple6b().setZero();
offset.setX( Length.valueOf(xOffset.asBigDecimal(), getControllerService().getCurrentUnit() ));
offset.setY( Length.valueOf(yOffset.asBigDecimal(), getControllerService().getCurrentUnit() ) );
offset.setZ( Length.valueOf(zOffset.asBigDecimal(), getControllerService().getCurrentUnit() ) );
if(aOffset != null){
offset.setA( Angle.valueOf(aOffset.asBigDecimal(), AngleUnit.DEGREE_ANGLE ) );
}
getControllerService().setCoordinateSystemOffset(cs, offset);
}
/**
* Handle the configuration changes received from the TinyG device
* @param responseEnvelope the response envelope
* @throws GkException GkException
*/
private void handleConfigurationModification(JsonObject responseEnvelope) throws GkException {
TinyGConfiguration cfg = getControllerService().getConfiguration();
cfg.setFromJson(responseEnvelope);
getControllerService().setConfiguration(cfg);
}
private void handleGCodeResponse(JsonValue jsonValue, ITinyGStatus status) throws GkException {
String receivedCommand = jsonValue.asString();
getControllerService().handleGCodeResponse(receivedCommand, status);
}
protected void handleQueueReport(JsonValue queueReport) throws GkException {
getControllerService().setAvailablePlannerBuffer(queueReport.asInt());
}
private void handleMessage(JsonValue message) throws GkException {
getApplicativeLogService().warning(message.asString(), "TinyG");
getControllerService().setMessage(message.asString());
}
private void handleProbeReport(JsonValue probeReport) throws GkException {
if(probeReport.isObject()){
Tuple6b probePosition = null;
JsonObject probeReportObject = (JsonObject) probeReport;
JsonValue eProbeResult = probeReportObject.get(TinyGv097.PROBE_REPORT_SUCCESS);
boolean probeSuccess = (eProbeResult.asInt() == 1);
if(probeSuccess){
JsonValue xProbeResult = probeReportObject.get(TinyGv097.PROBE_REPORT_POSITION_X);
JsonValue yProbeResult = probeReportObject.get(TinyGv097.PROBE_REPORT_POSITION_Y);
JsonValue zProbeResult = probeReportObject.get(TinyGv097.PROBE_REPORT_POSITION_Z);
JsonValue aProbeResult = probeReportObject.get(TinyGv097.PROBE_REPORT_POSITION_A);
JsonValue bProbeResult = probeReportObject.get(TinyGv097.PROBE_REPORT_POSITION_B);
JsonValue cProbeResult = probeReportObject.get(TinyGv097.PROBE_REPORT_POSITION_C);
probePosition = new Tuple6b();
if(xProbeResult != null){
probePosition.setX( Length.valueOf(xProbeResult.asBigDecimal(), getControllerService().getCurrentUnit()) );
}
if(yProbeResult != null){
probePosition.setY( Length.valueOf(yProbeResult.asBigDecimal(), getControllerService().getCurrentUnit()) );
}
if(zProbeResult != null){
probePosition.setZ( Length.valueOf(zProbeResult.asBigDecimal(), getControllerService().getCurrentUnit()) );
}
if(aProbeResult != null){
probePosition.setA( Angle.valueOf(aProbeResult.asBigDecimal(), AngleUnit.DEGREE_ANGLE) );
}
if(bProbeResult != null){
probePosition.setB( Angle.valueOf(bProbeResult.asBigDecimal(), AngleUnit.DEGREE_ANGLE) );
}
if(cProbeResult != null){
probePosition.setC( Angle.valueOf(cProbeResult.asBigDecimal(), AngleUnit.DEGREE_ANGLE) );
}
}
getControllerService().handleProbeResult(probeSuccess, probePosition);
}
}
/** (inheritDoc)
* @see org.goko.controller.tinyg.commons.AbstractTinyGCommunicator#handleStatusReport(com.eclipsesource.json.JsonObject)
*/
@Override
protected void handleStatusReport(JsonObject statusReport) throws GkException {
if(statusReport.isObject()){
JsonObject statusReportObject = (JsonObject) statusReport;
EnumUnit units = findUnits(statusReportObject);
Tuple6b workPosition = findWorkPosition(statusReportObject, units);
MachineState state = findState(statusReportObject);
EnumDistanceMode distanceMode = findDistanceMode(statusReportObject);
Tuple6b machinePosition= findMachinePosition(statusReportObject, units);
Speed velocity = findVelocity(statusReportObject, units);
Speed feedrate = findFeedrate(statusReportObject, units);
CoordinateSystem cs = findCoordinateSystem(statusReportObject);
EnumPlane plane = findPlane(statusReportObject);
EnumMotionMode motionMode = findMotionMode(statusReportObject);
GCodeContext gcodeContext = new GCodeContext(getControllerService().getGCodeContext());
gcodeContext.setPosition(workPosition);
gcodeContext.setMachinePosition(machinePosition);
gcodeContext.setDistanceMode(distanceMode);
gcodeContext.setUnit(units);
gcodeContext.setCoordinateSystem(cs);
gcodeContext.setFeedrate(feedrate);
gcodeContext.setPlane(plane);
gcodeContext.setMotionMode(motionMode);
if(state != null){
getControllerService().setState(state);
}
if(velocity != null){
getControllerService().setVelocity(velocity);
}
getControllerService().updateGCodeContext(gcodeContext);
}
}
/** (inheritDoc)
* @see org.goko.controller.tinyg.commons.AbstractTinyGCommunicator#handleErrorReport(com.eclipsesource.json.JsonObject)
*/
@Override
protected void handleErrorReport(JsonObject errorReportBody) throws GkException {
// TODO
}
/** (inheritDoc)
* @see org.goko.controller.tinyg.commons.AbstractTinyGCommunicator#handleMalformedJson(java.lang.String)
*/
@Override
protected void handleMalformedJson(String data) {
}
/** (inheritDoc)
* @see org.goko.controller.tinyg.commons.AbstractTinyGCommunicator#handleNonJsonData(java.lang.String)
*/
@Override
protected void handleNonJsonData(String data) throws GkException {
// FIXME : add a condition so if TinyG continues to answer as non json, there is no infinite loop
// tinyg.refreshStatus();
}
/**
* Extract state from status report
* @param statusReport
* @return
*/
private MachineState findState(JsonObject statusReport){
JsonValue statReport = statusReport.get(TinyGv097.STATUS_REPORT_STATE);
if(statReport != null){
return TinyGControllerUtility.getState(statReport.asInt());
}
return null;
}
protected void updateCoordinateSystem(ICoordinateSystem cs) throws GkException{
send("{\""+cs.getCode()+"\":\"\"}", true);
}
/**
* Check if the current configured flow control matches the one for the connection
* @throws GkException GkException
*/
protected void checkExecutionControl() throws GkException{
TinyGConfiguration configuration = getControllerService().getConfiguration();
BigDecimal flowControl = configuration.getSetting(TinyGConfiguration.SYSTEM_SETTINGS, TinyGConfiguration.ENABLE_FLOW_CONTROL, BigDecimal.class);
// We always need to use flow control
if(ObjectUtils.equals(flowControl, TinyGConfigurationValue.FLOW_CONTROL_OFF)){
throw new GkFunctionalException("TNG-001");
}
// Make sure the current connection use the same flow control
ISerialConnection connexion = getConnectionService().getCurrentConnection();
BigDecimal configuredFlowControl = configuration.getSetting(TinyGConfiguration.SYSTEM_SETTINGS, TinyGConfiguration.ENABLE_FLOW_CONTROL, BigDecimal.class);
if(configuredFlowControl.equals(TinyGConfigurationValue.FLOW_CONTROL_RTS_CTS)){ // TinyG expects RtsCts but the serial connection does not use it
if((connexion.getFlowControl() & SerialParameter.FLOWCONTROL_RTSCTS) != SerialParameter.FLOWCONTROL_RTSCTS){
throw new GkFunctionalException("TNG-005");
}
}else if(configuredFlowControl.equals(TinyGConfigurationValue.FLOW_CONTROL_XON_XOFF)){ // TinyG expects XonXoff but the serial connection does not use it
if((connexion.getFlowControl() & SerialParameter.FLOWCONTROL_XONXOFF) != SerialParameter.FLOWCONTROL_XONXOFF){
throw new GkFunctionalException("TNG-006");
}
}
}
/**
* Entry point for Kill Alarm action
* @throws GkException GkException
*/
public void killAlarm() throws GkException{
send(buildJsonQuery(TinyGv097.KILL_ALARM_HEADER), true);
}
/**
* Entry point for Stop Motion action
* @throws GkException GkException
*/
public void stopMotion() throws GkException{
getConnectionService().clearOutputBuffer();
sendImmediately(TinyGv097.FEED_HOLD, true);
sendImmediately(TinyGv097.QUEUE_FLUSH, true);
}
/**
* Entry point for Pause Motion action
* @throws GkException GkException
*/
public void pauseMotion() throws GkException{
sendImmediately(TinyGv097.FEED_HOLD, false);
}
/**
* Entry point for Resume motion action
* @throws GkException GkException
*/
public void resumeMotion() throws GkException{
sendImmediately(TinyGv097.CYCLE_START, false);
}
/**
* Entry point for Start Motion action
* @throws GkException GkException
*/
public void startMotion() throws GkException{
sendImmediately(TinyGv097.CYCLE_START, false);
}
/**
* Entry point for Reset action
* @throws GkException GkException
*/
public void resetTinyG() throws GkException{
sendImmediately(TinyGv097.RESET_COMMAND, false);
}
/**
* Entry point for Turn Spindle On action
* @throws GkException GkException
*/
public void turnSpindleOn() throws GkException{
send(TinyGv097.TURN_SPINDLE_ON_GCODE, true);
}
/**
* Entry point for Turn Spindle Off action
* @throws GkException GkException
*/
public void turnSpindleOff() throws GkException{
send(TinyGv097.TURN_SPINDLE_OFF_GCODE, true);
}
/**
* Entry point for Reset Zero action
* @throws GkException GkException
*/
public void resetZero(List<String> axes) throws GkException{
List<Byte> lstBytes = GkUtils.toBytesList("G28.3");
if(CollectionUtils.isNotEmpty(axes)){
for (String axe : axes) {
lstBytes.addAll(GkUtils.toBytesList(axe+"0"));
}
}else{
lstBytes.addAll( GkUtils.toBytesList("X0Y0Z0"));
}
send(lstBytes, true);
}
}