Merge pull request #183 from thedropbears/xbox-controls

Added xbox controls for driving

components/chassis.py

@@ -81,13 +81,13 @@ def __init__(
         self.drive.setInverted(drive_reversed)
         self.drive.configVoltageCompSaturation(12, timeoutMs=10)
         self.drive.enableVoltageCompensation(True)
-        self.drive_ff = SimpleMotorFeedforwardMeters(kS=0.71164, kV=2.55, kA=0.1178)
+        self.drive_ff = SimpleMotorFeedforwardMeters(kS=0.18877, kV=2.7713, kA=0.18824)
         self.drive.configVelocityMeasurementPeriod(
             ctre.SensorVelocityMeasPeriod.Period_5Ms
         )
         self.drive.configVelocityMeasurementWindow(8)
 
-        self.drive.config_kP(0, 0.0023546, 10)
+        self.drive.config_kP(0, 0.011489, 10)
         self.drive.config_kI(0, 0, 10)
         self.drive.config_kD(0, 0, 10)
 

components/vision.py

@@ -109,12 +109,12 @@ def execute(self) -> None:
 
         results = self.camera.getLatestResult()
         self.has_target = results.hasTargets()
+        self.timestamp = wpilib.Timer.getFPGATimestamp() - results.getLatency()
         if not self.has_target:
             return
         if results.getLatency() == self.last_latency and wpilib.RobotBase.isReal():
             return
         self.last_latency = results.getLatency()
-        self.timestamp = wpilib.Timer.getFPGATimestamp() - results.getLatency()
         self.target_pitch = math.radians(results.getBestTarget().getPitch())
         # PhotonVision has yaw reversed from our RH coordinate system
         self.target_yaw = -math.radians(results.getBestTarget().getYaw())

controllers/leds.py

@@ -31,7 +31,7 @@ def execute(self) -> None:
             self.status_lights.set_disabled()
         elif (
             self.chassis.translation_velocity.norm() > self.shooter_control.MAX_SPEED
-            or self.chassis.rotation_velocity.radians()
+            or abs(self.chassis.rotation_velocity.radians())
             > self.shooter_control.MAX_ROTATION
         ):
             self.status_lights.set(DisplayType.SOLID, LedColours.PINK)

controllers/shooter.py

@@ -150,7 +150,7 @@ def tracking(self) -> None:
                 and self.distance > self.MIN_DIST
                 and self.distance < self.MAX_DIST
                 and self.chassis.translation_velocity.norm() < self.MAX_SPEED
-                and self.chassis.rotation_velocity.radians() < self.MAX_ROTATION
+                and abs(self.chassis.rotation_velocity.radians()) < self.MAX_ROTATION
                 and accel < 0.5
             ):
                 self.next_state("firing")

robot.py

@@ -16,9 +16,10 @@
 from controllers.indexer import IndexerController
 from controllers.leds import LedController
 from components.vision import Vision
+from wpimath.filter import SlewRateLimiter
 
 from controllers.shooter import ShooterController
-from utilities.scalers import rescale_js, scale_value
+from utilities.scalers import apply_deadzone, rescale_js, scale_value
 
 from utilities import git
 
@@ -38,7 +39,7 @@ class MyRobot(magicbot.MagicRobot):
     turret: Turret
     vision: Vision
 
-    lock_motion_while_shooting = magicbot.tunable(False)
+    lock_motion_while_shooting = magicbot.tunable(True)
     test_chassis_speed = magicbot.tunable(2)
 
     def createObjects(self) -> None:
@@ -58,10 +59,18 @@ def createObjects(self) -> None:
         self.chassis_4_drive = ctre.WPI_TalonFX(7)
         self.chassis_4_steer = ctre.WPI_TalonFX(8)
 
-        self.joystick = wpilib.Joystick(0)
-        self.recorded_joystick_state = (0.0, 0.0, 0.0)
+        self.recorded_drive_state = (0.0, 0.0, 0.0)
         self.recorded_is_local_driving = False
-        self.codriver = wpilib.XboxController(1)
+        self.gamepad = wpilib.XboxController(1)
+        self.joystick_x_filter = SlewRateLimiter(
+            Chassis.max_attainable_wheel_speed / 0.3
+        )
+        self.joystick_y_filter = SlewRateLimiter(
+            Chassis.max_attainable_wheel_speed / 0.3
+        )
+        self.joystick_z_filter = SlewRateLimiter(7 / 0.2)
+        # joystick used for test mode
+        self.joystick = wpilib.Joystick(0)
 
         self.shooter_left_motor = ctre.WPI_TalonFX(11)
         self.shooter_right_motor = ctre.WPI_TalonFX(10)
