--- /dev/null
+package lejos.rcxcomm;\r
+\r
+import lejos.nxt.*;\r
+\r
+/**\r
+ * Emulation of the RCX Serial class with mindstorms NRLINK adapter.\r
+ * \r
+ * @author Lawrie Griffiths\r
+ *\r
+ */\r
+public class Serial {\r
+ private static RCXLink link;\r
+ private static byte[] buf1 = new byte[1];\r
+ private static byte[] packet = new byte[32];\r
+ private static boolean gotOpcode = false;\r
+ private static boolean gotPacket = false;\r
+ private static boolean skipping = false;\r
+ private static int paramsRead;\r
+ private static int paramsRequired;\r
+ private static int checkSum;\r
+ private static byte lastOpcode = 0;\r
+ \r
+ private Serial() { \r
+ }\r
+ \r
+ /**\r
+ * Set the sensor port\r
+ * \r
+ * @param port the sensor port the link is connected to\r
+ */\r
+ public static void setPort(SensorPort port) {\r
+ link = new RCXLink(port);\r
+ link.setDefaultSpeed();\r
+ link.flush();\r
+ }\r
+\r
+ /**\r
+ * Read an assembled packet. NRLink only seems to read\r
+ * one byte at a time reliably, and does not\r
+ * return zero bytes. \r
+ * \r
+ * @param aBuffer the buffer to return the packet into\r
+ * @return the number of bytes in the packet\r
+ */\r
+ public static int readPacket (byte[] aBuffer) {\r
+ if (!gotPacket) return 0;\r
+ gotPacket = false;\r
+ gotOpcode = false;\r
+ for(int i=0;i<paramsRequired+1;i++) aBuffer[i] = packet[i];\r
+ return paramsRequired+1;\r
+ }\r
+ \r
+ /**\r
+ * Test if a packet is available\r
+ * \r
+ * @return true iff a packiet is available\r
+ */\r
+ public static boolean isPacketAvailable() {\r
+ if (gotPacket) return true;\r
+ \r
+ int available = link.bytesAvailable();\r
+ \r
+ // After a failure, skip to next header\r
+ \r
+ if (skipping) {\r
+ while(available> 0) {\r
+ readByte(buf1);\r
+ //LCD.drawInt(buf1[0] & 0xFF, 4, 8, 7);\r
+ //LCD.refresh();\r
+ try {Thread.sleep(50);} catch (InterruptedException e) {}\r
+ available--;\r
+ if (headerByte(buf1[0])) {\r
+ skipping = false;\r
+ break;\r
+ }\r
+ }\r
+ if (skipping) return false;\r
+ gotOpcode = false;\r
+ available = link.bytesAvailable();\r
+ }\r
+ \r
+ // If we don't have the opcode yet, skip header bytes\r
+ \r
+ if (!gotOpcode) {\r
+ byte op = 0;\r
+ \r
+ // skip header bytes, and check complement \r
+ // for candidate opcode.\r
+ \r
+ while (available > 1) { // Need 2 bytes\r
+ readByte(buf1);\r
+ available--;\r
+ op = buf1[0];\r
+ if (!headerByte(op)) {\r
+ readByte(buf1);\r
+ available--;\r
+ if (complement(buf1[0], op)) {\r
+ gotOpcode = true;\r
+ break;\r
+ } else { \r
+ // Skip to header if complement check failed \r
+ skipping = true;\r
+ return false;\r
+ }\r
+ }\r
+ }\r
+ \r
+ // If we have the op code, calculate number of parameters\r
+ // and start the checksum\r
+ \r
+ if (!gotOpcode) return false; \r
+ available = link.bytesAvailable();\r
+ packet[0] = op;\r
+ int pq = op & 0x7;\r
+ paramsRequired = 0;\r
+ if (pq > 1 && pq < 6) paramsRequired = pq; \r
+ if (pq == 7) paramsRequired = 1;\r
+ paramsRead = 0;\r
+ checkSum = (op & 0xFF);\r
+ //LCD.drawInt(opCode & 0xFF,4, 0,6);\r
+ //LCD.drawInt(paramsRequired,4, 0,7);\r
+ //LCD.refresh();\r
+ //try {Thread.sleep(500);} catch (InterruptedException e) {}\r
+ }\r
+ \r
+ // If we don't have all the parameters, get them\r
+ // and check the complements\r
+ \r
+ if (paramsRead < paramsRequired) {\r
+ while (available > 1) {\r
+ readByte(buf1);\r
+ available--;\r
+ byte param = buf1[0];\r
+ readByte(buf1);\r
+ available--;\r
+ if (complement(buf1[0], param)) {\r
+ paramsRead++;\r
+ checkSum += (param & 0xFF);\r
+ packet[paramsRead] = param;\r
+ } else {\r
+ skipping = true;\r
+ //LCD.drawInt(param &0xFF, 4, 0, 4);\r
+ //LCD.drawInt(buf1[0] & 0xFF, 4, 8, 4);\r
+ //LCD.refresh();\r
+ return false;\r
+ }\r
+ if (paramsRead == paramsRequired) break;\r
+ }\r
+ if (paramsRead != paramsRequired) return false;\r
+ available = link.bytesAvailable();\r
+ }\r
+\r
+ // Check the checksum and its complement\r
+ \r
+ if (available > 1) {\r
+ readByte(buf1);\r
+ available--;\r
+ byte checkDigit = buf1[0];\r
+ readByte(buf1);\r
+ available--;\r
+ //LCD.drawInt(checkDigit &0xFF, 4, 0, 5);\r
+ //LCD.drawInt(checkSum & 0xFF, 4, 8, 5);\r
+ //LCD.refresh();\r
+ if (complement(buf1[0], checkDigit) &&\r
+ (checkDigit & 0xFF) == (checkSum & 0xFF)) {\r
+ gotPacket = true;\r
+ lastOpcode = packet[0];\r
+ } else {\r
+ // Skip to header if complement check\r
+ // or checksum fails\r
+ skipping = true;\r
+ }\r
+ }\r
+\r
+ return gotPacket;\r
+ }\r
+ \r
+ /**\r
+ * Send a packet\r
+ * \r
+ * @param aBuffer the buffer containing the packet\r
+ * @param aOffset the offset in the buffer - must be zero\r
+ * @param aLen the length of the packet\r
+ * @return true iff the packet was successfully sent\r
+ */\r
+ public static boolean sendPacket (byte[] aBuffer, int aOffset, int aLen) {\r
+ sleep();\r
+ link.defineAndRun(aBuffer, aLen); // assumes offset 0\r
+ sleep();\r
+ return true;\r
+ }\r
+\r
+ /**\r
+ * Set long range\r
+ *\r
+ */\r
+ public static void setRangeLong() {\r
+ link.setRangeLong();\r
+ }\r
+ \r
+ /**\r
+ * Set short range\r
+ * \r
+ */\r
+ public static void setRangeShort() {\r
+ link.setRangeLong();\r
+ }\r
+ \r
+ /**\r
+ * Reset the link - null\r
+ *\r
+ */\r
+ public static void resetSerial() {\r
+ }\r
+ \r
+ /**\r
+ * Wait until the packet is sent - null\r
+ *\r
+ */\r
+ public static void waitTillSent() { \r
+ }\r
+ \r
+ /**\r
+ * Get the RCXLink object associated with the Serial class\r
+ * \r
+ * @return the link\r
+ */\r
+ public static RCXLink getLink() {\r
+ return link;\r
+ }\r
+\r
+ private static void sleep() {\r
+ try {\r
+ Thread.sleep(100);\r
+ } catch (InterruptedException e) {}\r
+ }\r
+ \r
+ private static boolean headerByte(byte b) {\r
+ return(b == 0x55 || b == (byte) 0xFF || b == 0x00);\r
+ \r
+ }\r
+ \r
+ private static void readByte(byte [] b) {\r
+ b[0] = 0;\r
+ link.readBytes(b);\r
+ }\r
+ \r
+ private static boolean complement(byte b1, byte b2) {\r
+ return ((b1 &0xFF) + (b2 & 0xFF) == 0xFF);\r
+ }\r
+}\r
OSDN Git Service
