public final class ModernRoboticsUsbDcMotorController extends ModernRoboticsUsbController implements DcMotorController, VoltageSensor
This is an implementation of DcMotorController
Modern Robotics USB DC Motor Controllers have a Voltage Sensor that measures the current voltage of the main robot battery.
Use HardwareDeviceManager
to create an instance of this class
ModernRoboticsUsbController.WRITE_STATUS
ModernRoboticsUsbDevice.CreateReadWriteRunnable
HardwareDevice.Manufacturer
Modifier and Type | Field and Description |
---|---|
protected static int |
ADDRESS_BATTERY_VOLTAGE |
protected static int[] |
ADDRESS_MAX_DIFFERENTIAL_CONTROL_LOOP_COEFFICIENT_MAP |
protected static int[] |
ADDRESS_MOTOR_CURRENT_ENCODER_VALUE_MAP |
protected static int[] |
ADDRESS_MOTOR_GEAR_RATIO_MAP |
protected static int[] |
ADDRESS_MOTOR_MODE_MAP |
protected static int[] |
ADDRESS_MOTOR_POWER_MAP
map of motors to memory addresses
|
protected static int[] |
ADDRESS_MOTOR_TARGET_ENCODER_VALUE_MAP |
protected static int |
ADDRESS_MOTOR1_CURRENT_ENCODER_VALUE |
protected static int |
ADDRESS_MOTOR1_D_COEFFICIENT |
protected static int |
ADDRESS_MOTOR1_GEAR_RATIO |
protected static int |
ADDRESS_MOTOR1_I_COEFFICIENT |
protected static int |
ADDRESS_MOTOR1_MODE |
protected static int |
ADDRESS_MOTOR1_P_COEFFICIENT |
protected static int |
ADDRESS_MOTOR1_POWER |
protected static int |
ADDRESS_MOTOR1_TARGET_ENCODER_VALUE |
protected static int |
ADDRESS_MOTOR2_CURRENT_ENCODER_VALUE |
protected static int |
ADDRESS_MOTOR2_D_COEFFICIENT |
protected static int |
ADDRESS_MOTOR2_GEAR_RATIO |
protected static int |
ADDRESS_MOTOR2_I_COEFFICIENT |
protected static int |
ADDRESS_MOTOR2_MODE |
protected static int |
ADDRESS_MOTOR2_P_COEFFICIENT |
protected static int |
ADDRESS_MOTOR2_POWER |
protected static int |
ADDRESS_MOTOR2_TARGET_ENCODER_VALUE |
protected static int |
ADDRESS_PID_PARAMS_LOCK
"I2c register addresses" used in the controller hardware
|
protected static int |
ADDRESS_UNUSED |
protected static double |
apiPowerMax |
protected static double |
apiPowerMin |
protected static double |
BATTERY_MAX_MEASURABLE_VOLTAGE |
protected static int |
BATTERY_MAX_MEASURABLE_VOLTAGE_INT |
protected static byte |
bPowerBrake |
protected static byte |
bPowerFloat |
protected static byte |
bPowerMax
const values used by motor controller
|
protected static byte |
bPowerMin |
static int |
BUSY_THRESHOLD |
protected static byte |
cbEncoder |
protected static int |
cbRatioPIDParams |
protected static byte |
CHANNEL_MODE_FLAG_BUSY |
protected static byte |
CHANNEL_MODE_FLAG_ERROR |
protected static byte |
CHANNEL_MODE_FLAG_LOCK |
protected static byte |
CHANNEL_MODE_FLAG_NO_TIMEOUT |
protected static byte |
CHANNEL_MODE_FLAG_REVERSE |
protected static byte |
CHANNEL_MODE_FLAG_SELECT_RESET |
protected static byte |
CHANNEL_MODE_FLAG_SELECT_RUN_CONSTANT_SPEED |
protected static byte |
CHANNEL_MODE_FLAG_SELECT_RUN_POWER_CONTROL_ONLY
channel mode flags used by controller
|
protected static byte |
CHANNEL_MODE_FLAG_SELECT_RUN_TO_POSITION |
protected static byte |
CHANNEL_MODE_FLAG_UNUSED |
protected static int |
CHANNEL_MODE_MASK_BUSY |
protected static int |
CHANNEL_MODE_MASK_EMPTY_D5 |
protected static int |
CHANNEL_MODE_MASK_ERROR |
protected static int |
CHANNEL_MODE_MASK_LOCK |
protected static int |
CHANNEL_MODE_MASK_NO_TIMEOUT |
protected static int |
CHANNEL_MODE_MASK_REVERSE |
protected static int |
CHANNEL_MODE_MASK_SELECTION
channel mode masks used by controller
|
protected static byte |
CHANNEL_MODE_UNKNOWN |
protected static boolean |
DEBUG_LOGGING
Enable DEBUG_LOGGING logging
|
protected static byte |
DEFAULT_D_COEFFICIENT |
protected static byte |
DEFAULT_I_COEFFICIENT |
protected static byte |
DEFAULT_P_COEFFICIENT |
protected static int |
DIFFERENTIAL_CONTROL_LOOP_COEFFICIENT_MAX |
protected static int |
MONITOR_LENGTH
const values used by this class
|
protected static int |
MOTOR_FIRST |
protected static int |
MOTOR_LAST |
protected static int |
MOTOR_MAX |
protected static byte |
PID_PARAMS_LOCK_DISABLE |
protected static byte |
PID_PARAMS_LOCK_ENABLE |
protected ReadWriteRunnableSegment |
pidParamsLockSegment |
protected static byte |
RATIO_MAX |
protected static byte |
RATIO_MIN |
protected ReadWriteRunnableSegment[] |
rgPidParamsSegment |
protected static byte |
START_ADDRESS |
static java.lang.String |
TAG |
callbackLock, callbackWaiterCount, concurrentClientLock, readCompletionCount, readWriteRunnableIsRunning, writeStatus
createReadWriteRunnable, readWriteRunnable, readWriteService
Constructor and Description |
---|
ModernRoboticsUsbDcMotorController(Context context,
SerialNumber serialNumber,
OpenRobotUsbDevice openRobotUsbDevice,
SyncdDevice.Manager manager)
Use HardwareDeviceManager to create an instance of this class
|
Modifier and Type | Method and Description |
---|---|
protected void |
createSegments() |
protected void |
doArm() |
void |
doClose()
Close this device
|
protected void |
doCloseFromArmed() |
protected void |
doCloseFromOther() |
protected void |
doDisarm() |
protected void |
doPretend() |
java.lang.String |
getConnectionInfo()
Get connection information about this device in a human readable format
|
java.lang.String |
getDeviceName()
Device Name
|
DifferentialControlLoopCoefficients |
getDifferentialControlLoopCoefficients(int motor) |
double |
getGearRatio(int motor) |
HardwareDevice.Manufacturer |
getManufacturer()
Returns an indication of the manufacturer of this device.
|
int |
getMotorCurrentPosition(int motor)
Get the current motor position
|
DcMotor.RunMode |
getMotorMode(int motor)
Get the current motor mode.
|
double |
getMotorPower(int motor)
Get the current motor power
|
boolean |
getMotorPowerFloat(int motor)
Is motor power set to float?
|
int |
getMotorTargetPosition(int motor)
Get the current motor target position
|
MotorConfigurationType |
getMotorType(int motor)
Retrieves the motor type configured for this motor
|
DcMotor.ZeroPowerBehavior |
getMotorZeroPowerBehavior(int motor)
Returns the current zero power behavior of the motor.
|
protected java.lang.String |
getTag() |
double |
getVoltage()
Get battery voltage.
|
void |
initializeHardware() |
boolean |
isBusy(int motor)
Is the motor busy?
|
static DcMotor.RunMode |
modeFromByte(byte flag) |
static byte |
modeToByte(DcMotor.RunMode mode) |
protected void |
paranoidSleep(int ms) |
void |
resetDeviceConfigurationForOpMode()
Resets the device's configuration to that which is expected at the beginning of an OpMode.
|
void |
resetDeviceConfigurationForOpMode(int motor)
Reset the state we hold for the given motor so that it's clean at the start of an opmode
|
void |
setDifferentialControlLoopCoefficients(int motor,
DifferentialControlLoopCoefficients pid) |
protected void |
setEEPromLock(boolean enable) |
void |
setGearRatio(int motor,
double ratio) |
void |
setMotorMode(int motor,
DcMotor.RunMode mode)
Set the current motor mode.
|
void |
setMotorPower(int motor,
double power)
Set the current motor power
|
protected void |
setMotorPowerFloat(int motor) |
void |
setMotorTargetPosition(int motor,
int position)
Set the motor target position.
|
void |
setMotorType(int motor,
MotorConfigurationType motorType)
Informs the motor controller of the type of a particular motor.
|
void |
setMotorZeroPowerBehavior(int motor,
DcMotor.ZeroPowerBehavior zeroPowerBehavior)
Sets the behavior of the motor when zero power is applied.
|
protected void |
updateMotorParamsIfNecessary(int motor) |
protected void |
writeSegment(ReadWriteRunnableSegment segment,
byte[] data) |
isOkToReadOrWrite, read, readComplete, shutdownComplete, startupComplete, write, writeComplete
armDevice, disarmDevice, getCreateReadWriteRunnable, getOpenRobotUsbDevice, getPretendDevice, getReadWriteRunnable, getVersion, read8, readFromWriteCache, readFromWriteCache, write8, write8, write8
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
close, getVersion
public static final java.lang.String TAG
protected static final boolean DEBUG_LOGGING
protected static final int MONITOR_LENGTH
protected static final int MOTOR_FIRST
protected static final int MOTOR_LAST
protected static final int MOTOR_MAX
protected static final byte bPowerMax
protected static final byte bPowerBrake
protected static final byte bPowerMin
protected static final byte bPowerFloat
protected static final byte RATIO_MIN
protected static final byte RATIO_MAX
protected static final double apiPowerMin
protected static final double apiPowerMax
protected static final int DIFFERENTIAL_CONTROL_LOOP_COEFFICIENT_MAX
protected static final int BATTERY_MAX_MEASURABLE_VOLTAGE_INT
protected static final double BATTERY_MAX_MEASURABLE_VOLTAGE
protected static final byte DEFAULT_P_COEFFICIENT
protected static final byte DEFAULT_I_COEFFICIENT
protected static final byte DEFAULT_D_COEFFICIENT
protected static final byte START_ADDRESS
protected static final int CHANNEL_MODE_MASK_SELECTION
protected static final int CHANNEL_MODE_MASK_LOCK
protected static final int CHANNEL_MODE_MASK_REVERSE
protected static final int CHANNEL_MODE_MASK_NO_TIMEOUT
protected static final int CHANNEL_MODE_MASK_EMPTY_D5
protected static final int CHANNEL_MODE_MASK_ERROR
protected static final int CHANNEL_MODE_MASK_BUSY
protected static final byte CHANNEL_MODE_FLAG_SELECT_RUN_POWER_CONTROL_ONLY
protected static final byte CHANNEL_MODE_FLAG_SELECT_RUN_CONSTANT_SPEED
protected static final byte CHANNEL_MODE_FLAG_SELECT_RUN_TO_POSITION
protected static final byte CHANNEL_MODE_FLAG_SELECT_RESET
protected static final byte CHANNEL_MODE_FLAG_LOCK
protected static final byte CHANNEL_MODE_FLAG_REVERSE
protected static final byte CHANNEL_MODE_FLAG_NO_TIMEOUT
protected static final byte CHANNEL_MODE_FLAG_UNUSED
protected static final byte CHANNEL_MODE_FLAG_ERROR
protected static final byte CHANNEL_MODE_FLAG_BUSY
protected static final byte CHANNEL_MODE_UNKNOWN
protected static final int ADDRESS_PID_PARAMS_LOCK
protected static final byte PID_PARAMS_LOCK_DISABLE
protected static final byte PID_PARAMS_LOCK_ENABLE
protected static final int ADDRESS_MOTOR1_TARGET_ENCODER_VALUE
protected static final int ADDRESS_MOTOR1_MODE
protected static final int ADDRESS_MOTOR1_POWER
protected static final int ADDRESS_MOTOR2_POWER
protected static final int ADDRESS_MOTOR2_MODE
protected static final int ADDRESS_MOTOR2_TARGET_ENCODER_VALUE
protected static final int ADDRESS_MOTOR1_CURRENT_ENCODER_VALUE
protected static final int ADDRESS_MOTOR2_CURRENT_ENCODER_VALUE
protected static final int ADDRESS_BATTERY_VOLTAGE
protected static final int ADDRESS_MOTOR1_GEAR_RATIO
protected static final int ADDRESS_MOTOR1_P_COEFFICIENT
protected static final int ADDRESS_MOTOR1_I_COEFFICIENT
protected static final int ADDRESS_MOTOR1_D_COEFFICIENT
protected static final int ADDRESS_MOTOR2_GEAR_RATIO
protected static final int ADDRESS_MOTOR2_P_COEFFICIENT
protected static final int ADDRESS_MOTOR2_I_COEFFICIENT
protected static final int ADDRESS_MOTOR2_D_COEFFICIENT
protected static final int ADDRESS_UNUSED
protected static final int[] ADDRESS_MOTOR_POWER_MAP
protected static final int[] ADDRESS_MOTOR_MODE_MAP
protected static final int[] ADDRESS_MOTOR_TARGET_ENCODER_VALUE_MAP
protected static final int[] ADDRESS_MOTOR_CURRENT_ENCODER_VALUE_MAP
protected static final int[] ADDRESS_MOTOR_GEAR_RATIO_MAP
protected static final int[] ADDRESS_MAX_DIFFERENTIAL_CONTROL_LOOP_COEFFICIENT_MAP
public static final int BUSY_THRESHOLD
protected static final byte cbEncoder
protected static final int cbRatioPIDParams
protected ReadWriteRunnableSegment pidParamsLockSegment
protected ReadWriteRunnableSegment[] rgPidParamsSegment
public ModernRoboticsUsbDcMotorController(Context context, SerialNumber serialNumber, OpenRobotUsbDevice openRobotUsbDevice, SyncdDevice.Manager manager) throws RobotCoreException, java.lang.InterruptedException
RobotCoreException
java.lang.InterruptedException
protected java.lang.String getTag()
public void initializeHardware()
initializeHardware
in class ModernRoboticsUsbDevice
protected void createSegments()
protected void doArm() throws RobotCoreException, java.lang.InterruptedException
RobotCoreException
java.lang.InterruptedException
protected void doPretend() throws RobotCoreException, java.lang.InterruptedException
RobotCoreException
java.lang.InterruptedException
protected void doDisarm() throws RobotCoreException, java.lang.InterruptedException
RobotCoreException
java.lang.InterruptedException
protected void doCloseFromArmed()
protected void doCloseFromOther()
public HardwareDevice.Manufacturer getManufacturer()
HardwareDevice
getManufacturer
in interface HardwareDevice
public java.lang.String getDeviceName()
getDeviceName
in interface HardwareDevice
getDeviceName
in class ModernRoboticsUsbDevice
public java.lang.String getConnectionInfo()
HardwareDevice
getConnectionInfo
in interface HardwareDevice
public void resetDeviceConfigurationForOpMode()
HardwareDevice
resetDeviceConfigurationForOpMode
in interface HardwareDevice
public void doClose()
public void resetDeviceConfigurationForOpMode(int motor)
DcMotorController
resetDeviceConfigurationForOpMode
in interface DcMotorController
public void setMotorType(int motor, MotorConfigurationType motorType)
DcMotorController
setMotorType
in interface DcMotorController
motor
- port of the motor in questionmotorType
- the motor type.public MotorConfigurationType getMotorType(int motor)
DcMotorController
getMotorType
in interface DcMotorController
motor
- the motor in questionMotorConfigurationType.getUnspecifiedMotorType()
if no type has been configuredprotected void updateMotorParamsIfNecessary(int motor)
protected void setEEPromLock(boolean enable)
protected void writeSegment(ReadWriteRunnableSegment segment, byte[] data)
protected void paranoidSleep(int ms)
public void setMotorMode(int motor, DcMotor.RunMode mode)
DcMotorController
DcMotor.RunMode
setMotorMode
in interface DcMotorController
motor
- port of motormode
- run modepublic DcMotor.RunMode getMotorMode(int motor)
DcMotorController
getMotorMode
in interface DcMotorController
motor
- port of motorpublic void setMotorPower(int motor, double power)
DcMotorController
setMotorPower
in interface DcMotorController
motor
- port of motorpower
- from -1.0 to 1.0public double getMotorPower(int motor)
DcMotorController
getMotorPower
in interface DcMotorController
motor
- port of motorpublic boolean isBusy(int motor)
DcMotorController
isBusy
in interface DcMotorController
motor
- port of motorpublic void setMotorZeroPowerBehavior(int motor, DcMotor.ZeroPowerBehavior zeroPowerBehavior)
DcMotorController
setMotorZeroPowerBehavior
in interface DcMotorController
zeroPowerBehavior
- the behavior of the motor when zero power is applied.public DcMotor.ZeroPowerBehavior getMotorZeroPowerBehavior(int motor)
DcMotorController
getMotorZeroPowerBehavior
in interface DcMotorController
protected void setMotorPowerFloat(int motor)
public boolean getMotorPowerFloat(int motor)
DcMotorController
getMotorPowerFloat
in interface DcMotorController
motor
- port of motorpublic void setMotorTargetPosition(int motor, int position)
DcMotorController
setMotorTargetPosition
in interface DcMotorController
motor
- port of motorposition
- range from Integer.MIN_VALUE to Integer.MAX_VALUEpublic int getMotorTargetPosition(int motor)
DcMotorController
getMotorTargetPosition
in interface DcMotorController
motor
- port of motorpublic int getMotorCurrentPosition(int motor)
DcMotorController
getMotorCurrentPosition
in interface DcMotorController
motor
- port of motorpublic double getVoltage()
getVoltage
in interface VoltageSensor
public void setGearRatio(int motor, double ratio)
public double getGearRatio(int motor)
public void setDifferentialControlLoopCoefficients(int motor, DifferentialControlLoopCoefficients pid)
public DifferentialControlLoopCoefficients getDifferentialControlLoopCoefficients(int motor)
public static byte modeToByte(DcMotor.RunMode mode)
public static DcMotor.RunMode modeFromByte(byte flag)