@@ -130,75 +139,63 @@ def disabledPeriodic(self) -> None:
         self.status_lights.execute()
 
     def teleopPeriodic(self) -> None:
-        # handle chassis inputs
-        throttle = scale_value(self.joystick.getThrottle(), 1, -1, 0.1, 1)
-        spin_rate = 4.0
+        # left trigger increases turn speed
+        spin_rate = scale_value(self.gamepad.getLeftTriggerAxis(), 0, 1, 2.5, 7)
         # Don't update these values while firing
         if (
             not self.lock_motion_while_shooting
             or self.shooter_control.current_state != "firing"
         ):
             joystick_x = (
-                -rescale_js(self.joystick.getY(), deadzone=0.05, exponential=1.5)
-                * 4
-                * throttle
+                -apply_deadzone(self.gamepad.getLeftY(), 0.15)
+                * Chassis.max_attainable_wheel_speed
             )
             joystick_y = (
-                -rescale_js(self.joystick.getX(), deadzone=0.05, exponential=1.5)
-                * 4
-                * throttle
+                -apply_deadzone(self.gamepad.getLeftX(), 0.15)
+                * Chassis.max_attainable_wheel_speed
             )
             joystick_z = (
-                -rescale_js(self.joystick.getZ(), deadzone=0.3, exponential=25.0)
+                -rescale_js(self.gamepad.getRightX(), deadzone=0.15, exponential=4.0)
                 * spin_rate
             )
-            self.recorded_joystick_state = (joystick_x, joystick_y, joystick_z)
-            self.recorded_is_local_driving = self.joystick.getRawButton(6)
+            self.recorded_drive_state = (
+                self.joystick_x_filter.calculate(joystick_x),
+                self.joystick_y_filter.calculate(joystick_y),
+                self.joystick_z_filter.calculate(joystick_z),
+            )
+            self.recorded_is_local_driving = self.gamepad.getAButton()
 
-        # Drive in field oriented mode unless button 6 is pressed
+        # Drive in field oriented mode unless A button is pressed
         if self.recorded_is_local_driving:
-            self.chassis.drive_local(*self.recorded_joystick_state)
+            self.chassis.drive_local(*self.recorded_drive_state)
         else:
-            self.chassis.drive_field(*self.recorded_joystick_state)
-
-        if self.joystick.getRawButtonPressed(9):
-            self.shooter_control.lead_shots = True
-        if self.joystick.getRawButtonPressed(10):
-            self.shooter_control.lead_shots = False
+            self.chassis.drive_field(*self.recorded_drive_state)
 
-        if self.joystick.getRawButtonPressed(7):
-            self.indexer_control.ignore_colour = True
-        if self.joystick.getRawButtonPressed(8):
-            self.indexer_control.ignore_colour = False
-
-        # reset heading to intake facing directly downfield
-        if self.codriver.getYButtonPressed():
-            self.chassis.zero_yaw()
-
-        if self.joystick.getTrigger():
+        if self.gamepad.getRightTriggerAxis() > 0.3:
             self.shooter_control.fire()
 
-        if self.joystick.getRawButtonPressed(2):
-            self.indexer_control.wants_to_intake = (
-                not self.indexer_control.wants_to_intake
-            )
-
-        # hold down 11 to intake untill full, no auto-retract
-        self.intake.auto_retract = not self.joystick.getRawButton(11)
-        if self.joystick.getRawButton(11):
+        # right bumper puts intake down, hold to hold intake down
+        self.intake.auto_retract = not self.gamepad.getLeftBumper()
+        if self.gamepad.getLeftBumper():
             self.indexer_control.wants_to_intake = True
 
-        if self.codriver.getBButtonPressed() or self.joystick.getRawButton(3):
+        # left bumper raises intake
+        if self.gamepad.getRightBumper():
+            self.indexer_control.wants_to_intake = False
+
+        if self.gamepad.getYButtonPressed():
             self.indexer_control.engage("forced_clearing", force=True)
 
-        self.indexer_control.catflap_active = (
-            self.codriver.getXButton() or self.joystick.getRawButton(5)
-        )
+        self.indexer_control.catflap_active = self.gamepad.getXButton()
 
         # Failsafe
-        if self.codriver.getAButton() or self.joystick.getRawButton(4):
+        if self.gamepad.getBButton():
             self.chassis.set_pose_failsafe()
 
+        # d pad up to zero heading
+        if self.gamepad.getPOV() == 0:
+            self.chassis.zero_yaw()
+
     def testPeriodic(self) -> None:
         # hold y and use joystick throttle to set flywheel speed
         throttle = scale_value(self.joystick.getThrottle(), 1, -1, 0, 1)