add sysId example

SysId/constants.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+
+class DriveConstants:
+    # The PWM IDs for the drivetrain motor controllers.
+    kLeftMotor1Port = 0
+    kLeftMotor2Port = 1
+    kRightMotor1Port = 2
+    kRightMotor2Port = 3
+
+    # Encoders and their respective motor controllers.
+    kLeftEncoderPorts = (0, 1)
+    kRightEncoderPorts = (2, 3)
+    kLeftEncoderReversed = False
+    kRightEncoderReversed = True
+
+    # Encoder counts per revolution/rotation.
+    kEncoderCPR = 1024.0
+    kWheelDiameterInches = 6.0
+
+    # Assumes the encoders are directly mounted on the wheel shafts
+    kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR
+
+
+# autonomous
+class AutonomousConstants:
+    kTimeoutSeconds = 3.0
+    kDriveDistanceMetres = 2.0
+    kDriveSpeed = 0.5
+
+
+class OIConstants:
+    kDriverControllerPort = 0

SysId/robot.py

@@ -0,0 +1,27 @@
+import wpilib
+
+
+class MyRobot(wpilib.TimedRobot):
+    def robotInit(self) -> None:
+        pass
+
+    def robotPeriodic(self) -> None:
+        pass
+
+    def disabledInit(self) -> None:
+        pass
+
+    def disabledPeriodic(self) -> None:
+        pass
+
+    def autonomousInit(self) -> None:
+        pass
+
+    def teleopInit(self) -> None:
+        pass
+
+    def testInit(self) -> None:
+        pass
+
+    def testPeriodic(self) -> None:
+        pass

SysId/subsystems/drive.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command, Subsystem
+from commands2.sysid import SysIdRoutine
+
+from wpilib import Encoder, PWMSparkMax, RobotController
+from wpilib.drive import DifferentialDrive
+
+from wpimath.units import volts
+
+from constants import DriveConstants
+
+
+class Drive(Subsystem):
+    def __init__(self) -> None:
+        super().__init__()
+
+        # The motors on the left side of the drive
+        self.left_motor = PWMSparkMax(DriveConstants.kLeftMotor1Port)
+        self.left_motor.addFollower(PWMSparkMax(DriveConstants.kLeftMotor2Port))
+
+        # The motors on the right side of the drive
+        self.right_motor = PWMSparkMax(DriveConstants.kRightMotor1Port)
+        self.right_motor.addFollower(PWMSparkMax(DriveConstants.kRightMotor2Port))
+
+        # At least one side of the drive train needs to be inverted. This ensures that positive voltages sent to each motor group result in forward motion.
+        self.right_motor.setInverted(True)
+
+        # The robot's drive
+        self.drive = DifferentialDrive(self.left_motor, self.right_motor)
+
+        # The left-side drive encoder
+        self.left_encoder = Encoder(
+            DriveConstants.kLeftEncoderPorts[0],
+            DriveConstants.kLeftEncoderPorts[1],
+            DriveConstants.kLeftEncoderReversed,
+        )
+
+        # The right-side drive encoder
+        self.right_encoder = Encoder(
+            DriveConstants.kRightEncoderPorts[0],
+            DriveConstants.kRightEncoderPorts[1],
+            DriveConstants.kRightEncoderReversed,
+        )
+
+        # Sets the distance per pulse for the encoders
+        self.left_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+        self.right_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+
+        def drive(voltage: volts):
+            self.left_motor.setVoltage(voltage)
+            self.right_motor.setVoltage(voltage)
+
+        def log():
+            log.motor("drive-left").voltage(
+                self.left_motor.get() * RobotController.getBatteryVoltage()
+            ).distance(self.left_encoder.getDistance()).velocity(
+                self.left_encoder.getRate()
+            )
+
+            log.motor("drive-right").voltage(
+                self.right_motor.get() * RobotController.getBatteryVoltage()
+            ).distance(self.right_encoder.getDistance()).velocity(
+                self.right_encoder.getRate()
+            )
+
+        self.sys_id_routine = SysIdRoutine(
+            SysIdRoutine.Config(), SysIdRoutine.Mechanism(drive, log, self)
+        )
+
+    def arcadeDriveCommand(self, fwd: float, rot: float) -> Command:
+        """Returns a command that drives the robot with arcade controls.
+
+        :param fwd: the commanded forward movement
+        :param rot: the commanded rotation
+        """
+
+        return self.run(self.drive.arcadeDrive(fwd, rot))
+
+    def sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command:
+        return self.sys_id_routine.quasistatic(direction)
+
+    def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command:
+        return self.sys_id_routine.dynamic(direction)

SysId/sysidroutinebot.py

@@ -0,0 +1,39 @@
+from commands2 import Command
+from commands2.button import CommandXboxController
+from commands2.sysid import SysIdRoutine
+
+from subsystems.drive import Drive
+
+from constants import OIConstants
+
+
+class SysIdRoutineBot:
+    def __init__(self) -> None:
+        self.drive = Drive()
+        self.controller = CommandXboxController(OIConstants.kDriverControllerPort)
+
+    def configureBindings(self) -> None:
+        # Control the drive with split-stick arcade controls
+        self.drive.setDefaultCommand(
+            self.drive.arcadeDriveCommand(
+                self.controller.getLeftY(), self.controller.getRightX()
+            )
+        )
+
+        # Bind full set of SysId routine tests to buttons; a complete routine should run each of these
+        # once.
+        self.controller.a().whileTrue(
+            self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.b().whileTrue(
+            self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)
+        )
+        self.controller.x().whileTrue(
+            self.drive.sysIdDynamic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.y().whileTrue(
+            self.drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)
+        )
+
+    def getAutonomousCommand(self) -> Command:
+        return self.drive.run(lambda: None)