Update examples and tests

Author: auscompgeek

examples/get-set-params/robot.py

@@ -30,18 +30,18 @@ def robotInit(self):
         #   CANError.kOk
         #   CANError.kError
         #   CANError.kTimeout
-        if self.motor.setIdleMode(rev.IdleMode.kCoast) is not rev.CANError.kOk:
+        if self.motor.setIdleMode(rev.CANSparkMax.IdleMode.kCoast) != rev.REVLibError.kOk:
             wpilib.SmartDashboard.putString("Idle Mode", "Error")
 
         # Similarly, parameters will have a get() method which allows you to
         # retrieve their values from the controller
-        if self.motor.getIdleMode() is rev.IdleMode.kCoast:
+        if self.motor.getIdleMode() == rev.CANSparkMax.IdleMode.kCoast:
             wpilib.SmartDashboard.putString("Idle Mode", "Coast")
         else:
             wpilib.SmartDashboard.putString("Idle Mode", "Brake")
 
         # Set ramp rate to 0
-        if self.motor.setOpenLoopRampRate(0) is not rev.CANError.kOk:
+        if self.motor.setOpenLoopRampRate(0) != rev.REVLibError.kOk:
             wpilib.SmartDashboard.putString("Ramp Rate", "Error")
 
         # Read back ramp value

examples/limit-switch/robot.py

@@ -29,10 +29,10 @@ def robotInit(self):
         # rev.CANDigitalInput.LimitSwitchPolarity.kNormallyOpen
         # rev.CANDigitalInput.LimitSwitchPolarity.kNormallyClosed
         self.forwardLimit = self.motor.getForwardLimitSwitch(
-            rev.LimitSwitchPolarity.kNormallyClosed
+            rev.SparkLimitSwitch.Type.kNormallyClosed
         )
         self.reverseLimit = self.motor.getReverseLimitSwitch(
-            rev.LimitSwitchPolarity.kNormallyClosed
+            rev.SparkLimitSwitch.Type.kNormallyClosed
         )
 
         self.forwardLimit.enableLimitSwitch(False)

examples/maxswerve/robotcontainer.py

@@ -4,6 +4,7 @@
 import wpimath
 import wpilib
 
+from commands2 import cmd
 from wpimath.controller import PIDController, ProfiledPIDControllerRadians
 from wpimath.geometry import Pose2d, Rotation2d, Translation2d
 from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator
@@ -48,7 +49,7 @@ def __init__(self) -> None:
                     True,
                     True,
                 ),
-                [self.robotDrive],
+                self.robotDrive,
             )
         )
 
@@ -95,7 +96,7 @@ def getAutonomousCommand(self) -> commands2.Command:
         )
         thetaController.enableContinuousInput(-math.pi, math.pi)
 
