nxtOSEK/lejos_nxj/src/java/classes/lejos/nxt/comm/NXTCommand.java

   1 package lejos.nxt.comm;\r
   2 \r
   3 import java.io.*;\r
   4 \r
   5 /**\r
   6  * \r
   7  * Remote access to a NXT via Bluetooth using LCP.\r
   8  *\r
   9  */\r
  10 public class NXTCommand implements NXTProtocol {\r
  11         \r
  12         private NXTComm nxtComm = new NXTComm();\r
  13         private boolean verifyCommand = false;\r
  14         \r
  15         public void open(String name) throws IOException {\r
  16                 boolean open = nxtComm.open(name);\r
  17                 if (!open) throw new IOException("Open failed");\r
  18         }\r
  19         \r
  20         /**\r
  21          * Small helper method to send request to NXT and return verification result.\r
  22          * @param request\r
  23          * @return 0 for success\r
  24          */\r
  25         private byte sendRequest(byte [] request) throws IOException {\r
  26                 byte verify = 0; // default of 0 means success\r
  27                 if(verifyCommand)\r
  28                         request[0] = DIRECT_COMMAND_REPLY;\r
  29                 \r
  30                 nxtComm.sendData(request);\r
  31                 if(verifyCommand) {\r
  32                         byte [] reply = nxtComm.readData();\r
  33                         verify = reply[2];\r
  34                 }\r
  35                 return verify;\r
  36         }\r
  37         \r
  38         /**\r
  39          * \r
  40          * @param port - Output port (0 - 2 or 0xFF for all three)\r
  41          * @param power - Setpoint for power. (-100 to 100)\r
  42          * @param mode - Setting the modes MOTORON, BRAKE, and/or REGULATED. This parameter is a bitfield, so to put it in brake mode and regulated, use BRAKEMODE + REGULATED\r
  43          * @param regulationMode - see NXTProtocol for enumerations \r
  44          * @param turnRatio - Need two motors? (-100 to 100)\r
  45          * @param runState - see NXTProtocol for enumerations\r
  46          * @param tachoLimit - Number of degrees(?) to rotate before stopping.\r
  47          */\r
  48         public byte setOutputState(int port, byte power, int mode, int regulationMode, int turnRatio, int runState, int tachoLimit) throws IOException {\r
  49                 synchronized(this) {\r
  50                         byte [] request = {DIRECT_COMMAND_NOREPLY, SET_OUTPUT_STATE, (byte)port, power, (byte)mode, (byte)regulationMode, (byte)turnRatio, (byte)runState, (byte)tachoLimit, (byte)(tachoLimit>>>8), (byte)(tachoLimit>>>16), (byte)(tachoLimit>>>24)};\r
  51                         return sendRequest(request);\r
  52                 }\r
  53         }\r
  54         \r
  55         /**\r
  56          * Retrieves tacho count.\r
  57          * @param port - 0 to 3\r
  58          * @return tacho count\r
  59          */\r
  60         public int getTachoCount(int port) throws IOException {\r
  61                 synchronized(this) {\r
  62                         byte [] request = {DIRECT_COMMAND_REPLY, GET_OUTPUT_STATE, (byte)port};\r
  63                         nxtComm.sendData(request);\r
  64                         byte [] reply = nxtComm.readData();\r
  65         \r
  66                         int tachoCount = (0xFF & reply[13]) | ((0xFF & reply[14]) << 8)| ((0xFF & reply[15]) << 16)| ((0xFF & reply[16]) << 24);\r
  67                         return tachoCount;\r
  68                 }\r
  69         }\r
  70         \r
  71         /**\r
  72          * Resets the tachometer\r
  73          * @param port Output port (0-2)\r
  74          * @param relative TRUE: position relative to last movement, FALSE: absolute position\r
  75          */\r
  76         public byte resetMotorPosition(int port, boolean relative) throws IOException {\r
  77                 synchronized(this) {\r
  78                         byte boolVal = 0;\r
  79                         if(relative) boolVal = (byte)0xFF;\r
  80                         byte [] request = {DIRECT_COMMAND_NOREPLY, RESET_MOTOR_POSITION, (byte)port, boolVal};\r
  81                         return sendRequest(request);\r
  82                 }\r
  83         }\r
  84         \r
  85         public void setVerify(boolean verify) {\r
  86                 verifyCommand = verify;\r
  87         }\r
  88         \r
  89         /**\r
  90          * Call the close() command when your program ends, otherwise you\r
  91          * will have to turn the NXT brick off/on before you run another\r
  92          * program using iCommand.\r
  93          *\r
  94          */\r
  95         public void close() throws IOException {\r
  96                 nxtComm.close();\r
  97         }\r
  98         public int getBatteryLevel() throws IOException {\r
  99                 synchronized (this) {\r
 100                         byte [] request = {DIRECT_COMMAND_REPLY, GET_BATTERY_LEVEL};\r
 101                         nxtComm.sendData(request);\r
 102                         byte [] reply = nxtComm.readData();\r
 103                         int batteryLevel = (0xFF & reply[3]) | ((0xFF & reply[4]) << 8);\r
 104                         return batteryLevel;\r
 105                 }\r
 106         }\r
 107         \r
 108         public InputValues getInputValues(int port) throws IOException {\r
 109                 synchronized (this) {\r
 110                         byte [] request = {DIRECT_COMMAND_REPLY, GET_INPUT_VALUES, (byte)port};\r
 111                         nxtComm.sendData(request);\r
 112                         byte [] reply = nxtComm.readData();\r
 113                         InputValues inputValues = new InputValues();\r
 114                         inputValues.inputPort = reply[3];\r
 115                         // 0 is false, 1 is true.\r
 116                         inputValues.valid = (reply[4] != 0);\r
 117                         // 0 is false, 1 is true. \r
 118                         inputValues.isCalibrated = (reply[5] == 0);\r
 119                         inputValues.sensorType = reply[6];\r
 120                         inputValues.sensorMode = reply[7];\r
 121                         inputValues.rawADValue = (0xFF & reply[8]) | ((0xFF & reply[9]) << 8);\r
 122                         inputValues.normalizedADValue = (0xFF & reply[10]) | ((0xFF & reply[11]) << 8);\r
 123                         inputValues.scaledValue = (short)((0xFF & reply[12]) | (reply[13] << 8));\r
 124                         inputValues.calibratedValue = (short)((0xFF & reply[14]) | (reply[15] << 8));\r
 125                         return inputValues;\r
 126                 }\r
 127         }\r
 128         \r
 129         /**\r
 130          * Tells the NXT what type of sensor you are using and the mode to operate in.\r
 131          * @param port - 0 to 3\r
 132          * @param sensorType - Enumeration for sensor type (see NXTProtocol) \r
 133          * @param sensorMode - Enumeration for sensor mode (see NXTProtocol)\r
 134          */\r
 135         public byte setInputMode(int port, int sensorType, int sensorMode) throws IOException {\r
 136                 synchronized(this) {\r
 137                         byte [] request = {DIRECT_COMMAND_NOREPLY, SET_INPUT_MODE, (byte)port, (byte)sensorType, (byte)sensorMode};\r
 138                         return sendRequest(request);\r
 139                 }\r
 140         }\r
 141         \r
 142         /**\r
 143          * Plays a tone on NXT speaker. If a new tone is sent while the previous tone is playing,\r
 144          * the new tone command will stop the old tone command.\r
 145          * @param frequency \r
 146          * @param duration - In milliseconds.\r
 147          * @return - Returns true if command worked, false if it failed.\r
 148          */\r
 149         public byte playTone(int frequency, int duration) throws IOException {\r
 150                 synchronized (this) {\r
 151                         byte [] request = {DIRECT_COMMAND_NOREPLY, PLAY_TONE, (byte)frequency, (byte)(frequency>>>8), (byte)duration, (byte)(duration>>>8)};\r
 152                         return sendRequest(request);\r
 153                 }               \r
 154         }\r
 155         \r
 156         public DeviceInfo getDeviceInfo() throws IOException {\r
 157                 synchronized (this) {\r
 158                         byte [] request = {SYSTEM_COMMAND_REPLY, GET_DEVICE_INFO};\r
 159                         char[] name = new char[14];\r
 160                         nxtComm.sendData(request);\r
 161                         byte [] reply = nxtComm.readData();\r
 162                         DeviceInfo d = new DeviceInfo();\r
 163                         d.status = reply[2];\r
 164                         int i = 0;\r
 165                         for(;reply[3+i] != 0 && i<14;i++) name[i] = (char) reply[3+i]; \r
 166                         d.NXTname = new String(name,0,i);\r
 167                         d.bluetoothAddress = getAddressString(reply);\r
 168                         d.signalStrength = (0xFF & reply[25]) | ((0xFF & reply[26]) << 8)| ((0xFF & reply[27]) << 16)| ((0xFF & reply[28]) << 24);\r
 169                         d.freeFlash = (0xFF & reply[29]) | ((0xFF & reply[30]) << 8)| ((0xFF & reply[31]) << 16)| ((0xFF & reply[32]) << 24);\r
 170                         return d;\r
 171                 }\r
 172         }\r
 173         \r
 174         private static final char[] cs = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};\r
 175 \r
 176         private String getAddressString(byte [] reply) {\r
 177                 char[] caddr = new char[20];            \r
 178                 int ci = 0;\r
 179                 \r
 180                 for(int i=0; i<7; i++) {\r
 181                         int nr = reply[i+18] & 0xFF;    \r
 182                         caddr[ci++] = cs[nr / 16];\r
 183                         caddr[ci++] = cs[nr % 16];\r
 184                         if (i != 6) caddr[ci++] = ':';\r
 185                 }\r
 186                 return new String(caddr, 0, 20);\r
 187         }\r
 188         \r
 189         public FirmwareInfo getFirmwareVersion() throws IOException {\r
 190                 synchronized (this) {\r
 191                         byte [] request = {SYSTEM_COMMAND_REPLY, GET_FIRMWARE_VERSION};\r
 192                         nxtComm.sendData(request);\r
 193                         byte [] reply = nxtComm.readData();\r
 194                         FirmwareInfo info = new FirmwareInfo();\r
 195                         info.status = reply[2];\r
 196                         char[] cc = new char[3];\r
 197                         if(info.status == 0) {\r
 198                                 cc[1] = '.';\r
 199                                 cc[0] = (char) ('0' + reply[4]);\r
 200                                 cc[2] = (char) ('0' + reply[3]);\r
 201                                 info.protocolVersion = new String(cc,0,3);\r
 202                                 cc[0] = (char) ('0' + reply[6]);\r
 203                                 cc[2] = (char) ('0' + reply[5]);\r
 204                                 info.firmwareVersion = new String(cc,0,3);\r
 205                         }\r
 206                         return info;\r
 207                 }\r
 208         }\r
 209         \r
 210         /**\r
 211          * Deletes user flash memory (not including system modules).\r
 212          * @return 0 for success\r
 213          */\r
 214         public byte deleteUserFlash() throws IOException {\r
 215                 synchronized (this) {\r
 216                         byte [] request = {SYSTEM_COMMAND_REPLY, DELETE_USER_FLASH};\r
 217                         nxtComm.sendData(request);\r
 218                         byte [] reply = nxtComm.readData();\r
 219                         return reply[2];\r
 220                 }\r
 221         }\r
 222 }\r