def robotInit(self):
"""Robot-wide initialization code should go here"""
self.lstick = wpilib.Joystick(0)
self.rstick = wpilib.Joystick(1)
self.l_motor = wpilib.Jaguar(1)
self.r_motor = wpilib.Jaguar(2)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(0)
self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)
self.motor = wpilib.Jaguar(4)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
self.left_encoder = wpilib.Encoder(1, 2)
self.right_encoder = wpilib.Encoder(3, 4)
self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
self.chassis_speeds = ChassisSpeeds()
self.chassis_speeds.vx = 0.0
self.chassis_speeds.omega = 0.0
if is_sim:
self.physics = physics.PhysicsEngine()
self.last_tm = time.time()
def __init__(self, kS, kV, trackwidth, trajectory):
'''
Creates a controller for following a PathWeaver trajectory.
__init__(self, kS: Volts, kV: Volts * Seconds / Meters, trackwidth: Meters, trajectory: wpilib.trajectory.Trajectory)
:param kS: The kS gain determined by characterizing the Robot's drivetrain
:param kV: The kV gain determined by characterizing the Robot's drivetrain
:param trackwidth: The horizontal distance between the left and right wheels of the tank drive.
:param trajectory: The trajectory to follow. This can be generated by PathWeaver, or made by hand.
'''
self.kS = kS
self.kV = kV
self.trajectory = trajectory
self.odometry = DifferentialDriveOdometry(
Rotation2d(radians(0)), self.trajectory.initialPose())
self.ramsete = RamseteController(2, 0.7)
self.drive_kinematics = DifferentialDriveKinematics(trackwidth)
self.are_wheel_speeds_zero = False
self.timer = Timer()
def __init__(
self,
x_wheelbase: units.Quantity = 2 * units.feet,
speed: units.Quantity = 5 * units.fps,
deadzone: typing.Optional[DeadzoneCallable] = None,
):
"""
:param x_wheelbase: The distance between right and left wheels.
:param speed: Speed of robot (see above)
:param deadzone: A function that adjusts the output of the motor (see :func:`linear_deadzone`)
"""
trackwidth = units.meters.m_from(x_wheelbase, name="x_wheelbase")
self.kinematics = DifferentialDriveKinematics(trackwidth)
self.speed = units.mps.m_from(speed, name="speed")
self.wheelSpeeds = DifferentialDriveWheelSpeeds()
self.deadzone = deadzone
def setup(self) -> None:
self.left_front.setInverted(False)
self.right_front.setInverted(True)
for motor in (
self.left_front,
self.left_rear,
self.right_front,
self.right_rear,
):
motor.setIdleMode(rev.IdleMode.kCoast)
self.left_rear.follow(self.left_front)
self.right_rear.follow(self.right_front)
self.left_encoder = rev.CANEncoder(self.left_front)
self.right_encoder = rev.CANEncoder(self.right_front)
rev_to_m = WHEEL_CIRCUMFERENCE / GEAR_RATIO
for enc in (self.left_encoder, self.right_encoder):
enc.setPositionConversionFactor(rev_to_m)
enc.setVelocityConversionFactor(rev_to_m / 60)
self.left_pid: rev.CANPIDController = self.left_front.getPIDController(
)
self.right_pid: rev.CANPIDController = self.right_front.getPIDController(
)
self.ff_calculator = SimpleMotorFeedforwardMeters(kS=0.194,
kV=2.79,
kA=0.457)
for pid in (self.left_pid, self.right_pid):
# TODO: needs tuning
pid.setP(5.19 / 10)
pid.setI(0)
pid.setD(0)
pid.setIZone(0)
pid.setFF(0)
pid.setOutputRange(-1, 1)
self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
self.odometry = DifferentialDriveOdometry(self._get_heading())
KV = 1.96 # volts * seconds / distance
KA = 0.528 # volts * seconds^2 / distance
TRACK_WIDTH = 0.481 # Units: meters
MAX_GENERATION_VELOCITY = 2.3 # Units: m/s
MAX_GENERATION_VOLTAGE = 5 # Units: volts
TRAJECTORY_DIRECTORY = 'trajectories'
PICKLE_FILE_NAME = 'trajectories.pickle'
PICKLE_FILE = path.join(path.dirname(sys.modules['__main__'].__file__),
TRAJECTORY_DIRECTORY, PICKLE_FILE_NAME)
# Unimportant due to DifferentialDriveVoltageConstraint
MAX_GENERATION_ACCELERATION = 1.5 # Units: m/s^2.
DRIVE_FEEDFORWARD = SimpleMotorFeedforwardMeters(KS, KV, KA)
KINEMATICS = DifferentialDriveKinematics(TRACK_WIDTH)
DEFAULT_CONSTRAINTS: Tuple[TrajectoryConstraint,
...] = (DifferentialDriveKinematicsConstraint(
KINEMATICS, MAX_GENERATION_VELOCITY),
DifferentialDriveVoltageConstraint(
DRIVE_FEEDFORWARD, KINEMATICS,
MAX_GENERATION_VOLTAGE))
@dataclass
class TrajectoryData:
"""Hold metadata for a wpilib trajectory"""
start: Pose2d
interior_waypoints: List[Translation2d]
end: Pose2d
config: TrajectoryConfig = TrajectoryConfig(MAX_GENERATION_VELOCITY,
class Chassis:
left_rear: rev.CANSparkMax
left_front: rev.CANSparkMax
right_rear: rev.CANSparkMax
right_front: rev.CANSparkMax
imu: NavX
open_loop = magicbot.tunable(False)
vx = magicbot.will_reset_to(0.0)
vz = magicbot.will_reset_to(0.0)
def setup(self) -> None:
self.left_front.setInverted(False)
self.right_front.setInverted(True)
self.disable_brake_mode()
self.left_rear.follow(self.left_front)
self.right_rear.follow(self.right_front)
self.left_encoder = rev.CANEncoder(self.left_front)
self.right_encoder = rev.CANEncoder(self.right_front)
rev_to_m = WHEEL_CIRCUMFERENCE / GEAR_RATIO
for enc in (self.left_encoder, self.right_encoder):
enc.setPositionConversionFactor(rev_to_m)
enc.setVelocityConversionFactor(rev_to_m / 60)
self.left_pid: rev.CANPIDController = self.left_front.getPIDController(
)
self.right_pid: rev.CANPIDController = self.right_front.getPIDController(
)
self.ff_calculator = SimpleMotorFeedforwardMeters(kS=0.194,
kV=2.79,
kA=0.457)
for pid in (self.left_pid, self.right_pid):
# TODO: needs tuning
pid.setP(5.19 / 10)
pid.setI(0)
pid.setD(0)
pid.setIZone(0)
pid.setFF(0)
pid.setOutputRange(-1, 1)
self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
self.odometry = DifferentialDriveOdometry(self._get_heading())
# default_position on the field
self.reset_odometry(Pose2d(-3, 0, Rotation2d(math.pi)))
def execute(self) -> None:
# XXX: https://github.com/robotpy/robotpy-wpilib/issues/635
chassis_speeds = ChassisSpeeds()
chassis_speeds.vx = self.vx
chassis_speeds.omega = self.vz
speeds = self.kinematics.toWheelSpeeds(chassis_speeds)
left_ff = self.ff_calculator.calculate(speeds.left)
right_ff = self.ff_calculator.calculate(speeds.right)
if self.open_loop:
self.left_front.setVoltage(left_ff)
self.right_front.setVoltage(right_ff)
else:
self.left_pid.setReference(speeds.left,
rev.ControlType.kVelocity,
arbFeedforward=left_ff)
self.right_pid.setReference(speeds.right,
rev.ControlType.kVelocity,
arbFeedforward=right_ff)
self.odometry.update(
self._get_heading(),
self.left_encoder.getPosition(),
self.right_encoder.getPosition(),
)
def drive(self, vx: float, vz: float) -> None:
"""Sets the desired robot velocity.
Arguments:
vx: Forwards linear velocity in m/s.
vz: Angular velocity in rad/s, anticlockwise positive.
"""
self.vx = vx
self.vz = vz
def disable_closed_loop(self) -> None:
"""Run the drivetrain in open loop mode (feedforward only)."""
self.open_loop = True
def enable_closed_loop(self) -> None:
"""Run the drivetrain in velocity closed loop mode."""
self.open_loop = False
def enable_brake_mode(self) -> None:
for motor in (
self.left_front,
self.left_rear,
self.right_front,
self.right_rear,
):
motor.setIdleMode(rev.IdleMode.kBrake)
def disable_brake_mode(self) -> None:
for motor in (
self.left_front,
self.left_rear,
self.right_front,
self.right_rear,
):
motor.setIdleMode(rev.IdleMode.kCoast)
def _get_heading(self) -> Rotation2d:
"""Get the current heading of the robot from the IMU, anticlockwise positive."""
return Rotation2d(self.imu.getYaw())
def get_pose(self) -> Pose2d:
"""Get the current position of the robot on the field."""
return self.odometry.getPose()
@magicbot.feedback
def get_heading(self) -> float:
"""Get the current heading of the robot."""
return self.get_pose().rotation().radians()
@magicbot.feedback
def get_left_velocity(self) -> float:
return self.left_encoder.getVelocity()
@magicbot.feedback
def get_right_velocity(self) -> float:
return self.right_encoder.getVelocity()
def reset_odometry(self, pose: Pose2d) -> None:
"""Resets the odometry to start with the given pose.
This is intended to be called at the start of autonomous.
"""
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
self.odometry.resetPosition(pose, self._get_heading())
def find_field_angle(self, field_point: Translation2d) -> float:
"""Return the theoretical angle (in radians) to the target based on odometry"""
pose = self.get_pose()
rel = field_point - pose.translation()
rel_heading = Rotation2d(rel.y, rel.x) + pose.rotation()
return rel_heading.radians()
def find_power_port_angle(self) -> float:
return self.find_field_angle(POWER_PORT_POSITION)
class MyRobot(wpilib.TimedRobot):
"""Main robot class"""
def robotInit(self):
"""Robot-wide initialization code should go here"""
self.lstick = wpilib.Joystick(0)
self.rstick = wpilib.Joystick(1)
self.l_motor = wpilib.Jaguar(1)
self.r_motor = wpilib.Jaguar(2)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(0)
self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)
self.motor = wpilib.Jaguar(4)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
self.left_encoder = wpilib.Encoder(1, 2)
self.right_encoder = wpilib.Encoder(3, 4)
self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
self.chassis_speeds = ChassisSpeeds()
self.chassis_speeds.vx = 0.0
self.chassis_speeds.omega = 0.0
if is_sim:
self.physics = physics.PhysicsEngine()
self.last_tm = time.time()
if is_sim:
# TODO: this needs to be builtin
def robotPeriodic(self):
now = time.time()
tm_diff = now - self.last_tm
self.last_tm = now
self.physics.update_sim(now, tm_diff)
def autonomousInit(self):
"""Called when autonomous mode is enabled"""
self.timer = wpilib.Timer()
self.timer.start()
def autonomousPeriodic(self):
# Get WheelSPeeds out of Inverse Kinematics...
# XXX: https://github.com/robotpy/robotpy-wpilib/issues/635
#speeds = self.kinematics.toWheelSpeeds(self.chassis_speeds)
# Uncomment to see wheel speed
# print(speeds.left, speeds.right)
# Get Chassis Speeds using Kinematics
#sampleWheelSpeeds = DifferentialDriveWheelSpeeds()
#sampleWheelSpeeds.left = 2.0
#sampleWheelSpeeds.right = 2.0
#speeds2 = self.kinematics.toChassisSpeeds(sampleWheelSpeeds)
#print(speeds2.vx, speeds2.omega)
if self.timer.get() < 2.0:
speeds = self.kinematics.toWheelSpeeds(self.chassis_speeds)
self.chassis_speeds.vx = -.5
self.drive.tankDrive(speeds.left, speeds.right)
else:
self.drive.tankDrive(-.5, .5)
def teleopPeriodic(self):
"""Called when operation control mode is enabled"""
self.drive.arcadeDrive(self.lstick.getY(), self.lstick.getX())
# self.drive.arcadeDrive(self.lstick.getRawAxis(1), self.lstick.getRawAxis(3))
# Move a motor with a Joystick
y = self.rstick.getY()
# stop movement backwards when 1 is on
if self.limit1.get():
y = max(0, y)
# stop movement forwards when 2 is on
if self.limit2.get():
y = min(0, y)
self.motor.set(y)
class Path:
def __init__(self, kS, kV, trackwidth, trajectory):
'''
Creates a controller for following a PathWeaver trajectory.
__init__(self, kS: Volts, kV: Volts * Seconds / Meters, trackwidth: Meters, trajectory: wpilib.trajectory.Trajectory)
:param kS: The kS gain determined by characterizing the Robot's drivetrain
:param kV: The kV gain determined by characterizing the Robot's drivetrain
:param trackwidth: The horizontal distance between the left and right wheels of the tank drive.
:param trajectory: The trajectory to follow. This can be generated by PathWeaver, or made by hand.
'''
self.kS = kS
self.kV = kV
self.trajectory = trajectory
self.odometry = DifferentialDriveOdometry(
Rotation2d(radians(0)), self.trajectory.initialPose())
self.ramsete = RamseteController(2, 0.7)
self.drive_kinematics = DifferentialDriveKinematics(trackwidth)
self.are_wheel_speeds_zero = False
self.timer = Timer()
def is_done(self):
'''
is_done(self) -> bool
Returns whether or not the path is done.
'''
return self.are_wheel_speeds_zero
def reset(self, chassis, gyro):
'''
Re-initializes all of the data in the controller as if the path has not yet been executed.
This method MUST be called in teleopInit and autonomousInit directly before the controller is used.
reset(self, chassis: traits.DriveTrain, gyro: traits.Gyro)
:param chassis: An object that implements the DriveTrain trait. This object's encoders will be reset by this function
:param gyro: An object that implements the Gyro trait. This object's angle will be reset by this function
'''
# Assert the objects implement the proper traits
assert chassis.implements(DriveTrain)
assert gyro.implements(Gyro)
# The encoders and gyro need to be reset so that the Ramsete
# controller is fed data that looks new.
# By reseting these sensors, we look like weve never run the path at all!
chassis.reset_encoders()
gyro.reset()
self.are_wheel_speeds_zero = False
# The odometry object also needs to be re-initialized
# so that it forgets the state from the previous run
self.odometry = DifferentialDriveOdometry(
Rotation2d(radians(0)), self.trajectory.initialPose())
self.timer.start()
def follow(self, chassis, gyro):
'''
This updates the Path and drives the chassis to follow the path
update(self, chassis: traits.DriveTrain, gyro: traits.Gyro)
:param chassis: An object that implements the DriveTrain trait. When this function is called,
the object's motors will be driven to follow the last set trajectory.
:param gyro: An object that implements the gyro trait.
'''
# Assert the objects implement the proper traits
assert chassis.implements(DriveTrain)
assert gyro.implements(Gyro)
if self.is_done():
return
# Set the chassis to low gear for more precise movements
chassis.set_low_gear()
# If a trajectory has been set, run it
if self.trajectory is not None:
# Get the accumulated left and right distance of the chass
ld, rd = chassis.get_left_distance(), chassis.get_right_distance()
# Ramsete requires the counterclockwise angle of the Robot
angle = gyro.get_counterclockwise_degrees()
# Get the current position of the robot
current_pose = self.odometry.update(Rotation2d(radians(angle)), ld,
rd)
# Calculate the target position using the trajectory, and get the chassis wheel speeds
target_pose = self.trajectory.sample(self.timer.get())
chassis_speed = self.ramsete.calculate(current_pose, target_pose)
wheel_speeds = self.drive_kinematics.toWheelSpeeds(chassis_speed)
l, r = wheel_speeds.left, wheel_speeds.right
if abs(l) == 0 and abs(r) == 0:
self.are_wheel_speeds_zero = True
# Convert the left and right wheel speeds to volts using the characterized constants,
# and then convert those to percent values from -1 to 1
chassis.tank_drive((self.kS + l * self.kV) / 12,
(self.kS + r * self.kV) / 12)
class Chassis:
# chassis physical constants
BUMPER_WIDTH = 3.25 * units.meters_per_inch
ROBOT_WIDTH = 30 * units.meters_per_inch + BUMPER_WIDTH
ROBOT_LENGTH = 30 * units.meters_per_inch + BUMPER_WIDTH
ROBOT_MASS = 100 * units.kilograms_per_pound
TRACK_WIDTH = 24 * units.meters_per_inch
TRACK_RADIUS = TRACK_WIDTH / 2
WHEEL_DIAMETER = 6 * units.meters_per_inch
WHEEL_RADIUS = WHEEL_DIAMETER / 2
WHEEL_CIRCUMFERENCE = 2 * np.pi * WHEEL_RADIUS
GEAR_RATIO = (48 / 14) * (50 / 16) # 10.7142861
# conversions
RADIANS_PER_METER = (2 * np.pi * GEAR_RATIO) / WHEEL_CIRCUMFERENCE
METERS_PER_RADIAN = WHEEL_CIRCUMFERENCE / (2 * np.pi * GEAR_RATIO)
# motor config
LEFT_INVERTED = True
RIGHT_INVERTED = False
# motor coefs
KS = 0.149
KV = 2.4
KA = 0.234
# velocity pidf gains
VL_KP = 0.000363
VL_KI = 0
VL_KD = 0
VL_KF = 0
VR_KP = 0.000363
VR_KI = 0
VR_KD = 0
VR_KF = 0
# joystick
MAX_JOYSTICK_OUTPUT = 1
# static objeccts
feedforward_l = motorfeedforward.MotorFeedforward(KS, KV, KA)
feedforward_r = motorfeedforward.MotorFeedforward(KS, KV, KA)
kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
# required devices
dm_l: lazytalonfx.LazyTalonFX
dm_r: lazytalonfx.LazyTalonFX
ds_l: lazytalonfx.LazyTalonFX
ds_r: lazytalonfx.LazyTalonFX
imu: lazypigeonimu.LazyPigeonIMU
# constraints
MAX_VELOCITY = 3
class _Mode(Enum):
Idle = 0
PercentOutput = 1
Velocity = 2
def __init__(self):
self.mode = self._Mode.Idle
self.odometry = DifferentialDriveOdometry(Rotation2d.fromDegrees(0))
self.desired_output = WheelState()
self.desired_velocity = WheelState()
self.feedforward = WheelState()
self.wheel_left = motorstate.MotorState()
self.wheel_right = motorstate.MotorState()
self.nt = NetworkTables.getTable(f"/components/chassis")
def setup(self):
self.dm_l.setInverted(self.LEFT_INVERTED)
self.dm_r.setInverted(self.RIGHT_INVERTED)
self.dm_l.setRadiansPerUnit(self.RADIANS_PER_METER)
self.dm_r.setRadiansPerUnit(self.RADIANS_PER_METER)
self.dm_l.setPIDF(
0,
self.VL_KP,
self.VL_KI,
self.VL_KD,
self.VL_KF,
)
self.dm_r.setPIDF(
0,
self.VR_KP,
self.VR_KI,
self.VR_KD,
self.VR_KF,
)
def on_enable(self):
pass
def on_disable(self):
self.stop()
def stop(self) -> None:
self.mode = self._Mode.Idle
if wpilib.RobotBase.isSimulation():
self._setSimulationOutput(1, 0)
self._setSimulationOutput(3, 0)
def setOutput(self, output_l: float, output_r: float) -> None:
self.mode = self._Mode.PercentOutput
self.desired_output.left = output_l
self.desired_output.right = output_r
def setWheelVelocity(self, velocity_l: float, velocity_r: float) -> None:
self.mode = self._Mode.Velocity
self.desired_velocity.left = velocity_l
self.desired_velocity.right = velocity_r
def setChassisVelocity(self, velocity_x: float, velocity_y: float,
velocity_omega) -> None:
state = ChassisSpeeds(velocity_x, velocity_y, -velocity_omega)
velocity = self.kinematics.toWheelSpeeds(state)
self.setWheelVelocity(velocity.left, velocity.right)
def setTankDrive(self, throttle, rotation):
self.mode = self._Mode.PercentOutput
self.desired_output.left = throttle + rotation
self.desired_output.right = throttle - rotation
# self.desired_output.norm(self.MAX_JOYSTICK_OUTPUT)
def setForzaDrive(self, throttle, reverse, rotation):
if throttle <= 0:
throttle = -reverse
self.setTankDrive(throttle, rotation)
def ntPutLeftRight(self, key, value):
self.nt.putNumber(f"{key}_left", value.left)
self.nt.putNumber(f"{key}_right", value.right)
def updateNetworkTables(self):
"""Update network table values related to component."""
self.wheel_left.putNT(self.nt, "wheel_left")
self.wheel_right.putNT(self.nt, "wheel_right")
self.ntPutLeftRight("desired_output", self.desired_output)
self.ntPutLeftRight("desired_velocity", self.desired_velocity)
self.ntPutLeftRight("feedforward", self.feedforward)
def getHeading(self):
return self.getPose().rotation().radians()
def getPose(self):
if wpilib.RobotBase.isSimulation():
x = hal.simulation.SimDeviceSim("Field2D").getDouble("x").get()
y = hal.simulation.SimDeviceSim("Field2D").getDouble("y").get()
rot = hal.simulation.SimDeviceSim("Field2D").getDouble("rot").get()
rot = (units.angle_range(rot * units.radians_per_degree) *
units.degrees_per_radian)
return Pose2d(x, y, Rotation2d.fromDegrees(rot))
else:
return self.odometry.getPose()
def _setSimulationOutput(self, id, output):
hal.simulation.SimDeviceSim(f"Talon FX[{id}]").getDouble(
"Motor Output").set(output)
def _getSimulationPosition(self, id):
return (hal.simulation.SimDeviceSim(
f"Custom Talon FX[{id}]").getDouble("Position").get())
def execute(self):
dt = 0.02
self.wheel_left.update(self.dm_l.getPosition(), dt)
self.wheel_right.update(self.dm_r.getPosition(), dt)
if wpilib.RobotBase.isSimulation():
self.wheel_left.position = self._getSimulationPosition(1)
self.wheel_right.position = self._getSimulationPosition(3)
self.odometry.update(
Rotation2d(self.getHeading()),
self.wheel_left.position,
self.wheel_right.position,
)
if self.mode == self._Mode.Idle:
self.dm_l.setOutput(0)
self.dm_r.setOutput(0)
elif self.mode == self._Mode.PercentOutput:
self.dm_l.setOutput(self.desired_output.left)
self.dm_r.setOutput(self.desired_output.right)
elif self.mode == self._Mode.Velocity:
self.feedforward.left = (
self.feedforward_l.calculate(self.desired_velocity.left, dt) /
12)
self.feedforward.right = (
self.feedforward_r.calculate(self.desired_velocity.right, dt) /
12)
if not wpilib.RobotBase.isSimulation():
self.dm_l.setVelocity(
self.desired_velocity.left,
self.feedforward.left,
)
self.dm_r.setVelocity(
self.desired_velocity.right,
self.feedforward.right,
)
else:
self.dm_l.setOutput(self.desired_velocity.left /
self.MAX_VELOCITY)
self.dm_r.setOutput(self.desired_velocity.right /
self.MAX_VELOCITY)
self.updateNetworkTables()
class FourMotorDrivetrain:
"""
Four motors, each side chained together. The motion equations are
as follows::
FWD = (L+R)/2
RCCW = (R-L)/W
* L is forward speed of the left wheel(s), all in sync
* R is forward speed of the right wheel(s), all in sync
* W is wheelbase in feet
.. note:: :class:`wpilib.drive.DifferentialDrive` assumes that to make
the robot go forward, the left motors must be set to 1, and
the right to -1
.. versionadded:: 2018.2.0
"""
#: Wheel speeds you can use for encoder calculations (updated by calculate)
wheelSpeeds: DifferentialDriveWheelSpeeds
def __init__(
self,
x_wheelbase: units.Quantity = 2 * units.feet,
speed: units.Quantity = 5 * units.fps,
deadzone: typing.Optional[DeadzoneCallable] = None,
):
"""
:param x_wheelbase: The distance between right and left wheels.
:param speed: Speed of robot (see above)
:param deadzone: A function that adjusts the output of the motor (see :func:`linear_deadzone`)
"""
trackwidth = units.meters.m_from(x_wheelbase, name="x_wheelbase")
self.kinematics = DifferentialDriveKinematics(trackwidth)
self.speed = units.mps.m_from(speed, name="speed")
self.wheelSpeeds = DifferentialDriveWheelSpeeds()
self.deadzone = deadzone
def calculate(self, lf_motor: float, lr_motor: float, rf_motor: float,
rr_motor: float) -> ChassisSpeeds:
"""
Given motor values, computes resulting chassis speeds of robot
:param lf_motor: Left front motor value (-1 to 1); 1 is forward
:param lr_motor: Left rear motor value (-1 to 1); 1 is forward
:param rf_motor: Right front motor value (-1 to 1); -1 is forward
:param rr_motor: Right rear motor value (-1 to 1); -1 is forward
:returns: ChassisSpeeds that can be passed to 'drive'
.. versionadded:: 2020.1.0
"""
if self.deadzone:
lf_motor = self.deadzone(lf_motor)
lr_motor = self.deadzone(lr_motor)
rf_motor = self.deadzone(rf_motor)
rr_motor = self.deadzone(rr_motor)
l = (lf_motor + lr_motor) * 0.5 * self.speed
r = -(rf_motor + rr_motor) * 0.5 * self.speed
self.wheelSpeeds.left = l
self.wheelSpeeds.right = r
return self.kinematics.toChassisSpeeds(self.wheelSpeeds)