diff --git a/src/main/java/de/fhg/iais/roberta/connection/wired/ClearBufferThread.java b/src/main/java/de/fhg/iais/roberta/connection/wired/ClearBufferThread.java new file mode 100644 index 0000000..a5f4bef --- /dev/null +++ b/src/main/java/de/fhg/iais/roberta/connection/wired/ClearBufferThread.java @@ -0,0 +1,59 @@ +package de.fhg.iais.roberta.connection.wired; + +import org.apache.commons.lang3.SystemUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import com.fazecast.jSerialComm.SerialPort; + + +public class ClearBufferThread extends Thread { + private final Logger LOG = LoggerFactory.getLogger(ClearBufferThread.class); + private final String portName; + private Thread thread; + private SerialPort serialPort; + private boolean exitThread = false; + + public ClearBufferThread(String portName) { + this.portName = (SystemUtils.IS_OS_WINDOWS ? "" : "/dev/") + portName; + } + + public boolean exit() throws InterruptedException { + if ( thread != null && thread.isAlive() ) { + exitThread = true; + } + if ( serialPort != null && serialPort.isOpen() ) { + return serialPort.closePort(); + } + return true; + } + + @Override + public void run() { + byte[] buffer = new byte[4096]; + exitThread = false; + while ( !exitThread && serialPort.bytesAvailable() >= 0 ) { + serialPort.readBytes(buffer, Math.min(serialPort.bytesAvailable(), buffer.length)); + try { + Thread.sleep(100); + } catch ( InterruptedException e ) { + LOG.info(e.getMessage()); + } + } + LOG.info("Stop clearing buffer"); + } + + public void start(SerialPort serialPortObject) { + LOG.info("Start clearing buffer until next program upload"); + initSerialPort(serialPortObject); + thread = new Thread(this, "Clear buffer of " + portName); + thread.start(); + } + + private void initSerialPort(SerialPort serialPortObject) { + serialPort = serialPortObject; + if ( !serialPort.isOpen() ) { + serialPort.openPort(); + } + } +} diff --git a/src/main/java/de/fhg/iais/roberta/connection/wired/SerialRobotDetector.java b/src/main/java/de/fhg/iais/roberta/connection/wired/SerialRobotDetector.java index 26c3eed..3a58974 100644 --- a/src/main/java/de/fhg/iais/roberta/connection/wired/SerialRobotDetector.java +++ b/src/main/java/de/fhg/iais/roberta/connection/wired/SerialRobotDetector.java @@ -1,6 +1,7 @@ package de.fhg.iais.roberta.connection.wired; import de.fhg.iais.roberta.connection.wired.mBot2.Mbot2; +import de.fhg.iais.roberta.connection.wired.legoLargeHub.LegoLargeHub; import static de.fhg.iais.roberta.util.WiredRobotIdFileHelper.load; import java.io.BufferedReader; @@ -80,6 +81,10 @@ public List detectRobots() { case MBOT2: detectedRobots.add(new Mbot2(wiredRobotType, device.port)); break; + case SPIKEPRIME: + case ROBOTINVENTOR: + detectedRobots.add(new LegoLargeHub(wiredRobotType, device.port)); + break; case EV3: case NONE: throw new IllegalStateException("Robot type not supported"); diff --git a/src/main/java/de/fhg/iais/roberta/connection/wired/WiredRobotType.java b/src/main/java/de/fhg/iais/roberta/connection/wired/WiredRobotType.java index 9db33d9..3cb9fce 100644 --- a/src/main/java/de/fhg/iais/roberta/connection/wired/WiredRobotType.java +++ b/src/main/java/de/fhg/iais/roberta/connection/wired/WiredRobotType.java @@ -15,6 +15,8 @@ public enum WiredRobotType { UNOWIFIREV2("unowifirev2", "Arduino Uno Wifi Rev2", true), NANO33BLE("nano33ble", "Arduino Nano 33 BLE", true), ROB3RTA ("rob3rta", "ROB3RTA", true), + SPIKEPRIME ("spikeprime", "Spike Prime", true), + ROBOTINVENTOR ("robotinventor", "Robot Inventor", true), NONE ("none", "none", false); private final String text; diff --git a/src/main/java/de/fhg/iais/roberta/connection/wired/legoLargeHub/LegoLargeHub.java b/src/main/java/de/fhg/iais/roberta/connection/wired/legoLargeHub/LegoLargeHub.java new file mode 100644 index 0000000..34615e0 --- /dev/null +++ b/src/main/java/de/fhg/iais/roberta/connection/wired/legoLargeHub/LegoLargeHub.java @@ -0,0 +1,23 @@ +package de.fhg.iais.roberta.connection.wired.legoLargeHub; + +import de.fhg.iais.roberta.connection.IConnector; +import de.fhg.iais.roberta.connection.IRobot; +import de.fhg.iais.roberta.connection.wired.AbstractWiredRobot; +import de.fhg.iais.roberta.connection.wired.WiredRobotType; + +public class LegoLargeHub extends AbstractWiredRobot { + /** + * Constructor for wired robots. + * + * @param type the robot type + * @param port the robot port + */ + public LegoLargeHub(WiredRobotType type, String port) { + super(type, port); + } + + @Override + public IConnector createConnector() { + return new LegoLargeHubConnector(this); + } +} diff --git a/src/main/java/de/fhg/iais/roberta/connection/wired/legoLargeHub/LegoLargeHubCommunicator.java b/src/main/java/de/fhg/iais/roberta/connection/wired/legoLargeHub/LegoLargeHubCommunicator.java new file mode 100644 index 0000000..3825534 --- /dev/null +++ b/src/main/java/de/fhg/iais/roberta/connection/wired/legoLargeHub/LegoLargeHubCommunicator.java @@ -0,0 +1,263 @@ +package de.fhg.iais.roberta.connection.wired.legoLargeHub; + +import java.io.File; +import java.io.IOException; +import java.nio.charset.StandardCharsets; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Base64; +import java.util.List; +import java.util.concurrent.TimeUnit; +import java.util.regex.Matcher; +import java.util.regex.Pattern; + +import org.apache.commons.codec.digest.DigestUtils; +import org.apache.commons.io.FileUtils; +import org.apache.commons.lang3.RandomStringUtils; +import org.apache.commons.lang3.SystemUtils; +import org.json.JSONException; +import org.json.JSONObject; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import com.fazecast.jSerialComm.SerialPort; + +import de.fhg.iais.roberta.connection.wired.ClearBufferThread; +import de.fhg.iais.roberta.connection.wired.IWiredRobot; +import de.fhg.iais.roberta.util.Pair; + +public class LegoLargeHubCommunicator { + + private static final Logger LOG = LoggerFactory.getLogger(LegoLargeHubCommunicator.class); + + private final IWiredRobot robot; + private final ClearBufferThread clearBufferThread; + private final int slotId = 0; + + private SerialPort serialPort; + private List payloads; + private byte[] fileContentEncoded; + + + private boolean transferIdAdded = false; + + LegoLargeHubCommunicator(IWiredRobot robot) { + this.robot = robot; + this.clearBufferThread = new ClearBufferThread(robot.getPort()); + initSerialPort(this.robot.getPort()); + } + + public JSONObject getDeviceInfo() { + JSONObject deviceInfo = new JSONObject(); + + deviceInfo.put("firmwarename", this.robot.getType().toString()); + deviceInfo.put("robot", this.robot.getType().toString()); + deviceInfo.put("brickname", this.robot.getType().getPrettyText()); + return deviceInfo; + } + + public Pair handleUpload(String absolutePath) { + Pair result; + try { + stopClearBufferThrad(); + + extractFileInformation(absolutePath); + createJsonPayloads(); + + result = sendPayloads(); + LOG.info(result.getSecond()); + } catch ( Exception e ) { + LOG.info(e.getMessage()); + result = new Pair<>(1, "An error occured while uploading. If this happens again please reconnect the robot with the computer"); + } + + startClearBufferThread(); + transferIdAdded = false; + return result; + } + + private void initSerialPort(String portName) { + portName = (SystemUtils.IS_OS_WINDOWS ? "" : "/dev/") + portName; // to hide the parameter, which should not be used + serialPort = SerialPort.getCommPort(portName); + serialPort.setBaudRate(115200); + LOG.info("Serial Communication is initialized: {} {} {}", + serialPort.getSystemPortName(), + serialPort.getDescriptivePortName(), + serialPort.getPortDescription()); + } + + private void extractFileInformation(String filePath) throws IOException { + File file = new File(filePath); + fileContentEncoded = FileUtils.readFileToByteArray(file); + } + + private void createJsonPayloads() { + payloads = new ArrayList<>(); + + createProgramTerminatePayload(); + createStartWriteProgramPayload(); + createWritePackagePayload(); + createExecuteProgramPayload(); + + LOG.info("Created " + payloads.size() + " payloads"); + } + + private void createProgramTerminatePayload() { + assemblePayload("program_terminate", new JSONObject()); + } + + private void createStartWriteProgramPayload() { + JSONObject params = new JSONObject(); + JSONObject meta = new JSONObject(); + long nowTime = System.currentTimeMillis() / 1000; + + meta.put("created", nowTime); + meta.put("modified", nowTime); + meta.put("name", "NepoProg.py"); + meta.put("type", "python"); + meta.put("project_id", "OpenRoberta"); + + params.put("slotid", slotId); + params.put("size", fileContentEncoded.length); + params.put("meta", meta); + + assemblePayload("start_write_program", params); + } + + private void createWritePackagePayload() { + List paramList = new ArrayList<>(); + int maxDataSize = 512; + int rest = 0; + int end; + for ( int i = 0; i < fileContentEncoded.length; i += end ) { + JSONObject param = new JSONObject(); + end = Math.min(maxDataSize, fileContentEncoded.length - rest); + rest += end; + param.put("data", Base64.getEncoder().encodeToString(Arrays.copyOfRange(fileContentEncoded, i, i + end))); + + paramList.add(param); + } + for ( JSONObject params : paramList ) { + assemblePayload("write_package", params); + } + } + + private void createExecuteProgramPayload() { + JSONObject params = new JSONObject(); + params.put("slotid", slotId); + assemblePayload("program_execute", params); + } + + private void assemblePayload(String mode, JSONObject params) { + JSONObject payload = new JSONObject(); + + payload.put("m", mode); + payload.put("p", params); + payload.put("i", RandomStringUtils.randomAlphanumeric(4)); + + payloads.add(payload); + } + + private void addTransferIdToWritePackage(String transferId) { + for ( JSONObject payload : payloads ) { + if ( payload.getString("m").equals("write_package") ) { + payload.getJSONObject("p").put("transferid", transferId); + } + } + } + + private Pair sendPayloads() throws InterruptedException, JSONException { + Pair result = new Pair<>(0, "Program successfully uploaded"); + + if ( !serialPort.isOpen() ) { + serialPort.openPort(); + } + LOG.info("Program upload starts"); + for ( int i = 0; i < payloads.size(); i++ ) { + JSONObject payload = payloads.get(i); + String payloadAsString = payload + "\r"; + byte[] payloadAsBytes = payloadAsString.getBytes(StandardCharsets.UTF_8); + int payloadLength = payloadAsBytes.length; + int bytesWritten = serialPort.writeBytes(payloadAsBytes, payloadLength); + + if ( bytesWritten == payloadLength ) { + Pair pair = receiveResponse(payload); + if ( pair.getFirst() == 1 ) { + return new Pair<>(1, pair.getSecond()); + } + LOG.info("Payload " + (i + 1) + " of " + payloads.size() + " uploaded"); + TimeUnit.MILLISECONDS.sleep(100); + } else { + return new Pair<>(1, "Robot seems to be disconnected. Please reconnect the robot with the computer and upload the program again"); + } + } + return result; + } + + private Pair receiveResponse(JSONObject payload) throws InterruptedException, JSONException { + String id = payload.getString("i"); + Pattern findResponsePattern = Pattern.compile("\\{.*" + id + ".*}"); + short bufSize = 2048; + byte[] buffer = new byte[bufSize]; + long time = System.currentTimeMillis(); + while ( (System.currentTimeMillis()) - time < 10000 ) { + int bytesAvailable = serialPort.bytesAvailable(); + if ( bytesAvailable < 0 ) { + return new Pair<>(1, "Robot seems to be disconnected. Please reconnect the robot with the computer and upload the program again"); + } + serialPort.readBytes(buffer, Math.min(bytesAvailable, bufSize)); + String answer = new String(buffer, StandardCharsets.UTF_8); + Matcher responseMatcher = findResponsePattern.matcher(answer); + if ( responseMatcher.find() ) { + return checkResponse(responseMatcher.group(), id, payload); + } + TimeUnit.MILLISECONDS.sleep(200); + } + LOG.error("Timeout: No response received from the robot"); + return new Pair<>(1, "No response received from the robot"); + } + + private Pair checkResponse(String response, String id, JSONObject payload) throws JSONException { + try { + JSONObject jsonAnswer = new JSONObject(response); + if ( jsonAnswer.has("e") ) { + String error = new String(Base64.getDecoder().decode(jsonAnswer.getString("e")), StandardCharsets.UTF_8); + LOG.error("The robot threw an error. Please try uploading the program again.\n\n" + error); + return new Pair<>(1, "An error occured on the robot while transmitting the payload:\n\n" + error); + } + if ( jsonAnswer.has("i") && jsonAnswer.getString("i").equals(id) ) { + if ( !jsonAnswer.get("r").equals(JSONObject.NULL) ) { + JSONObject r = jsonAnswer.getJSONObject("r"); + if ( !transferIdAdded && r.has("transferid") ) { + addTransferIdToWritePackage(jsonAnswer.getJSONObject("r").getString("transferid")); + transferIdAdded = true; + } + if ( r.has("checksum") ) { + String data = payload.getJSONObject("p").getString("data"); + String checksum = jsonAnswer.getJSONObject("r").getString("checksum"); + return validateChecksum(checksum, data); + } + } + return new Pair<>(0, "Everything worked"); + } + return new Pair<>(1, "IDs didn't match"); + } catch ( JSONException e ) { + LOG.info("Broken response detected. Ignoring and continuing with upload"); + return new Pair<>(0, "Ignoring and sending next payload"); + } + } + + private Pair validateChecksum(String checksum, String base64Data) { + byte[] decoded = Base64.getDecoder().decode(base64Data); + String hash = new DigestUtils("SHA-256").digestAsHex(decoded); + return checksum.equals(hash) ? new Pair<>(0, "Checksum is correct") : new Pair<>(1, "The SHA-256 checksums didnt match"); + } + + private void startClearBufferThread() { + clearBufferThread.start(serialPort); + } + + private void stopClearBufferThrad() throws InterruptedException { + clearBufferThread.exit(); + } +} \ No newline at end of file diff --git a/src/main/java/de/fhg/iais/roberta/connection/wired/legoLargeHub/LegoLargeHubConnector.java b/src/main/java/de/fhg/iais/roberta/connection/wired/legoLargeHub/LegoLargeHubConnector.java new file mode 100644 index 0000000..432a5a5 --- /dev/null +++ b/src/main/java/de/fhg/iais/roberta/connection/wired/legoLargeHub/LegoLargeHubConnector.java @@ -0,0 +1,128 @@ +package de.fhg.iais.roberta.connection.wired.legoLargeHub; + +import java.io.File; +import java.io.FileNotFoundException; +import java.io.FileOutputStream; +import java.io.IOException; + +import org.json.JSONException; +import org.json.JSONObject; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import de.fhg.iais.roberta.connection.AbstractConnector; +import de.fhg.iais.roberta.connection.wired.arduino.ArduinoConnector; +import de.fhg.iais.roberta.util.OraTokenGenerator; +import de.fhg.iais.roberta.util.Pair; + +public class LegoLargeHubConnector extends AbstractConnector { + private static final Logger LOG = LoggerFactory.getLogger(ArduinoConnector.class); + + private LegoLargeHubCommunicator legoLargeHubCommunicator = null; + + protected LegoLargeHubConnector(LegoLargeHub robot) { + super(robot); + } + + @Override + protected void runLoopBody() { + switch ( this.state ) { + case DISCOVER: + this.legoLargeHubCommunicator = new LegoLargeHubCommunicator(this.robot); + this.fire(State.WAIT_FOR_CONNECT_BUTTON_PRESS); + break; + case CONNECT_BUTTON_IS_PRESSED: + this.token = OraTokenGenerator.generateToken(); + this.fire(State.WAIT_FOR_SERVER); + + this.brickData = this.legoLargeHubCommunicator.getDeviceInfo(); + this.brickData.put(KEY_TOKEN, this.token); + this.brickData.put(KEY_CMD, CMD_REGISTER); + LOG.info(this.brickData.toString()); + try { + JSONObject serverResponse = this.serverCommunicator.pushRequest(this.brickData); + String command = serverResponse.getString("cmd"); + switch ( command ) { + case CMD_REPEAT: + this.fire(State.WAIT_FOR_CMD); + LOG.info("Robot successfully registered with token {}, waiting for commands", this.token); + break; + case CMD_ABORT: + LOG.info("registration timeout"); + this.fire(State.TOKEN_TIMEOUT); + this.fire(State.DISCOVER); + break; + default: + LOG.error("Unexpected command {} from server", command); + this.reset(State.ERROR_HTTP); + } + } catch ( IOException | JSONException e ) { + LOG.error("CONNECT {}", e.getMessage()); + this.reset(State.ERROR_HTTP); + } + break; + case WAIT_FOR_CMD: + this.brickData = this.legoLargeHubCommunicator.getDeviceInfo(); + this.brickData.put(KEY_TOKEN, this.token); + this.brickData.put(KEY_CMD, CMD_PUSH); + try { + JSONObject serverResponse = this.serverCommunicator.pushRequest(this.brickData); + String cmdKey = serverResponse.getString(KEY_CMD); + switch ( cmdKey ) { + case CMD_REPEAT: + break; + case CMD_DOWNLOAD: + LOG.info("Download user program"); + try { + Pair program = this.serverCommunicator.downloadProgram(this.brickData); + File tmp = File.createTempFile(program.getSecond(), ""); + tmp.deleteOnExit(); + + if ( !tmp.exists() ) { + throw new FileNotFoundException("File " + tmp.getAbsolutePath() + " does not exist."); + } + + try (FileOutputStream os = new FileOutputStream(tmp)) { + os.write(program.getFirst()); + } + this.fire(State.WAIT_UPLOAD); + Pair result = this.legoLargeHubCommunicator.handleUpload(tmp.getAbsolutePath()); + if ( result.getFirst() != 0 ) { + this.fire(State.ERROR_UPLOAD_TO_ROBOT.setAdditionalInfo(result.getSecond())); + this.fire(State.WAIT_FOR_CMD); + } + } catch ( FileNotFoundException e ) { + LOG.info("File not found: {}", e.getMessage()); + this.fire(State.ERROR_UPLOAD_TO_ROBOT); + this.fire(State.WAIT_FOR_CMD); + } catch ( IOException io ) { + LOG.info("Download and run failed: {}", io.getMessage()); + LOG.info("Do not give up yet - make the next push request"); + this.fire(State.ERROR_UPLOAD_TO_ROBOT); + this.fire(State.WAIT_FOR_CMD); + } + break; + case CMD_CONFIGURATION: + LOG.info("Configuration"); + break; + case CMD_UPDATE: + LOG.info("Firmware update not necessary and not supported!"); + break; + case CMD_ABORT: + LOG.error("Unexpected response from server: {}", cmdKey); + this.reset(State.ERROR_HTTP); + break; + } + } catch ( IOException | JSONException e ) { + LOG.error("WAIT_FOR_CMD {}", e.getMessage()); + this.reset(State.ERROR_HTTP); + } + break; + case WAIT_UPLOAD: + this.fire(State.WAIT_FOR_CMD); + break; + default: + break; + } + } +} diff --git a/src/main/resources/wired-robot-ids.txt b/src/main/resources/wired-robot-ids.txt index 17baf85..ca76e4d 100644 --- a/src/main/resources/wired-robot-ids.txt +++ b/src/main/resources/wired-robot-ids.txt @@ -3,19 +3,21 @@ # vendorID,productID,wiredRobotType # # Available wired robot types are: -# Arduino Uno: uno -# Arduino Nano: nano -# Arduino Mega: mega -# BOB3: bob3 -# Bot'n Roll: ardu -# Mbot: mbot -# Micro:bit/Calliope mini: microbit -# Arduino Uno Wifi Rev2: unowifirev2 -# Bionics4Education: festobionic -# Arduino Nano 33 BLE: nano33ble -# ROB3RTA: rob3rta -# FestoBionicFlower festobionicflower -# Mbot2 mbot2 +# Arduino Uno: uno +# Arduino Nano: nano +# Arduino Mega: mega +# BOB3: bob3 +# Bot'n Roll: ardu +# Mbot: mbot +# Mbot2: mbot2 +# Micro:bit/Calliope mini: microbit +# Arduino Uno Wifi Rev2: unowifirev2 +# Bionics4Education: festobionic +# Arduino Nano 33 BLE: nano33ble +# ROB3RTA: rob3rta +# FestoBionicFlower: festobionicflower +# Spike Prime: spikeprime +# Robot Inventor robotinventor 2341,0043,uno 2A03,0043,uno @@ -31,6 +33,8 @@ 16C0,0933,rob3rta 0403,6001,festobionicflower 1a86,7523,mbot2 +0694,0009,spikeprime +0694,0010,robotinventor # nano33ble: 005A for flashing, 805A for serial communication 2341,005A,nano33ble