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

   1 package lejos.nxt.comm;
   2
   3 import java.io.*;
   4 import lejos.nxt.*;
   5
   6 /**
   7  *
   8  * Implements the Lego Communication Protocol,
   9  * with some extensions for lejos NXJ.
  10  *
  11  */
  12 public class LCP {
  13         private static byte[] i2cReply = new byte[16];
  14         private static int i2cLen = 0;
  15     private static File[] files = null;
  16     private static String[] fileNames = null;
  17     private static int fileIdx = -1;
  18     private static String currentProgram = null;
  19     private static File file = null;
  20     private static FileOutputStream out = null;
  21     private static FileInputStream in = null;
  22     private static int numFiles;
  23
  24         // Command types constants. Indicates type of packet being sent or received.
  25         public static byte DIRECT_COMMAND_REPLY = 0x00;
  26         public static byte SYSTEM_COMMAND_REPLY = 0x01;
  27         public static byte REPLY_COMMAND = 0x02;
  28         public static byte DIRECT_COMMAND_NOREPLY = (byte)0x80; // Avoids ~100ms latency
  29         public static byte SYSTEM_COMMAND_NOREPLY = (byte)0x81; // Avoids ~100ms latency
  30
  31         // Direct Commands
  32         public static final byte START_PROGRAM = 0x00;
  33         public static final byte STOP_PROGRAM = 0x01;
  34         public static final byte PLAY_SOUND_FILE = 0x02;
  35         public static final byte PLAY_TONE = 0x03;
  36         public static final byte SET_OUTPUT_STATE = 0x04;
  37         public static final byte SET_INPUT_MODE = 0x05;
  38         public static final byte GET_OUTPUT_STATE = 0x06;
  39         public static final byte GET_INPUT_VALUES = 0x07;
  40         public static final byte RESET_SCALED_INPUT_VALUE = 0x08;
  41         public static final byte MESSAGE_WRITE = 0x09;
  42         public static final byte RESET_MOTOR_POSITION = 0x0A;
  43         public static final byte GET_BATTERY_LEVEL = 0x0B;
  44         public static final byte STOP_SOUND_PLAYBACK = 0x0C;
  45         public static final byte KEEP_ALIVE = 0x0D;
  46         public static final byte LS_GET_STATUS = 0x0E;
  47         public static final byte LS_WRITE = 0x0F;
  48         public static final byte LS_READ = 0x10;
  49         public static final byte GET_CURRENT_PROGRAM_NAME = 0x11;
  50
  51         // NXJ additions
  52         public static byte NXJ_DISCONNECT = 0x20;
  53         public static byte NXJ_DEFRAG = 0x21;
  54
  55         // System Commands:
  56         public static final byte OPEN_READ = (byte)0x80;
  57         public static final byte OPEN_WRITE = (byte)0x81;
  58         public static final byte READ = (byte)0x82;
  59         public static final byte WRITE = (byte)0x83;
  60         public static final byte CLOSE = (byte)0x84;
  61         public static final byte DELETE = (byte)0x85;
  62         public static final byte FIND_FIRST = (byte)0x86;
  63         public static final byte FIND_NEXT = (byte)0x87;
  64         public static final byte GET_FIRMWARE_VERSION = (byte)0x88;
  65         public static final byte OPEN_WRITE_LINEAR = (byte)0x89;
  66         public static final byte OPEN_READ_LINEAR = (byte)0x8A;
  67         public static final byte OPEN_WRITE_DATA = (byte)0x8B;
  68         public static final byte OPEN_APPEND_DATA = (byte)0x8C;
  69         public static final byte BOOT = (byte)0x97;
  70         public static final byte SET_BRICK_NAME = (byte)0x98;
  71         public static final byte GET_DEVICE_INFO = (byte)0x9B;
  72         public static final byte DELETE_USER_FLASH = (byte)0xA0;
  73         public static final byte POLL_LENGTH = (byte)0xA1;
  74         public static final byte POLL = (byte)0xA2;
  75
  76         public static final byte NXJ_FIND_FIRST = (byte)0xB6;
  77         public static final byte NXJ_FIND_NEXT = (byte)0xB7;
  78
  79         // Error codes
  80
  81         public static final byte FILE_NOT_FOUND = (byte)0x86;
  82
  83         private LCP()
  84         {
  85         }
  86
  87         /**
  88          * Emulates a Lego firmware Direct or System command
  89          * @param cmd the buffer containing the command
  90          * @param cmdLen the legth of the command
  91          */
  92         public static int emulateCommand(byte[] cmd, int cmdLen, byte[] reply)
  93         {
  94             int len = 3;
  95
  96             for(int i=0;i<reply.length;i++)reply[i] = 0;
  97
  98                 reply[0] = REPLY_COMMAND;;
  99                 reply[1] = cmd[1];
 100
 101                 byte cmdId = cmd[1];
 102
 103                 // START PROGRAM
 104                 if (cmdId == START_PROGRAM) {
 105                         int filenameLength = 0;
 106                         init_files();
 107                         for(int i=0;i<20 && cmd[i+2] != 0;i++) filenameLength++;
 108                         char[] chars = new char[filenameLength];
 109                         for(int i=0;i<filenameLength;i++) chars[i] = (char) cmd[i+2];
 110                         currentProgram = new String(chars,0,filenameLength);
 111                         if (fileNames != null) {
 112                                 for(int i=0;i<fileNames.length;i++) {
 113                                         if (currentProgram.equals(fileNames[i])) {
 114                                                 LCD.clear();
 115                                                 LCD.refresh();
 116                                                 files[i].exec();
 117                                         }
 118                                 }
 119                         }
 120                 }
 121
 122                 // GET CURRENT PROGRAM NAME
 123
 124                 if (cmdId == GET_CURRENT_PROGRAM_NAME) {
 125                         if (currentProgram != null) {
 126                                 for(int i=0;i<currentProgram.length() && i < 19;i++)
 127                                         reply[3+i] = (byte) currentProgram.charAt(i);
 128                         }
 129                 }
 130
 131                 // GET BATTERY LEVEL
 132                 if (cmdId == GET_BATTERY_LEVEL) {
 133                         int mv = Battery.getVoltageMilliVolt();
 134
 135                         reply[3] = getLSB(mv);
 136                         reply[4] = getMSB(mv);
 137                         len = 5;
 138                 }
 139
 140                 // PLAYTONE
 141                 if (cmdId == PLAY_TONE)
 142                 {
 143                         Sound.playTone(getInt(cmd,2), getInt(cmd,4));
 144                 }
 145
 146                 // GET FIRMWARE VERSION
 147                 if (cmdId == GET_FIRMWARE_VERSION)
 148                 {
 149                         reply[3] = 2;
 150                         reply[4] = 1;
 151                         reply[5] = 3;
 152                         reply[6] = 1;
 153                         len = 7;
 154                 }
 155
 156                 // GET DEVICE INFO
 157                 if (cmdId == GET_DEVICE_INFO)
 158                 {
 159             byte [] name = Bluetooth.getFriendlyName();
 160             for(int i=0;i<15;i++) reply[3+i] = name[i];
 161             byte [] address = Bluetooth.getLocalAddress();
 162             for(int i=0;i<7;i++) reply[18+i] = address[i];
 163             int freeMem = File.freeMemory();
 164                         reply[29] = (byte) (freeMem & 0xFF);
 165                         reply[30] = (byte) ((freeMem >> 8) & 0xFF);
 166                         reply[31] = (byte) ((freeMem >> 16) & 0xFF);
 167                         reply[32] = (byte) ((freeMem >> 24) & 0xFF);
 168                         len = 33;
 169                 }
 170
 171                 // SET BRICK NAME
 172                 if (cmdId == SET_BRICK_NAME)
 173                 {
 174             byte [] name = new byte[16];
 175             for(int i=0;i<16;i++) name[i] = cmd[i+2];
 176             Bluetooth.setFriendlyName(name);
 177                         len = 4;
 178                 }
 179
 180                 // GETOUTPUTSTATE
 181                 if (cmdId == GET_OUTPUT_STATE) {
 182                         byte port = cmd[2];
 183                         Motor m;
 184                         if(port == 0)
 185                                 m = Motor.A;
 186                         else if(port == 1)
 187                                 m = Motor.B;
 188                         else m = Motor.C;
 189                         int tacho = m.getTachoCount();
 190
 191                         reply[3] = port;
 192                         reply[4] = (byte)(m.getSpeed() * 100 / 900); // Power
 193                         // MODE CALCULATION:
 194                         byte mode = 0;
 195                         if (m.isMoving()) mode = 0x01; // 0x01 = MOTORON
 196                         reply[5] = mode; // Only contains isMoving (MOTORON) at moment
 197                         // REGULATION_MODE CALCULATION:
 198                         byte regulation_mode = 0; // 0 = idle
 199                         if (m.isMoving()) mode = 0x01; // 0x01 = MOTOR_SPEED
 200                         // !! This returns same as run state (below). Whats the diff?
 201                         reply[6] = regulation_mode; // Regulation mode
 202                         // TURN RATIO CALC (ignored):
 203                         byte turn_ratio = 0; // NXJ uses Pilot. Omitting.
 204                         reply[7] = turn_ratio; // Turn ratio
 205                         // RUN_STATE CALCULATION:
 206                         byte run_state = 0;
 207                         if (m.isMoving()) run_state = 0x20; // 0x20 = RUNNING
 208                         reply[8] = run_state; // Run state
 209                         // 9 - 12 = Tacho Limit. Ignored?
 210                         reply[13] = (byte) (tacho & 0xFF);
 211                         reply[14] = (byte) ((tacho >> 8) & 0xFF);
 212                         reply[15] = (byte) ((tacho >> 16) & 0xFF);
 213                         reply[16] = (byte) ((tacho >> 24) & 0xFF);
 214                         len = 25;
 215                 }
 216
 217                 // GETINPUTVALUES
 218                 if (cmdId == GET_INPUT_VALUES) {
 219                         byte port = cmd[2];
 220                         int raw = SensorPort.PORTS[port].readRawValue();
 221                         int scaled = SensorPort.PORTS[port].readValue();
 222                         int norm = 1024 - raw;
 223
 224                         reply[3] = port;
 225                         reply[4] = 1;
 226                         reply[5] = 0;
 227                         reply[6] = (byte) SensorPort.PORTS[port].getType();
 228                         reply[7] = (byte) SensorPort.PORTS[port].getMode();
 229                         reply[8] = getLSB(raw);
 230                         reply[9] = getMSB(raw);
 231                         reply[10] = getLSB(norm);
 232                         reply[11] = getMSB(norm);
 233                         reply[12] = getLSB(scaled);
 234                         reply[13] = getMSB(scaled);
 235                         reply[14] = 0;
 236                         reply[15] = 0;
 237                         len = 16;
 238                 }
 239
 240                 // SETINPUTMODE
 241                 if (cmdId == SET_INPUT_MODE) {
 242                         byte port = cmd[2];
 243                         int sensorType = (cmd[3] & 0xFF);
 244                         int sensorMode = (cmd[4] & 0xFF);
 245                         SensorPort.PORTS[port].setTypeAndMode(sensorType, sensorMode);
 246                 }
 247
 248                 // SETOUTPUTSTATE
 249                 if(cmdId == SET_OUTPUT_STATE) {
 250                         byte motorid = cmd[2];
 251                         byte power = cmd[3];
 252                         int speed = (Math.abs(power) * 900) / 100;
 253                         byte mode = cmd[4];
 254                         byte regMode = cmd[5];
 255                         byte turnRatio = cmd[6];
 256                         byte runState = cmd[7];
 257                         int tacholimit = (0xFF & cmd[8]) | ((0xFF & cmd[9]) << 8)| ((0xFF & cmd[10]) << 16)| ((0xFF & cmd[11]) << 24);
 258
 259                         // Initialize motor:
 260                         Motor m = null;
 261
 262                         for(int i=0;i<3;i++)
 263                         {
 264                                 if(motorid == 0 || (motorid < 0 && i == 0))
 265                                         m = Motor.A;
 266                                 else if (motorid == 1 || (motorid < 0 && i == 1))
 267                                         m = Motor.B;
 268                                 else if (motorid == 2 || (motorid < 0 && i == 2))
 269                                     m = Motor.C;
 270
 271                                 m.setSpeed(speed);
 272
 273                                 if(power < 0) tacholimit = -tacholimit;
 274
 275                                 // Check if command is to STOP:
 276                                 if(power == 0) m.stop();
 277
 278                                 // Check if doing tacho rotation
 279                                 if(tacholimit != 0)
 280                                         m.rotate(tacholimit, true); // Returns immediately
 281
 282                                 if((mode | 0x01) != 0 && power != 0 && tacholimit == 0) { // MOTORON
 283                                         if(power>0) m.forward();
 284                                         else m.backward();
 285                                 }
 286
 287                                 if (motorid >= 0) break;
 288                         }
 289                 }
 290
 291                 // RESETMOTORPOSITION
 292                 if (cmdId == RESET_MOTOR_POSITION)
 293                 {
 294                         MotorPort.resetTachoCountById(cmd[2]);
 295                 }
 296
 297                 // KEEPALIVE
 298                 if (cmdId == KEEP_ALIVE)
 299                 {
 300                         len = 7;
 301                 }
 302
 303                 // LSWRITE
 304                 if (cmdId == LS_WRITE)
 305                 {
 306                         byte port = cmd[2];
 307                         byte txLen = cmd[3];
 308                         byte rxLen = cmd[4];
 309                         SensorPort.i2cEnableById(port);
 310                         try {Thread.sleep(100);} catch(InterruptedException ie) {}
 311                         int ret = SensorPort.i2cStartById(port, cmd[5] >> 1, cmd[6], rxLen, i2cReply, rxLen, 0);
 312
 313                         while (SensorPort.i2cBusyById(port) != 0) {
 314                                 Thread.yield();
 315                         }
 316                         try {Thread.sleep(100);} catch(InterruptedException ie) {}
 317                         i2cLen = rxLen;
 318                 }
 319
 320                 // LSREAD
 321                 if (cmdId == LS_READ)
 322                 {
 323                         reply[3] = (byte) i2cLen;
 324                         for(int i=0;i<16;i++) reply[i+4] = i2cReply[i];
 325                         len = 20;
 326                         i2cLen = 0;
 327                 }
 328
 329                 // LSGETSTATUS
 330                 if (cmdId == LS_GET_STATUS)
 331                 {
 332                         reply[3] = (byte) i2cLen;
 333                         len = 4;
 334                 }
 335
 336                 // OPEN READ
 337                 if (cmdId == OPEN_READ)
 338                 {
 339                         int filenameLength = 0;
 340                         init_files();
 341                         for(int i=0;i<20 && cmd[i+2] != 0;i++) filenameLength++;
 342                         char[] chars = new char[filenameLength];
 343                         for(int i=0;i<filenameLength;i++) chars[i] = (char) cmd[i+2];
 344                         file = new File(new String(chars,0,filenameLength));
 345             try {
 346                 in = new FileInputStream(file);
 347                 int size = file.length();
 348                 cmd[4] = (byte) (size & 0xFF);
 349                         cmd[5] = (byte) ((size >> 8) & 0xFF);
 350                         cmd[6] = (byte) ((size >> 16) & 0xFF);
 351                         cmd[7] = (byte) ((size >> 24) & 0xFF);
 352             } catch (Exception e) {
 353                 reply[2] = FILE_NOT_FOUND;
 354             }
 355                         len = 8;
 356                 }
 357
 358                 // OPEN WRITE
 359                 if (cmdId == OPEN_WRITE)
 360                 {
 361                         int filenameLength = 0;
 362                         init_files();
 363                         for(int i=0;i<20 && cmd[i+2] != 0;i++) filenameLength++;
 364                         char[] chars = new char[filenameLength];
 365                         for(int i=0;i<filenameLength;i++) chars[i] = (char) cmd[i+2];
 366                         file = new File(new String(chars,0,filenameLength));
 367                         int size = cmd[22] & 0xFF;
 368                         size += ((cmd[23] & 0xFF) << 8);
 369                         size += ((cmd[24] & 0xFF) << 16);
 370                         size += ((cmd[25] & 0xFF) << 24);
 371                         if (file.exists()) {
 372                                 file.delete();
 373                                 numFiles--;
 374                         }
 375                         file.createNewFile();
 376                         fileNames = new String[++numFiles];
 377                         for(int j=0;j<numFiles;j++) fileNames[j] = files[j].getName();
 378                         out = new FileOutputStream(file);
 379
 380                         len = 4;
 381                 }
 382
 383                 // OPEN WRITE LINEAR
 384                 if (cmdId == OPEN_WRITE_LINEAR)
 385                 {
 386                         len = 4;
 387                 }
 388
 389                 // OPEN WRITE DATA
 390                 if (cmdId == OPEN_WRITE_DATA)
 391                 {
 392                         len = 4;
 393                 }
 394
 395                 // OPEN APPEND  DATA
 396                 if (cmdId == OPEN_APPEND_DATA)
 397                 {
 398                         reply[2] = FILE_NOT_FOUND;
 399                         len = 8;
 400                 }
 401
 402                 // DEFRAG
 403                 if (cmdId == NXJ_DEFRAG)
 404                 {
 405                         File.defrag();
 406                 }
 407
 408                 // FIND FIRST
 409                 if (cmdId == FIND_FIRST || cmdId == NXJ_FIND_FIRST)
 410                 {
 411                         init_files();
 412                         if (cmdId == FIND_FIRST) len = 28;
 413                         else len = 32;
 414                         if (numFiles == 0)
 415                         {
 416                                 reply[2] = FILE_NOT_FOUND;
 417                         }
 418                         else
 419                         {
 420                                 for(int i=0;i<fileNames[0].length();i++) reply[4+i] = (byte) fileNames[0].charAt(i);
 421                                 fileIdx = 1;
 422                 int size = files[0].length();
 423                 reply[24] = (byte) (size & 0xFF);
 424                         reply[25] = (byte) ((size >> 8) & 0xFF);
 425                         reply[26] = (byte) ((size >> 16) & 0xFF);
 426                         reply[27] = (byte) ((size >> 24) & 0xFF);
 427
 428                         if (cmdId == NXJ_FIND_FIRST) {
 429                                 int startPage = files[0].getPage();
 430                                 reply[28] = (byte) (startPage & 0xFF);
 431                                 reply[29] = (byte) ((startPage >> 8) & 0xFF);
 432                                 reply[30] = (byte) ((startPage >> 16) & 0xFF);
 433                                 reply[31] = (byte) ((startPage >> 24) & 0xFF);
 434                         }
 435                         }
 436                 }
 437
 438                 // FIND NEXT
 439                 if (cmdId == FIND_NEXT || cmdId == NXJ_FIND_NEXT)
 440                 {
 441                         if (cmdId == FIND_NEXT) len = 28;
 442                         else len = 32;
 443                         if (fileNames == null || fileIdx >= fileNames.length) reply[2] = FILE_NOT_FOUND;
 444                         else
 445                         {
 446                                 for(int i=0;i<fileNames[fileIdx].length();i++) reply[4+i] = (byte) fileNames[fileIdx].charAt(i);
 447                 int size = files[fileIdx].length();
 448                 reply[24] = (byte) (size & 0xFF);
 449                         reply[25] = (byte) ((size >> 8) & 0xFF);
 450                         reply[26] = (byte) ((size >> 16) & 0xFF);
 451                         reply[27] = (byte) ((size >> 24) & 0xFF);
 452
 453                         if (cmdId == NXJ_FIND_NEXT) {
 454                                 int startPage = files[fileIdx].getPage();
 455                                 reply[28] = (byte) (startPage & 0xFF);
 456                                 reply[29] = (byte) ((startPage >> 8) & 0xFF);
 457                                 reply[30] = (byte) ((startPage >> 16) & 0xFF);
 458                                 reply[31] = (byte) ((startPage >> 24) & 0xFF);
 459                         }
 460
 461                                 fileIdx++;
 462                         }
 463                 }
 464
 465                 // READ
 466                 if (cmdId == READ)
 467                 {
 468             int numBytes = ((cmd[4] & 0xFF) << 8) + (cmd[3] & 0xFF);
 469             int bytesRead = 0;
 470
 471             try {
 472                 bytesRead = in.read(reply,6, numBytes);
 473             } catch (IOException ioe) {}
 474                         reply[4] = (byte) (bytesRead & 0xFF);
 475                         reply[5] = (byte) ((bytesRead << 8) & 0xFF);
 476                         len = bytesRead + 6;
 477                 }
 478
 479                 // WRITE
 480                 if (cmdId == WRITE)
 481                 {
 482                         int dataLen = cmdLen - 3;
 483                         try {
 484                                 out.write(cmd,3,dataLen);
 485                         } catch (Exception ioe) {
 486                                 //LCD.drawString("Exception", 0, 7);
 487                                 //LCD.refresh();
 488                         }
 489                         reply[4] = (byte) (dataLen &0xFF);
 490                         reply[5] = (byte) ((dataLen >> 8) & 0xFF);
 491                         len = 6;
 492                 }
 493
 494                 // DELETE
 495                 if (cmdId == DELETE)
 496                 {
 497                         int filenameLength = 0;
 498                         boolean deleted = false;
 499                         len = 23;
 500                         for(int i=0;i<20 && cmd[i+2] != 0;i++) filenameLength++;
 501                         char[] chars = new char[filenameLength];
 502                         for(int i=0;i<filenameLength;i++) chars[i] = (char) cmd[i+2];
 503                         String fileName = new String(chars,0,filenameLength);
 504                         if (fileNames != null) {
 505                                 for(int i=0;i<fileNames.length;i++) {
 506                                         if (fileName.equals(fileNames[i])) {
 507                                                 files[i].delete();
 508                                                 for(int j=0;j<filenameLength;j++) reply[j+3] = (byte) chars[j];
 509                                                 deleted = true;
 510                                                 fileNames = new String[--numFiles];
 511                                                 for(int j=0;j<numFiles;j++) fileNames[j] = files[j].getName();
 512                                                 break;
 513                                         }
 514                                 }
 515                         }
 516                         if (!deleted) reply[2] = FILE_NOT_FOUND;
 517                 }
 518
 519                 // CLOSE
 520                 if (cmdId == CLOSE)
 521                 {
 522                         if (out != null) {
 523                                 try {
 524                                         out.flush();
 525                                         out.close();
 526                                 } catch (Exception ioe) {
 527                                         //LCD.drawString("Exception",0,7);
 528                                         //LCD.refresh();
 529                                 }
 530                                 out = null;
 531                         }
 532                         len = 4;
 533                 }
 534
 535                 return len;
 536         }
 537
 538         private static int getInt(byte [] cmd, int i)
 539         {
 540                 return (cmd[i] & 0xFF) + ((cmd[i+1] & 0xFF) << 8);
 541         }
 542
 543         private static byte getLSB(int i)
 544         {
 545                 return (byte) (i & 0xFF);
 546         }
 547
 548         private static byte getMSB(int i)
 549         {
 550                 return (byte) ((i >> 8) & 0xFF);
 551         }
 552
 553         private static void init_files() {
 554                 if (files == null) {
 555                         files = File.listFiles();
 556                         numFiles = 0;
 557                         for(int i=0;i<files.length && files[i] != null;i++) numFiles++;
 558                         fileNames = new String[numFiles];
 559                         for(int i=0;i<numFiles;i++) fileNames[i] = files[i].getName();
 560                 }
 561         }
 562 }
 563