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
tstep = 0.01
while angle < angle_target:
result = tank.calculate(l_motor, r_motor, tstep)
#
# 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,
robot_mass,
drivetrain_gear_ratio,
motors_per_side,
robot_wheelbase,
robot_width,
robot_length,
wheel_diameter,
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,
robot_mass,
drivetrain_gear_ratio,
motors_per_side,
robot_wheelbase,
"""
import math
import typing
from wpilib.geometry import Transform2d
from .motor_cfgs import MotorModelConfig
from .units import units, Helpers
import logging
logger = logging.getLogger("pyfrc.physics")
# default parameters for a kitbot
_bumper_length = 3.25 * units.inch
_kitbot_wheelbase = 21.0 * units.inch
_kitbot_width = _kitbot_wheelbase + _bumper_length * 2
_kitbot_length = 30.0 * units.inch + _bumper_length * 2
_inertia_units = (units.foot ** 2) * units.pound
_bm_units = units.foot * units.pound
class MotorModel:
"""
Motor model used by the :class:`TankModel`. You should not need to create
this object if you're using the :class:`TankModel` class.
"""
@units.wraps(None, (None, None, "tm_kv", "tm_ka", "volts"))