LegoMindstormsNxtBase.java

Go to the documentation of this file.

// -*- mode: java; c-basic-offset: 2; -*-
// Copyright 2009-2011 Google, All Rights reserved
// Copyright 2011-2012 MIT, All rights reserved
// Released under the Apache License, Version 2.0
// http://www.apache.org/licenses/LICENSE-2.0
 
package com.google.appinventor.components.runtime;
 
import com.google.appinventor.components.annotations.DesignerProperty;
import com.google.appinventor.components.annotations.PropertyCategory;
import com.google.appinventor.components.annotations.SimpleObject;
import com.google.appinventor.components.annotations.SimpleProperty;
import com.google.appinventor.components.common.PropertyTypeConstants;
import com.google.appinventor.components.runtime.util.ErrorMessages;
 
import android.util.Log;
 
import java.io.UnsupportedEncodingException;
import java.util.Collections;
import java.util.HashMap;
import java.util.Map;
 
@SimpleObject
public class LegoMindstormsNxtBase extends AndroidNonvisibleComponent
    implements BluetoothConnectionListener, Component, Deleteable {
  private static final int TOY_ROBOT = 0x0804; // from android.bluetooth.BluetoothClass.Device.
 
  private static final Map<Integer, String> ERROR_MESSAGES;
  static {
    ERROR_MESSAGES = new HashMap<Integer, String>();
    ERROR_MESSAGES.put(0x20, "Pending communication transaction in progress");
    ERROR_MESSAGES.put(0x40, "Specified mailbox queue is empty");
    ERROR_MESSAGES.put(0x81, "No more handles");
    ERROR_MESSAGES.put(0x82, "No space");
    ERROR_MESSAGES.put(0x83, "No more files");
    ERROR_MESSAGES.put(0x84, "End of file expected");
    ERROR_MESSAGES.put(0x85, "End of file");
    ERROR_MESSAGES.put(0x86, "Not a linear file");
    ERROR_MESSAGES.put(0x87, "File not found");
    ERROR_MESSAGES.put(0x88, "Handle already closed");
    ERROR_MESSAGES.put(0x89, "No linear space");
    ERROR_MESSAGES.put(0x8A, "Undefined error");
    ERROR_MESSAGES.put(0x8B, "File is busy");
    ERROR_MESSAGES.put(0x8C, "No write buffers");
    ERROR_MESSAGES.put(0x8D, "Append not possible");
    ERROR_MESSAGES.put(0x8E, "File is full");
    ERROR_MESSAGES.put(0x8F, "File exists");
    ERROR_MESSAGES.put(0x90, "Module not found");
    ERROR_MESSAGES.put(0x91, "Out of boundary");
    ERROR_MESSAGES.put(0x92, "Illegal file name");
    ERROR_MESSAGES.put(0x93, "Illegal handle");
    ERROR_MESSAGES.put(0xBD, "Request failed (i.e. specified file not found)");
    ERROR_MESSAGES.put(0xBE, "Unknown command opcode");
    ERROR_MESSAGES.put(0xBF, "Insane packet");
    ERROR_MESSAGES.put(0xC0, "Data contains out-of-range values");
    ERROR_MESSAGES.put(0xDD, "Communication bus error");
    ERROR_MESSAGES.put(0xDE, "No free memory in communication buffer");
    ERROR_MESSAGES.put(0xDF, "Specified channel/connection is not valid");
    ERROR_MESSAGES.put(0xE0, "Specified channel/connection not configured or busy");
    ERROR_MESSAGES.put(0xEC, "No active program");
    ERROR_MESSAGES.put(0xED, "Illegal size specified");
    ERROR_MESSAGES.put(0xEE, "Illegal mailbox queue ID specified");
    ERROR_MESSAGES.put(0xEF, "Attempted to access invalid field of a structure");
    ERROR_MESSAGES.put(0xF0, "Bad input or output specified");
    ERROR_MESSAGES.put(0xFB, "Insufficient memory available");
    ERROR_MESSAGES.put(0xFF, "Bad arguments");
  }
 
  protected final String logTag;
 
  // TODO(lizlooney) - allow communication via USB if possible.
  protected BluetoothClient bluetooth;
 
 
  protected LegoMindstormsNxtBase(ComponentContainer container, String logTag) {
    super(container.$form());
    this.logTag = logTag;
  }
 
  protected LegoMindstormsNxtBase() {
    super(null);
    logTag = null;
  }
 
  public final void Initialize() {
  }
 
  @SimpleProperty(
      description = "The BluetoothClient component that should be used for communication.",
      category = PropertyCategory.BEHAVIOR, userVisible = false)
  public BluetoothClient BluetoothClient() {
    return bluetooth;
  }
 
  @DesignerProperty(editorType = PropertyTypeConstants.PROPERTY_TYPE_BLUETOOTHCLIENT,
      defaultValue = "")
  @SimpleProperty(userVisible = false)
  public void BluetoothClient(BluetoothClient bluetoothClient) {
    if (bluetooth != null) {
      bluetooth.removeBluetoothConnectionListener(this);
      bluetooth.detachComponent(this);
      bluetooth = null;
    }
 
    if (bluetoothClient != null) {
      bluetooth = bluetoothClient;
      bluetooth.attachComponent(this, Collections.singleton(TOY_ROBOT));
      bluetooth.addBluetoothConnectionListener(this);
      if (bluetooth.IsConnected()) {
        // We missed the real afterConnect event.
        afterConnect(bluetooth);
      }
    }
  }
 
  protected final void setOutputState(String functionName, int port, int power, int mode,
      int regulationMode, int turnRatio, int runState, long tachoLimit) {
    power = sanitizePower(power);
    byte[] command = new byte[12];
    command[0] = (byte) 0x80;  // Direct command telegram, no response
    command[1] = (byte) 0x04;  // SETOUTPUTSTATE command
    copyUBYTEValueToBytes(port, command, 2);
    copySBYTEValueToBytes(power, command, 3);
    copyUBYTEValueToBytes(mode, command, 4);
    copyUBYTEValueToBytes(regulationMode, command, 5);
    copySBYTEValueToBytes(turnRatio, command, 6);
    copyUBYTEValueToBytes(runState, command, 7);
    // NOTE(lizlooney) - the LEGO MINDSTORMS NXT Direct Commands documentation (AKA Appendix 2)
    // says to use bytes 8-12 for the ULONG tacho limit. That's 5 bytes!
    // I've tested sending a 5th byte and it is ignored. Paul Gyugyi confirmed that the code for
    // the NXT firmware only uses 4 bytes. I'm pretty sure the documentation was supposed to say
    // bytes 8-11.
    copyULONGValueToBytes(tachoLimit, command, 8);
    sendCommand(functionName, command);
  }
 
  protected final void setInputMode(String functionName, int port, int sensorType, int sensorMode) {
    byte[] command = new byte[5];
    command[0] = (byte) 0x80;  // Direct command telegram, no response
    command[1] = (byte) 0x05;  // SETINPUTMODE command
    copyUBYTEValueToBytes(port, command, 2);
    copyUBYTEValueToBytes(sensorType, command, 3);
    copyUBYTEValueToBytes(sensorMode, command, 4);
    sendCommand(functionName, command);
  }
 
  protected final byte[] getInputValues(String functionName, int port) {
    byte[] command = new byte[3];
    command[0] = (byte) 0x00;  // Direct command telegram, response required
    command[1] = (byte) 0x07;  // GETINPUTVALUES command
    copyUBYTEValueToBytes(port, command, 2);
    byte[] returnPackage = sendCommandAndReceiveReturnPackage(functionName, command);
    if (evaluateStatus(functionName, returnPackage, command[1])) {
      if (returnPackage.length == 16) {
        return returnPackage;
      } else {
        Log.w(logTag, functionName + ": unexpected return package length " +
            returnPackage.length + " (expected 16)");
      }
    }
    return null;
  }
 
  protected final void resetInputScaledValue(String functionName, int port) {
    byte[] command = new byte[3];
    command[0] = (byte) 0x80;  // Direct command telegram, no response
    command[1] = (byte) 0x08;  // RESETINPUTSCALEDVALUE command
    copyUBYTEValueToBytes(port, command, 2);
    sendCommand(functionName, command);
  }
 
  protected final int lsGetStatus(String functionName, int port) {
    byte[] command = new byte[3];
    command[0] = (byte) 0x00;  // Direct command telegram, response required
    command[1] = (byte) 0x0E;  // LSGETSTATUS command
    copyUBYTEValueToBytes(port, command, 2);
    byte[] returnPackage = sendCommandAndReceiveReturnPackage(functionName, command);
    if (evaluateStatus(functionName, returnPackage, command[1])) {
      if (returnPackage.length == 4) {
        return getUBYTEValueFromBytes(returnPackage, 3);
      } else {
        Log.w(logTag, functionName + ": unexpected return package length " +
            returnPackage.length + " (expected 4)");
      }
    }
    return 0;
  }
 
  protected final void lsWrite(String functionName, int port, byte[] data, int rxDataLength) {
    if (data.length > 16) {
      throw new IllegalArgumentException("length must be <= 16");
    }
    byte[] command = new byte[5 + data.length];
    command[0] = (byte) 0x00;  // Direct command telegram, response required
    command[1] = (byte) 0x0F;  // LSWRITE command
    copyUBYTEValueToBytes(port, command, 2);
    copyUBYTEValueToBytes(data.length, command, 3);
    copyUBYTEValueToBytes(rxDataLength, command, 4);
    System.arraycopy(data, 0, command, 5, data.length);
    byte[] returnPackage = sendCommandAndReceiveReturnPackage(functionName, command);
    evaluateStatus(functionName, returnPackage, command[1]);
  }
 
  protected final byte[] lsRead(String functionName, int port) {
    byte[] command = new byte[3];
    command[0] = (byte) 0x00;  // Direct command telegram, response required
    command[1] = (byte) 0x10;  // LSREAD command
    copyUBYTEValueToBytes(port, command, 2);
    byte[] returnPackage = sendCommandAndReceiveReturnPackage(functionName, command);
    if (evaluateStatus(functionName, returnPackage, command[1])) {
      if (returnPackage.length == 20) {
        return returnPackage;
      } else {
        Log.w(logTag, functionName + ": unexpected return package length " +
            returnPackage.length + " (expected 20)");
      }
    }
    return null;
  }
 
 
  /*
   * Checks whether the bluetooth property has been set or whether this
   * component is connected to a robot and, if necessary, dispatches the
   * appropriate error.
   *
   * Returns true if everything is ok, false if there was an error.
   */
  protected final boolean checkBluetooth(String functionName) {
    if (bluetooth == null) {
      form.dispatchErrorOccurredEvent(this, functionName,
          ErrorMessages.ERROR_NXT_BLUETOOTH_NOT_SET);
      return false;
    }
    if (!bluetooth.IsConnected()) {
      form.dispatchErrorOccurredEvent(this, functionName,
          ErrorMessages.ERROR_NXT_NOT_CONNECTED_TO_ROBOT);
      return false;
    }
    return true;
  }
 
  protected final byte[] sendCommandAndReceiveReturnPackage(String functionName, byte[] command) {
    sendCommand(functionName, command);
    return receiveReturnPackage(functionName);
  }
 
  protected final void sendCommand(String functionName, byte[] command) {
    byte[] header = new byte[2];
    copyUWORDValueToBytes(command.length, header, 0);
    bluetooth.write(functionName, header);
    bluetooth.write(functionName, command);
  }
 
  private byte[] receiveReturnPackage(String functionName) {
    byte[] header = bluetooth.read(functionName, 2);
    if (header.length == 2) {
      int length = getUWORDValueFromBytes(header, 0);
      byte[] returnPackage = bluetooth.read(functionName, length);
      if (returnPackage.length >= 3) {
        return returnPackage;
      }
    }
 
    form.dispatchErrorOccurredEvent(this, functionName,
        ErrorMessages.ERROR_NXT_INVALID_RETURN_PACKAGE);
    return new byte[0];
  }
 
  protected final boolean evaluateStatus(String functionName, byte[] returnPackage, byte command) {
    int status = getStatus(functionName, returnPackage, command);
    if (status == 0) {
      return true;
    } else {
      handleError(functionName, status);
      return false;
    }
  }
 
  protected final int getStatus(String functionName, byte[] returnPackage, byte command) {
    if (returnPackage.length >= 3) {
      if (returnPackage[0] != (byte) 0x02) {
        Log.w(logTag, functionName + ": unexpected return package byte 0: 0x" +
            Integer.toHexString(returnPackage[0] & 0xFF) + " (expected 0x02)");
      }
      if (returnPackage[1] != command) {
        Log.w(logTag, functionName + ": unexpected return package byte 1: 0x" +
            Integer.toHexString(returnPackage[1] & 0xFF) + " (expected 0x" +
            Integer.toHexString(command & 0xFF) + ")");
      }
      return getUBYTEValueFromBytes(returnPackage, 2);
    } else {
      Log.w(logTag, functionName + ": unexpected return package length " +
          returnPackage.length + " (expected >= 3)");
    }
    return -1;
  }
 
  private void handleError(String functionName, int status) {
    if (status < 0) {
      // Real status bytes received from the NXT are unsigned.
      // -1 is returned from getStatus when the returnPackage is not even big enough to contain a
      // status byte. In that case, we've already called form.dispatchErrorOccurredEvent from
      // receiveReturnPackage.
    } else {
      String errorMessage = ERROR_MESSAGES.get(status);
      if (errorMessage != null) {
        form.dispatchErrorOccurredEvent(this, functionName,
            ErrorMessages.ERROR_NXT_ERROR_CODE_RECEIVED, errorMessage);
      } else {
        form.dispatchErrorOccurredEvent(this, functionName,
            ErrorMessages.ERROR_NXT_ERROR_CODE_RECEIVED,
            "Error code 0x" + Integer.toHexString(status & 0xFF));
      }
    }
  }
 
  protected final void copyBooleanValueToBytes(boolean value, byte[] bytes, int offset) {
    bytes[offset] = value ? (byte) 1 : (byte) 0;
  }
 
  protected final void copySBYTEValueToBytes(int value, byte[] bytes, int offset) {
    bytes[offset] = (byte) value;
  }
 
  protected final void copyUBYTEValueToBytes(int value, byte[] bytes, int offset) {
    bytes[offset] = (byte) value;
  }
 
  protected final void copySWORDValueToBytes(int value, byte[] bytes, int offset) {
    bytes[offset] = (byte) (value & 0xff);
    value = value >> 8;
    bytes[offset + 1] = (byte) (value & 0xff);
  }
 
  protected final void copyUWORDValueToBytes(int value, byte[] bytes, int offset) {
    bytes[offset] = (byte) (value & 0xff);
    value = value >> 8;
    bytes[offset + 1] = (byte) (value & 0xff);
  }
 
  protected final void copySLONGValueToBytes(int value, byte[] bytes, int offset) {
    bytes[offset] = (byte) (value & 0xff);
    value = value >> 8;
    bytes[offset + 1] = (byte) (value & 0xff);
    value = value >> 8;
    bytes[offset + 2] = (byte) (value & 0xff);
    value = value >> 8;
    bytes[offset + 3] = (byte) (value & 0xff);
  }
 
  protected final void copyULONGValueToBytes(long value, byte[] bytes, int offset) {
    bytes[offset] = (byte) (value & 0xff);
    value = value >> 8;
    bytes[offset + 1] = (byte) (value & 0xff);
    value = value >> 8;
    bytes[offset + 2] = (byte) (value & 0xff);
    value = value >> 8;
    bytes[offset + 3] = (byte) (value & 0xff);
  }
 
  protected final void copyStringValueToBytes(String value, byte[] bytes, int offset,
      int maxCount) {
    if (value.length() > maxCount) {
      value = value.substring(0, maxCount);
    }
    byte[] valueBytes;
    try {
      valueBytes = value.getBytes("ISO-8859-1");
    } catch (UnsupportedEncodingException e) {
      Log.w(logTag, "UnsupportedEncodingException: " + e.getMessage());
      valueBytes = value.getBytes();
    }
    int lengthToCopy = Math.min(maxCount, valueBytes.length);
    System.arraycopy(valueBytes, 0, bytes, offset, lengthToCopy);
  }
 
  protected final boolean getBooleanValueFromBytes(byte[] bytes, int offset) {
    return bytes[offset] != 0;
  }
 
  protected final int getSBYTEValueFromBytes(byte[] bytes, int offset) {
    return bytes[offset];
  }
 
  protected final int getUBYTEValueFromBytes(byte[] bytes, int offset) {
    return bytes[offset] & 0xFF;
  }
 
  protected final int getSWORDValueFromBytes(byte[] bytes, int offset) {
    return (bytes[offset] & 0xFF) |
           (bytes[offset + 1] << 8);
  }
 
  protected final int getUWORDValueFromBytes(byte[] bytes, int offset) {
    return (bytes[offset] & 0xFF) |
           ((bytes[offset + 1] & 0xFF) << 8);
  }
 
  protected final int getSLONGValueFromBytes(byte[] bytes, int offset) {
    return (bytes[offset] & 0xFF) |
           ((bytes[offset + 1] & 0xFF) << 8) |
           ((bytes[offset + 2] & 0xFF) << 16) |
           (bytes[offset + 3] << 24);
  }
 
  protected final long getULONGValueFromBytes(byte[] bytes, int offset) {
    return (bytes[offset] & 0xFFL) |
           ((bytes[offset + 1] & 0xFFL) << 8) |
           ((bytes[offset + 2] & 0xFFL) << 16) |
           ((bytes[offset + 3] & 0xFFL) << 24);
  }
 
  protected final String getStringValueFromBytes(byte[] bytes, int offset) {
    // Determine length by looking for the null termination byte.
    int length = 0;
    for (int i = offset; i < bytes.length; i++) {
      if (bytes[i] == 0) {
        length = i - offset;
        break;
      }
    }
    return getStringValueFromBytes(bytes, offset, length);
  }
 
  protected final String getStringValueFromBytes(byte[] bytes, int offset, int count) {
    try {
      return new String(bytes, offset, count, "ISO-8859-1");
    } catch (UnsupportedEncodingException e) {
      Log.w(logTag, "UnsupportedEncodingException: " + e.getMessage());
      return new String(bytes, offset, count);
    }
  }
 
  protected final int convertMotorPortLetterToNumber(String motorPortLetter) {
    if (motorPortLetter.length() == 1) {
      return convertMotorPortLetterToNumber(motorPortLetter.charAt(0));
    }
    throw new IllegalArgumentException("Illegal motor port letter " + motorPortLetter);
  }
 
  protected final int convertMotorPortLetterToNumber(char motorPortLetter) {
    if (motorPortLetter == 'A' || motorPortLetter == 'a') {
      return 0;
    } else if (motorPortLetter == 'B' || motorPortLetter == 'b') {
      return 1;
    } else if (motorPortLetter == 'C' || motorPortLetter == 'c') {
      return 2;
    }
    throw new IllegalArgumentException("Illegal motor port letter " + motorPortLetter);
  }
 
  protected final int convertSensorPortLetterToNumber(String sensorPortLetter) {
    if (sensorPortLetter.length() == 1) {
      return convertSensorPortLetterToNumber(sensorPortLetter.charAt(0));
    }
    throw new IllegalArgumentException("Illegal sensor port letter " + sensorPortLetter);
  }
 
  protected final int convertSensorPortLetterToNumber(char sensorPortLetter) {
    if (sensorPortLetter == '1') {
      return 0;
    } else if (sensorPortLetter == '2') {
      return 1;
    } else if (sensorPortLetter == '3') {
      return 2;
    } else if (sensorPortLetter == '4') {
      return 3;
    }
    throw new IllegalArgumentException("Illegal sensor port letter " + sensorPortLetter);
  }
 
  protected final int sanitizePower(int power) {
    if (power < -100) {
      Log.w(logTag, "power " + power + " is invalid, using -100.");
      power = -100;
    }
    if (power > 100) {
      Log.w(logTag, "power " + power + " is invalid, using 100.");
      power = 100;
    }
    return power;
  }
 
  protected final int sanitizeTurnRatio(int turnRatio) {
    if (turnRatio < -100) {
      Log.w(logTag, "turnRatio " + turnRatio + " is invalid, using -100.");
      turnRatio = -100;
    }
    if (turnRatio > 100) {
      Log.w(logTag, "turnRatio " + turnRatio + " is invalid, using 100.");
      turnRatio = 100;
    }
    return turnRatio;
  }
 
  // BluetoothConnectionListener implementation
 
  @Override
  public void afterConnect(BluetoothConnectionBase bluetoothConnection) {
    // Subclasses may wish to do something.
  }
 
  @Override
  public void beforeDisconnect(BluetoothConnectionBase bluetoothConnection) {
    // Subclasses may wish to do something.
  }
 
  // Deleteable implementation
 
  @Override
  public void onDelete() {
    if (bluetooth != null) {
      bluetooth.removeBluetoothConnectionListener(this);
      bluetooth.detachComponent(this);
      bluetooth = null;
    }
  }
}