Add SysId example (#111)

Author: LucienMorey

SysId/constants.py

@@ -0,0 +1,39 @@
+#
+# 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,53 @@
+#!/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 CommandScheduler, TimedCommandRobot
+
+from sysidroutinebot import SysIdRoutineBot
+
+
+class MyRobot(TimedCommandRobot):
+    """The VM is configured to automatically run this class, and to call the functions corresponding to
+    each mode, as described in the TimedRobot documentation. If you change the name of this class or
+    the package after creating this project, you must also update the build.gradle file in the
+    project.
+    """
+
+    def robotInit(self) -> None:
+        """This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+        self.robot = SysIdRoutineBot()
+
+        self.robot.configureBindings()
+
+        self.autonomous_command = self.robot.getAutonomousCommand()
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+        pass
+
+    def disabledPeriodic(self) -> None:
+        pass
+
+    def autonomousInit(self) -> None:
+        self.autonomous_command.schedule()
+
+    def teleopInit(self) -> None:
+        # This makes sure that the autonomous stops running when
+        # teleop starts running. If you want the autonomous to
+        # continue until interrupted by another command, remove
+        # this line or comment it out.
+        self.autonomous_command.cancel()
+
+    def testInit(self) -> None:
+        # Cancels all running commands at the start of test mode.
+        CommandScheduler.getInstance().cancelAll()
+
+    def testPeriodic(self) -> None:
+        """This function is called periodically during test mode."""
+        pass

SysId/subsystems/drive.py

@@ -0,0 +1,102 @@
+#
+# 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.sysid import SysIdRoutineLog
+
+from wpilib import Encoder, PWMSparkMax, RobotController
+from wpilib.drive import DifferentialDrive
+
+from wpimath.units import volts
+
+from constants import DriveConstants
+
+from typing import Callable
+
+
+class Drive(Subsystem):
+    def __init__(self) -> None:
+        # 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)
+
+        # Tell SysId how to plumb the driving voltage to the motors.
+        def drive(voltage: volts) -> None:
+            self.left_motor.setVoltage(voltage)
+            self.right_motor.setVoltage(voltage)
+
+        # Tell SysId to make generated commands require this subsystem, suffix test state in
+        # WPILog with this subsystem's name ("drive")
+        self.sys_id_routine = SysIdRoutine(
+            SysIdRoutine.Config(),
+            SysIdRoutine.Mechanism(drive, self.log, self),
+        )
+
+    # Tell SysId how to record a frame of data for each motor on the mechanism being
+    # characterized.
+    def log(self, sys_id_routine: SysIdRoutineLog) -> None:
+        # Record a frame for the left motors.  Since these share an encoder, we consider
+        # the entire group to be one motor.
+        sys_id_routine.motor("drive-left").voltage(
+            self.left_motor.get() * RobotController.getBatteryVoltage()
+        ).position(self.left_encoder.getDistance()).velocity(
+            self.left_encoder.getRate()
+        )
+        # Record a frame for the right motors.  Since these share an encoder, we consider
+        # the entire group to be one motor.
+        sys_id_routine.motor("drive-right").voltage(
+            self.right_motor.get() * RobotController.getBatteryVoltage()
+        ).position(self.right_encoder.getDistance()).velocity(
+            self.right_encoder.getRate()
+        )
+
+    def arcadeDriveCommand(
+        self, fwd: Callable[[], float], rot: Callable[[], float]
+    ) -> Command:
+        """Returns a command that drives the robot with arcade controls.
+
+        :param fwd: the commanded forward movement
+        :param rot: the commanded rotation
+        """
+
+        # A split-stick arcade command, with forward/backward controlled by the left
+        # hand, and turning controlled by the right.
+        return self.run(lambda: 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,69 @@
+#
+# 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
+from commands2.button import CommandXboxController
+from commands2.sysid import SysIdRoutine
+
+from subsystems.drive import Drive
+
+from constants import OIConstants
+
+
+class SysIdRoutineBot:
+    """This class is where the bulk of the robot should be declared. Since Command-based is a
+    "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot`
+    periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+    subsystems, commands, and button mappings) should be declared here.
+    """
+
+    def __init__(self) -> None:
+        # The robot's subsystems
+        self.drive = Drive()
+
+        # The driver's controller
+        self.controller = CommandXboxController(OIConstants.kDriverControllerPort)
+
+    def configureBindings(self) -> None:
+        """Use this method to define bindings between conditions and commands. These are useful for
+        automating robot behaviors based on button and sensor input.
+
+        Should be called during :meth:`.Robot.robotInit`.
+
+        Event binding methods are available on the :class:`.Trigger` class.
+        """
+
+        # Control the drive with split-stick arcade controls
+        self.drive.setDefaultCommand(
+            self.drive.arcadeDriveCommand(
+                lambda: -self.controller.getLeftY(),
+                lambda: -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:
+        """Use this to define the command that runs during autonomous.
+
+        Scheduled during :meth:`.Robot.autonomousInit`.
+        """
+
+        # Do nothing
+        return self.drive.run(lambda: None)

run_tests.sh

@@ -59,6 +59,7 @@ BASE_TESTS="
   StateSpaceFlywheel
   StateSpaceFlywheelSysId
   SwerveBot
+  SysId
   TankDrive
   TankDriveXboxController
   Timed/src