[robotpy] Fixup errors missed in #8479 (#8660)

Looks like a race condition happened in the landing of the examples PR
and #8479 which caused some of the python examples to fail.
This commit is contained in:
PJ Reiniger
2026-03-07 00:21:43 -05:00
committed by GitHub
parent b29bde123f
commit ccfb3ab1cd
18 changed files with 55 additions and 55 deletions

View File

@@ -76,7 +76,7 @@ class Arm:
# In this method, we update our simulation of what our arm is doing
# First, we set our "inputs" (voltages)
self.armSim.setInput(
[self.motor.get() * wpilib.RobotController.getBatteryVoltage()]
[self.motor.getDutyCycle() * wpilib.RobotController.getBatteryVoltage()]
)
# Next, we update it. The standard loop time is 20ms.
@@ -111,4 +111,4 @@ class Arm:
self.motor.setVoltage(pidOutput)
def stop(self) -> None:
self.motor.set(0.0)
self.motor.setDutyCycle(0.0)

View File

@@ -242,8 +242,8 @@ class Drivetrain:
# simulation, and write the simulated positions and velocities to our
# simulated encoder and gyro.
self.drivetrainSimulator.setInputs(
self.leftLeader.get() * wpilib.RobotController.getInputVoltage(),
self.rightLeader.get() * wpilib.RobotController.getInputVoltage(),
self.leftLeader.getDutyCycle() * wpilib.RobotController.getInputVoltage(),
self.rightLeader.getDutyCycle() * wpilib.RobotController.getInputVoltage(),
)
self.drivetrainSimulator.update(0.02)

View File

@@ -8,7 +8,7 @@ import wpilib
import enum
class ExampleSmartMotorController(wpilib.MotorController):
class ExampleSmartMotorController:
"""A simplified stub class that simulates the API of a common "smart" motor controller.
Has no actual functionality.
"""

View File

