First commit. Untested.
This commit is contained in:
6
.classpath
Normal file
6
.classpath
Normal file
@@ -0,0 +1,6 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<classpath>
|
||||||
|
<classpathentry kind="src" path="src"/>
|
||||||
|
<classpathentry kind="con" path="org.lejos.nxt.ldt.LEJOS_LIBRARY_CONTAINER/nxt"/>
|
||||||
|
<classpathentry kind="output" path="bin"/>
|
||||||
|
</classpath>
|
5
.gitignore
vendored
Normal file
5
.gitignore
vendored
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
bin/
|
||||||
|
*.class
|
||||||
|
*.nxj
|
||||||
|
*.nxd
|
||||||
|
*~
|
23
.project
Normal file
23
.project
Normal file
@@ -0,0 +1,23 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<projectDescription>
|
||||||
|
<name>libnxtar</name>
|
||||||
|
<comment></comment>
|
||||||
|
<projects>
|
||||||
|
</projects>
|
||||||
|
<buildSpec>
|
||||||
|
<buildCommand>
|
||||||
|
<name>org.eclipse.jdt.core.javabuilder</name>
|
||||||
|
<arguments>
|
||||||
|
</arguments>
|
||||||
|
</buildCommand>
|
||||||
|
<buildCommand>
|
||||||
|
<name>org.lejos.nxt.ldt.leJOSBuilder</name>
|
||||||
|
<arguments>
|
||||||
|
</arguments>
|
||||||
|
</buildCommand>
|
||||||
|
</buildSpec>
|
||||||
|
<natures>
|
||||||
|
<nature>org.lejos.nxt.ldt.leJOSNature</nature>
|
||||||
|
<nature>org.eclipse.jdt.core.javanature</nature>
|
||||||
|
</natures>
|
||||||
|
</projectDescription>
|
22
LICENSE
Normal file
22
LICENSE
Normal file
@@ -0,0 +1,22 @@
|
|||||||
|
Copyright (c) 2014, Miguel Angel Astor Romero
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
|
||||||
|
1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
list of conditions and the following disclaimer.
|
||||||
|
2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
this list of conditions and the following disclaimer in the documentation
|
||||||
|
and/or other materials provided with the distribution.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||||
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
|
||||||
|
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
28
README.md
Normal file
28
README.md
Normal file
@@ -0,0 +1,28 @@
|
|||||||
|
NxtAR: A generic software architecture for Augmented Reality based mobile robot control.
|
||||||
|
========================================================================================
|
||||||
|
|
||||||
|
libnxtarcontrol
|
||||||
|
---------------
|
||||||
|
|
||||||
|
### Abstract ###
|
||||||
|
|
||||||
|
NxtAR is a generic software architecture for the development of Augmented Reality games
|
||||||
|
and applications centered around mobile robot control. This is a reference implementation
|
||||||
|
with support for [LEGO Mindstorms NXT][1] mobile robots.
|
||||||
|
|
||||||
|
### Module description ###
|
||||||
|
|
||||||
|
The **libnxtarcontrol** library is an stand-alone implementation of the robot control protocol
|
||||||
|
used by the different applications in the NxtAR reference implementation, in particular, the
|
||||||
|
control protocol used by the [NxtAR-bot][2] application. This library is intended for use with
|
||||||
|
the [LejOS][3] operating system.
|
||||||
|
|
||||||
|
### Module installation and usage. ###
|
||||||
|
|
||||||
|
Add the libnxtarcontrol_XXXXXX.jar file to your LejOS project's classpath and compile your
|
||||||
|
application normally, following the LejOS project [guidelines][4].
|
||||||
|
|
||||||
|
[1]: http://www.lego.com/en-us/mindstorms/?domainredir=mindstorms.lego.com
|
||||||
|
[2]: https://github.com/sagge-miky/NxtAR-bot
|
||||||
|
[3]: http://www.lejos.org/nxj.php
|
||||||
|
[4]: http://www.lejos.org/nxt/nxj/tutorial/Preliminaries/CompileAndRun.htm
|
@@ -0,0 +1,93 @@
|
|||||||
|
/* Copyright (c) 2014, Miguel Angel Astor Romero
|
||||||
|
* All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*/
|
||||||
|
package ve.ucv.ciens.icaro.libnxtarcontrol;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>An immutable and pure data class that represents an action decoded from a protocol
|
||||||
|
* data message.</p>
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero
|
||||||
|
* @version 1.0
|
||||||
|
* @since December 15, 2014
|
||||||
|
*/
|
||||||
|
public class DecodedControlAction{
|
||||||
|
/**
|
||||||
|
* <p>All recognized actions.</p>
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero
|
||||||
|
* @version 1.0
|
||||||
|
* @since December 16, 2014
|
||||||
|
*/
|
||||||
|
public enum Action{
|
||||||
|
MOVE_FORWARD, MOVE_BACKWARDS, STOP, RECENTER, USER_1, USER_2, USER_3;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>All motor ports and possible combinations without repetitions.<p>
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero
|
||||||
|
* @version 1.0
|
||||||
|
* @since December 16, 2014
|
||||||
|
*/
|
||||||
|
public enum Motor{
|
||||||
|
MOTOR_A, MOTOR_B, MOTOR_C, MOTOR_AB, MOTOR_AC, MOTOR_BC, MOTOR_ABC;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Data fields.
|
||||||
|
public final Action action;
|
||||||
|
public final Motor motor;
|
||||||
|
public final int speed;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Create a new ControlAction object using {@link DecodedControlAction#STOP} as
|
||||||
|
* the default action, {@link DecodedControlAction#MOTOR_ABC} as motor flag, and
|
||||||
|
* 0 as default speed.</p>
|
||||||
|
*/
|
||||||
|
public DecodedControlAction(){
|
||||||
|
this(Action.STOP, Motor.MOTOR_ABC, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Create a new ControlAction object using the specified action. The motor
|
||||||
|
* flag is set to {@link DecodedControlAction#MOTOR_ABC} and the speed is set to 100.</p>
|
||||||
|
*
|
||||||
|
* @param action The action flag to set.
|
||||||
|
*/
|
||||||
|
public DecodedControlAction(Action action){
|
||||||
|
this(action, Motor.MOTOR_ABC, 100);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Create a new ControlAction object using the specified action and motor flag. The
|
||||||
|
* speed is set to 100.</p>
|
||||||
|
*
|
||||||
|
* @param action The action flag to set.
|
||||||
|
* @param motor The motor flag to set.
|
||||||
|
*/
|
||||||
|
public DecodedControlAction(Action action, Motor motor){
|
||||||
|
this(action, motor, 100);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Create a new ControlAction object using the specified action, motor flag and speed.</p>
|
||||||
|
*
|
||||||
|
* @param action The action flag to set.
|
||||||
|
* @param motor The motor flag to set.
|
||||||
|
* @param speed The speed to set. Will be clamped to the range [-100, 100].
|
||||||
|
*/
|
||||||
|
public DecodedControlAction(Action action, Motor motor, int speed){
|
||||||
|
this.action = action;
|
||||||
|
this.motor = motor;
|
||||||
|
this.speed = speed < -100 ? -100 : (speed > 100 ? 100 : speed);
|
||||||
|
}
|
||||||
|
}
|
515
src/ve/ucv/ciens/icaro/libnxtarcontrol/NxtARControlProtocol.java
Normal file
515
src/ve/ucv/ciens/icaro/libnxtarcontrol/NxtARControlProtocol.java
Normal file
@@ -0,0 +1,515 @@
|
|||||||
|
/* Copyright (c) 2014, Miguel Angel Astor Romero
|
||||||
|
* All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*/
|
||||||
|
package ve.ucv.ciens.icaro.libnxtarcontrol;
|
||||||
|
|
||||||
|
import java.io.DataInput;
|
||||||
|
import java.io.DataInputStream;
|
||||||
|
import java.io.IOException;
|
||||||
|
import java.util.LinkedList;
|
||||||
|
|
||||||
|
import lejos.nxt.BasicMotorPort;
|
||||||
|
import lejos.nxt.Battery;
|
||||||
|
import ve.ucv.ciens.icaro.libnxtarcontrol.DecodedControlAction.Action;
|
||||||
|
import ve.ucv.ciens.icaro.libnxtarcontrol.DecodedControlAction.Motor;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>A wrapper around the NxtAR robot control protocol for the LejOS operating system.</p>
|
||||||
|
*
|
||||||
|
* @see <a href="http://www.lejos.org">The LejOS operating system.</a>
|
||||||
|
* @see <a href="https://github.com/sagge-miky/NxtAR-core">NxtAR-core Github repository.</a>
|
||||||
|
* @author Miguel Angel Astor Romero
|
||||||
|
* @version 1.0
|
||||||
|
* @since December 15, 2014
|
||||||
|
*/
|
||||||
|
public class NxtARControlProtocol {
|
||||||
|
private enum MotorPort{
|
||||||
|
MOTOR_A, MOTOR_B, MOTOR_C;
|
||||||
|
}
|
||||||
|
|
||||||
|
private enum MotorAction{
|
||||||
|
FORWARD, BACKWARD, STOP;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Binary flags used for decoding messages.
|
||||||
|
// Motor ports.
|
||||||
|
private static final byte MOTOR_A = (byte)0x01;
|
||||||
|
private static final byte MOTOR_B = (byte)0x02;
|
||||||
|
private static final byte MOTOR_C = (byte)0x04;
|
||||||
|
// Direction of movement (forward or backward).
|
||||||
|
private static final byte DIRECTION = (byte)0x08;
|
||||||
|
// Possible actions.
|
||||||
|
private static final byte RECENTER = (byte)0x10;
|
||||||
|
private static final byte USER_1 = (byte)0x20;
|
||||||
|
private static final byte USER_2 = (byte)0x40;
|
||||||
|
private static final byte USER_3 = (byte)0x80;
|
||||||
|
|
||||||
|
private DataInputStream inputStream;
|
||||||
|
private LinkedList<UserActionListener> userActionListeners;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Create a new ARControl object.</p>
|
||||||
|
*
|
||||||
|
* @param inputStream An opened input stream used to read protocol messages from.
|
||||||
|
* @throws IllegalArgumentException If inputStream is null.
|
||||||
|
*/
|
||||||
|
public NxtARControlProtocol(final DataInputStream inputStream) throws IllegalArgumentException{
|
||||||
|
if(inputStream == null)
|
||||||
|
throw new IllegalArgumentException("Input stream is null.");
|
||||||
|
|
||||||
|
this.inputStream = inputStream;
|
||||||
|
this.userActionListeners = new LinkedList<UserActionListener>();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Changes the input stream associated with this ARControl for the input stream passed as
|
||||||
|
* parameter. The currently set input stream is closed before replacing it.</p>
|
||||||
|
*
|
||||||
|
* @param inputStream An opened input stream.
|
||||||
|
* @throws IOException If an error happened while closing the previous input stream.
|
||||||
|
* @throws IllegalArgumentException If the input stream is null.
|
||||||
|
*/
|
||||||
|
public void setInputStream(DataInputStream inputStream) throws IOException, IllegalArgumentException{
|
||||||
|
if(inputStream == null)
|
||||||
|
throw new IllegalArgumentException("Input stream is null.");
|
||||||
|
|
||||||
|
try{
|
||||||
|
this.inputStream.close();
|
||||||
|
}catch(IOException io){
|
||||||
|
throw io;
|
||||||
|
}
|
||||||
|
|
||||||
|
this.inputStream = null;
|
||||||
|
this.inputStream = inputStream;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Attempts to read a 2-byte message and returns it as is.</p>
|
||||||
|
*
|
||||||
|
* @return The two bytes read from the associated connection as an array.
|
||||||
|
* @throws IOException If reading the message fails. It is the same IOException
|
||||||
|
* as thrown by {@link DataInput#readByte()} if any.
|
||||||
|
*/
|
||||||
|
public byte[] readRawControlMessage() throws IOException{
|
||||||
|
byte[] msg = new byte[2];
|
||||||
|
try{
|
||||||
|
synchronized (inputStream) {
|
||||||
|
msg[0] = inputStream.readByte();
|
||||||
|
msg[1] = inputStream.readByte();
|
||||||
|
}
|
||||||
|
}catch (IOException io){
|
||||||
|
throw io;
|
||||||
|
}
|
||||||
|
return msg;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
*
|
||||||
|
* @param blocking
|
||||||
|
* @return
|
||||||
|
*/
|
||||||
|
public boolean readAndExecuteMessage() throws IOException{
|
||||||
|
boolean success = false;
|
||||||
|
byte[] msg;
|
||||||
|
DecodedControlAction controlAction;
|
||||||
|
|
||||||
|
try{
|
||||||
|
msg = readRawControlMessage();
|
||||||
|
}catch(IOException io){
|
||||||
|
msg = null;
|
||||||
|
throw io;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(msg != null){
|
||||||
|
try{
|
||||||
|
controlAction = decodeMessage(msg);
|
||||||
|
}catch(IllegalArgumentException ia){
|
||||||
|
controlAction = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(controlAction != null){
|
||||||
|
switch(controlAction.action){
|
||||||
|
case MOVE_BACKWARDS:
|
||||||
|
switch(controlAction.motor){
|
||||||
|
case MOTOR_A:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_AB:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_ABC:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_AC:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_B:
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_BC:
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_C:
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOVE_FORWARD:
|
||||||
|
switch(controlAction.motor){
|
||||||
|
case MOTOR_A:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_AB:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_ABC:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_AC:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_B:
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_BC:
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_C:
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case RECENTER:
|
||||||
|
switch(controlAction.motor){
|
||||||
|
case MOTOR_A:
|
||||||
|
recenterMotor(MotorPort.MOTOR_A);
|
||||||
|
break;
|
||||||
|
case MOTOR_AB:
|
||||||
|
recenterMotor(MotorPort.MOTOR_A);
|
||||||
|
recenterMotor(MotorPort.MOTOR_B);
|
||||||
|
break;
|
||||||
|
case MOTOR_ABC:
|
||||||
|
recenterMotor(MotorPort.MOTOR_A);
|
||||||
|
recenterMotor(MotorPort.MOTOR_B);
|
||||||
|
recenterMotor(MotorPort.MOTOR_C);
|
||||||
|
break;
|
||||||
|
case MOTOR_AC:
|
||||||
|
recenterMotor(MotorPort.MOTOR_A);
|
||||||
|
recenterMotor(MotorPort.MOTOR_C);
|
||||||
|
break;
|
||||||
|
case MOTOR_B:
|
||||||
|
recenterMotor(MotorPort.MOTOR_B);
|
||||||
|
break;
|
||||||
|
case MOTOR_BC:
|
||||||
|
recenterMotor(MotorPort.MOTOR_B);
|
||||||
|
recenterMotor(MotorPort.MOTOR_C);
|
||||||
|
break;
|
||||||
|
case MOTOR_C:
|
||||||
|
recenterMotor(MotorPort.MOTOR_C);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case STOP:
|
||||||
|
switch(controlAction.motor){
|
||||||
|
case MOTOR_A:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_AB:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_ABC:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_AC:
|
||||||
|
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_B:
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_BC:
|
||||||
|
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
|
||||||
|
break;
|
||||||
|
case MOTOR_C:
|
||||||
|
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case USER_1:
|
||||||
|
case USER_2:
|
||||||
|
case USER_3:
|
||||||
|
notifyListeners(controlAction.action, controlAction.motor, controlAction.speed);
|
||||||
|
success = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Decodes a protocol message encoded as a byte array of two elements as specified
|
||||||
|
* in the package definition.</p>
|
||||||
|
*
|
||||||
|
* <p>User actions have precedence over motor recentering and
|
||||||
|
* this in turn has precedence over other movement actions.
|
||||||
|
* User actions have precedence in decreasing order; that is, user action 1 has
|
||||||
|
* precedence over user actions 2 and 3, etc.</p>
|
||||||
|
*
|
||||||
|
* <p>If the message indicates a movement (forward or backward) with all motors off,
|
||||||
|
* then it is interpreted as a request to stop all motors. A recenter or user action
|
||||||
|
* with all motors off will be decoded as is and must be interpreted by the user.</p>
|
||||||
|
*
|
||||||
|
* @param message A byte array of size two encoding a message recognized by the protocol. If the array
|
||||||
|
* has 3 or more elements then only the first 2 are used during the decoding process.
|
||||||
|
* @return A {@link DecodedControlAction} instance containing the decoded message.
|
||||||
|
* @throws IllegalArgumentException If the array is null or has less than 2 elements.
|
||||||
|
*/
|
||||||
|
public DecodedControlAction decodeMessage(byte[] message) throws IllegalArgumentException{
|
||||||
|
Action action = Action.STOP;
|
||||||
|
Motor motor = Motor.MOTOR_ABC;
|
||||||
|
DecodedControlAction controlAction;
|
||||||
|
|
||||||
|
if(message == null)
|
||||||
|
throw new IllegalArgumentException("Message is null.");
|
||||||
|
|
||||||
|
if(message.length < 2)
|
||||||
|
throw new IllegalArgumentException("Message is too short. Length of message is " + message.length);
|
||||||
|
|
||||||
|
// Decode the message.
|
||||||
|
boolean motorA = (message[0] & MOTOR_A) > 0 ? true : false;
|
||||||
|
boolean motorB = (message[0] & MOTOR_B) > 0 ? true : false;
|
||||||
|
boolean motorC = (message[0] & MOTOR_C) > 0 ? true : false;
|
||||||
|
boolean recenter = (message[0] & RECENTER) > 0 ? true : false;
|
||||||
|
boolean user1 = (message[0] & USER_1) > 0 ? true : false;
|
||||||
|
boolean user2 = (message[0] & USER_2) > 0 ? true : false;
|
||||||
|
boolean user3 = (message[0] & USER_3) > 0 ? true : false;
|
||||||
|
int direction = (message[0] & DIRECTION) > 0 ? BasicMotorPort.FORWARD : BasicMotorPort.BACKWARD;
|
||||||
|
|
||||||
|
// Set the action flag.
|
||||||
|
if(user1 || user2 || user3){
|
||||||
|
if (user1) action = Action.USER_1;
|
||||||
|
else if(user2) action = Action.USER_2;
|
||||||
|
else if(user3) action = Action.USER_3;
|
||||||
|
}else if(recenter){
|
||||||
|
action = Action.RECENTER;
|
||||||
|
}else{
|
||||||
|
if(direction == BasicMotorPort.FORWARD){
|
||||||
|
action = Action.MOVE_FORWARD;
|
||||||
|
}else if(direction == BasicMotorPort.BACKWARD){
|
||||||
|
action = Action.MOVE_BACKWARDS;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the motor flag.
|
||||||
|
if(motorA && motorB && motorC){
|
||||||
|
motor = Motor.MOTOR_ABC;
|
||||||
|
}else if(motorA && motorB && !motorC){
|
||||||
|
motor = Motor.MOTOR_AB;
|
||||||
|
}else if(motorA && !motorB && motorC){
|
||||||
|
motor = Motor.MOTOR_AC;
|
||||||
|
}else if(!motorA && motorB && motorC){
|
||||||
|
motor = Motor.MOTOR_BC;
|
||||||
|
}else if(motorA && !motorB && !motorC){
|
||||||
|
motor = Motor.MOTOR_A;
|
||||||
|
}else if(!motorA && motorB && !motorC){
|
||||||
|
motor = Motor.MOTOR_B;
|
||||||
|
}else if(!motorA && !motorB && motorC){
|
||||||
|
motor = Motor.MOTOR_C;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check for stop condition.
|
||||||
|
if(action == Action.MOVE_FORWARD || action == Action.MOVE_BACKWARDS){
|
||||||
|
if(!motorA && !motorB && !motorC){
|
||||||
|
action = Action.STOP;
|
||||||
|
motor = Motor.MOTOR_ABC;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
controlAction = new DecodedControlAction(action, motor, message[1]);
|
||||||
|
|
||||||
|
return controlAction;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Adds an {@link UserActionListener} to this object's listeners list calling it's
|
||||||
|
* {@link UserActionListener#onListenerRegistered()} method. Adding a listener that
|
||||||
|
* is already registered does nothing.</p>
|
||||||
|
*
|
||||||
|
* @param listener The listener to add.
|
||||||
|
* @throws IllegalArgumentException If listener is null.
|
||||||
|
*/
|
||||||
|
public synchronized void registerUserActionListener(UserActionListener listener) throws IllegalArgumentException{
|
||||||
|
if(listener == null)
|
||||||
|
throw new IllegalArgumentException("Listener is null.");
|
||||||
|
|
||||||
|
if(!userActionListeners.contains(listener)){
|
||||||
|
userActionListeners.add(listener);
|
||||||
|
listener.onListenerRegistered();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Removes an {@link UserActionListener} from this object's listeners list calling it's
|
||||||
|
* {@link UserActionListener#onListenerRemoved()} method. Removing a listener that
|
||||||
|
* is NOT on the list does nothing.</p>
|
||||||
|
*
|
||||||
|
* @param listener The listener to remove.
|
||||||
|
* @throws IllegalArgumentException If listener is null.
|
||||||
|
*/
|
||||||
|
public synchronized void removeUserActionListener(UserActionListener listener) throws IllegalArgumentException{
|
||||||
|
if(listener == null)
|
||||||
|
throw new IllegalArgumentException("Listener is null.");
|
||||||
|
|
||||||
|
if(userActionListeners.contains(listener)){
|
||||||
|
userActionListeners.remove(listener);
|
||||||
|
listener.onListenerRemoved();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Iterates over all the registered {@link UserActionListener} objects calling the methods
|
||||||
|
* respective to the {@link Action} received. Only user actions are processed, all other actions are
|
||||||
|
* ignored.</p>
|
||||||
|
*
|
||||||
|
* @param userAction The action that triggered the notification.
|
||||||
|
*/
|
||||||
|
private void notifyListeners(Action userAction, Motor motorFlag, int speed){
|
||||||
|
switch(userAction){
|
||||||
|
case USER_1:
|
||||||
|
for(UserActionListener listener : userActionListeners)
|
||||||
|
listener.onUserAction1(motorFlag, speed);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case USER_2:
|
||||||
|
for(UserActionListener listener : userActionListeners)
|
||||||
|
listener.onUserAction2(motorFlag, speed);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case USER_3:
|
||||||
|
for(UserActionListener listener : userActionListeners)
|
||||||
|
listener.onUserAction3(motorFlag, speed);
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Issues a movement command to the specified motor.</p>
|
||||||
|
*
|
||||||
|
* @param port The motor port to control.
|
||||||
|
* @param action The direction of movement.
|
||||||
|
* @param speed The speed of movement.
|
||||||
|
*/
|
||||||
|
private void controlMotor(MotorPort port, MotorAction action, int speed){
|
||||||
|
switch(port){
|
||||||
|
case MOTOR_A:
|
||||||
|
lejos.nxt.Motor.A.setSpeed(speed * Battery.getVoltage());
|
||||||
|
switch(action){
|
||||||
|
case BACKWARD:
|
||||||
|
lejos.nxt.Motor.A.backward();
|
||||||
|
break;
|
||||||
|
case FORWARD:
|
||||||
|
lejos.nxt.Motor.A.forward();
|
||||||
|
break;
|
||||||
|
case STOP:
|
||||||
|
lejos.nxt.Motor.A.stop(true);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_B:
|
||||||
|
lejos.nxt.Motor.B.setSpeed(speed * Battery.getVoltage());
|
||||||
|
switch(action){
|
||||||
|
case BACKWARD:
|
||||||
|
lejos.nxt.Motor.B.backward();
|
||||||
|
break;
|
||||||
|
case FORWARD:
|
||||||
|
lejos.nxt.Motor.B.forward();
|
||||||
|
break;
|
||||||
|
case STOP:
|
||||||
|
lejos.nxt.Motor.B.stop(true);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_C:
|
||||||
|
lejos.nxt.Motor.C.setSpeed(speed * Battery.getVoltage());
|
||||||
|
switch(action){
|
||||||
|
case BACKWARD:
|
||||||
|
lejos.nxt.Motor.C.backward();
|
||||||
|
break;
|
||||||
|
case FORWARD:
|
||||||
|
lejos.nxt.Motor.C.forward();
|
||||||
|
break;
|
||||||
|
case STOP:
|
||||||
|
lejos.nxt.Motor.C.stop(true);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Returns a motor to it's initial position. This method blocks until
|
||||||
|
* the motor is on position.</p>
|
||||||
|
*
|
||||||
|
* @param port The motor to recenter.
|
||||||
|
*/
|
||||||
|
private void recenterMotor(MotorPort port){
|
||||||
|
int rotation, tacho;
|
||||||
|
|
||||||
|
switch(port){
|
||||||
|
case MOTOR_A:
|
||||||
|
lejos.nxt.Motor.A.setSpeed(50 * Battery.getVoltage());
|
||||||
|
tacho = lejos.nxt.Motor.A.getTachoCount() % 360;
|
||||||
|
rotation = -(tacho);
|
||||||
|
lejos.nxt.Motor.A.rotate(rotation, false);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_B:
|
||||||
|
lejos.nxt.Motor.B.setSpeed(50 * Battery.getVoltage());
|
||||||
|
tacho = lejos.nxt.Motor.B.getTachoCount() % 360;
|
||||||
|
rotation = -(tacho);
|
||||||
|
lejos.nxt.Motor.B.rotate(rotation, false);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_C:
|
||||||
|
lejos.nxt.Motor.C.setSpeed(50 * Battery.getVoltage());
|
||||||
|
tacho = lejos.nxt.Motor.C.getTachoCount() % 360;
|
||||||
|
rotation = -(tacho);
|
||||||
|
lejos.nxt.Motor.C.rotate(rotation, false);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
@@ -0,0 +1,50 @@
|
|||||||
|
/* Copyright (c) 2014, Miguel Angel Astor Romero
|
||||||
|
* All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*/
|
||||||
|
package ve.ucv.ciens.icaro.libnxtarcontrol;
|
||||||
|
|
||||||
|
import ve.ucv.ciens.icaro.libnxtarcontrol.DecodedControlAction.Action;
|
||||||
|
import ve.ucv.ciens.icaro.libnxtarcontrol.DecodedControlAction.Motor;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>An object to be notified when an user action has been received in a protocol message.</p>
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero
|
||||||
|
* @version 1.0
|
||||||
|
* @since December 16, 2014
|
||||||
|
*/
|
||||||
|
public interface UserActionListener {
|
||||||
|
/**
|
||||||
|
* <p>Executes a set of instructions just after the listener has been registered with an {@link NxtARControlProtocol instance}.</p>
|
||||||
|
*/
|
||||||
|
public void onListenerRegistered();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Executes a set of instructions when a {@link DecodedControlAction} is decoded with {@link Action#USER_1}.</p>
|
||||||
|
*/
|
||||||
|
public void onUserAction1(Motor motorFlag, int speed);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Executes a set of instructions when a {@link DecodedControlAction} is decoded with {@link Action#USER_2}.</p>
|
||||||
|
*/
|
||||||
|
public void onUserAction2(Motor motorFlag, int speed);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Executes a set of instructions when a {@link DecodedControlAction} is decoded with {@link Action#USER_3}.</p>
|
||||||
|
*/
|
||||||
|
public void onUserAction3(Motor motorFlag, int speed);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>Executes a set of instructions just after the listener has been removed from an {@link NxtARControlProtocol instance}.</p>
|
||||||
|
*/
|
||||||
|
public void onListenerRemoved();
|
||||||
|
}
|
30
src/ve/ucv/ciens/icaro/libnxtarcontrol/package-info.java
Normal file
30
src/ve/ucv/ciens/icaro/libnxtarcontrol/package-info.java
Normal file
@@ -0,0 +1,30 @@
|
|||||||
|
/**
|
||||||
|
* <p>This package contains a wrapper library and reference implementation of the NxtAR
|
||||||
|
* robot control protocol for the LejOS operating system.</p>
|
||||||
|
*
|
||||||
|
* <p>The protocol is based on 2 byte long data messages that encode a set of motor ports, actions
|
||||||
|
* on those motors and the speed of the motors. The first byte encodes a set actions and motors, while the
|
||||||
|
* second byte contains an integer number representing the speed to set to the enabled motors. The first
|
||||||
|
* byte's bits encode the following data (from least significant to most significant bit): </p>
|
||||||
|
*
|
||||||
|
* <ul>
|
||||||
|
* <li>Motor port A enabled.</li>
|
||||||
|
* <li>Motor port B enabled.</li>
|
||||||
|
* <li>Motor port C enabled.</li>
|
||||||
|
* <li>Direction of movement. 0 for backward movement, 1 for forward movement.</li>
|
||||||
|
* <li>Request to return the enabled motors to their initial positions.</li>
|
||||||
|
* <li>Customizable user action 1.</li>
|
||||||
|
* <li>Customizable user action 2.</li>
|
||||||
|
* <li>Customizable user action 3.</li>
|
||||||
|
* </ul>
|
||||||
|
*
|
||||||
|
* <p>This bits are decoded using the binary masks defined in {@link ve.ucv.ciens.icaro.libnxtarcontrol.NxtARControlProtocol}.
|
||||||
|
* This mechanism allows to set the state or execute an action on multiple motors simultaneously. The user actions are
|
||||||
|
* executed using callback objects registered with the aforementioned class.</p>
|
||||||
|
*
|
||||||
|
* <p>The {@link ve.ucv.ciens.icaro.libnxtarcontrol.NxtARControlProtocol} class includes methods to
|
||||||
|
* read, decode and execute a protocol message, independently of how those messages are generated. An example
|
||||||
|
* would be messages read from a Bluetooth or USB stream, as well as messages generated by the application
|
||||||
|
* that is using the library.</p>
|
||||||
|
*/
|
||||||
|
package ve.ucv.ciens.icaro.libnxtarcontrol;
|
Reference in New Issue
Block a user