Skip to content

Commit c65f34e

Browse files
auscompgeekauscompgeek
committed
Update examples and tests
1 parent 1da55d6 commit c65f34e

File tree

5 files changed

+64
-53
lines changed

5 files changed

+64
-53
lines changed

‎examples/get-set-params/robot.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -30,18 +30,18 @@ def robotInit(self):
3030
# CANError.kOk
3131
# CANError.kError
3232
# CANError.kTimeout
33-
if self.motor.setIdleMode(rev.IdleMode.kCoast) is not rev.CANError.kOk:
33+
if self.motor.setIdleMode(rev.CANSparkMax.IdleMode.kCoast) != rev.REVLibError.kOk:
3434
wpilib.SmartDashboard.putString("Idle Mode", "Error")
3535

3636
# Similarly, parameters will have a get() method which allows you to
3737
# retrieve their values from the controller
38-
if self.motor.getIdleMode() is rev.IdleMode.kCoast:
38+
if self.motor.getIdleMode() == rev.CANSparkMax.IdleMode.kCoast:
3939
wpilib.SmartDashboard.putString("Idle Mode", "Coast")
4040
else:
4141
wpilib.SmartDashboard.putString("Idle Mode", "Brake")
4242

4343
# Set ramp rate to 0
44-
if self.motor.setOpenLoopRampRate(0) is not rev.CANError.kOk:
44+
if self.motor.setOpenLoopRampRate(0) != rev.REVLibError.kOk:
4545
wpilib.SmartDashboard.putString("Ramp Rate", "Error")
4646

4747
# Read back ramp value

‎examples/limit-switch/robot.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,10 +29,10 @@ def robotInit(self):
2929
# rev.CANDigitalInput.LimitSwitchPolarity.kNormallyOpen
3030
# rev.CANDigitalInput.LimitSwitchPolarity.kNormallyClosed
3131
self.forwardLimit = self.motor.getForwardLimitSwitch(
32-
rev.LimitSwitchPolarity.kNormallyClosed
32+
rev.SparkLimitSwitch.Type.kNormallyClosed
3333
)
3434
self.reverseLimit = self.motor.getReverseLimitSwitch(
35-
rev.LimitSwitchPolarity.kNormallyClosed
35+
rev.SparkLimitSwitch.Type.kNormallyClosed
3636
)
3737

3838
self.forwardLimit.enableLimitSwitch(False)

‎examples/maxswerve/robotcontainer.py‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
import wpimath
55
import wpilib
66

7+
from commands2 import cmd
78
from wpimath.controller import PIDController, ProfiledPIDControllerRadians
89
from wpimath.geometry import Pose2d, Rotation2d, Translation2d
910
from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator
@@ -48,7 +49,7 @@ def __init__(self) -> None:
4849
True,
4950
True,
5051
),
51-
[self.robotDrive],
52+
self.robotDrive,
5253
)
5354
)
5455

@@ -95,7 +96,7 @@ def getAutonomousCommand(self) -> commands2.Command:
9596
)
9697
thetaController.enableContinuousInput(-math.pi, math.pi)
9798

