/**
   * Sets the velocities of the Create's left and right motors in millimeters per second, and runs
   * the motors for a set amount of time determined by <code>runningTime</code>. After the time has
   * elapsed the motors are stopped.
   *
   * @param leftMotor Velocity of Create's left wheel in millimeters per second
   * @param rightMotor Velocity of Create's right wheel in millimeters per second
   * @param runningTime Time in milliseconds for the motors to run
   */
  public void moveMotors(int leftMotor, int rightMotor, int runningTime) {

    // Speeds above 500mm/s are not valid, so don't send them
    if (leftMotor < -500 || leftMotor > 500 || rightMotor < -500 || rightMotor > 500) {
      System.out.println("Invalid motor commands, values must be between -500 and 500");
    } else {
      // Get a serial service
      final SerialIOService serialIOService = getQwerkController().getSerialIOService();
      if (serialIOService != null) {
        try {
          // Convert negative speeds to values over 32767 to send the data in two's complement form
          if (rightMotor < 0) {
            rightMotor += 65536;
          }

          if (leftMotor < 0) {
            leftMotor += 65536;
          }

          // Create the movement packet
          byte[] data = new byte[6];

          data[0] = (byte) 131; // Change mode to 'safe'
          data[1] = (byte) 145; // Send the command for driving the wheels
          data[2] = (byte) (rightMotor / 256); // Send the high 8-bits for right wheel speed
          data[3] = (byte) (rightMotor % 256); // Send the low 8-bits for right wheel speed
          data[4] = (byte) (leftMotor / 256);
          data[5] = (byte) (leftMotor % 256);

          // Write the data to the Create through the Qwerk's serial port
          serialIOService.getSerialPort(QwerkSerialPortDevice.DEV_TTY_AM1.getName()).write(data);
          System.out.println(dateFormatter.format(new Date()) + "Data written successfully.");
          sleepUnlessStop(runningTime);
          stopMoving();
        } catch (SerialIOException e) {
          final String msg = "SerialIOException while writing to the serial port";
          System.out.println(dateFormatter.format(new Date()) + msg);
        }
      } else {
        System.out.println(dateFormatter.format(new Date()) + "SerialIOService is null!");
      }
    }
  }
  /**
   * Cause the Create to rotate by the number of degrees specified by <code>travelAngle</code>
   *
   * @param travelAngle Specifies the number of degrees to rotate - positive values cause counter
   *     clockwise rotation, negative values cause clockwise rotation
   */
  public void moveAngle(int travelAngle) {
    int leftMotor = 0;
    int rightMotor = 0;

    if (travelAngle > 0) {
      leftMotor = 65406;
      rightMotor = 130;
    } else if (travelAngle < 0) {
      leftMotor = 130;
      rightMotor = 65406;
    }

    // Get a serial service
    final SerialIOService serialIOService = getQwerkController().getSerialIOService();
    if (serialIOService != null) {
      try {
        // Convert negative distances to values over 32767 to send the data in two's complement form
        int sleepTime;

        if (travelAngle < 0) {
          sleepTime = -travelAngle * 20;
          travelAngle += 65536;
        } else {
          sleepTime = travelAngle * 20;
        }
        updateSensors();
        // Create the movement packet
        byte[] data = new byte[15];

        data[0] = (byte) 131; // Change mode to 'safe'
        data[1] = (byte) 145; // Send the command for driving the wheels
        data[2] = (byte) (rightMotor / 256); // Send the high 8-bits for right wheel speed
        data[3] = (byte) (rightMotor % 256); // Send the low 8-bits for right wheel speed
        data[4] = (byte) (leftMotor / 256);
        data[5] = (byte) (leftMotor % 256);
        data[6] = (byte) 157; // Change mode to 'safe'
        data[7] = (byte) (travelAngle / 256); // Send the high 8-bits for travel distance
        data[8] = (byte) (travelAngle % 256); // Send the low 8-bits for travel distance
        data[9] = (byte) 131; // Change mode to 'safe'
        data[10] = (byte) 145; // Send the command for driving the wheels
        data[11] = (byte) (0); // Send the high 8-bits for right wheel speed
        data[12] = (byte) (0); // Send the low 8-bits for right wheel speed
        data[13] = (byte) (0);
        data[14] = (byte) (0);

        // Write the data to the Create through the Qwerk's serial port
        serialIOService.getSerialPort(QwerkSerialPortDevice.DEV_TTY_AM1.getName()).write(data);
        System.out.println(dateFormatter.format(new Date()) + "Data written successfully.");
        stopMoving();
        if (travelAngle < 0) {
          travelAngle = -travelAngle;
        }
        sleepUnlessStop(sleepTime);
        updateSensors();
      } catch (SerialIOException e) {
        final String msg = "SerialIOException while writing to the serial port";
        System.out.println(dateFormatter.format(new Date()) + msg);
      }
    } else {
      System.out.println(dateFormatter.format(new Date()) + "SerialIOService is null!");
    }
  }