def __init__(self, channel):
"""Allocate a PWM given a channel.
:param channel: The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
:type channel: int
"""
SensorBase.checkPWMChannel(channel)
self.channel = channel
self._port = hal.initializeDigitalPort(hal.getPort(channel))
self.freq = 1/self.kDefaultPwmPeriod * 1000
print('PWM::__init__ channel: {0}, freq: {1}'.format( channel, self.freq))
if not hal.allocatePWMChannel(self._port):
raise IndexError("PWM channel %d is already allocated" % channel)
#self.pwmObj = hal_data['pwm'][digital_port.pin]['pwmObj']
# Need this to free on unit test wpilib reset
Resource._add_global_resource(self)
hal.setPWM(self._port, 0)
self.__finalizer = weakref.finalize(self, _freePWM, self._port)
self.eliminateDeadband = True
hal.HALReport(hal.HALUsageReporting.kResourceType_PWM, channel)
def __init__(self, channel):
"""Constructor.
:param channel: The PWM channel that the SD540 is attached to. 0-9 are on-board, 10-19 are on the MXP port
.. note ::
Note that the SD540 uses the following bounds for PWM values. These
values should work reasonably well for most controllers, but if users
experience issues such as asymmetric behavior around the deadband or
inability to saturate the controller in either direction, calibration is
recommended. The calibration procedure can be found in the SD540 User
Manual available from Mindsensors.
- 2.05ms = full "forward"
- 1.55ms = the "high end" of the deadband range
- 1.50ms = center of the deadband range (off)
- 1.44ms = the "low end" of the deadband range
- .94ms = full "reverse"
"""
super().__init__(channel)
self.setBounds(2.05, 1.55, 1.50, 1.44, .94)
self.setPeriodMultiplier(self.PeriodMultiplier.k1X)
self.setRaw(self.centerPwm)
self.setZeroLatch()
self.isInverted = False
hal.HALReport(hal.HALUsageReporting.kResourceType_MindsensorsSD540,
self.getChannel())
LiveWindow.addActuatorChannel("SD540", self.getChannel(), self)
def __init__(self):
"""Creates a preference class that will automatically read the file in
a different thread. Any call to its methods will be blocked until the
thread is finished reading.
"""
# The actual values (str->str)
self.values = {}
# The keys in the order they were read from the file
self.keylist = []
# The comments that were in the file sorted by which key they appeared
# over (str->str)
self.comments = {}
# The comment at the end of the file
self.endComment = ""
# The semaphore for beginning reads and writes to the file
self.fileLock = threading.Condition()
# The semaphore for reading from the table
self.lock = threading.RLock()
# We synchronized on fileLock and then wait
# for it to know that the reading thread has started
with self.fileLock:
reader = threading.Thread(target=self._read,
name="Preferences Read")
reader.start()
self.fileLock.wait()
hal.HALReport(hal.HALUsageReporting.kResourceType_Preferences, 0)
def __init__(self, channel):
"""Construct an analog output on a specified MXP channel.
:param channel: The channel number to represent.
"""
if not hal.checkAnalogOutputChannel(channel):
raise IndexError(
"Analog output channel %d cannot be allocated. Channel is not present."
% channel)
try:
AnalogOutput.channels.allocate(self, channel)
except IndexError as e:
raise IndexError("Analog output channel %d is already allocated" %
channel) from e
self.channel = channel
port = hal.getPort(channel)
self._port = hal.initializeAnalogOutputPort(port)
LiveWindow.addSensorChannel("AnalogOutput", channel, self)
hal.HALReport(hal.HALUsageReporting.kResourceType_AnalogChannel,
channel, 1)
self.__finalizer = weakref.finalize(self, _freeAnalogOutput,
self._port)
def __init__(self, channel):
"""Constructor for a Talon (original or Talon SR)
:param channel: The PWM channel that the Talon is attached to. 0-9 are on-board, 10-19 are on the MXP port
:type channel: int
.. note ::
The Talon uses the following bounds for PWM values. These values
should work reasonably well for most controllers, but if users
experience issues such as asymmetric behavior around the deadband
or inability to saturate the controller in either direction,
calibration is recommended. The calibration procedure can be
found in the Talon User Manual available from CTRE.
- 2.037ms = full "forward"
- 1.539ms = the "high end" of the deadband range
- 1.513ms = center of the deadband range (off)
- 1.487ms = the "low end" of the deadband range
- 0.989ms = full "reverse"
"""
super().__init__(channel)
self.setBounds(2.037, 1.539, 1.513, 1.487, 0.989)
self.setPeriodMultiplier(self.PeriodMultiplier.k1X)
self.setRaw(self.centerPwm)
self.setZeroLatch()
self.isInverted = False
LiveWindow.addActuatorChannel("Talon", self.getChannel(), self)
hal.HALReport(hal.HALUsageReporting.kResourceType_Talon,
self.getChannel())
def getTable():
if SmartDashboard.table is None:
from networktables import NetworkTable
SmartDashboard.table = NetworkTable.getTable("SmartDashboard")
hal.HALReport(hal.HALUsageReporting.kResourceType_SmartDashboard,
hal.HALUsageReporting.kSmartDashboard_Instance)
return SmartDashboard.table
def __init__(self, port, range):
"""Constructor. Use this when the device is the first/only device on
the bus
:param port: The SPI port that the accelerometer is connected to
:type port: :class:`.SPI.Port`
:param range: The range (+ or -) that the accelerometer will measure.
:type range: :class:`.ADXL345_SPI.Range`
"""
self.spi = SPI(port)
self.spi.setClockRate(500000)
self.spi.setMSBFirst()
self.spi.setSampleDataOnFalling()
self.spi.setClockActiveLow()
self.spi.setChipSelectActiveHigh()
# Turn on the measurements
self.spi.write([self.kPowerCtlRegister, self.kPowerCtl_Measure])
self.setRange(range)
hal.HALReport(hal.HALUsageReporting.kResourceType_ADXL345,
hal.HALUsageReporting.kADXL345_SPI)
LiveWindow.addSensor("ADXL345_SPI", port, self)
def __init__(self, channel):
"""Constructor.
:param channel: The PWM channel that the VictorSP is attached to. 0-9 are on-board, 10-19 are on the MXP port.
:type channel: int
.. note ::
The Talon uses the following bounds for PWM values. These values
should work reasonably well for most controllers, but if users
experience issues such as asymmetric behavior around the deadband
or inability to saturate the controller in either direction,
calibration is recommended. The calibration procedure can be
found in the VictorSP User Manual.
- 2.004ms = full "forward"
- 1.520ms = the "high end" of the deadband range
- 1.500ms = center of the deadband range (off)
- 1.480ms = the "low end" of the deadband range
- 0.997ms = full "reverse"
"""
super().__init__(channel)
self.setBounds(2.004, 1.52, 1.50, 1.48, .997)
self.setPeriodMultiplier(self.PeriodMultiplier.k1X)
# never call the raw functions
#self.setRaw(self.centerPwm)
self.setZeroLatch()
self.isInverted = False
LiveWindow.addActuatorChannel("VictorSP", self.getChannel(), self)
hal.HALReport(hal.HALUsageReporting.kResourceType_VictorSP,
self.getChannel())
def main(robot_cls):
"""Starting point for the applications."""
RobotBase.initializeHardwareConfiguration()
hal.HALReport(hal.HALUsageReporting.kResourceType_Language,
hal.HALUsageReporting.kLanguage_Python)
try:
robot = robot_cls()
robot.prestart()
except:
from .driverstation import DriverStation
DriverStation.reportError("ERROR Could not instantiate robot",
True)
logger.exception("Robots don't quit!")
logger.exception("Could not instantiate robot " +
robot_cls.__name__ + "!")
return False
# Add a check to see if the user forgot to call super().__init__()
if not hasattr(robot, '_RobotBase__initialized'):
logger.error(
"If your robot class has an __init__ function, it must call super().__init__()!"
)
return False
if not hal.HALIsSimulation():
try:
import wpilib
with open('/tmp/frc_versions/FRC_Lib_Version.ini', 'w') as fp:
fp.write('RobotPy %s' % wpilib.__version__)
except:
logger.warning("Could not write FRC version file to disk")
try:
robot.startCompetition()
except KeyboardInterrupt:
logger.exception(
"THIS IS NOT AN ERROR: The user hit CTRL-C to kill the robot")
logger.info("Exiting because of keyboard interrupt")
return True
except:
from .driverstation import DriverStation
DriverStation.reportError("ERROR Unhandled exception", True)
logger.warn("Robots don't quit!")
logger.exception(
"---> The startCompetition() method (or methods called by it) should have handled the exception."
)
return False
else:
# startCompetition never returns unless exception occurs....
from .driverstation import DriverStation
DriverStation.reportError("ERROR startCompetition() returned",
False)
logger.warn("Robots don't quit!")
logger.error(
"---> Unexpected return from startCompetition() method.")
return False
def __init__(self, *args, **kwargs):
"""Constructor.
Arguments can be supplied as positional or keyword. Acceptable
positional argument combinations are:
- channel
- moduleNumber, channel
Alternatively, the above names can be used as keyword arguments.
:param moduleNumber: The CAN ID of the PCM the solenoid is attached to
:type moduleNumber: int
:param channel: The channel on the PCM to control (0..7)
:type channel: int
"""
# keyword arguments
channel = kwargs.pop("channel", None)
moduleNumber = kwargs.pop("moduleNumber", None)
if kwargs:
warnings.warn("unknown keyword arguments: %s" % kwargs.keys(),
RuntimeWarning)
# positional arguments
if len(args) == 1:
channel = args[0]
elif len(args) == 2:
moduleNumber, channel = args
elif len(args) != 0:
raise ValueError(
"don't know how to handle %d positional arguments" % len(args))
if moduleNumber is None:
moduleNumber = SensorBase.getDefaultSolenoidModule()
if channel is None:
raise ValueError("must specify channel")
SensorBase.checkSolenoidModule(moduleNumber)
SensorBase.checkSolenoidChannel(channel)
super().__init__(moduleNumber)
self.channel = channel
try:
self.allocated.allocate(self, channel)
except IndexError as e:
raise IndexError(
"Solenoid channel %d on module %d is already allocated" %
(channel, moduleNumber)) from e
self.port = self.ports[channel]
LiveWindow.addActuatorModuleChannel("Solenoid", moduleNumber, channel,
self)
hal.HALReport(hal.HALUsageReporting.kResourceType_Solenoid, channel,
moduleNumber)
def __init__(self, range=Accelerometer.Range.k8G):
"""Constructor.
:param range: The range the accelerometer will measure. Defaults to
+/-8g if unspecified.
:type range: :class:`.Accelerometer.Range`
"""
self.setRange(range)
hal.HALReport(hal.HALUsageReporting.kResourceType_Accelerometer, 0, 0,
"Built-in accelerometer")
LiveWindow.addSensor("BuiltInAccel", 0, self)
def __init__(self, channel):
"""Create an instance of a digital output.
:param channel: the DIO channel for the digital output. 0-9 are on-board, 10-25 are on the MXP
"""
super().__init__(channel, False)
self._pwmGenerator = None
self._pwmGenerator_finalizer = None
hal.HALReport(hal.HALUsageReporting.kResourceType_DigitalOutput,
channel)
def __init__(self, port):
'''
:param port: The port on the driver station that the controller is
plugged into.
:type port: int
'''
self.ds = wpilib.DriverStation.getInstance()
self.port = port
hal.HALReport(hal.HALUsageReporting.kResourceType_Joystick, port)
def _initRelay(self):
SensorBase.checkRelayChannel(self.channel)
try:
if (self.direction == self.Direction.kBoth or
self.direction == self.Direction.kForward):
Relay.relayChannels.allocate(self, self.channel * 2)
hal.HALReport(hal.HALUsageReporting.kResourceType_Relay,
self.channel)
if (self.direction == self.Direction.kBoth or
self.direction == self.Direction.kReverse):
Relay.relayChannels.allocate(self, self.channel * 2 + 1)
hal.HALReport(hal.HALUsageReporting.kResourceType_Relay,
self.channel + 128)
except IndexError as e:
raise IndexError("Relay channel %d is already allocated" % self.channel) from e
self._port = hal.initializeDigitalPort(hal.getPort(self.channel))
self._port_finalizer = weakref.finalize(self, _freeRelay, self._port)
self.setSafetyEnabled(False)
def __init__(self, channel):
"""Create an instance of a Digital Input class. Creates a digital
input given a channel.
:param channel: the DIO channel for the digital input. 0-9 are on-board, 10-25 are on the MXP
:type channel: int
"""
super().__init__(channel, True)
hal.HALReport(hal.HALUsageReporting.kResourceType_DigitalInput,
channel)
LiveWindow.addSensorChannel("DigitalInput", channel, self)
def __init__(self, channel, directionSensitive=False):
"""Construct a GearTooth sensor.
:param channel: The DIO channel index or DigitalSource that the sensor
is connected to.
:type channel: int
:param directionSensitive: True to enable the pulse length decoding in
hardware to specify count direction. Defaults to False.
:type directionSensitive: bool
"""
super().__init__(channel)
self.enableDirectionSensing(directionSensitive)
if hasattr(self.upSource, "getChannel"):
if directionSensitive:
hal.HALReport(hal.HALUsageReporting.kResourceType_GearTooth,
self.upSource.getChannel(), 0, "D")
else:
hal.HALReport(hal.HALUsageReporting.kResourceType_GearTooth,
self.upSource.getChannel(), 0)
LiveWindow.addSensorChannel("GearTooth", self.upSource.getChannel(),
self)
def startCompetition(self):
"""Start a competition.
This code tracks the order of the field starting to ensure that
everything happens in the right order. Repeatedly run the correct
method, either Autonomous or OperatorControl when the robot is
enabled. After running the correct method, wait for some state to
change, either the other mode starts or the robot is disabled. Then
go back and wait for the robot to be enabled again.
"""
hal.HALReport(hal.HALUsageReporting.kResourceType_Framework,
hal.HALUsageReporting.kFramework_Simple)
self.robotMain()
if hasattr(self, '_no_robot_main'):
# first and one-time initialization
LiveWindow.setEnabled(False)
self.robotInit()
#We call this now (not in prestart like default) so that the robot
#won't enable until the initialization has finished. This is useful
#because otherwise it's sometimes possible to enable the robot before
#the code is ready.
hal.HALNetworkCommunicationObserveUserProgramStarting()
while True:
if self.isDisabled():
self.ds.InDisabled(True)
self.disabled()
self.ds.InDisabled(False)
while self.isDisabled():
Timer.delay(0.01)
elif self.isAutonomous():
self.ds.InAutonomous(True)
self.autonomous()
self.ds.InAutonomous(False)
while self.isAutonomous() and not self.isDisabled():
Timer.delay(0.01)
elif self.isTest():
LiveWindow.setEnabled(True)
self.ds.InTest(True)
self.test()
self.ds.InTest(False)
while self.isTest() and self.isEnabled():
Timer.delay(0.01)
LiveWindow.setEnabled(False)
else:
self.ds.InOperatorControl(True)
self.operatorControl()
self.ds.InOperatorControl(False)
while self.isOperatorControl() and not self.isDisabled():
Timer.delay(0.01)
def __init__(self, port, deviceAddress):
"""Constructor.
:param port: The I2C port the device is connected to.
:param deviceAddress: The address of the device on the I2C bus.
"""
self.port = port
self.deviceAddress = deviceAddress
hal.i2CInitialize(self.port)
self._i2c_finalizer = weakref.finalize(self, _freeI2C, self.port)
hal.HALReport(hal.HALUsageReporting.kResourceType_I2C, deviceAddress)
def __init__(self):
self.channelIndex = -1
with self.mutex:
for i, v in enumerate(self.filterAllocated):
if not v:
self.channelIndex = i
self.filterAllocated[i] = True
hal.HALReport(
hal.HALUsageReporting.kResourceType_DigitalFilter,
self.channelIndex, 0)
break
else:
raise ValueError("No more filters available")
def __init__(self, pingChannel, echoChannel, units=Unit.kInches):
"""Create an instance of the Ultrasonic Sensor.
This is designed to supchannel the Daventech SRF04 and Vex ultrasonic
sensors.
:param pingChannel: The digital output channel that sends the pulse
to initiate the sensor sending the ping.
:param echoChannel: The digital input channel that receives the echo.
The length of time that the echo is high represents the round
trip time of the ping, and the distance.
:param units: The units returned in either kInches or kMillimeters
"""
# Convert to DigitalInput and DigitalOutput if necessary
self.pingAllocated = False
self.echoAllocated = False
if not hasattr(pingChannel, 'channel'):
from .digitaloutput import DigitalOutput
pingChannel = DigitalOutput(pingChannel)
self.pingAllocated = True
if not hasattr(echoChannel, 'channel'):
from .digitalinput import DigitalInput
echoChannel = DigitalInput(echoChannel)
self.echoAllocated = True
self.pingChannel = pingChannel
self.echoChannel = echoChannel
self.units = units
self.pidSource = self.PIDSourceType.kDisplacement
self.enabled = True # make it available for round robin scheduling
# set up counter for this sensor
self.counter = Counter(self.echoChannel)
self.counter.setMaxPeriod(1.0)
self.counter.setSemiPeriodMode(True)
self.counter.reset()
isAutomatic = Ultrasonic.isAutomaticMode()
self.setAutomaticMode(False)
Ultrasonic.sensors.add(self)
if isAutomatic:
self.setAutomaticMode(True)
Resource._add_global_resource(self)
Ultrasonic.instances += 1
hal.HALReport(hal.HALUsageReporting.kResourceType_Ultrasonic,
Ultrasonic.instances)
LiveWindow.addSensor("Ultrasonic", self.echoChannel.getChannel(), self)
def mecanumDrive_Cartesian(self, x, y, rotation, gyroAngle):
"""Drive method for Mecanum wheeled robots.
A method for driving with Mecanum wheeled robots. There are 4 wheels
on the robot, arranged so that the front and back wheels are toed in
45 degrees. When looking at the wheels from the top, the roller
axles should form an X across the robot.
This is designed to be directly driven by joystick axes.
:param x: The speed that the robot should drive in the X direction.
[-1.0..1.0]
:param y: The speed that the robot should drive in the Y direction.
This input is inverted to match the forward == -1.0 that
joysticks produce. [-1.0..1.0]
:param rotation: The rate of rotation for the robot that is
completely independent of the translation. [-1.0..1.0]
:param gyroAngle: The current angle reading from the gyro. Use this
to implement field-oriented controls.
"""
if not RobotDrive.kMecanumCartesian_Reported:
hal.HALReport(hal.HALUsageReporting.kResourceType_RobotDrive,
self.getNumMotors(),
hal.HALUsageReporting.kRobotDrive_MecanumCartesian)
RobotDrive.kMecanumCartesian_Reported = True
xIn = x
yIn = y
# Negate y for the joystick.
yIn = -yIn
# Compenstate for gyro angle.
xIn, yIn = RobotDrive.rotateVector(xIn, yIn, gyroAngle)
wheelSpeeds = [0]*self.kMaxNumberOfMotors
wheelSpeeds[self.MotorType.kFrontLeft] = xIn + yIn + rotation
wheelSpeeds[self.MotorType.kFrontRight] = -xIn + yIn - rotation
wheelSpeeds[self.MotorType.kRearLeft] = -xIn + yIn + rotation
wheelSpeeds[self.MotorType.kRearRight] = xIn + yIn - rotation
RobotDrive.normalize(wheelSpeeds)
self.frontLeftMotor.set(wheelSpeeds[self.MotorType.kFrontLeft] * self.maxOutput, self.syncGroup)
self.frontRightMotor.set(wheelSpeeds[self.MotorType.kFrontRight] * self.maxOutput, self.syncGroup)
self.rearLeftMotor.set(wheelSpeeds[self.MotorType.kRearLeft] * self.maxOutput, self.syncGroup)
self.rearRightMotor.set(wheelSpeeds[self.MotorType.kRearRight] * self.maxOutput, self.syncGroup)
if self.syncGroup != 0:
CANJaguar.updateSyncGroup(self.syncGroup)
self.feed()
def __init__(self, channel):
"""Constructor.
:param channel: The PWM channel that the Victor is attached to. 0-9 are on-board, 10-19 are on the MXP port
:type channel: int
"""
super().__init__(channel)
self.setBounds(2.027, 1.525, 1.507, 1.49, 1.026)
self.setPeriodMultiplier(self.PeriodMultiplier.k2X)
self.setRaw(self.centerPwm)
self.setZeroLatch()
LiveWindow.addActuatorChannel("Victor", self.getChannel(), self)
hal.HALReport(hal.HALUsageReporting.kResourceType_Victor,
self.getChannel())
def __init__(self, channel):
"""Create a new instance of Accelerometer from either an existing
AnalogChannel or from an analog channel port index.
:param channel: port index or an already initialized :class:`.AnalogInput`
"""
if not hasattr(channel, "getAverageVoltage"):
channel = AnalogInput(channel)
self.analogChannel = channel
self.voltsPerG = 1.0
self.zeroGVoltage = 2.5
hal.HALReport(hal.HALUsageReporting.kResourceType_Accelerometer,
self.analogChannel.getChannel())
LiveWindow.addSensorChannel("Accelerometer",
self.analogChannel.getChannel(), self)
def __init__(self, trigger, outputType):
"""Create an object that represents one of the four outputs from an
analog trigger.
Because this class derives from DigitalSource, it can be passed into
routing functions for Counter, Encoder, etc.
:param trigger: The trigger for which this is an output.
:param outputType: An enum that specifies the output on the trigger
to represent.
"""
self.trigger = trigger
self.outputType = outputType
hal.HALReport(hal.HALUsageReporting.kResourceType_AnalogTriggerOutput,
trigger.index, outputType)
def __init__(self, port, range):
"""Constructor.
:param port: The I2C port the accelerometer is attached to.
:param range: The range (+ or -) that the accelerometer will measure.
"""
self.i2c = I2C(port, self.kAddress)
# Turn on the measurements
self.i2c.write(self.kPowerCtlRegister, self.kPowerCtl_Measure)
self.setRange(range)
hal.HALReport(hal.HALUsageReporting.kResourceType_ADXL345,
hal.HALUsageReporting.kADXL345_I2C)
LiveWindow.addSensor("ADXL345_I2C", port, self)
def drive(self, outputMagnitude, curve):
"""Drive the motors at "outputMagnitude" and "curve".
Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0
represents stopped and not turning. ``curve < 0`` will turn left and ``curve > 0``
will turn right.
The algorithm for steering provides a constant turn radius for any normal
speed range, both forward and backward. Increasing m_sensitivity causes
sharper turns for fixed values of curve.
This function will most likely be used in an autonomous routine.
:param outputMagnitude: The speed setting for the outside wheel in a turn,
forward or backwards, +1 to -1.
:param curve: The rate of turn, constant for different forward speeds. Set
``curve < 0`` for left turn or ``curve > 0`` for right turn.
Set ``curve = e^(-r/w)`` to get a turn radius r for wheelbase w of your robot.
Conversely, turn radius r = -ln(curve)*w for a given value of curve and
wheelbase w.
"""
if not RobotDrive.kArcadeRatioCurve_Reported:
hal.HALReport(hal.HALUsageReporting.kResourceType_RobotDrive,
self.getNumMotors(),
hal.HALUsageReporting.kRobotDrive_ArcadeRatioCurve)
RobotDrive.kArcadeRatioCurve_Reported = True
if curve < 0:
value = math.log(-curve)
ratio = (value - self.sensitivity) / (value + self.sensitivity)
if ratio == 0:
ratio = .0000000001
leftOutput = outputMagnitude / ratio
rightOutput = outputMagnitude
elif curve > 0:
value = math.log(curve)
ratio = (value - self.sensitivity) / (value + self.sensitivity)
if ratio == 0:
ratio = .0000000001
leftOutput = outputMagnitude
rightOutput = outputMagnitude / ratio
else:
leftOutput = outputMagnitude
rightOutput = outputMagnitude
self.setLeftRightMotorOutputs(leftOutput, rightOutput)
def __init__(self, channel):
"""Constructor.
* By default `kDefaultMaxServoPWM` ms is used as the maxPWM value
* By default `kDefaultMinServoPWM` ms is used as the minPWM value
:param channel: The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on the MXP port.
:type channel: int
"""
super().__init__(channel)
self.setBounds(self.kDefaultMaxServoPWM, 0, 0, 0,
self.kDefaultMinServoPWM)
self.setPeriodMultiplier(self.PeriodMultiplier.k4X)
LiveWindow.addActuatorChannel("Servo", self.getChannel(), self)
hal.HALReport(hal.HALUsageReporting.kResourceType_Servo,
self.getChannel())
def mecanumDrive_Polar(self, magnitude, direction, rotation):
"""Drive method for Mecanum wheeled robots.
A method for driving with Mecanum wheeled robots. There are 4 wheels
on the robot, arranged so that the front and back wheels are toed in
45 degrees. When looking at the wheels from the top, the roller
axles should form an X across the robot.
:param magnitude: The speed that the robot should drive in a given
direction.
:param direction: The direction the robot should drive in degrees.
The direction and maginitute are independent of the rotation rate.
:param rotation: The rate of rotation for the robot that is completely
independent of the magnitute or direction. [-1.0..1.0]
"""
if not RobotDrive.kMecanumPolar_Reported:
hal.HALReport(hal.HALUsageReporting.kResourceType_RobotDrive,
self.getNumMotors(),
hal.HALUsageReporting.kRobotDrive_MecanumPolar)
RobotDrive.kMecanumPolar_Reported = True
# Normalized for full power along the Cartesian axes.
magnitude = RobotDrive.limit(magnitude) * math.sqrt(2.0)
# The rollers are at 45 degree angles.
dirInRad = math.radians(direction + 45.0)
cosD = math.cos(dirInRad)
sinD = math.sin(dirInRad)
wheelSpeeds = [0]*self.kMaxNumberOfMotors
wheelSpeeds[self.MotorType.kFrontLeft] = sinD * magnitude + rotation
wheelSpeeds[self.MotorType.kFrontRight] = cosD * magnitude - rotation
wheelSpeeds[self.MotorType.kRearLeft] = cosD * magnitude + rotation
wheelSpeeds[self.MotorType.kRearRight] = sinD * magnitude - rotation
RobotDrive.normalize(wheelSpeeds)
self.frontLeftMotor.set(wheelSpeeds[self.MotorType.kFrontLeft] * self.invertedMotors[self.MotorType.kFrontLeft] * self.maxOutput, self.syncGroup)
self.frontRightMotor.set(wheelSpeeds[self.MotorType.kFrontRight] * self.invertedMotors[self.MotorType.kFrontRight] * self.maxOutput, self.syncGroup)
self.rearLeftMotor.set(wheelSpeeds[self.MotorType.kRearLeft] * self.invertedMotors[self.MotorType.kRearLeft] * self.maxOutput, self.syncGroup)
self.rearRightMotor.set(wheelSpeeds[self.MotorType.kRearRight] * self.invertedMotors[self.MotorType.kRearRight] * self.maxOutput, self.syncGroup)
if self.syncGroup != 0:
CANJaguar.updateSyncGroup(self.syncGroup)
self.feed()
def __init__(self):
"""Instantiates a Scheduler.
"""
hal.HALReport(hal.HALUsageReporting.kResourceType_Command,
hal.HALUsageReporting.kCommand_Scheduler)
# Active Commands
self.commandTable = collections.OrderedDict()
# The set of all Subsystems
self.subsystems = set()
# Whether or not we are currently adding a command
self.adding = False
# Whether or not we are currently disabled
self.disabled = False
# A list of all Commands which need to be added
self.additions = []
# A list of all Buttons. It is created lazily.
self.buttons = []
self.runningCommandsChanged = False
def __init__(self, port, simPort=None):
"""Constructor
:param port: the physical SPI port
:type port: :class:`.SPI.Port`
:param simPort: This must be an object that implements all of
the spi* functions from hal_impl that you use.
See ``test_spi.py`` for an example.
"""
if port not in [
self.Port.kOnboardCS0, self.Port.kOnboardCS1,
self.Port.kOnboardCS2, self.Port.kOnboardCS3, self.Port.kMXP
]:
raise ValueError("Invalid value '%s' for SPI port" % port)
if hal.HALIsSimulation():
if simPort is None:
# If you want more functionality, implement your own mock
from hal_impl.spi_helpers import SPISimBase
simPort = SPISimBase()
msg = "Using stub simulator for SPI port %s" % port
warnings.warn(msg)
# Just check for basic functionality
assert hasattr(simPort, 'spiInitialize')
assert hasattr(simPort, 'spiClose')
self._port = (simPort, port)
else:
self._port = port
self.bitOrder = 0
self.clockPolarity = 0
self.dataOnTrailing = 0
hal.spiInitialize(self._port)
self.__finalizer = weakref.finalize(self, _freeSPI, self._port)
SPI.devices += 1
hal.HALReport(hal.HALUsageReporting.kResourceType_SPI, SPI.devices)
