/**
* 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!");
}
}
