"""
.. warning:: These drivetrain models are not particularly realistic, and
if you are using a tank drive style drivetrain you should use
the :class:`.TankModel` instead.
Based on input from various drive motors, these helper functions
simulate moving the robot in various ways. Many thanks to
`Ether <http://www.chiefdelphi.com/forums/member.php?u=34863>`_
for assistance with the motion equations.
When specifying the robot speed to the below functions, the following
may help you determine the approximate speed of your robot:
* Slow: 4ft/s
* Typical: 5 to 7ft/s
* Fast: 8 to 12ft/s
Obviously, to get the best simulation results, you should try to
estimate the speed of your robot accurately.
Here's an example usage of the drivetrains::
import hal.simulation
from pyfrc.physics import drivetrains
class PhysicsEngine:
def __init__(self, physics_controller):
self.physics_controller = physics_controller
self.drivetrain = drivetrains.TwoMotorDrivetrain(deadzone=drivetrains.linear_deadzone(0.2))
self.l_motor = hal.simulation.PWMSim(1)
self.r_motor = hal.simulation.PWMSim(2)
def update_sim(self, now, tm_diff):
l_motor = self.l_motor.getSpeed()
r_motor = self.r_motor.getSpeed()
speeds = self.drivetrain.calculate(l_motor, r_motor)
self.physics_controller.drive(speeds, tm_diff)
# optional: compute encoder
# l_encoder = self.drivetrain.wheelSpeeds.left * tm_diff
.. versionchanged:: 2020.1.0
The input speeds and output rotation angles were changed to reflect
the current WPILib drivetrain/field objects. Wheelbases and default
speeds all require units.
"""
import math
import typing
from .units import units, Helpers
from wpimath.geometry import Translation2d
from wpimath.kinematics import (
ChassisSpeeds,
DifferentialDriveKinematics,
DifferentialDriveWheelSpeeds,
MecanumDriveKinematics,
MecanumDriveWheelSpeeds,
)
DeadzoneCallable = typing.Callable[[float], float]
[docs]
def linear_deadzone(deadzone: float) -> DeadzoneCallable:
"""
Real motors won't actually move unless you give them some minimum amount
of input. This computes an output speed for a motor and causes it to
'not move' if the input isn't high enough. Additionally, the output is
adjusted linearly to compensate.
Example: For a deadzone of 0.2:
* Input of 0.0 will result in 0.0
* Input of 0.2 will result in 0.0
* Input of 0.3 will result in ~0.12
* Input of 1.0 will result in 1.0
This returns a function that computes the deadzone. You should pass the
returned function to one of the drivetrain simulation functions as the
``deadzone`` parameter.
:param motor_input: The motor input (between -1 and 1)
:param deadzone: Minimum input required for the motor to move (between 0 and 1)
"""
assert 0.0 < deadzone < 1.0
scale_param = 1.0 - deadzone
def _linear_deadzone(motor_input):
abs_motor_input = abs(motor_input)
if abs_motor_input < deadzone:
return 0.0
else:
return math.copysign(
(abs_motor_input - deadzone) / scale_param, motor_input
)
return _linear_deadzone
[docs]
class TwoMotorDrivetrain:
"""
Two center-mounted motors with a simple drivetrain. 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 motor output is 1, and the
right motor output is -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
[docs]
def calculate(self, l_motor: float, r_motor: float) -> ChassisSpeeds:
"""
Given motor values, computes resulting chassis speeds of robot
:param l_motor: Left motor value (-1 to 1); 1 is forward
:param r_motor: Right motor value (-1 to 1); -1 is forward
:returns: ChassisSpeeds that can be passed to 'drive'
.. versionadded:: 2020.1.0
"""
if self.deadzone:
l_motor = self.deadzone(l_motor)
r_motor = self.deadzone(r_motor)
l = l_motor * self.speed
r = -r_motor * self.speed
self.wheelSpeeds.left = l
self.wheelSpeeds.right = r
return self.kinematics.toChassisSpeeds(self.wheelSpeeds)
[docs]
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
[docs]
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)
[docs]
class MecanumDrivetrain:
"""
Four motors, each with a mecanum wheel attached to it.
.. note:: :class:`wpilib.drive.MecanumDrive` assumes that to make
the robot go forward, the left motor outputs are 1, and the
right motor outputs are -1
.. versionadded:: 2018.2.0
"""
#: Use this to compute encoder data after calculate is called
wheelSpeeds: MecanumDriveWheelSpeeds
def __init__(
self,
x_wheelbase: units.Quantity = 2 * units.feet,
y_wheelbase: units.Quantity = 3 * units.feet,
speed: units.Quantity = 5 * units.fps,
deadzone: typing.Optional[DeadzoneCallable] = None,
):
"""
:param x_wheelbase: The distance between right and left wheels.
:param y_wheelbase: The distance between forward and rear wheels.
:param speed: Speed of robot (see above)
:param deadzone: A function that adjusts the output of the motor (see :func:`linear_deadzone`)
"""
x2 = units.meters.m_from(x_wheelbase, name="x_wheelbase") / 2.0
y2 = units.meters.m_from(y_wheelbase, name="y_wheelbase") / 2.0
self.kinematics = MecanumDriveKinematics(
Translation2d(x2, y2),
Translation2d(x2, -y2),
Translation2d(-x2, y2),
Translation2d(-x2, -y2),
)
self.speed = units.mps.m_from(speed, name="speed")
self.deadzone = deadzone
self.wheelSpeeds = MecanumDriveWheelSpeeds()
[docs]
def calculate(
self,
lf_motor: float,
lr_motor: float,
rf_motor: float,
rr_motor: float,
) -> ChassisSpeeds:
"""
: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)
# Calculate speed of each wheel
lr = lr_motor * self.speed
rr = -rr_motor * self.speed
lf = lf_motor * self.speed
rf = -rf_motor * self.speed
self.wheelSpeeds.frontLeft = lf
self.wheelSpeeds.rearLeft = lr
self.wheelSpeeds.frontRight = rf
self.wheelSpeeds.rearRight = rr
return self.kinematics.toChassisSpeeds(self.wheelSpeeds)
[docs]
def four_motor_swerve_drivetrain(
lr_motor: float,
rr_motor: float,
lf_motor: float,
rf_motor: float,
lr_angle: float,
rr_angle: float,
lf_angle: float,
rf_angle: float,
x_wheelbase=2,
y_wheelbase=2,
speed=5,
deadzone=None,
) -> ChassisSpeeds:
"""
Four motors that can be rotated in any direction
If any motors are inverted, then you will need to multiply that motor's
value by -1.
:param lr_motor: Left rear motor value (-1 to 1); 1 is forward
:param rr_motor: Right rear motor value (-1 to 1); 1 is forward
:param lf_motor: Left front motor value (-1 to 1); 1 is forward
:param rf_motor: Right front motor value (-1 to 1); 1 is forward
:param lr_angle: Left rear motor angle in degrees (0 to 360 measured clockwise from forward position)
:param rr_angle: Right rear motor angle in degrees (0 to 360 measured clockwise from forward position)
:param lf_angle: Left front motor angle in degrees (0 to 360 measured clockwise from forward position)
:param rf_angle: Right front motor angle in degrees (0 to 360 measured clockwise from forward position)
:param x_wheelbase: The distance in feet between right and left wheels.
:param y_wheelbase: The distance in feet between forward and rear wheels.
:param speed: Speed of robot in feet per second (see above)
:param deadzone: A function that adjusts the output of the motor (see :func:`linear_deadzone`)
:returns: ChassisSpeeds that can be passed to 'drive'
.. versionchanged:: 2020.1.0
The output rotation angle was changed from CW to CCW to reflect the
current WPILib drivetrain/field objects
"""
if deadzone:
lf_motor = deadzone(lf_motor)
lr_motor = deadzone(lr_motor)
rf_motor = deadzone(rf_motor)
rr_motor = deadzone(rr_motor)
# Calculate speed of each wheel
lr = lr_motor * speed
rr = rr_motor * speed
lf = lf_motor * speed
rf = rf_motor * speed
# Calculate angle in radians
lr_rad = math.radians(lr_angle)
rr_rad = math.radians(rr_angle)
lf_rad = math.radians(lf_angle)
rf_rad = math.radians(rf_angle)
# Calculate wheelbase radius
wheelbase_radius = math.hypot(x_wheelbase / 2.0, y_wheelbase / 2.0)
# Calculates the Vx and Vy components
# Sin an Cos inverted because forward is 0 on swerve wheels
Vx = (
(math.sin(lr_rad) * lr)
+ (math.sin(rr_rad) * rr)
+ (math.sin(lf_rad) * lf)
+ (math.sin(rf_rad) * rf)
)
Vy = (
(math.cos(lr_rad) * lr)
+ (math.cos(rr_rad) * rr)
+ (math.cos(lf_rad) * lf)
+ (math.cos(rf_rad) * rf)
)
# Adjusts the angle corresponding to a diameter that is perpendicular to the radius (add or subtract 45deg)
lr_rad = (lr_rad + (math.pi / 4)) % (2 * math.pi)
rr_rad = (rr_rad - (math.pi / 4)) % (2 * math.pi)
lf_rad = (lf_rad - (math.pi / 4)) % (2 * math.pi)
rf_rad = (rf_rad + (math.pi / 4)) % (2 * math.pi)
# Finds the rotational velocity by finding the torque and adding them up
Vw = wheelbase_radius * (
(math.cos(lr_rad) * -lr)
+ (math.cos(rr_rad) * rr)
+ (math.cos(lf_rad) * -lf)
+ (math.cos(rf_rad) * rf)
)
Vx *= 0.25
Vy *= 0.25
Vw *= 0.25
return ChassisSpeeds.fromFeet(Vx, Vy, Vw)