-        swerveControllerCommand = commands2.Swerve4ControllerCommand(
+        swerveControllerCommand = commands2.SwerveControllerCommand(
             exampleTrajectory,
             self.robotDrive.getPose,  # Functional interface to feed supplier
             DriveConstants.kDriveKinematics,
@@ -104,13 +105,16 @@ def getAutonomousCommand(self) -> commands2.Command:
             PIDController(AutoConstants.kPYController, 0, 0),
             thetaController,
             self.robotDrive.setModuleStates,
-            [self.robotDrive],
+            (self.robotDrive,),
         )
 
         # Reset odometry to the starting pose of the trajectory.
         self.robotDrive.resetOdometry(exampleTrajectory.initialPose())
 
         # Run path following command, then stop at the end.
         return swerveControllerCommand.andThen(
-            lambda: self.robotDrive.drive(0, 0, 0, False, False)
+            cmd.run(
+                lambda: self.robotDrive.drive(0, 0, 0, False, False),
+                self.robotDrive,
+            )
         )

examples/maxswerve/subsystems/drivesubsystem.py

@@ -3,7 +3,7 @@
 
 import wpilib
 
-from commands2 import SubsystemBase
+from commands2 import Subsystem
 from wpimath.filter import SlewRateLimiter
 from wpimath.geometry import Pose2d, Rotation2d
 from wpimath.kinematics import (
@@ -18,7 +18,7 @@
 from .maxswervemodule import MAXSwerveModule
 
 
-class DriveSubsystem(SubsystemBase):
+class DriveSubsystem(Subsystem):
     def __init__(self) -> None:
         super().__init__()
 
@@ -48,7 +48,7 @@ def __init__(self) -> None:
         )
 
         # The gyro sensor
-        self.gyro = wpilib.ADIS16470_IMU()
+        self.gyro = wpilib.ADIS16448_IMU()
 
         # Slew rate filter variables for controlling lateral acceleration
         self.currentRotation = 0.0
@@ -63,22 +63,24 @@ def __init__(self) -> None:
         self.odometry = SwerveDrive4Odometry(
             DriveConstants.kDriveKinematics,
             Rotation2d.fromDegrees(self.gyro.getAngle()),
-            [
+            (
                 self.frontLeft.getPosition(),
                 self.frontRight.getPosition(),
                 self.rearLeft.getPosition(),
                 self.rearRight.getPosition(),
-            ],
+            ),
         )
 
     def periodic(self) -> None:
         # Update the odometry in the periodic block
         self.odometry.update(
             Rotation2d.fromDegrees(self.gyro.getAngle()),
-            self.frontLeft.getPosition(),
-            self.frontRight.getPosition(),
-            self.rearLeft.getPosition(),
-            self.rearRight.getPosition(),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.rearLeft.getPosition(),
+                self.rearRight.getPosition(),
+            ),
         )
 
     def getPose(self) -> Pose2d:
@@ -96,11 +98,13 @@ def resetOdometry(self, pose: Pose2d) -> None:
         """
         self.odometry.resetPosition(
             Rotation2d.fromDegrees(self.gyro.getAngle()),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.rearLeft.getPosition(),
+                self.rearRight.getPosition(),
+            ),
             pose,
-            self.frontLeft.getPosition(),
-            self.frontRight.getPosition(),
-            self.rearLeft.getPosition(),
-            self.rearRight.getPosition(),
         )
 
     def drive(
@@ -202,13 +206,13 @@ def drive(
             if fieldRelative
             else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)
         )
-        swerveModuleStates = SwerveDrive4Kinematics.desaturateWheelSpeeds(
+        fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds(
             swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond
         )
-        self.frontLeft.setDesiredState(swerveModuleStates[0])
-        self.frontRight.setDesiredState(swerveModuleStates[1])
-        self.rearLeft.setDesiredState(swerveModuleStates[2])
-        self.rearRight.setDesiredState(swerveModuleStates[3])
+        self.frontLeft.setDesiredState(fl)
+        self.frontRight.setDesiredState(fr)
+        self.rearLeft.setDesiredState(rl)
+        self.rearRight.setDesiredState(rr)
 
     def setX(self) -> None:
         """Sets the wheels into an X formation to prevent movement."""
@@ -219,18 +223,23 @@ def setX(self) -> None:
         self.rearLeft.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(-45)))
         self.rearRight.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(45)))
 
-    def setModuleStates(self, desiredStates: typing.Tuple[SwerveModuleState]) -> None:
+    def setModuleStates(
+        self,
+        desiredStates: typing.Tuple[
+            SwerveModuleState, SwerveModuleState, SwerveModuleState, SwerveModuleState
+        ],
+    ) -> None:
         """Sets the swerve ModuleStates.
 
         :param desiredStates: The desired SwerveModule states.
         """
-        desiredStates = SwerveDrive4Kinematics.desaturateWheelSpeeds(
+        fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds(
             desiredStates, DriveConstants.kMaxSpeedMetersPerSecond
         )
-        self.frontLeft.setDesiredState(desiredStates[0])
-        self.frontRight.setDesiredState(desiredStates[1])
-        self.rearLeft.setDesiredState(desiredStates[2])
-        self.rearRight.setDesiredState(desiredStates[3])
+        self.frontLeft.setDesiredState(fl)
+        self.frontRight.setDesiredState(fr)
+        self.rearLeft.setDesiredState(rl)
+        self.rearRight.setDesiredState(rr)
 
     def resetEncoders(self) -> None:
         """Resets the drive encoders to currently read a position of 0."""
@@ -248,7 +257,7 @@ def getHeading(self) -> float:
 
         :returns: the robot's heading in degrees, from -180 to 180
         """
