diff --git a/robotpyExamples/ArmSimulation/subsystems/arm.py b/robotpyExamples/ArmSimulation/subsystems/arm.py index c4c9cac66d..71487c25fc 100644 --- a/robotpyExamples/ArmSimulation/subsystems/arm.py +++ b/robotpyExamples/ArmSimulation/subsystems/arm.py @@ -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) diff --git a/robotpyExamples/DifferentialDrivePoseEstimator/drivetrain.py b/robotpyExamples/DifferentialDrivePoseEstimator/drivetrain.py index c25611d74a..3886931522 100644 --- a/robotpyExamples/DifferentialDrivePoseEstimator/drivetrain.py +++ b/robotpyExamples/DifferentialDrivePoseEstimator/drivetrain.py @@ -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) diff --git a/robotpyExamples/DriveDistanceOffboard/examplesmartmotorcontroller.py b/robotpyExamples/DriveDistanceOffboard/examplesmartmotorcontroller.py index 1fc2077727..5f7d4f056d 100644 --- a/robotpyExamples/DriveDistanceOffboard/examplesmartmotorcontroller.py +++ b/robotpyExamples/DriveDistanceOffboard/examplesmartmotorcontroller.py @@ -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. """ diff --git a/robotpyExamples/DriveDistanceOffboard/subsystems/drivesubsystem.py b/robotpyExamples/DriveDistanceOffboard/subsystems/drivesubsystem.py index 9a95716f09..4baf52accc 100644 --- a/robotpyExamples/DriveDistanceOffboard/subsystems/drivesubsystem.py +++ b/robotpyExamples/DriveDistanceOffboard/subsystems/drivesubsystem.py @@ -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( diff --git a/robotpyExamples/ElevatorExponentialSimulation/subsystems/elevator.py b/robotpyExamples/ElevatorExponentialSimulation/subsystems/elevator.py index ee16abd3bb..f20ee52b7c 100644 --- a/robotpyExamples/ElevatorExponentialSimulation/subsystems/elevator.py +++ b/robotpyExamples/ElevatorExponentialSimulation/subsystems/elevator.py @@ -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.""" diff --git a/robotpyExamples/ElevatorSimulation/subsystems/elevator.py b/robotpyExamples/ElevatorSimulation/subsystems/elevator.py index 15f69e6604..7ea78107fc 100644 --- a/robotpyExamples/ElevatorSimulation/subsystems/elevator.py +++ b/robotpyExamples/ElevatorSimulation/subsystems/elevator.py @@ -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.""" diff --git a/robotpyExamples/FlywheelBangBangController/robot.py b/robotpyExamples/FlywheelBangBangController/robot.py index a25c72bc0f..67eac98d36 100755 --- a/robotpyExamples/FlywheelBangBangController/robot.py +++ b/robotpyExamples/FlywheelBangBangController/robot.py @@ -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()) diff --git a/robotpyExamples/Mechanism2d/robot.py b/robotpyExamples/Mechanism2d/robot.py index 76f46566fa..cbc465a206 100755 --- a/robotpyExamples/Mechanism2d/robot.py +++ b/robotpyExamples/Mechanism2d/robot.py @@ -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)) diff --git a/robotpyExamples/MotorControl/robot.py b/robotpyExamples/MotorControl/robot.py index 9b63940d5d..a41b2428e8 100755 --- a/robotpyExamples/MotorControl/robot.py +++ b/robotpyExamples/MotorControl/robot.py @@ -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()) diff --git a/robotpyExamples/PotentiometerPID/robot.py b/robotpyExamples/PotentiometerPID/robot.py index c672f24c56..01133c8503 100644 --- a/robotpyExamples/PotentiometerPID/robot.py +++ b/robotpyExamples/PotentiometerPID/robot.py @@ -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(): diff --git a/robotpyExamples/SimpleDifferentialDriveSimulation/drivetrain.py b/robotpyExamples/SimpleDifferentialDriveSimulation/drivetrain.py index bb891297f1..52eedbe96d 100644 --- a/robotpyExamples/SimpleDifferentialDriveSimulation/drivetrain.py +++ b/robotpyExamples/SimpleDifferentialDriveSimulation/drivetrain.py @@ -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) diff --git a/robotpyExamples/SwerveBot/drivetrain.py b/robotpyExamples/SwerveBot/drivetrain.py index 129eae9566..c245a5fdd6 100644 --- a/robotpyExamples/SwerveBot/drivetrain.py +++ b/robotpyExamples/SwerveBot/drivetrain.py @@ -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.""" diff --git a/robotpyExamples/SwerveBot/swervemodule.py b/robotpyExamples/SwerveBot/swervemodule.py index 310da91dc4..a890f8549c 100644 --- a/robotpyExamples/SwerveBot/swervemodule.py +++ b/robotpyExamples/SwerveBot/swervemodule.py @@ -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( diff --git a/robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py b/robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py index 4509b2584c..ec303329d5 100644 --- a/robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py +++ b/robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py @@ -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.""" diff --git a/robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py b/robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py index 74a16f73ca..a4f3101d86 100644 --- a/robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py +++ b/robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py @@ -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( diff --git a/robotpyExamples/SysId/subsystems/shooter.py b/robotpyExamples/SysId/subsystems/shooter.py index 0417626207..841f6f8b75 100644 --- a/robotpyExamples/SysId/subsystems/shooter.py +++ b/robotpyExamples/SysId/subsystems/shooter.py @@ -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() diff --git a/robotpyExamples/UnitTest/subsystems/intake.py b/robotpyExamples/UnitTest/subsystems/intake.py index feba22ef1a..a4007c2eef 100644 --- a/robotpyExamples/UnitTest/subsystems/intake.py +++ b/robotpyExamples/UnitTest/subsystems/intake.py @@ -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 diff --git a/robotpyExamples/XrpReference/subsystems/drivetrain.py b/robotpyExamples/XrpReference/subsystems/drivetrain.py index 516be57292..aa4c8c68ea 100644 --- a/robotpyExamples/XrpReference/subsystems/drivetrain.py +++ b/robotpyExamples/XrpReference/subsystems/drivetrain.py @@ -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)