def handle_chassis_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)
if joystick.getRawButtonPressed(3):
# TODO reset heading
pass
def handle_chassis_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)
if joystick.getRawButtonPressed(3):
self.chassis.reset_odometry(
geometry.Pose2d(-3, 0, geometry.Rotation2d(
math.pi))) # the starting position on the field
self.target_estimator.reset()
def teleopPeriodic(self):
"""Runs the motors with tank steering"""
throttle = scale_value(self.joystick.getThrottle(), 1, -1, 0.1, 1)
# this is where the joystick inputs get converted to numbers that are sent
# to the chassis component. we rescale them using the rescale_js function,
# in order to make their response exponential, and to set a dead zone -
# which just means if it is under a certain value a 0 will be sent
# TODO: Tune these constants for whatever robot they are on
joystick_vx = (
-rescale_js(self.joystick.getY(), deadzone=0.1, exponential=1.5) *
4 * throttle)
joystick_vy = (
-rescale_js(self.joystick.getX(), deadzone=0.1, exponential=1.5) *
4 * throttle)
joystick_vz = (
-rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=20.0) *
self.spin_rate)
if joystick_vx or joystick_vy or joystick_vz:
# Drive in field oriented mode unless button 6 is held
if not self.joystick.getRawButton(6):
rotation = self.gyro.getRotation2d()
chassis_speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
joystick_vx, joystick_vy, joystick_vz, rotation)
else:
chassis_speeds = ChassisSpeeds(joystick_vx, joystick_vy,
joystick_vz)
for state, module in zip(
self.kinematics.toSwerveModuleStates(chassis_speeds),
self.modules):
new_state = SwerveModuleState.optimize(state,
module.get_rotation())
module.set(new_state)
else:
for module in self.modules:
module.stop()
# Reset the heading when button 7 is pressed
if self.joystick.getRawButtonPressed(7):
self.gyro.reset()
def handle_shooter_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
if joystick.getTrigger():
self.shooter_controller.fire_input()
if gamepad.getBackButton() and gamepad.getBumperPressed(
Hand.kLeftHand):
# Disable turret in case of catastrophic malfunction
# Make this toggle to allow re-enabling turret in case it was accidentally disabled
self.shooter.toggle()
self.turret.toggle()
# Hold to stay in manual aiming mode
if gamepad.getXButton():
self.shooter_controller.manual_slew(
rescale_js(gamepad.getX(Hand.kLeftHand), 0.1) *
-self.MANUAL_SLEW_SPEED)
def handle_chassis_inputs(self, joystick: wpilib.Joystick) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)