Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def test_tankdrive_get_distance():
"""Test TankModel.get_distance() by driving in a quarter circle.
If the turn angle is 90deg, then x should equal y. get_distance() runs in little steps,
so multiple calls should be equivalent to a single call"""
# just use the default robot
tank = tankmodel.TankModel.theory(
motor_cfgs.MOTOR_CFG_CIM,
robot_mass=90 * units.lbs,
gearing=10.71,
nmotors=2,
x_wheelbase=2.0 * units.feet,
wheel_diameter=6 * units.inch,
)
# slight turn to the right
l_motor = 1.0
r_motor = -0.9
# get the motors up to speed, so that subsequent calls produce the same result
tank.calculate(l_motor, r_motor, 2.0)
# figure out how much time is needed to turn 90 degrees
angle = 0.0
angle_target = -math.pi / 2.0 # 90 degrees
total_time = 0.0
#
# See the notes for the other physics sample
#
import math
from pyfrc.physics import motor_cfgs, tankmodel
from pyfrc.physics.units import units
class PhysicsEngine(object):
"""
Simulates a 4-wheel robot using Tank Drive joystick control
"""
WHEEL_DIAMETER = 6 * units.inch
ENCODER_PULSE_PER_REV = 360
ENCODER_SCALE = units.foot.m_from(WHEEL_DIAMETER) * math.pi / 360
def __init__(self, physics_controller):
"""
:param physics_controller: `pyfrc.physics.core.Physics` object
to communicate simulation effects to
"""
self.physics_controller = physics_controller
#
# Two ways of initializing the realistic physics -- either use the
# ka/kv that you computed for your robot, or use the theoretical model
#
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 __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
"""
:param physics_controller: `pyfrc.physics.core.Physics` object
to communicate simulation effects to
"""
self.physics_controller = physics_controller
#
# Two ways of initializing the realistic physics -- either use the
# ka/kv that you computed for your robot, or use the theoretical model
#
# Change these parameters to fit your robot!
# -> these parameters are for the test robot mentioned in the paper
motor_cfg = motor_cfgs.MOTOR_CFG_CIM
robot_mass = 110 * units.lbs
bumper_width = 3.25 * units.inch
robot_wheelbase = 22 * units.inch
robot_width = 23 * units.inch + bumper_width * 2
robot_length = 32 * units.inch + bumper_width * 2
wheel_diameter = 3.8 * units.inch
drivetrain_gear_ratio = 6.1
motors_per_side = 3
# Uses theoretical parameters by default, change this if you've
# actually measured kv/ka for your robot
if True:
self.drivetrain = tankmodel.TankModel.theory(
motor_cfg,
ka = motor_config.nominalVoltage / max_acceleration
kv = units.tm_kv.from_(kv, name="kv")
ka = units.tm_ka.from_(ka, name="ka")
logger.info(
"Motor config: %d %s motors @ %.2f gearing with %.1f diameter wheels",
nmotors,
motor_config.name,
gearing,
wheel_diameter.m,
)
logger.info(
"- Theoretical: vmax=%.3f ft/s, amax=%.3f ft/s^2, kv=%.3f, ka=%.3f",
max_velocity.m_as(units.foot / units.second),
max_acceleration.m_as(units.foot / units.second ** 2),
kv.m,
ka.m,
)
return cls(
motor_config,
robot_mass,
x_wheelbase,
robot_width,
robot_length,
kv,
ka,
vintercept,
kv,
ka,
drivetrain_gear_ratio,
motors_per_side,
robot_wheelbase,
robot_width,
robot_length,
wheel_diameter,
)
else:
# These are the parameters for kv/ka that you computed for your robot
# -> this example uses the values from the paper
l_kv = 0.81 * units.tm_kv
l_ka = 0.21 * units.tm_ka
l_vintercept = 1.26 * units.volts
r_kv = 0.81 * units.tm_kv
r_ka = 0.21 * units.tm_ka
r_vintercept = 1.26 * units.volts
self.drivetrain = tankmodel.TankModel(
motor_cfg,
robot_mass,
robot_wheelbase,
robot_width,
robot_length,
l_kv,
l_ka,
l_vintercept,
r_kv,
r_ka,
r_vintercept,
)