private int getEncoderValue(int channel) {
// use latest tachoMonitor value for the motor if available
if (tachoMonitorAlive()) {
return tachoMonitor.getTachoCount(channel);
}
// .. otherwise, query the controller to get it
getData(REGISTER_MAP[REG_IDX_ENCODER_CURRENT][channel], buf, 4);
return EndianTools.decodeIntBE(buf, 0);
}
/**
* Execute a command. designed to never block because it is shared across two motors. The rotate
* WAITS are done in the TetrixEncoderMotor class
*
* @param command the command
* @param operand the value from the caller. Mostly not used and set to 0
* @param channel the channel: MOTOR_1, MOTOR_2
* @return a value depending on the command
*/
synchronized int doCommand(int command, int operand, int channel) {
byte workingByte = 0;
int commandRetVal = 0;
switch (command) {
case CMD_FORWARD:
if (motorState[channel] == STATE_RUNNING_FWD) break;
if (motorType[channel] == MOTTYPE_REGULATED) {
if (motorState[channel] != STATE_STOPPED) {
((TetrixRegulatedMotor) motors[channel])
.doListenerState(TetrixRegulatedMotor.LISTENERSTATE_STOP);
}
((TetrixRegulatedMotor) motors[channel])
.doListenerState(TetrixRegulatedMotor.LISTENERSTATE_START);
}
motorGo(channel, command);
break;
case CMD_BACKWARD:
if (motorState[channel] == STATE_RUNNING_BKWD) break;
if (motorType[channel] == MOTTYPE_REGULATED) {
if (motorState[channel] != STATE_STOPPED) {
((TetrixRegulatedMotor) motors[channel])
.doListenerState(TetrixRegulatedMotor.LISTENERSTATE_STOP);
}
((TetrixRegulatedMotor) motors[channel])
.doListenerState(TetrixRegulatedMotor.LISTENERSTATE_START);
}
motorGo(channel, command);
break;
case CMD_FLT:
workingByte = -128;
case CMD_STOP:
if (command == CMD_STOP) workingByte = 0;
sendData(REGISTER_MAP[REG_IDX_POWER][channel], workingByte);
Delay.msDelay(50);
if (motorType[channel] == MOTTYPE_REGULATED
&& motorState[channel] != STATE_STOPPED
&& motorState[channel] != STATE_ROTATE_TO) {
((TetrixRegulatedMotor) motors[channel])
.doListenerState(TetrixRegulatedMotor.LISTENERSTATE_STOP);
}
motorState[channel] = STATE_STOPPED;
break;
case CMD_SETPOWER:
motorParams[MOTPARAM_POWER][channel] = operand;
// if not running, exit
if (motorState[channel] == STATE_STOPPED) break;
// set the power if running to take effect immediately
workingByte = (byte) motorParams[MOTPARAM_POWER][channel];
if (motorState[channel] == STATE_RUNNING_BKWD) {
workingByte *= -1;
}
sendData(REGISTER_MAP[REG_IDX_POWER][channel], workingByte);
break;
case CMD_ROTATE:
case CMD_ROTATE_TO:
if (motorType[channel] == MOTTYPE_REGULATED) {
if (motorState[channel] != STATE_STOPPED) {
((TetrixRegulatedMotor) motors[channel])
.doListenerState(TetrixRegulatedMotor.LISTENERSTATE_STOP);
}
((TetrixRegulatedMotor) motors[channel])
.doListenerState(TetrixRegulatedMotor.LISTENERSTATE_START);
}
rotate(channel, operand, command);
break;
case CMD_GETPOWER:
commandRetVal = motorParams[MOTPARAM_POWER][channel];
break;
case CMD_GETTACHO:
commandRetVal = getEncoderValue(channel);
break;
case CMD_RESETTACHO:
// reset encoder/tacho
setMode(channel, true);
Delay.msDelay(30); // small delay to allow encoder value reset in controller to happen
motorState[channel] = STATE_STOPPED;
break;
case CMD_SETREVERSE:
motorParams[MOTPARAM_REVERSED][channel] = operand;
setMode(channel, false);
break;
case CMD_ISMOVING:
commandRetVal = MOTPARAM_OP_TRUE;
if (motorState[channel] == STATE_STOPPED) commandRetVal = MOTPARAM_OP_FALSE;
// over-ride previous decision if regulated motor and it is moving
if (tachoMonitorAlive()) {
commandRetVal = tachoMonitor.isMoving(channel) ? MOTPARAM_OP_TRUE : MOTPARAM_OP_FALSE;
}
break;
case CMD_SETREGULATE:
motorParams[MOTPARAM_REGULATED][channel] = operand; // 1=true, 0=false
break;
case CMD_GETSPEED:
commandRetVal = 0;
if (tachoMonitorAlive()) {
commandRetVal = tachoMonitor.getSpeed(channel);
}
break;
case CMD_GETLIMITANGLE:
commandRetVal = limitangle[channel];
break;
default:
throw new IllegalArgumentException("Invalid Command");
}
return commandRetVal;
}
boolean tachoMonitorAlive() {
return tachoMonitor != null && tachoMonitor.isAlive();
}