-        return Rotation2d.fromDegrees(self.gyro.getAngle()).getDegrees()
+        return Rotation2d.fromDegrees(self.gyro.getAngle()).degrees()
 
     def getTurnRate(self) -> float:
         """Returns the turn rate of the robot.

tests/test_cansparkmax.py

@@ -1,32 +1,30 @@
 import rev
+from rev import REVLibError
 
 
 def test_setfeedbackdevice():
     s = rev.CANSparkMax(1, rev.CANSparkLowLevel.MotorType.kBrushless)
     e = s.getEncoder()
     p = s.getPIDController()
-    p.setFeedbackDevice(e)
-
-
-# def test_init(rev):
-#     rev.CANSparkMax(0, rev.MotorType.kBrushless)
+    assert p.setFeedbackDevice(e) == REVLibError.kOk
 
 
 def test_get_fwd_limit():
     sm = rev.CANSparkMax(2, rev.CANSparkLowLevel.MotorType.kBrushless)
-    sm.getForwardLimitSwitch(rev.SparkLimitSwitch.Type.kNormallyOpen)
+    switch = sm.getForwardLimitSwitch(rev.SparkLimitSwitch.Type.kNormallyOpen)
+    switch.get()
 
 
-# def test_current_limit(rev, hal_data):
-#     sm = rev.CANSparkMax(1, rev.MotorType.kBrushless)
+def test_current_limit():
+    sm = rev.CANSparkMax(1, rev.CANSparkLowLevel.MotorType.kBrushless)
 
-#     sm.setSecondaryCurrentLimit(50)
+    sm.setSecondaryCurrentLimit(50)
 
 #     assert hal_data["CAN"]["sparkmax-1"]["currentChop"] == 50.0
 #     assert isinstance(hal_data["CAN"]["sparkmax-1"]["currentChop"], float)
 #     assert hal_data["CAN"]["sparkmax-1"]["currentChopCycles"] == 0
 
-#     sm.setSecondaryCurrentLimit(52.5, 5)
+    sm.setSecondaryCurrentLimit(52.5, 5)
 
 #     assert hal_data["CAN"]["sparkmax-1"]["currentChop"] == 52.5
 #     assert hal_data["CAN"]["sparkmax-1"]["currentChopCycles"] == 5
@@ -39,17 +37,17 @@ def test_get_fwd_limit():
 #     assert not rev_sw.get()
 
 
-# def test_frame_period(rev, hal_data):
-#     sm = rev.CANSparkMax(2, rev.MotorType.kBrushed)
-#     sm.setPeriodicFramePeriod(rev.PeriodicFrame.kStatus2, 20)
+# def test_frame_period():
+#     sm = rev.CANSparkMax(2, rev.CANSparkLowLevel.MotorType.kBrushed)
+#     sm.setPeriodicFramePeriod(rev.CANSparkLowLevel.PeriodicFrame.kStatus2, 20)
 #     assert (
 #         hal_data["CAN"]["sparkmax-2"]["frame_period"][rev.PeriodicFrame.kStatus2] == 20
 #     )
 
 
-# def test_pid_set(rev, hal_data):
-#     sm = rev.CANSparkMax(0, rev.MotorType.kBrushless)
-#     pid = sm.getPIDController()
-#     pid.setOutputRange(-1, 1)
-#     pid.setP(0.005)
-#     pid.setReference(5, rev.ControlType.kPosition)
+def test_pid_set():
+    sm = rev.CANSparkMax(0, rev.CANSparkLowLevel.MotorType.kBrushless)
+    pid = sm.getPIDController()
+    pid.setOutputRange(-1, 1)
+    pid.setP(0.005)
+    pid.setReference(5, rev.CANSparkBase.ControlType.kPosition)