Fix for devices affected by the OpenCV 1244 bug.
This commit is contained in:
@@ -15,7 +15,7 @@
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with MINDdroid. If not, see <http://www.gnu.org/licenses/>.
|
||||
**/
|
||||
**/
|
||||
|
||||
package com.lego.minddroid;
|
||||
|
||||
@@ -40,439 +40,490 @@ import android.os.Message;
|
||||
* by the owners, i.e. calling the send/recive methods by themselves.
|
||||
*/
|
||||
public class BTCommunicator extends Thread {
|
||||
public static final int MOTOR_A = 0;
|
||||
public static final int MOTOR_B = 1;
|
||||
public static final int MOTOR_C = 2;
|
||||
public static final int MOTOR_B_ACTION = 40;
|
||||
public static final int MOTOR_RESET = 10;
|
||||
public static final int DO_BEEP = 51;
|
||||
public static final int DO_ACTION = 52;
|
||||
public static final int READ_MOTOR_STATE = 60;
|
||||
public static final int GET_FIRMWARE_VERSION = 70;
|
||||
public static final int DISCONNECT = 99;
|
||||
public static final int MOTOR_A = 0;
|
||||
public static final int MOTOR_B = 1;
|
||||
public static final int MOTOR_C = 2;
|
||||
public static final int MOTOR_B_ACTION = 40;
|
||||
public static final int MOTOR_RESET = 10;
|
||||
public static final int DO_BEEP = 51;
|
||||
public static final int DO_ACTION = 52;
|
||||
public static final int READ_MOTOR_STATE = 60;
|
||||
public static final int GET_FIRMWARE_VERSION = 70;
|
||||
public static final int DISCONNECT = 99;
|
||||
public static final int SET_LIGHT_SENSOR = 89;
|
||||
public static final int GET_LIGHT_SENSOR = 90;
|
||||
public static final int GET_TOUCH_SENSOR = 92;
|
||||
|
||||
public static final int DISPLAY_TOAST = 1000;
|
||||
public static final int STATE_CONNECTED = 1001;
|
||||
public static final int STATE_CONNECTERROR = 1002;
|
||||
public static final int STATE_CONNECTERROR_PAIRING = 1022;
|
||||
public static final int MOTOR_STATE = 1003;
|
||||
public static final int STATE_RECEIVEERROR = 1004;
|
||||
public static final int STATE_SENDERROR = 1005;
|
||||
public static final int FIRMWARE_VERSION = 1006;
|
||||
public static final int FIND_FILES = 1007;
|
||||
public static final int START_PROGRAM = 1008;
|
||||
public static final int STOP_PROGRAM = 1009;
|
||||
public static final int GET_PROGRAM_NAME = 1010;
|
||||
public static final int PROGRAM_NAME = 1011;
|
||||
public static final int SAY_TEXT = 1030;
|
||||
public static final int VIBRATE_PHONE = 1031;
|
||||
public static final int DISPLAY_TOAST = 1000;
|
||||
public static final int STATE_CONNECTED = 1001;
|
||||
public static final int STATE_CONNECTERROR = 1002;
|
||||
public static final int STATE_CONNECTERROR_PAIRING = 1022;
|
||||
public static final int MOTOR_STATE = 1003;
|
||||
public static final int STATE_RECEIVEERROR = 1004;
|
||||
public static final int STATE_SENDERROR = 1005;
|
||||
public static final int FIRMWARE_VERSION = 1006;
|
||||
public static final int FIND_FILES = 1007;
|
||||
public static final int START_PROGRAM = 1008;
|
||||
public static final int STOP_PROGRAM = 1009;
|
||||
public static final int GET_PROGRAM_NAME = 1010;
|
||||
public static final int PROGRAM_NAME = 1011;
|
||||
public static final int SAY_TEXT = 1030;
|
||||
public static final int VIBRATE_PHONE = 1031;
|
||||
public static final int UPDATE_SENSOR = 1042;
|
||||
|
||||
public static final int NO_DELAY = 0;
|
||||
public static final int NO_DELAY = 0;
|
||||
|
||||
private static final UUID SERIAL_PORT_SERVICE_CLASS_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
|
||||
// this is the only OUI registered by LEGO, see http://standards.ieee.org/regauth/oui/index.shtml
|
||||
public static final String OUI_LEGO = "00:16:53";
|
||||
private static final UUID SERIAL_PORT_SERVICE_CLASS_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
|
||||
// this is the only OUI registered by LEGO, see http://standards.ieee.org/regauth/oui/index.shtml
|
||||
public static final String OUI_LEGO = "00:16:53";
|
||||
|
||||
private Resources mResources;
|
||||
private BluetoothAdapter btAdapter;
|
||||
private BluetoothSocket nxtBTsocket = null;
|
||||
private OutputStream nxtOutputStream = null;
|
||||
private InputStream nxtInputStream = null;
|
||||
private boolean connected = false;
|
||||
private Resources mResources;
|
||||
private BluetoothAdapter btAdapter;
|
||||
private BluetoothSocket nxtBTsocket = null;
|
||||
private OutputStream nxtOutputStream = null;
|
||||
private InputStream nxtInputStream = null;
|
||||
private boolean connected = false;
|
||||
|
||||
private Handler uiHandler;
|
||||
private String mMACaddress;
|
||||
private BTConnectable myOwner;
|
||||
private Handler uiHandler;
|
||||
private String mMACaddress;
|
||||
private BTConnectable myOwner;
|
||||
|
||||
private byte[] returnMessage;
|
||||
private byte[] returnMessage;
|
||||
|
||||
public BTCommunicator(BTConnectable myOwner, Handler uiHandler, BluetoothAdapter btAdapter, Resources resources) {
|
||||
this.myOwner = myOwner;
|
||||
this.uiHandler = uiHandler;
|
||||
this.btAdapter = btAdapter;
|
||||
this.mResources = resources;
|
||||
}
|
||||
public BTCommunicator(BTConnectable myOwner, Handler uiHandler, BluetoothAdapter btAdapter, Resources resources) {
|
||||
this.myOwner = myOwner;
|
||||
this.uiHandler = uiHandler;
|
||||
this.btAdapter = btAdapter;
|
||||
this.mResources = resources;
|
||||
}
|
||||
|
||||
public Handler getHandler() {
|
||||
return myHandler;
|
||||
}
|
||||
public Handler getHandler() {
|
||||
return myHandler;
|
||||
}
|
||||
|
||||
public byte[] getReturnMessage() {
|
||||
return returnMessage;
|
||||
}
|
||||
public byte[] getReturnMessage() {
|
||||
return returnMessage;
|
||||
}
|
||||
|
||||
public void setMACAddress(String mMACaddress) {
|
||||
this.mMACaddress = mMACaddress;
|
||||
}
|
||||
public void setMACAddress(String mMACaddress) {
|
||||
this.mMACaddress = mMACaddress;
|
||||
}
|
||||
|
||||
/**
|
||||
* @return The current status of the connection
|
||||
*/
|
||||
public boolean isConnected() {
|
||||
return connected;
|
||||
}
|
||||
/**
|
||||
* @return The current status of the connection
|
||||
*/
|
||||
public boolean isConnected() {
|
||||
return connected;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates the connection, waits for incoming messages and dispatches them. The thread will be terminated
|
||||
* on closing of the connection.
|
||||
*/
|
||||
@Override
|
||||
public void run() {
|
||||
/**
|
||||
* Creates the connection, waits for incoming messages and dispatches them. The thread will be terminated
|
||||
* on closing of the connection.
|
||||
*/
|
||||
@Override
|
||||
public void run() {
|
||||
|
||||
try {
|
||||
createNXTconnection();
|
||||
}
|
||||
catch (IOException e) { }
|
||||
try {
|
||||
createNXTconnection();
|
||||
|
||||
/* NUEVO */
|
||||
setInputMode(0, LCPMessage.LIGHT_ACTIVE, LCPMessage.RAWMODE);
|
||||
setInputMode(1, LCPMessage.LIGHT_ACTIVE, LCPMessage.RAWMODE);
|
||||
/* NUEVO */
|
||||
}
|
||||
catch (IOException e) { }
|
||||
|
||||
while (connected) {
|
||||
try {
|
||||
returnMessage = receiveMessage();
|
||||
if ((returnMessage.length >= 2) && ((returnMessage[0] == LCPMessage.REPLY_COMMAND) ||
|
||||
(returnMessage[0] == LCPMessage.DIRECT_COMMAND_NOREPLY)))
|
||||
dispatchMessage(returnMessage);
|
||||
while (connected) {
|
||||
try {
|
||||
returnMessage = receiveMessage();
|
||||
if ((returnMessage.length >= 2) && ((returnMessage[0] == LCPMessage.REPLY_COMMAND) ||
|
||||
(returnMessage[0] == LCPMessage.DIRECT_COMMAND_NOREPLY)))
|
||||
dispatchMessage(returnMessage);
|
||||
|
||||
} catch (IOException e) {
|
||||
// don't inform the user when connection is already closed
|
||||
if (connected)
|
||||
sendState(STATE_RECEIVEERROR);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
} catch (IOException e) {
|
||||
// don't inform the user when connection is already closed
|
||||
if (connected)
|
||||
sendState(STATE_RECEIVEERROR);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a bluetooth connection with SerialPortServiceClass_UUID
|
||||
* @see <a href=
|
||||
* "http://lejos.sourceforge.net/forum/viewtopic.php?t=1991&highlight=android"
|
||||
* />
|
||||
* On error the method either sends a message to it's owner or creates an exception in the
|
||||
* case of no message handler.
|
||||
*/
|
||||
public void createNXTconnection() throws IOException {
|
||||
try {
|
||||
BluetoothSocket nxtBTSocketTemporary;
|
||||
BluetoothDevice nxtDevice = null;
|
||||
nxtDevice = btAdapter.getRemoteDevice(mMACaddress);
|
||||
if (nxtDevice == null) {
|
||||
if (uiHandler == null)
|
||||
throw new IOException();
|
||||
else {
|
||||
sendToast(mResources.getString(R.string.no_paired_nxt));
|
||||
sendState(STATE_CONNECTERROR);
|
||||
return;
|
||||
}
|
||||
}
|
||||
nxtBTSocketTemporary = nxtDevice.createRfcommSocketToServiceRecord(SERIAL_PORT_SERVICE_CLASS_UUID);
|
||||
try {
|
||||
nxtBTSocketTemporary.connect();
|
||||
}
|
||||
catch (IOException e) {
|
||||
if (myOwner.isPairing()) {
|
||||
if (uiHandler != null) {
|
||||
sendToast(mResources.getString(R.string.pairing_message));
|
||||
sendState(STATE_CONNECTERROR_PAIRING);
|
||||
}
|
||||
else
|
||||
throw e;
|
||||
return;
|
||||
}
|
||||
/**
|
||||
* Create a bluetooth connection with SerialPortServiceClass_UUID
|
||||
* @see <a href=
|
||||
* "http://lejos.sourceforge.net/forum/viewtopic.php?t=1991&highlight=android"
|
||||
* />
|
||||
* On error the method either sends a message to it's owner or creates an exception in the
|
||||
* case of no message handler.
|
||||
*/
|
||||
public void createNXTconnection() throws IOException {
|
||||
try {
|
||||
BluetoothSocket nxtBTSocketTemporary;
|
||||
BluetoothDevice nxtDevice = null;
|
||||
nxtDevice = btAdapter.getRemoteDevice(mMACaddress);
|
||||
if (nxtDevice == null) {
|
||||
if (uiHandler == null)
|
||||
throw new IOException();
|
||||
else {
|
||||
sendToast(mResources.getString(R.string.no_paired_nxt));
|
||||
sendState(STATE_CONNECTERROR);
|
||||
return;
|
||||
}
|
||||
}
|
||||
nxtBTSocketTemporary = nxtDevice.createRfcommSocketToServiceRecord(SERIAL_PORT_SERVICE_CLASS_UUID);
|
||||
try {
|
||||
nxtBTSocketTemporary.connect();
|
||||
}
|
||||
catch (IOException e) {
|
||||
if (myOwner.isPairing()) {
|
||||
if (uiHandler != null) {
|
||||
sendToast(mResources.getString(R.string.pairing_message));
|
||||
sendState(STATE_CONNECTERROR_PAIRING);
|
||||
}
|
||||
else
|
||||
throw e;
|
||||
return;
|
||||
}
|
||||
|
||||
// try another method for connection, this should work on the HTC desire, credits to Michael Biermann
|
||||
try {
|
||||
Method mMethod = nxtDevice.getClass().getMethod("createRfcommSocket", new Class[] { int.class });
|
||||
nxtBTSocketTemporary = (BluetoothSocket) mMethod.invoke(nxtDevice, Integer.valueOf(1));
|
||||
nxtBTSocketTemporary.connect();
|
||||
}
|
||||
catch (Exception e1){
|
||||
if (uiHandler == null)
|
||||
throw new IOException();
|
||||
else
|
||||
sendState(STATE_CONNECTERROR);
|
||||
return;
|
||||
}
|
||||
}
|
||||
nxtBTsocket = nxtBTSocketTemporary;
|
||||
nxtInputStream = nxtBTsocket.getInputStream();
|
||||
nxtOutputStream = nxtBTsocket.getOutputStream();
|
||||
connected = true;
|
||||
} catch (IOException e) {
|
||||
if (uiHandler == null)
|
||||
throw e;
|
||||
else {
|
||||
if (myOwner.isPairing())
|
||||
sendToast(mResources.getString(R.string.pairing_message));
|
||||
sendState(STATE_CONNECTERROR);
|
||||
return;
|
||||
}
|
||||
}
|
||||
// everything was OK
|
||||
if (uiHandler != null)
|
||||
sendState(STATE_CONNECTED);
|
||||
}
|
||||
// try another method for connection, this should work on the HTC desire, credits to Michael Biermann
|
||||
try {
|
||||
Method mMethod = nxtDevice.getClass().getMethod("createRfcommSocket", new Class[] { int.class });
|
||||
nxtBTSocketTemporary = (BluetoothSocket) mMethod.invoke(nxtDevice, Integer.valueOf(1));
|
||||
nxtBTSocketTemporary.connect();
|
||||
}
|
||||
catch (Exception e1){
|
||||
if (uiHandler == null)
|
||||
throw new IOException();
|
||||
else
|
||||
sendState(STATE_CONNECTERROR);
|
||||
return;
|
||||
}
|
||||
}
|
||||
nxtBTsocket = nxtBTSocketTemporary;
|
||||
nxtInputStream = nxtBTsocket.getInputStream();
|
||||
nxtOutputStream = nxtBTsocket.getOutputStream();
|
||||
connected = true;
|
||||
} catch (IOException e) {
|
||||
if (uiHandler == null)
|
||||
throw e;
|
||||
else {
|
||||
if (myOwner.isPairing())
|
||||
sendToast(mResources.getString(R.string.pairing_message));
|
||||
sendState(STATE_CONNECTERROR);
|
||||
return;
|
||||
}
|
||||
}
|
||||
// everything was OK
|
||||
if (uiHandler != null)
|
||||
sendState(STATE_CONNECTED);
|
||||
}
|
||||
|
||||
/**
|
||||
* Closes the bluetooth connection. On error the method either sends a message
|
||||
* to it's owner or creates an exception in the case of no message handler.
|
||||
*/
|
||||
public void destroyNXTconnection() throws IOException {
|
||||
try {
|
||||
if (nxtBTsocket != null) {
|
||||
connected = false;
|
||||
nxtBTsocket.close();
|
||||
nxtBTsocket = null;
|
||||
}
|
||||
/**
|
||||
* Closes the bluetooth connection. On error the method either sends a message
|
||||
* to it's owner or creates an exception in the case of no message handler.
|
||||
*/
|
||||
public void destroyNXTconnection() throws IOException {
|
||||
try {
|
||||
if (nxtBTsocket != null) {
|
||||
connected = false;
|
||||
nxtBTsocket.close();
|
||||
nxtBTsocket = null;
|
||||
}
|
||||
|
||||
nxtInputStream = null;
|
||||
nxtOutputStream = null;
|
||||
nxtInputStream = null;
|
||||
nxtOutputStream = null;
|
||||
|
||||
} catch (IOException e) {
|
||||
if (uiHandler == null)
|
||||
throw e;
|
||||
else
|
||||
sendToast(mResources.getString(R.string.problem_at_closing));
|
||||
}
|
||||
}
|
||||
} catch (IOException e) {
|
||||
if (uiHandler == null)
|
||||
throw e;
|
||||
else
|
||||
sendToast(mResources.getString(R.string.problem_at_closing));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends a message on the opened OutputStream
|
||||
* @param message, the message as a byte array
|
||||
*/
|
||||
public void sendMessage(byte[] message) throws IOException {
|
||||
if (nxtOutputStream == null)
|
||||
throw new IOException();
|
||||
/**
|
||||
* Sends a message on the opened OutputStream
|
||||
* @param message, the message as a byte array
|
||||
*/
|
||||
public void sendMessage(byte[] message) throws IOException {
|
||||
if (nxtOutputStream == null)
|
||||
throw new IOException();
|
||||
|
||||
// send message length
|
||||
int messageLength = message.length;
|
||||
nxtOutputStream.write(messageLength);
|
||||
nxtOutputStream.write(messageLength >> 8);
|
||||
nxtOutputStream.write(message, 0, message.length);
|
||||
}
|
||||
// send message length
|
||||
int messageLength = message.length;
|
||||
nxtOutputStream.write(messageLength);
|
||||
nxtOutputStream.write(messageLength >> 8);
|
||||
nxtOutputStream.write(message, 0, message.length);
|
||||
}
|
||||
|
||||
/**
|
||||
* Receives a message on the opened InputStream
|
||||
* @return the message
|
||||
*/
|
||||
public byte[] receiveMessage() throws IOException {
|
||||
if (nxtInputStream == null)
|
||||
throw new IOException();
|
||||
/**
|
||||
* Receives a message on the opened InputStream
|
||||
* @return the message
|
||||
*/
|
||||
public byte[] receiveMessage() throws IOException {
|
||||
if (nxtInputStream == null)
|
||||
throw new IOException();
|
||||
|
||||
int length = nxtInputStream.read();
|
||||
length = (nxtInputStream.read() << 8) + length;
|
||||
byte[] returnMessage = new byte[length];
|
||||
nxtInputStream.read(returnMessage);
|
||||
return returnMessage;
|
||||
}
|
||||
int length = nxtInputStream.read();
|
||||
length = (nxtInputStream.read() << 8) + length;
|
||||
byte[] returnMessage = new byte[length];
|
||||
nxtInputStream.read(returnMessage);
|
||||
return returnMessage;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends a message on the opened OutputStream. In case of
|
||||
* an error the state is sent to the handler.
|
||||
* @param message, the message as a byte array
|
||||
*/
|
||||
private void sendMessageAndState(byte[] message) {
|
||||
if (nxtOutputStream == null)
|
||||
return;
|
||||
/**
|
||||
* Sends a message on the opened OutputStream. In case of
|
||||
* an error the state is sent to the handler.
|
||||
* @param message, the message as a byte array
|
||||
*/
|
||||
private void sendMessageAndState(byte[] message) {
|
||||
if (nxtOutputStream == null)
|
||||
return;
|
||||
|
||||
try {
|
||||
sendMessage(message);
|
||||
}
|
||||
catch (IOException e) {
|
||||
sendState(STATE_SENDERROR);
|
||||
}
|
||||
}
|
||||
try {
|
||||
sendMessage(message);
|
||||
}
|
||||
catch (IOException e) {
|
||||
sendState(STATE_SENDERROR);
|
||||
}
|
||||
}
|
||||
|
||||
private void dispatchMessage(byte[] message) {
|
||||
switch (message[1]) {
|
||||
private void dispatchMessage(byte[] message) {
|
||||
switch (message[1]) {
|
||||
|
||||
case LCPMessage.GET_OUTPUT_STATE:
|
||||
case LCPMessage.GET_OUTPUT_STATE:
|
||||
|
||||
if (message.length >= 25)
|
||||
sendState(MOTOR_STATE);
|
||||
if (message.length >= 25)
|
||||
sendState(MOTOR_STATE);
|
||||
|
||||
break;
|
||||
break;
|
||||
|
||||
case LCPMessage.GET_FIRMWARE_VERSION:
|
||||
case LCPMessage.GET_FIRMWARE_VERSION:
|
||||
|
||||
if (message.length >= 7)
|
||||
sendState(FIRMWARE_VERSION);
|
||||
if (message.length >= 7)
|
||||
sendState(FIRMWARE_VERSION);
|
||||
|
||||
break;
|
||||
break;
|
||||
|
||||
case LCPMessage.FIND_FIRST:
|
||||
case LCPMessage.FIND_NEXT:
|
||||
case LCPMessage.FIND_FIRST:
|
||||
case LCPMessage.FIND_NEXT:
|
||||
|
||||
if (message.length >= 28) {
|
||||
// Success
|
||||
if (message[2] == 0)
|
||||
sendState(FIND_FILES);
|
||||
}
|
||||
/*if (message.length >= 28) {
|
||||
// Success
|
||||
if (message[2] == 0)
|
||||
sendState(FIND_FILES);
|
||||
}*/
|
||||
|
||||
break;
|
||||
|
||||
case LCPMessage.GET_CURRENT_PROGRAM_NAME:
|
||||
break;
|
||||
|
||||
if (message.length >= 23) {
|
||||
sendState(PROGRAM_NAME);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case LCPMessage.SAY_TEXT:
|
||||
|
||||
if (message.length == 22) {
|
||||
sendState(SAY_TEXT);
|
||||
}
|
||||
|
||||
case LCPMessage.VIBRATE_PHONE:
|
||||
if (message.length == 3) {
|
||||
sendState(VIBRATE_PHONE);
|
||||
}
|
||||
}
|
||||
}
|
||||
case LCPMessage.GET_CURRENT_PROGRAM_NAME:
|
||||
|
||||
private void doBeep(int frequency, int duration) {
|
||||
byte[] message = LCPMessage.getBeepMessage(frequency, duration);
|
||||
sendMessageAndState(message);
|
||||
waitSomeTime(20);
|
||||
}
|
||||
|
||||
private void doAction(int actionNr) {
|
||||
byte[] message = LCPMessage.getActionMessage(actionNr);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
if (message.length >= 23) {
|
||||
sendState(PROGRAM_NAME);
|
||||
}
|
||||
|
||||
private void startProgram(String programName) {
|
||||
byte[] message = LCPMessage.getStartProgramMessage(programName);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
break;
|
||||
|
||||
private void stopProgram() {
|
||||
byte[] message = LCPMessage.getStopProgramMessage();
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
private void getProgramName() {
|
||||
byte[] message = LCPMessage.getProgramNameMessage();
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
private void changeMotorSpeed(int motor, int speed) {
|
||||
if (speed > 100)
|
||||
speed = 100;
|
||||
case LCPMessage.SAY_TEXT:
|
||||
|
||||
else if (speed < -100)
|
||||
speed = -100;
|
||||
if (message.length == 22) {
|
||||
sendState(SAY_TEXT);
|
||||
}
|
||||
|
||||
byte[] message = LCPMessage.getMotorMessage(motor, speed);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
break;
|
||||
|
||||
case LCPMessage.VIBRATE_PHONE:
|
||||
if (message.length == 3) {
|
||||
sendState(VIBRATE_PHONE);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
/* NUEVO */
|
||||
case LCPMessage.GET_INPUT_VALUES:
|
||||
if(message[3] == LCPMessage.PORT_0 || message[3] == LCPMessage.PORT_1){
|
||||
sendState(UPDATE_SENSOR);
|
||||
}
|
||||
|
||||
break;
|
||||
/* NUEVO */
|
||||
}
|
||||
}
|
||||
|
||||
private void rotateTo(int motor, int end) {
|
||||
byte[] message = LCPMessage.getMotorMessage(motor, -80, end);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
private void doBeep(int frequency, int duration) {
|
||||
byte[] message = LCPMessage.getBeepMessage(frequency, duration);
|
||||
sendMessageAndState(message);
|
||||
waitSomeTime(20);
|
||||
}
|
||||
|
||||
private void reset(int motor) {
|
||||
byte[] message = LCPMessage.getResetMessage(motor);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
private void doAction(int actionNr) {
|
||||
byte[] message = LCPMessage.getActionMessage(actionNr);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
private void readMotorState(int motor) {
|
||||
byte[] message = LCPMessage.getOutputStateMessage(motor);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
private void startProgram(String programName) {
|
||||
byte[] message = LCPMessage.getStartProgramMessage(programName);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
private void getFirmwareVersion() {
|
||||
byte[] message = LCPMessage.getFirmwareVersionMessage();
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
private void stopProgram() {
|
||||
byte[] message = LCPMessage.getStopProgramMessage();
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
private void findFiles(boolean findFirst, int handle) {
|
||||
byte[] message = LCPMessage.getFindFilesMessage(findFirst, handle, "*.*");
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
private void getProgramName() {
|
||||
byte[] message = LCPMessage.getProgramNameMessage();
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
private void waitSomeTime(int millis) {
|
||||
try {
|
||||
Thread.sleep(millis);
|
||||
private void changeMotorSpeed(int motor, int speed) {
|
||||
if (speed > 100)
|
||||
speed = 100;
|
||||
|
||||
} catch (InterruptedException e) {
|
||||
}
|
||||
}
|
||||
else if (speed < -100)
|
||||
speed = -100;
|
||||
|
||||
private void sendToast(String toastText) {
|
||||
Bundle myBundle = new Bundle();
|
||||
myBundle.putInt("message", DISPLAY_TOAST);
|
||||
myBundle.putString("toastText", toastText);
|
||||
sendBundle(myBundle);
|
||||
}
|
||||
byte[] message = LCPMessage.getMotorMessage(motor, speed);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
private void sendState(int message) {
|
||||
Bundle myBundle = new Bundle();
|
||||
myBundle.putInt("message", message);
|
||||
sendBundle(myBundle);
|
||||
}
|
||||
private void rotateTo(int motor, int end) {
|
||||
byte[] message = LCPMessage.getMotorMessage(motor, -80, end);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
private void sendBundle(Bundle myBundle) {
|
||||
Message myMessage = myHandler.obtainMessage();
|
||||
myMessage.setData(myBundle);
|
||||
uiHandler.sendMessage(myMessage);
|
||||
}
|
||||
private void reset(int motor) {
|
||||
byte[] message = LCPMessage.getResetMessage(motor);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
// receive messages from the UI
|
||||
final Handler myHandler = new Handler() {
|
||||
@Override
|
||||
public void handleMessage(Message myMessage) {
|
||||
private void readMotorState(int motor) {
|
||||
byte[] message = LCPMessage.getOutputStateMessage(motor);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
int message;
|
||||
private void getFirmwareVersion() {
|
||||
byte[] message = LCPMessage.getFirmwareVersionMessage();
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
switch (message = myMessage.getData().getInt("message")) {
|
||||
case MOTOR_A:
|
||||
case MOTOR_B:
|
||||
case MOTOR_C:
|
||||
changeMotorSpeed(message, myMessage.getData().getInt("value1"));
|
||||
break;
|
||||
case MOTOR_B_ACTION:
|
||||
rotateTo(MOTOR_B, myMessage.getData().getInt("value1"));
|
||||
break;
|
||||
case MOTOR_RESET:
|
||||
reset(myMessage.getData().getInt("value1"));
|
||||
break;
|
||||
case START_PROGRAM:
|
||||
startProgram(myMessage.getData().getString("name"));
|
||||
break;
|
||||
case STOP_PROGRAM:
|
||||
stopProgram();
|
||||
break;
|
||||
case GET_PROGRAM_NAME:
|
||||
getProgramName();
|
||||
break;
|
||||
case DO_BEEP:
|
||||
doBeep(myMessage.getData().getInt("value1"), myMessage.getData().getInt("value2"));
|
||||
break;
|
||||
case DO_ACTION:
|
||||
doAction(0);
|
||||
break;
|
||||
case READ_MOTOR_STATE:
|
||||
readMotorState(myMessage.getData().getInt("value1"));
|
||||
break;
|
||||
case GET_FIRMWARE_VERSION:
|
||||
getFirmwareVersion();
|
||||
break;
|
||||
case FIND_FILES:
|
||||
findFiles(myMessage.getData().getInt("value1") == 0, myMessage.getData().getInt("value2"));
|
||||
break;
|
||||
case DISCONNECT:
|
||||
// send stop messages before closing
|
||||
changeMotorSpeed(MOTOR_A, 0);
|
||||
changeMotorSpeed(MOTOR_B, 0);
|
||||
changeMotorSpeed(MOTOR_C, 0);
|
||||
waitSomeTime(500);
|
||||
try {
|
||||
destroyNXTconnection();
|
||||
}
|
||||
catch (IOException e) { }
|
||||
break;
|
||||
}
|
||||
}
|
||||
};
|
||||
private void findFiles(boolean findFirst, int handle) {
|
||||
byte[] message = LCPMessage.getFindFilesMessage(findFirst, handle, "*.*");
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
/* NUEVO */
|
||||
private void setInputMode(int port, byte sensor, byte mode){
|
||||
byte [] message = LCPMessage.getSetInputModeMessage(port, sensor, mode);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
|
||||
private void getInputValues(int port){
|
||||
byte [] message = LCPMessage.getGetInputValuesMessage(port);
|
||||
sendMessageAndState(message);
|
||||
}
|
||||
/* NUEVO */
|
||||
|
||||
private void waitSomeTime(int millis) {
|
||||
try {
|
||||
Thread.sleep(millis);
|
||||
|
||||
} catch (InterruptedException e) {
|
||||
}
|
||||
}
|
||||
|
||||
private void sendToast(String toastText) {
|
||||
Bundle myBundle = new Bundle();
|
||||
myBundle.putInt("message", DISPLAY_TOAST);
|
||||
myBundle.putString("toastText", toastText);
|
||||
sendBundle(myBundle);
|
||||
}
|
||||
|
||||
private void sendState(int message) {
|
||||
Bundle myBundle = new Bundle();
|
||||
myBundle.putInt("message", message);
|
||||
sendBundle(myBundle);
|
||||
}
|
||||
|
||||
private void sendBundle(Bundle myBundle) {
|
||||
Message myMessage = myHandler.obtainMessage();
|
||||
myMessage.setData(myBundle);
|
||||
uiHandler.sendMessage(myMessage);
|
||||
}
|
||||
|
||||
// receive messages from the UI
|
||||
final Handler myHandler = new Handler() {
|
||||
@Override
|
||||
public void handleMessage(Message myMessage) {
|
||||
|
||||
int message;
|
||||
|
||||
switch (message = myMessage.getData().getInt("message")) {
|
||||
case MOTOR_A:
|
||||
case MOTOR_B:
|
||||
case MOTOR_C:
|
||||
changeMotorSpeed(message, myMessage.getData().getInt("value1"));
|
||||
break;
|
||||
case MOTOR_B_ACTION:
|
||||
rotateTo(MOTOR_B, myMessage.getData().getInt("value1"));
|
||||
break;
|
||||
case MOTOR_RESET:
|
||||
reset(myMessage.getData().getInt("value1"));
|
||||
break;
|
||||
case START_PROGRAM:
|
||||
startProgram(myMessage.getData().getString("name"));
|
||||
break;
|
||||
case STOP_PROGRAM:
|
||||
stopProgram();
|
||||
break;
|
||||
case GET_PROGRAM_NAME:
|
||||
getProgramName();
|
||||
break;
|
||||
case DO_BEEP:
|
||||
doBeep(myMessage.getData().getInt("value1"), myMessage.getData().getInt("value2"));
|
||||
break;
|
||||
case DO_ACTION:
|
||||
doAction(0);
|
||||
break;
|
||||
case READ_MOTOR_STATE:
|
||||
readMotorState(myMessage.getData().getInt("value1"));
|
||||
break;
|
||||
case GET_FIRMWARE_VERSION:
|
||||
getFirmwareVersion();
|
||||
break;
|
||||
case FIND_FILES:
|
||||
findFiles(myMessage.getData().getInt("value1") == 0, myMessage.getData().getInt("value2"));
|
||||
break;
|
||||
case DISCONNECT:
|
||||
// send stop messages before closing
|
||||
changeMotorSpeed(MOTOR_A, 0);
|
||||
changeMotorSpeed(MOTOR_B, 0);
|
||||
changeMotorSpeed(MOTOR_C, 0);
|
||||
waitSomeTime(500);
|
||||
try {
|
||||
/* NUEVO */
|
||||
setInputMode(0, LCPMessage.NO_SENSOR, LCPMessage.RAWMODE);
|
||||
setInputMode(1, LCPMessage.NO_SENSOR, LCPMessage.RAWMODE);
|
||||
/* NUEVO */
|
||||
destroyNXTconnection();
|
||||
}
|
||||
catch (IOException e) { }
|
||||
break;
|
||||
/* NUEVO */
|
||||
case SET_LIGHT_SENSOR:
|
||||
setInputMode(0, LCPMessage.LIGHT_ACTIVE, LCPMessage.RAWMODE);
|
||||
break;
|
||||
case GET_LIGHT_SENSOR:
|
||||
getInputValues(0);
|
||||
waitSomeTime(30);
|
||||
break;
|
||||
case GET_TOUCH_SENSOR:
|
||||
getInputValues(1);
|
||||
waitSomeTime(30);
|
||||
break;
|
||||
/* NUEVO */
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
@@ -0,0 +1,222 @@
|
||||
package com.lego.minddroid;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
import java.nio.ByteOrder;
|
||||
import java.nio.FloatBuffer;
|
||||
import java.nio.IntBuffer;
|
||||
|
||||
import javax.microedition.khronos.egl.EGLConfig;
|
||||
import javax.microedition.khronos.opengles.GL10;
|
||||
|
||||
import android.hardware.Camera;
|
||||
import android.hardware.Camera.PreviewCallback;
|
||||
import android.opengl.GLSurfaceView.Renderer;
|
||||
import android.os.Handler;
|
||||
|
||||
public class CameraGLRenderer implements Renderer, PreviewCallback{
|
||||
private static final String TAG = "CAMERA_RENDERER";
|
||||
Quad t;
|
||||
int tex;
|
||||
int[] glCameraFrame = null;
|
||||
int[] glTexture = null;
|
||||
byte[] frame = null;
|
||||
int[] cameraTexture;
|
||||
public int w,h;
|
||||
protected MINDdroidCV mActivity;
|
||||
protected double[]buffer;
|
||||
protected int left, right;
|
||||
private Handler handler = new Handler();
|
||||
|
||||
static {
|
||||
System.loadLibrary("mixed_sample");
|
||||
}
|
||||
|
||||
public CameraGLRenderer(){
|
||||
handler.removeCallbacks(FrameProcessor);
|
||||
handler.postDelayed(FrameProcessor, 100);
|
||||
|
||||
w = 0; h = 0;
|
||||
t = new Quad();
|
||||
|
||||
frame = new byte[1];
|
||||
buffer = new double[10];
|
||||
glCameraFrame = new int[240*160];
|
||||
glTexture = new int[256*256];
|
||||
for(int i = 0; i < 256; ++i){
|
||||
for(int j = 0; j < 256; ++j){
|
||||
glTexture[i*256 + j] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public native void FindLight(int width, int height, byte yuv[], int[] rgba,double[] array);
|
||||
|
||||
public void onDrawFrame( GL10 gl ) {
|
||||
gl.glClearColor( 0.0f, 0.0f, 0.0f, 1.0f );
|
||||
gl.glClear( GL10.GL_COLOR_BUFFER_BIT );
|
||||
|
||||
gl.glMatrixMode(GL10.GL_MODELVIEW);
|
||||
gl.glLoadIdentity();
|
||||
|
||||
bindCameraTexture(gl);
|
||||
|
||||
gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
|
||||
gl.glEnableClientState(GL10.GL_TEXTURE_COORD_ARRAY);
|
||||
|
||||
gl.glVertexPointer(3, GL10.GL_FLOAT, 0, t.vertexBuffer);
|
||||
gl.glTexCoordPointer(2, GL10.GL_FLOAT, 0, t.texBuffer);
|
||||
gl.glDrawArrays(GL10.GL_TRIANGLE_STRIP, 0, 4);
|
||||
|
||||
gl.glDisableClientState(GL10.GL_TEXTURE_COORD_ARRAY);
|
||||
gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
|
||||
|
||||
gl.glFinish();
|
||||
}
|
||||
|
||||
public void setMActivity (MINDdroidCV activity){
|
||||
mActivity = activity;
|
||||
}
|
||||
|
||||
public void onSurfaceChanged( GL10 gl, int width, int height ) {
|
||||
gl.glViewport( 0, 0, width, height );
|
||||
}
|
||||
|
||||
public void onSurfaceCreated( GL10 gl, EGLConfig config ) {
|
||||
gl.glEnable(GL10.GL_TEXTURE_2D);
|
||||
}
|
||||
|
||||
void bindCameraTexture(GL10 gl) {
|
||||
if (cameraTexture==null)
|
||||
cameraTexture=new int[1];
|
||||
else
|
||||
gl.glDeleteTextures(1, cameraTexture, 0);
|
||||
|
||||
gl.glGenTextures(1, cameraTexture, 0);
|
||||
int tex = cameraTexture[0];
|
||||
gl.glBindTexture(GL10.GL_TEXTURE_2D, tex);
|
||||
synchronized (glCameraFrame) {
|
||||
int orgRow = 0;
|
||||
int destRow = 0;
|
||||
for(int i = 0; i < 160; ++i){
|
||||
System.arraycopy(glCameraFrame, orgRow, glTexture, destRow, 240);
|
||||
orgRow += 240;
|
||||
destRow += 256;
|
||||
}
|
||||
gl.glTexImage2D(GL10.GL_TEXTURE_2D, 0, GL10.GL_RGBA, 256, 256, 0, GL10.GL_RGBA, GL10.GL_UNSIGNED_BYTE, IntBuffer.wrap(glTexture));
|
||||
}
|
||||
gl.glTexParameterf(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_MIN_FILTER, GL10.GL_LINEAR);
|
||||
gl.glTexParameterf(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_WRAP_S, GL10.GL_CLAMP_TO_EDGE);
|
||||
gl.glTexParameterf(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_WRAP_T, GL10.GL_CLAMP_TO_EDGE);
|
||||
}
|
||||
|
||||
public void onPreviewFrame(byte[] new_frame, Camera camera){
|
||||
w = camera.getParameters().getPreviewSize().width;
|
||||
h = camera.getParameters().getPreviewSize().height;
|
||||
|
||||
synchronized(frame){
|
||||
frame = new_frame.clone();
|
||||
}
|
||||
}
|
||||
|
||||
private void processFrame(byte[] data) {
|
||||
synchronized (glCameraFrame) {
|
||||
FindLight(w, h, data, glCameraFrame, buffer);
|
||||
}
|
||||
}
|
||||
|
||||
void calculateMove() {
|
||||
// buffer[1] holds the light direction info if the phone is in landscape format
|
||||
// small values -> turn left
|
||||
// large values -> turn right
|
||||
// in portrait mode buffer[2] should be used
|
||||
// large values -> turn right
|
||||
// small values -> turn left
|
||||
if(mActivity.isConnected()){
|
||||
mActivity.sendBTCmessage(BTCommunicator.NO_DELAY, BTCommunicator.GET_LIGHT_SENSOR, 0, 0);
|
||||
mActivity.sendBTCmessage(40, BTCommunicator.GET_TOUCH_SENSOR, 0, 0);
|
||||
}
|
||||
if(mActivity.getLightSensorReading() > 600 || mActivity.getTouchSensorReading() > 0){
|
||||
left = 0;
|
||||
right = 0;
|
||||
}else{
|
||||
if( buffer[0] > 100 ) { // light is visible
|
||||
int forwardSpeed = 50;
|
||||
double upScale = 40;
|
||||
double direction = (buffer[1] - 240/2)/160;
|
||||
//double direction = -1.0 * (buffer[2] - w/2)/h;
|
||||
left = (int)(upScale * direction) + forwardSpeed;
|
||||
right = (int)(-1.0 * upScale * direction) + forwardSpeed;
|
||||
} else {
|
||||
left = 0;
|
||||
right = 0;
|
||||
}
|
||||
left = Math.min(Math.max(left,0),100);
|
||||
right = Math.min(Math.max(right,0),100);
|
||||
}
|
||||
|
||||
mActivity.updateMotorControl(left,right);
|
||||
}
|
||||
|
||||
private Runnable FrameProcessor = new Runnable(){
|
||||
public void run(){
|
||||
if(mActivity.isConnected()){
|
||||
synchronized(frame){
|
||||
processFrame(frame);
|
||||
}
|
||||
calculateMove();
|
||||
}
|
||||
if(handler != null)
|
||||
handler.postDelayed(this, 100);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
class Quad {
|
||||
public FloatBuffer vertexBuffer, texBuffer;
|
||||
|
||||
// number of coordinates per vertex in this array
|
||||
static final int COORDS_PER_VERTEX = 3;
|
||||
float triangleCoords[] = { // in counterclockwise order:
|
||||
-1.0f, -1.0f, 0.0f, // top
|
||||
1.0f, -1.0f, 0.0f, // bottom left
|
||||
-1.0f, 1.0f, 0.0f, // bottom right
|
||||
1.0f, 1.0f, 0.0f,
|
||||
};
|
||||
|
||||
float texCoords[] = {
|
||||
0.0f, 0.625f,
|
||||
0.9375f, 0.625f,
|
||||
0.0f, 0.0f,
|
||||
0.9375f, 0.0f
|
||||
};
|
||||
|
||||
public Quad() {
|
||||
// initialize vertex byte buffer for shape coordinates
|
||||
ByteBuffer bb = ByteBuffer.allocateDirect(
|
||||
// (number of coordinate values * 4 bytes per float)
|
||||
triangleCoords.length * 4);
|
||||
// use the device hardware's native byte order
|
||||
bb.order(ByteOrder.nativeOrder());
|
||||
|
||||
// create a floating point buffer from the ByteBuffer
|
||||
vertexBuffer = bb.asFloatBuffer();
|
||||
// add the coordinates to the FloatBuffer
|
||||
vertexBuffer.put(triangleCoords);
|
||||
// set the buffer to read the first coordinate
|
||||
vertexBuffer.position(0);
|
||||
|
||||
ByteBuffer bb2 = ByteBuffer.allocateDirect(
|
||||
// (number of coordinate values * 4 bytes per float)
|
||||
texCoords.length * 4);
|
||||
// use the device hardware's native byte order
|
||||
bb2.order(ByteOrder.nativeOrder());
|
||||
|
||||
// create a floating point buffer from the ByteBuffer
|
||||
texBuffer = bb2.asFloatBuffer();
|
||||
// add the coordinates to the FloatBuffer
|
||||
texBuffer.put(texCoords);
|
||||
// set the buffer to read the first coordinate
|
||||
texBuffer.position(0);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -20,6 +20,7 @@
|
||||
package com.lego.minddroid;
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Class for composing the proper messages for simple
|
||||
* communication over bluetooth
|
||||
@@ -97,6 +98,87 @@ public class LCPMessage {
|
||||
public static final byte DIRECTORY_FULL = (byte) 0xFC;
|
||||
public static final byte UNDEFINED_ERROR = (byte) 0x8A;
|
||||
public static final byte NOT_IMPLEMENTED = (byte) 0xFD;
|
||||
|
||||
// Sensor types
|
||||
public static final byte NO_SENSOR = 0x00;
|
||||
public static final byte SWITCH = 0x01;
|
||||
public static final byte TEMPERATURE = 0x02;
|
||||
public static final byte REFLECTION = 0x03;
|
||||
public static final byte ANGLE = 0x04;
|
||||
public static final byte LIGHT_ACTIVE = 0x05;
|
||||
public static final byte LIGHT_INACTIVE = 0x06;
|
||||
public static final byte SOUND_DB = 0x07;
|
||||
public static final byte SOUND_DBA = 0x08;
|
||||
public static final byte CUSTOM = 0x09;
|
||||
public static final byte LOWSPEED = 0x0A;
|
||||
public static final byte LOWSPEED_9V = 0x0B;
|
||||
public static final byte NO_OF_SENSOR_TYPES = 0x0C;
|
||||
|
||||
// Sensor modes
|
||||
public static final byte RAWMODE = 0x00;
|
||||
public static final byte BOOLEANMODE = 0x20;
|
||||
public static final byte TRANSITIONCNTMODE = 0x40;
|
||||
public static final byte PERIODCOUNTERMODE = 0x60;
|
||||
public static final byte PCTFULLSCALEMODE = (byte) 0x80;
|
||||
public static final byte CELSIUSMODE = (byte) 0xA0;
|
||||
public static final byte FRAHRENHEITMODE = (byte) 0xC0;
|
||||
public static final byte ANGLESTEPSMODE = (byte) 0xE0;
|
||||
public static final byte SLOPEMASK = 0x1F;
|
||||
public static final byte MODEMASK = (byte) 0xE0;
|
||||
|
||||
// Ports
|
||||
public static final byte PORT_0 = 0x00;
|
||||
public static final byte PORT_1 = 0x01;
|
||||
public static final byte PORT_2 = 0x02;
|
||||
public static final byte PORT_3 = 0x03;
|
||||
|
||||
public static byte[] getSetInputModeMessage(int input_port, byte sensor_type, byte sensor_mode){
|
||||
byte[] message = new byte[5];
|
||||
|
||||
message[0] = DIRECT_COMMAND_NOREPLY;
|
||||
message[1] = SET_INPUT_MODE;
|
||||
switch(input_port){
|
||||
case 0:
|
||||
message[2] = PORT_0;
|
||||
break;
|
||||
case 1:
|
||||
message[2] = PORT_1;
|
||||
break;
|
||||
case 2:
|
||||
message[2] = PORT_2;
|
||||
break;
|
||||
case 3:
|
||||
message[2] = PORT_3;
|
||||
break;
|
||||
}
|
||||
message[3] = sensor_type;
|
||||
message[4] = sensor_mode;
|
||||
|
||||
return message;
|
||||
}
|
||||
|
||||
public static byte[] getGetInputValuesMessage(int input_port){
|
||||
byte[] message = new byte[3];
|
||||
|
||||
message[0] = DIRECT_COMMAND_REPLY;
|
||||
message[1] = GET_INPUT_VALUES;
|
||||
switch(input_port){
|
||||
case 0:
|
||||
message[2] = PORT_0;
|
||||
break;
|
||||
case 1:
|
||||
message[2] = PORT_1;
|
||||
break;
|
||||
case 2:
|
||||
message[2] = PORT_2;
|
||||
break;
|
||||
case 3:
|
||||
message[2] = PORT_3;
|
||||
break;
|
||||
}
|
||||
|
||||
return message;
|
||||
}
|
||||
|
||||
// Firmware codes
|
||||
public static byte[] FIRMWARE_VERSION_LEJOSMINDDROID = { 0x6c, 0x4d, 0x49, 0x64 };
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -251,7 +251,6 @@ public class NXJUploader extends Activity implements UploadThreadListener, Dialo
|
||||
textView = (TextView) findViewById(R.id.nxt_name);
|
||||
textView.setText(text);
|
||||
break;
|
||||
|
||||
case DIALOG_FILE:
|
||||
textView = (TextView) findViewById(R.id.nxj_file_name);
|
||||
textView.setText(text);
|
||||
|
||||
@@ -1,29 +1,112 @@
|
||||
package com.lego.minddroid;
|
||||
|
||||
import android.content.Context;
|
||||
import android.graphics.Bitmap;
|
||||
import android.hardware.Camera;
|
||||
import android.util.Log;
|
||||
import android.view.SurfaceHolder;
|
||||
import android.view.SurfaceView;
|
||||
|
||||
class SampleView extends SampleViewBase {
|
||||
|
||||
public SampleView(Context context, MINDdroidCV uiActivity) {
|
||||
super(context,uiActivity);
|
||||
class SampleView extends SurfaceView implements SurfaceHolder.Callback {
|
||||
private SurfaceHolder mHolder;
|
||||
private Camera mCamera;
|
||||
private static final String TAG = "CAMERA_PREVIEW";
|
||||
private int mFrameWidth;
|
||||
private int mFrameHeight;
|
||||
protected CameraGLRenderer mGLRender;
|
||||
protected MINDdroidCV mActivity;
|
||||
protected double[]buffer;
|
||||
protected int left, right;
|
||||
|
||||
public SampleView(Context context, CameraGLRenderer renderer, MINDdroidCV uiActivity) {
|
||||
super(context);
|
||||
mCamera = Camera.open();
|
||||
mGLRender = renderer;
|
||||
mActivity = uiActivity;
|
||||
|
||||
// Install a SurfaceHolder.Callback so we get notified when the
|
||||
// underlying surface is created and destroyed.
|
||||
mHolder = getHolder();
|
||||
mHolder.addCallback(this);
|
||||
// deprecated setting, but required on Android versions prior to 3.0
|
||||
mHolder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
|
||||
|
||||
renderer.setMActivity(uiActivity);
|
||||
|
||||
Log.i(TAG, "Instantiated new " + this.getClass());
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Bitmap processFrame(byte[] data) {
|
||||
int frameSize = getFrameWidth() * getFrameHeight();
|
||||
int[] rgba = new int[frameSize];
|
||||
|
||||
FindLight(getFrameWidth(), getFrameHeight(), data, rgba,buffer);
|
||||
public void surfaceCreated(SurfaceHolder holder) {
|
||||
// The Surface has been created, now tell the camera where to draw the preview.
|
||||
try {
|
||||
|
||||
Bitmap bmp = Bitmap.createBitmap(getFrameWidth(), getFrameHeight(), Bitmap.Config.ARGB_8888);
|
||||
bmp.setPixels(rgba, 0/* offset */, getFrameWidth() /* stride */, 0, 0, getFrameWidth(), getFrameHeight());
|
||||
return bmp;
|
||||
/**/
|
||||
mCamera.setPreviewDisplay(mHolder);
|
||||
mCamera.startPreview();
|
||||
|
||||
} catch (Exception e) {
|
||||
Log.d(TAG, "Error setting camera preview: " + e.getMessage());
|
||||
}
|
||||
}
|
||||
|
||||
public native void FindLight(int width, int height, byte yuv[], int[] rgba,double[] array);
|
||||
|
||||
static {
|
||||
System.loadLibrary("mixed_sample");
|
||||
public int getFrameWidth() {
|
||||
return mFrameWidth;
|
||||
}
|
||||
|
||||
public int getFrameHeight() {
|
||||
return mFrameHeight;
|
||||
}
|
||||
|
||||
|
||||
public void surfaceDestroyed(SurfaceHolder holder) {
|
||||
// Surface will be destroyed when we return, so stop the preview.
|
||||
// Because the CameraDevice object is not a shared resource, it's very
|
||||
// important to release it when the activity is paused.
|
||||
try {
|
||||
if (mCamera!=null) {
|
||||
mCamera.stopPreview();
|
||||
mCamera.setPreviewCallback(null);
|
||||
mCamera.release(); // release the camera for other applications
|
||||
mCamera = null;
|
||||
}
|
||||
} catch (Exception e) {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public void surfaceChanged(SurfaceHolder holder, int format, int w, int h) {
|
||||
// If your preview can change or rotate, take care of those events here.
|
||||
// Make sure to stop the preview before resizing or reformatting it.
|
||||
|
||||
mFrameWidth = w;
|
||||
mFrameHeight = h;
|
||||
|
||||
if (mHolder.getSurface() == null){
|
||||
// preview surface does not exist
|
||||
return;
|
||||
}
|
||||
|
||||
// stop preview before making changes
|
||||
try {
|
||||
mCamera.stopPreview();
|
||||
} catch (Exception e){
|
||||
// ignore: tried to stop a non-existent preview
|
||||
}
|
||||
|
||||
// set preview size and make any resize, rotate or
|
||||
// reformatting changes here
|
||||
|
||||
// start preview with new settings
|
||||
try {
|
||||
Camera.Parameters p = mCamera.getParameters();
|
||||
p.setPreviewSize(240, 160);
|
||||
mCamera.setParameters(p);
|
||||
mGLRender.w = w;
|
||||
mGLRender.h = h;
|
||||
mCamera.setPreviewDisplay(mHolder);
|
||||
mCamera.startPreview();
|
||||
mCamera.setPreviewCallback(mGLRender);
|
||||
} catch (Exception e){
|
||||
Log.d(TAG, "Error starting camera preview: " + e.getMessage());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,182 +0,0 @@
|
||||
package com.lego.minddroid;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.text.DecimalFormat;
|
||||
import java.util.List;
|
||||
|
||||
import android.content.Context;
|
||||
import android.graphics.Bitmap;
|
||||
import android.graphics.Canvas;
|
||||
import android.graphics.Color;
|
||||
import android.graphics.Paint;
|
||||
import android.graphics.Rect;
|
||||
import android.hardware.Camera;
|
||||
import android.hardware.Camera.PreviewCallback;
|
||||
import android.util.Log;
|
||||
import android.view.SurfaceHolder;
|
||||
import android.view.SurfaceView;
|
||||
|
||||
public abstract class SampleViewBase extends SurfaceView implements SurfaceHolder.Callback, Runnable {
|
||||
private static final String TAG = "Sample::SurfaceView";
|
||||
|
||||
private Camera mCamera;
|
||||
private SurfaceHolder mHolder;
|
||||
private int mFrameWidth;
|
||||
private int mFrameHeight;
|
||||
private byte[] mFrame;
|
||||
private boolean mThreadRun;
|
||||
protected MINDdroidCV mActivity;
|
||||
protected double[]buffer;
|
||||
protected int left, right;
|
||||
|
||||
public SampleViewBase(Context context, MINDdroidCV uiActivity) {
|
||||
super(context);
|
||||
mActivity = uiActivity;
|
||||
|
||||
mHolder = getHolder();
|
||||
mHolder.addCallback(this);
|
||||
buffer = new double[10];
|
||||
Log.i(TAG, "Instantiated new " + this.getClass());
|
||||
}
|
||||
|
||||
public int getFrameWidth() {
|
||||
return mFrameWidth;
|
||||
}
|
||||
|
||||
public int getFrameHeight() {
|
||||
return mFrameHeight;
|
||||
}
|
||||
|
||||
public void surfaceChanged(SurfaceHolder _holder, int format, int width, int height) {
|
||||
Log.i(TAG, "surfaceCreated");
|
||||
if (mCamera != null) {
|
||||
Camera.Parameters params = mCamera.getParameters();
|
||||
List<Camera.Size> sizes = params.getSupportedPreviewSizes();
|
||||
mFrameWidth = width;
|
||||
mFrameHeight = height;
|
||||
|
||||
// selecting optimal camera preview size
|
||||
{
|
||||
double minDiff = Double.MAX_VALUE;
|
||||
for (Camera.Size size : sizes) {
|
||||
if (Math.abs(size.height - height) < minDiff) {
|
||||
mFrameWidth = size.width;
|
||||
mFrameHeight = size.height;
|
||||
minDiff = Math.abs(size.height - height);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
params.setPreviewSize(getFrameWidth(), getFrameHeight());
|
||||
mCamera.setParameters(params);
|
||||
//mCamera.setDisplayOrientation(180);
|
||||
|
||||
try {
|
||||
mCamera.setPreviewDisplay(null);
|
||||
} catch (IOException e) {
|
||||
Log.e(TAG, "mCamera.setPreviewDisplay fails: " + e);
|
||||
}
|
||||
mCamera.startPreview();
|
||||
}
|
||||
}
|
||||
|
||||
public void surfaceCreated(SurfaceHolder holder) {
|
||||
Log.i(TAG, "surfaceCreated");
|
||||
mCamera = Camera.open();
|
||||
mCamera.setPreviewCallback(new PreviewCallback() {
|
||||
public void onPreviewFrame(byte[] data, Camera camera) {
|
||||
synchronized (SampleViewBase.this) {
|
||||
mFrame = data;
|
||||
SampleViewBase.this.notify();
|
||||
}
|
||||
}
|
||||
});
|
||||
(new Thread(this)).start();
|
||||
}
|
||||
|
||||
public void surfaceDestroyed(SurfaceHolder holder) {
|
||||
Log.i(TAG, "surfaceDestroyed");
|
||||
mThreadRun = false;
|
||||
if (mCamera != null) {
|
||||
synchronized (this) {
|
||||
mCamera.stopPreview();
|
||||
mCamera.setPreviewCallback(null);
|
||||
mCamera.release();
|
||||
mCamera = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
protected abstract Bitmap processFrame(byte[] data);
|
||||
|
||||
public void run() {
|
||||
mThreadRun = true;
|
||||
Log.i(TAG, "Starting processing thread");
|
||||
while (mThreadRun) {
|
||||
Bitmap bmp = null;
|
||||
|
||||
synchronized (this) {
|
||||
try {
|
||||
this.wait();
|
||||
bmp = processFrame(mFrame);
|
||||
} catch (InterruptedException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
if (bmp != null) {
|
||||
Canvas canvas = mHolder.lockCanvas();
|
||||
calculateMove();
|
||||
if (canvas != null) {
|
||||
canvas.drawBitmap(bmp,0.0f,0.0f,null); //(canvas.getWidth() - getFrameWidth()) / 2, (canvas.getHeight() - getFrameHeight()) / 2, null);
|
||||
|
||||
//drawText(canvas,buffer);
|
||||
mHolder.unlockCanvasAndPost(canvas);
|
||||
}
|
||||
bmp.recycle();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void drawText(Canvas canvas,double[] buffer) {
|
||||
Paint paint = new Paint();
|
||||
paint.setStyle(Paint.Style.FILL);
|
||||
paint.setColor(Color.WHITE);
|
||||
DecimalFormat twoPlaces = new DecimalFormat("0.00");
|
||||
String todraw = ":" + twoPlaces.format(buffer[0]) + ":" +
|
||||
twoPlaces.format(buffer[1]) + ":" +
|
||||
twoPlaces.format(buffer[2]) + ":" +
|
||||
left + ":" + right + ":";
|
||||
Paint bpaint = new Paint();
|
||||
bpaint.setStyle(Paint.Style.FILL);
|
||||
bpaint.setColor(Color.BLUE);
|
||||
Rect rect = new Rect(0,0,250,50);
|
||||
canvas.drawRect(rect , bpaint);
|
||||
canvas.drawText(todraw, 0, todraw.length(), 10.0f, 10.0f, paint);
|
||||
}
|
||||
|
||||
void calculateMove() {
|
||||
// buffer[1] holds the light direction info if the phone is in landscape format
|
||||
// small values -> turn left
|
||||
// large values -> turn right
|
||||
// in portrait mode buffer[2] should be used
|
||||
// large values -> turn right
|
||||
// small values -> turn left
|
||||
if( buffer[0] > 100 ) { // light is visible
|
||||
int forwardSpeed = 50;
|
||||
double upScale = 40;
|
||||
//double direction = (buffer[1] - getFrameWidth()/2)/getFrameWidth();
|
||||
double direction = -1.0 * (buffer[2] - getFrameHeight()/2)/getFrameHeight();
|
||||
left = (int)(upScale * direction) + forwardSpeed;
|
||||
right = (int)(-1.0 * upScale * direction) + forwardSpeed;
|
||||
} else {
|
||||
left = 0;
|
||||
right = 0;
|
||||
}
|
||||
left = Math.min(Math.max(left,0),100);
|
||||
right = Math.min(Math.max(right,0),100);
|
||||
|
||||
mActivity.updateMotorControl(left,right);
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user