@@ -59,7 +59,7 @@ class DriveSubsystem(commands2.Subsystem):
)
# The robot's drive
self.drive = wpilib.DifferentialDrive(self.leftLeader, self.rightLeader)
self.drive = wpilib.DifferentialDrive(self.leftLeader.set, self.rightLeader.set)
self.profile = wpimath.TrapezoidProfile(
wpimath.TrapezoidProfile.Constraints(

View File

@@ -84,7 +84,7 @@ class Elevator:
# In this method, we update our simulation of what our elevator is doing
# First, we set our "inputs" (voltages)
self.elevatorSim.setInputVoltage(
self.motorSim.getVelocity() * wpilib.RobotController.getBatteryVoltage()
self.motorSim.getDutyCycle() * wpilib.RobotController.getBatteryVoltage()
)
# Next, we update it. The standard loop time is 20ms.
@@ -120,7 +120,7 @@ class Elevator:
def stop(self) -> None:
"""Stop the control loop and motor output."""
self.motor.set(0.0)
self.motor.setDutyCycle(0.0)
def reset(self) -> None:
"""Reset Exponential profile to begin from current position on enable."""

View File

@@ -68,7 +68,7 @@ class Elevator:
# In this method, we update our simulation of what our elevator is doing
# First, we set our "inputs" (voltages)
self.elevatorSim.setInputVoltage(
self.motorSim.getVelocity() * wpilib.RobotController.getBatteryVoltage()
self.motorSim.getDutyCycle() * wpilib.RobotController.getBatteryVoltage()
)
# Next, we update it. The standard loop time is 20ms.
@@ -99,7 +99,7 @@ class Elevator:
def stop(self) -> None:
"""Stop the control loop and motor output."""
self.controller.setGoal(0.0)
self.motor.set(0.0)
self.motor.setDutyCycle(0.0)
def updateTelemetry(self) -> None:
"""Update telemetry, including the mechanism visualization."""

View File

@@ -102,7 +102,7 @@ class MyRobot(wpilib.TimedRobot):
# To update our simulation, we set motor voltage inputs, update the
# simulation, and write the simulated velocities to our simulated encoder
self.flywheelSim.setInputVoltage(
self.flywheelMotor.get() * wpilib.RobotController.getInputVoltage()
self.flywheelMotor.getDutyCycle() * wpilib.RobotController.getInputVoltage()
)
self.flywheelSim.update(0.02)
self.encoderSim.setRate(self.flywheelSim.getAngularVelocity())

View File

@@ -56,5 +56,5 @@ class MyRobot(wpilib.TimedRobot):
self.wrist.setAngle(self.wristPot.get())
def teleopPeriodic(self):
self.elevatorMotor.set(self.joystick.getRawAxis(0))
self.wristMotor.set(self.joystick.getRawAxis(1))
self.elevatorMotor.setDutyCycle(self.joystick.getRawAxis(0))
self.wristMotor.setDutyCycle(self.joystick.getRawAxis(1))

View File

@@ -43,4 +43,4 @@ class MyRobot(wpilib.TimedRobot):
wpilib.SmartDashboard.putNumber("Encoder", self.encoder.getDistance())
def teleopPeriodic(self):
self.motor.set(self.joystick.getY())
self.motor.setDutyCycle(self.joystick.getY())

View File

@@ -56,7 +56,7 @@ class MyRobot(wpilib.TimedRobot):
pidOut = self.pidController.calculate(position)
# Apply PID output
self.elevatorMotor.set(pidOut)
self.elevatorMotor.setDutyCycle(pidOut)
# when the button is pressed once, the selected elevator setpoint is incremented
if self.joystick.getTriggerPressed():

View File

@@ -141,8 +141,8 @@ class Drivetrain:
# simulated encoder and gyro. We negate the right side so that positive
# voltages make the right side move forward.
self.drivetrainSimulator.setInputs(
self.leftLeader.get() * wpilib.RobotController.getInputVoltage(),
self.rightLeader.get() * wpilib.RobotController.getInputVoltage(),
self.leftLeader.getDutyCycle() * wpilib.RobotController.getInputVoltage(),
self.rightLeader.getDutyCycle() * wpilib.RobotController.getInputVoltage(),
)
self.drivetrainSimulator.update(0.02)

View File

@@ -71,16 +71,16 @@ class Drivetrain:
if fieldRelative:
robot_velocities = robot_velocities.toRobotRelative(self.imu.getRotation2d())
swerveModuleStates = self.kinematics.toSwerveModuleStates(
swerveModuleStates = self.kinematics.toSwerveModuleVelocities(
wpimath.ChassisVelocities.discretize(robot_velocities, periodSeconds)
)
wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
swerveModuleStates, kMaxVelocity
)
self.frontLeft.setDesiredState(swerveModuleStates[0])
self.frontRight.setDesiredState(swerveModuleStates[1])
self.backLeft.setDesiredState(swerveModuleStates[2])
self.backRight.setDesiredState(swerveModuleStates[3])
self.frontLeft.setDesiredVelocity(swerveModuleStates[0])
self.frontRight.setDesiredVelocity(swerveModuleStates[1])
self.backLeft.setDesiredVelocity(swerveModuleStates[2])
self.backRight.setDesiredVelocity(swerveModuleStates[3])
def updateOdometry(self) -> None:
"""Updates the field relative position of the robot."""

View File

@@ -76,12 +76,12 @@ class SwerveModule:
# to be continuous.
self.turningPIDController.enableContinuousInput(-math.pi, math.pi)
def getState(self) -> wpimath.SwerveModuleState:
"""Returns the current state of the module.
def getVelocity(self) -> wpimath.SwerveModuleVelocity:
"""Returns the current velocity of the module.
:returns: The current state of the module.
:returns: The current velocity of the module.
"""
return wpimath.SwerveModuleState(
return wpimath.SwerveModuleVelocity(
self.driveEncoder.getRate(),
wpimath.Rotation2d(self.turningEncoder.getDistance()),
)
@@ -96,32 +96,32 @@ class SwerveModule:
wpimath.Rotation2d(self.turningEncoder.getDistance()),
)
def setDesiredState(self, desiredState: wpimath.SwerveModuleState) -> None:
"""Sets the desired state for the module.
def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
"""Sets the desired velocity for the module.
:param desiredState: Desired state with velocity and angle.
:param desiredVelocity: Desired state with velocity and angle.
"""
encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
# Optimize the reference state to avoid spinning further than 90 degrees
desiredState.optimize(encoderRotation)
desiredVelocity.optimize(encoderRotation)
# Scale velocity by cosine of angle error. This scales down movement perpendicular to the desired
# direction of travel that can occur when modules change directions. This results in smoother
# driving.
desiredState.cosineScale(encoderRotation)
desiredVelocity.cosineScale(encoderRotation)
# Calculate the drive output from the drive PID controller.
driveOutput = self.drivePIDController.calculate(
self.driveEncoder.getRate(), desiredState.velocity
self.driveEncoder.getRate(), desiredVelocity.velocity
)
driveFeedforward = self.driveFeedforward.calculate(desiredState.velocity)
driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity)
# Calculate the turning motor output from the turning PID controller.
turnOutput = self.turningPIDController.calculate(
self.turningEncoder.getDistance(), desiredState.angle.radians()
self.turningEncoder.getDistance(), desiredVelocity.angle.radians()
)
turnFeedforward = self.turnFeedforward.calculate(

View File

@@ -81,13 +81,13 @@ class Drivetrain:
chassisVelocities = chassisVelocities.discretize(period)
states = self.kinematics.toSwerveModuleStates(chassisVelocities)
states = self.kinematics.toSwerveModuleVelocities(chassisVelocities)
wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(states, self.kMaxVelocity)
self.frontLeft.setDesiredState(states[0])
self.frontRight.setDesiredState(states[1])
self.backLeft.setDesiredState(states[2])
self.backRight.setDesiredState(states[3])
self.frontLeft.setDesiredVelocity(states[0])
self.frontRight.setDesiredVelocity(states[1])
self.backLeft.setDesiredVelocity(states[2])
self.backRight.setDesiredVelocity(states[3])
def updateOdometry(self) -> None:
"""Updates the field relative position of the robot."""

View File

@@ -77,12 +77,12 @@ class SwerveModule:
# to be continuous.
self.turningPIDController.enableContinuousInput(-math.pi, math.pi)
def getState(self) -> wpimath.SwerveModuleState:
def getState(self) -> wpimath.SwerveModuleVelocity :
"""Returns the current state of the module.
:returns: The current state of the module.
"""
return wpimath.SwerveModuleState(
return wpimath.SwerveModuleVelocity (
self.driveEncoder.getRate(),
wpimath.Rotation2d(self.turningEncoder.getDistance()),
)
@@ -97,31 +97,31 @@ class SwerveModule:
wpimath.Rotation2d(self.turningEncoder.getDistance()),
)
def setDesiredState(self, desiredState: wpimath.SwerveModuleState) -> None:
def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
"""Sets the desired state for the module.
:param desiredState: Desired state with velocity and angle.
:param desiredVelocity: Desired state with velocity and angle.
"""
encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
# Optimize the reference state to avoid spinning further than 90 degrees
desiredState.optimize(encoderRotation)
desiredVelocity.optimize(encoderRotation)
# Scale velocity by cosine of angle error. This scales down movement perpendicular to the
# desired direction of travel that can occur when modules change directions. This results
# in smoother driving.
desiredState.cosineScale(encoderRotation)
desiredVelocity.cosineScale(encoderRotation)
# Calculate the drive output from the drive PID controller.
driveOutput = self.drivePIDController.calculate(
self.driveEncoder.getRate(), desiredState.velocity
self.driveEncoder.getRate(), desiredVelocity.velocity
)
driveFeedforward = self.driveFeedforward.calculate(desiredState.velocity)
driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity)
# Calculate the turning motor output from the turning PID controller.
turnOutput = self.turningPIDController.calculate(
self.turningEncoder.getDistance(), desiredState.angle.radians()
self.turningEncoder.getDistance(), desiredVelocity.angle.radians()
)
turnFeedforward = self.turnFeedforward.calculate(

View File

@@ -93,7 +93,7 @@ class Shooter(Subsystem):
)
+ self.shooter_feedforward.calculate(target_velocity_radians)
)
self.feeder_motor.set(ShooterConstants.kFeederVelocity)
self.feeder_motor.setDutyCycle(ShooterConstants.kFeederVelocity)
def _stop_motors(interrupted: bool) -> None:
self.shooter_motor.stopMotor()