98-
swerveControllerCommand = commands2.Swerve4ControllerCommand(
99+
swerveControllerCommand = commands2.SwerveControllerCommand(
99100
exampleTrajectory,
100101
self.robotDrive.getPose, # Functional interface to feed supplier
101102
DriveConstants.kDriveKinematics,
@@ -104,13 +105,16 @@ def getAutonomousCommand(self) -> commands2.Command:
104105
PIDController(AutoConstants.kPYController, 0, 0),
105106
thetaController,
106107
self.robotDrive.setModuleStates,
107-
[self.robotDrive],
108+
(self.robotDrive,),
108109
)
109110

110111
# Reset odometry to the starting pose of the trajectory.
111112
self.robotDrive.resetOdometry(exampleTrajectory.initialPose())
112113

113114
# Run path following command, then stop at the end.
114115
return swerveControllerCommand.andThen(
115-
lambda: self.robotDrive.drive(0, 0, 0, False, False)
116+
cmd.run(
117+
lambda: self.robotDrive.drive(0, 0, 0, False, False),
118+
self.robotDrive,
119+
)
116120
)

‎examples/maxswerve/subsystems/drivesubsystem.py‎

Lines changed: 34 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33

44
import wpilib
55

6-
from commands2 import SubsystemBase
6+
from commands2 import Subsystem
77
from wpimath.filter import SlewRateLimiter
88
from wpimath.geometry import Pose2d, Rotation2d
99
from wpimath.kinematics import (
@@ -18,7 +18,7 @@
1818
from .maxswervemodule import MAXSwerveModule
1919

2020

21-
class DriveSubsystem(SubsystemBase):
21+
class DriveSubsystem(Subsystem):
2222
def __init__(self) -> None:
2323
super().__init__()
2424

@@ -48,7 +48,7 @@ def __init__(self) -> None:
4848
)
4949

5050
# The gyro sensor
51-
self.gyro = wpilib.ADIS16470_IMU()
51+
self.gyro = wpilib.ADIS16448_IMU()
5252

5353
# Slew rate filter variables for controlling lateral acceleration
5454
self.currentRotation = 0.0
@@ -63,22 +63,24 @@ def __init__(self) -> None:
6363
self.odometry = SwerveDrive4Odometry(
6464
DriveConstants.kDriveKinematics,
6565
Rotation2d.fromDegrees(self.gyro.getAngle()),
66-
[
66+
(
6767
self.frontLeft.getPosition(),
6868
self.frontRight.getPosition(),
6969
self.rearLeft.getPosition(),
7070
self.rearRight.getPosition(),
71-
],
71+
),
7272
)
7373

7474
def periodic(self) -> None:
7575
# Update the odometry in the periodic block
7676
self.odometry.update(
7777
Rotation2d.fromDegrees(self.gyro.getAngle()),
78-
self.frontLeft.getPosition(),
79-
self.frontRight.getPosition(),
80-
self.rearLeft.getPosition(),
81-
self.rearRight.getPosition(),
78+
(
79+
self.frontLeft.getPosition(),
80+
self.frontRight.getPosition(),
81+
self.rearLeft.getPosition(),
82+
self.rearRight.getPosition(),
83+
),
8284
)
8385

8486
def getPose(self) -> Pose2d:
@@ -96,11 +98,13 @@ def resetOdometry(self, pose: Pose2d) -> None:
9698
"""
9799
self.odometry.resetPosition(
98100
Rotation2d.fromDegrees(self.gyro.getAngle()),
101+
(
102+
self.frontLeft.getPosition(),
103+
self.frontRight.getPosition(),
104+
self.rearLeft.getPosition(),
105+
self.rearRight.getPosition(),
106+
),
99107
pose,
100-
self.frontLeft.getPosition(),
101-
self.frontRight.getPosition(),
102-
self.rearLeft.getPosition(),
103-
self.rearRight.getPosition(),
104108
)
105109

106110
def drive(
@@ -202,13 +206,13 @@ def drive(
202206
if fieldRelative
203207
else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)
204208
)
205-
swerveModuleStates = SwerveDrive4Kinematics.desaturateWheelSpeeds(
209+
fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds(
206210
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond
207211
)
208-
self.frontLeft.setDesiredState(swerveModuleStates[0])
209-
self.frontRight.setDesiredState(swerveModuleStates[1])
210-
self.rearLeft.setDesiredState(swerveModuleStates[2])
211-
self.rearRight.setDesiredState(swerveModuleStates[3])
212+
self.frontLeft.setDesiredState(fl)
213+
self.frontRight.setDesiredState(fr)
214+
self.rearLeft.setDesiredState(rl)
215+
self.rearRight.setDesiredState(rr)
212216

213217
def setX(self) -> None:
214218
"""Sets the wheels into an X formation to prevent movement."""
@@ -219,18 +223,23 @@ def setX(self) -> None:
219223
self.rearLeft.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(-45)))
220224
self.rearRight.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(45)))
221225

222-
def setModuleStates(self, desiredStates: typing.Tuple[SwerveModuleState]) -> None:
226+
def setModuleStates(
227+
self,
228+
desiredStates: typing.Tuple[
229+
SwerveModuleState, SwerveModuleState, SwerveModuleState, SwerveModuleState
230+
],
231+
) -> None:
223232
"""Sets the swerve ModuleStates.
224233
225234
:param desiredStates: The desired SwerveModule states.
226235
"""
227-
desiredStates = SwerveDrive4Kinematics.desaturateWheelSpeeds(
236+
fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds(
228237
desiredStates, DriveConstants.kMaxSpeedMetersPerSecond
229238
)
230-
self.frontLeft.setDesiredState(desiredStates[0])
231-
self.frontRight.setDesiredState(desiredStates[1])
232-
self.rearLeft.setDesiredState(desiredStates[2])
233-
self.rearRight.setDesiredState(desiredStates[3])
239+
self.frontLeft.setDesiredState(fl)
240+
self.frontRight.setDesiredState(fr)
241+
self.rearLeft.setDesiredState(rl)
242+
self.rearRight.setDesiredState(rr)
234243

235244
def resetEncoders(self) -> None:
236245
"""Resets the drive encoders to currently read a position of 0."""
@@ -248,7 +257,7 @@ def getHeading(self) -> float:
248257
249258
:returns: the robot's heading in degrees, from -180 to 180
250259
"""
251-
return Rotation2d.fromDegrees(self.gyro.getAngle()).getDegrees()
260+
return Rotation2d.fromDegrees(self.gyro.getAngle()).degrees()
252261

253262
def getTurnRate(self) -> float:
254263
"""Returns the turn rate of the robot.

‎tests/test_cansparkmax.py‎

Lines changed: 17 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,32 +1,30 @@
11
import rev
2+
from rev import REVLibError
23

34

45
def test_setfeedbackdevice():
56
s = rev.CANSparkMax(1, rev.CANSparkLowLevel.MotorType.kBrushless)
67
e = s.getEncoder()
78
p = s.getPIDController()
8-
p.setFeedbackDevice(e)
9-
10-
11-
# def test_init(rev):
12-
# rev.CANSparkMax(0, rev.MotorType.kBrushless)
9+
assert p.setFeedbackDevice(e) == REVLibError.kOk
1310

1411

1512
def test_get_fwd_limit():
1613
sm = rev.CANSparkMax(2, rev.CANSparkLowLevel.MotorType.kBrushless)
17-
sm.getForwardLimitSwitch(rev.SparkLimitSwitch.Type.kNormallyOpen)
14+
switch = sm.getForwardLimitSwitch(rev.SparkLimitSwitch.Type.kNormallyOpen)
15+
switch.get()
1816

1917

20-
# def test_current_limit(rev, hal_data):
21-
# sm = rev.CANSparkMax(1, rev.MotorType.kBrushless)
18+
def test_current_limit():
19+
sm = rev.CANSparkMax(1, rev.CANSparkLowLevel.MotorType.kBrushless)
2220

23-
# sm.setSecondaryCurrentLimit(50)
21+
sm.setSecondaryCurrentLimit(50)
2422

2523
# assert hal_data["CAN"]["sparkmax-1"]["currentChop"] == 50.0
2624
# assert isinstance(hal_data["CAN"]["sparkmax-1"]["currentChop"], float)
2725
# assert hal_data["CAN"]["sparkmax-1"]["currentChopCycles"] == 0
2826

29-
# sm.setSecondaryCurrentLimit(52.5, 5)
27+
sm.setSecondaryCurrentLimit(52.5, 5)
3028

3129
# assert hal_data["CAN"]["sparkmax-1"]["currentChop"] == 52.5
3230
# assert hal_data["CAN"]["sparkmax-1"]["currentChopCycles"] == 5
@@ -39,17 +37,17 @@ def test_get_fwd_limit():
3937
# assert not rev_sw.get()
4038

4139

42-
# def test_frame_period(rev, hal_data):
43-
# sm = rev.CANSparkMax(2, rev.MotorType.kBrushed)
44-
# sm.setPeriodicFramePeriod(rev.PeriodicFrame.kStatus2, 20)
40+
# def test_frame_period():
41+
# sm = rev.CANSparkMax(2, rev.CANSparkLowLevel.MotorType.kBrushed)
42+
# sm.setPeriodicFramePeriod(rev.CANSparkLowLevel.PeriodicFrame.kStatus2, 20)
4543
# assert (
4644
# hal_data["CAN"]["sparkmax-2"]["frame_period"][rev.PeriodicFrame.kStatus2] == 20
4745
# )
4846

4947

50-
# def test_pid_set(rev, hal_data):
51-
# sm = rev.CANSparkMax(0, rev.MotorType.kBrushless)
52-
# pid = sm.getPIDController()
53-
# pid.setOutputRange(-1, 1)
54-
# pid.setP(0.005)
55-
# pid.setReference(5, rev.ControlType.kPosition)
48+
def test_pid_set():
49+
sm = rev.CANSparkMax(0, rev.CANSparkLowLevel.MotorType.kBrushless)
50+
pid = sm.getPIDController()
51+
pid.setOutputRange(-1, 1)
52+
pid.setP(0.005)
53+
pid.setReference(5, rev.CANSparkBase.ControlType.kPosition)

0 commit comments

Comments
 (0)