private void stopThread() {
isRunning = false;
try {
joinX(500);
} catch (InterruptedException ex) {
}
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
if (isAlive())
DebugConsole.show(
"DEBUG: PrototypeSendorThread stopping failed (port: " + getPortLabel() + ")");
else
DebugConsole.show(
"DEBUG: PrototypeSensorThread successfully stopped (port: " + getPortLabel() + ")");
}
private synchronized void read(boolean check, int[] ain, int[] din) {
if (check) checkConnect();
if (ain.length != 5) {
new ShowError("Error in PrototypeSensor.read().\n" + "Parameter ain must have length 5.");
return;
}
if (din.length != 6) {
new ShowError("Error in PrototypeSensor.read().\n" + "Parameter din must have length 6.");
return;
}
byte[] in = getData(REGISTERBASE, 11);
// Convert to int
int[] data = new int[11];
for (int i = 0; i < 11; i++) data[i] = in[i] & 0xFF;
// Analog inputs: base -> upper 8 bits, base+1 -> lower 2 bits
for (int i = 0; i < 5; i++) ain[i] = 4 * data[2 * i] + data[2 * i + 1];
// Digital inputs
int bitValue = 1;
for (int i = 0; i < 6; i++) {
if ((control & bitValue) != 0) din[i] = -1;
else din[i] = (data[10] & bitValue) / bitValue;
bitValue *= 2;
}
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_MEDIUM)
DebugConsole.show("DEBUG: read() values:\n" + getAnalogValues(ain) + getDigitalValues(din));
}
protected void init() {
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_MEDIUM)
DebugConsole.show("DEBUG: Prototype.init() called (Port: " + getPortLabel() + ")");
super.init();
sendData(IOCONTROL, (byte) 0); // All inputs
sendData(SAMPLING, (byte) 4); // minimum sampling period
}
private static void runToolBus(final ToolBus toolbus) {
try {
toolbus.parsecup();
toolbus.prepare();
Thread thread =
new Thread(
new Runnable() {
public void run() {
toolbus.execute();
}
});
thread.setName(PLUGIN_ID);
thread.start();
DebugConsole debugConsole = new DebugConsole(toolbus);
debugConsole.show();
} catch (Exception rex) {
rex.printStackTrace();
throw new RuntimeException(rex);
}
}
/**
* Writes the given bit state (low/high) to the digital output channels. Only channels set as
* output will be affected. Input channel bits are ignored.
*
* @param dout an integer array of length 6 that holds the bit state: 0->low, 1->high
*/
public synchronized void write(int[] dout) {
checkConnect();
if (dout.length != 6) {
new ShowError("Error in PrototypeSensor.write().\n" + "Parameter dout must have length 6.");
return;
}
for (int i = 0; i < 6; i++) {
if (dout[i] < 0 || dout[i] > 1) {
new ShowError("Error in PrototypeSensor.read().\n" + "Parameter values must be 0 or 1.");
return;
}
}
int bitValue = 1;
int value = 0;
for (int i = 0; i < 6; i++) {
if (dout[i] == 1) value = value + dout[i] * bitValue;
bitValue *= 2;
}
sendData(DIGITALOUT, (byte) value);
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_MEDIUM)
DebugConsole.show("DEBUG: write():\n" + value);
}
public void run() {
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW) DebugConsole.show("PrTh started");
int[] ain = new int[5];
int[] din = new int[6];
for (int i = 0; i < 5; i++) ain[i] = 0;
for (int i = 0; i < 6; i++) din[i] = 0;
int[] ain_new = new int[5];
int[] din_new = new int[6];
isRunning = true;
while (isRunning) {
if (prototypeListener != null) {
Tools.delay(pollDelay);
boolean rc = readSensor(ain_new, din_new);
if (!rc) // Error
continue;
boolean isAnalogChanged = false;
for (int i = 0; i < 5; i++) {
if (ain_new[i] == ain[i]) ain_new[i] = -1;
else {
ain[i] = ain_new[i];
isAnalogChanged = true;
}
}
boolean isDigitalChanged = false;
for (int i = 0; i < 6; i++) {
if (din_new[i] == din[i]) din_new[i] = -1;
else {
din[i] = din_new[i];
isDigitalChanged = true;
}
}
if (isAnalogChanged) {
if (inAnalogCallback) {
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
DebugConsole.show(
"DEBUG: Prototype event 'ainChanged' (port: "
+ getPortLabel()
+ ") -------- rejected: Other callback underway!");
} else {
inAnalogCallback = true;
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
DebugConsole.show(
"DEBUG: Prototype event 'ainChanged' (port: " + getPortLabel() + ")");
prototypeListener.ainChanged(getPort(), ain_new);
inAnalogCallback = false;
}
}
if (isDigitalChanged) {
if (inDigitalCallback) {
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
DebugConsole.show(
"DEBUG: Prototype event 'dinChanged' (port: "
+ getPortLabel()
+ ") -------- rejected: Other callback underway!");
} else {
inDigitalCallback = true;
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
DebugConsole.show(
"DEBUG: Prototype event 'dinChanged' (port: " + getPortLabel() + ")");
prototypeListener.dinChanged(getPort(), din_new);
inDigitalCallback = false;
}
}
}
}
}
private PrototypeSensorThread() {
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW) DebugConsole.show("PrTh created");
}
protected void cleanup() {
if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_MEDIUM)
DebugConsole.show("DEBUG: Prototype.cleanup() called (Port: " + getPortLabel() + ")");
if (pst != null) pst.stopThread();
sendData(IOCONTROL, (byte) 0); // All inputs
}