View File

@@ -20,17 +20,17 @@ class Intake:
)
def deploy(self) -> None:
self.piston.set(wpilib.DoubleSolenoid.Value.kForward)
self.piston.setDutyCycle(wpilib.DoubleSolenoid.Value.kForward)
def retract(self) -> None:
self.piston.set(wpilib.DoubleSolenoid.Value.kReverse)
self.motor.set(0) # turn off the motor
self.piston.setDutyCycle(wpilib.DoubleSolenoid.Value.kReverse)
self.motor.setDutyCycle(0) # turn off the motor
def activate(self, velocity: float) -> None:
if self.isDeployed():
self.motor.set(velocity)
self.motor.setDutyCycle(velocity)
else: # if piston isn't open, do nothing
self.motor.set(0)
self.motor.setDutyCycle(0)
def isDeployed(self) -> bool:
return self.piston.get() == wpilib.DoubleSolenoid.Value.kForward

View File

@@ -31,7 +31,7 @@ class Drivetrain(commands2.Subsystem):
self.rightEncoder = wpilib.Encoder(6, 7)
# Set up the differential drive controller
self.drive = wpilib.DifferentialDrive(self.leftMotor.set, self.rightMotor.set)
self.drive = wpilib.DifferentialDrive(self.leftMotor.setDutyCycle, self.rightMotor.setDutyCycle)
# TODO: these don't work
# wpiutil.SendableRegistry.addChild(self.drive, self.leftMotor)