diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e71c91e2..f3770084 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -22,7 +22,7 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - os: ["ubuntu-22.04", "macos-14", "windows-2022"] + os: ["ubuntu-24.04", "macos-14", "windows-2022"] python_version: - '3.9' - '3.10' @@ -37,7 +37,7 @@ jobs: python-version: ${{ matrix.python_version }} - name: Install deps run: | - pip install 'robotpy[commands2,romi]<2026.0.0,>=2025.0.0b3' numpy pytest + pip install 'robotpy[commands2,romi]<2028,>=2027.0.0a2' numpy pytest - name: Run tests run: bash run_tests.sh shell: bash diff --git a/AddressableLED/robot.py b/AddressableLED/robot.py index ddd88cd8..1062a43e 100644 --- a/AddressableLED/robot.py +++ b/AddressableLED/robot.py @@ -12,9 +12,8 @@ class MyRobot(wpilib.TimedRobot): def robotInit(self): - # PWM Port 9 - # Must be a PWM header, not MXP or DIO - self.led = wpilib.AddressableLED(9) + # SmartIO port 1 + self.led = wpilib.AddressableLED(1) # LED Data self.ledData = [wpilib.AddressableLED.LEDData() for _ in range(kLEDBuffer)] @@ -28,7 +27,6 @@ def robotInit(self): # Set the data self.led.setData(self.ledData) - self.led.start() def robotPeriodic(self): # Fill the buffer with a rainbow diff --git a/ArmSimulation/physics.py b/ArmSimulation/physics.py index 85539726..96b59ca5 100644 --- a/ArmSimulation/physics.py +++ b/ArmSimulation/physics.py @@ -67,7 +67,7 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): Constants.kArmEncoderDistPerPulse, ) self.encoderSim = wpilib.simulation.EncoderSim(robot.arm.encoder) - self.motorSim = wpilib.simulation.PWMSim(robot.arm.motor.getChannel()) + self.motorSim = wpilib.simulation.PWMMotorControllerSim(robot.arm.motor) # Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm. self.mech2d = wpilib.Mechanism2d(60, 60) diff --git a/CANPDP/robot.py b/CANPDP/robot.py index 157fc3f3..b234e716 100755 --- a/CANPDP/robot.py +++ b/CANPDP/robot.py @@ -19,7 +19,7 @@ def robotInit(self): """Robot initialization function""" # Object for dealing with the Power Distribution Panel (PDP). - self.pdp = wpilib.PowerDistribution() + self.pdp = wpilib.PowerDistribution(0) # Put the PDP itself to the dashboard wpilib.SmartDashboard.putData("PDP", self.pdp) diff --git a/DutyCycleInput/robot.py b/DutyCycleInput/robot.py index 523d51e0..091cdfbe 100755 --- a/DutyCycleInput/robot.py +++ b/DutyCycleInput/robot.py @@ -12,7 +12,7 @@ class MyRobot(wpilib.TimedRobot): def robotInit(self): """Robot initialization function""" - self.dutyCycle = wpilib.DutyCycle(wpilib.DigitalInput(0)) + self.dutyCycle = wpilib.DutyCycle(0) def robotPeriodic(self): # Duty Cycle Frequency in Hz diff --git a/ElevatorSimulation/physics.py b/ElevatorSimulation/physics.py index 60eb36ad..84081408 100644 --- a/ElevatorSimulation/physics.py +++ b/ElevatorSimulation/physics.py @@ -56,7 +56,7 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): [0.01, 0.0], ) self.encoderSim = wpilib.simulation.EncoderSim(robot.encoder) - self.motorSim = wpilib.simulation.PWMSim(robot.motor.getChannel()) + self.motorSim = wpilib.simulation.PWMMotorControllerSim(robot.motor) # Create a Mechanism2d display of an elevator self.mech2d = wpilib.Mechanism2d(20, 50) diff --git a/FlywheelBangBangController/physics.py b/FlywheelBangBangController/physics.py index 326dcdd9..1e780de1 100644 --- a/FlywheelBangBangController/physics.py +++ b/FlywheelBangBangController/physics.py @@ -29,7 +29,9 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): self.physics_controller = physics_controller # Motors - self.flywheelMotor = wpilib.simulation.PWMSim(robot.flywheelMotor.getChannel()) + self.flywheelMotor = wpilib.simulation.PWMMotorControllerSim( + robot.flywheelMotor.getChannel() + ) # Sensors self.encoder = wpilib.simulation.EncoderSim(robot.encoder) diff --git a/GameData/robot.py b/GameData/robot.py index 02e857aa..88d175c5 100755 --- a/GameData/robot.py +++ b/GameData/robot.py @@ -8,6 +8,7 @@ import wpilib import ntcore +CAN_BUS = 0 PNUE_MOD_TYPE = wpilib.PneumaticsModuleType.CTREPCM @@ -18,10 +19,10 @@ class MyRobot(wpilib.TimedRobot): def robotInit(self): # A way of demonstrating the difference between the game data strings - self.blue = wpilib.Solenoid(PNUE_MOD_TYPE, 0) - self.red = wpilib.Solenoid(PNUE_MOD_TYPE, 1) - self.green = wpilib.Solenoid(PNUE_MOD_TYPE, 2) - self.yellow = wpilib.Solenoid(PNUE_MOD_TYPE, 3) + self.blue = wpilib.Solenoid(CAN_BUS, PNUE_MOD_TYPE, 0) + self.red = wpilib.Solenoid(CAN_BUS, PNUE_MOD_TYPE, 1) + self.green = wpilib.Solenoid(CAN_BUS, PNUE_MOD_TYPE, 2) + self.yellow = wpilib.Solenoid(CAN_BUS, PNUE_MOD_TYPE, 3) # Set game data to empty string by default self.gameData = "" # Get the SmartDashboard table from networktables diff --git a/HatchbotInlined/physics.py b/HatchbotInlined/physics.py index 42dbf78c..c1796534 100644 --- a/HatchbotInlined/physics.py +++ b/HatchbotInlined/physics.py @@ -38,10 +38,10 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): self.physics_controller = physics_controller # Motors - self.l_motor = wpilib.simulation.PWMSim( + self.l_motor = wpilib.simulation.PWMMotorControllerSim( robot.container.driveSubsystem.left1.getChannel() ) - self.r_motor = wpilib.simulation.PWMSim( + self.r_motor = wpilib.simulation.PWMMotorControllerSim( robot.container.driveSubsystem.right1.getChannel() ) diff --git a/HatchbotInlined/subsystems/hatchsubsystem.py b/HatchbotInlined/subsystems/hatchsubsystem.py index 180a8465..1f9b068a 100644 --- a/HatchbotInlined/subsystems/hatchsubsystem.py +++ b/HatchbotInlined/subsystems/hatchsubsystem.py @@ -16,9 +16,10 @@ def __init__(self) -> None: super().__init__() self.hatchSolenoid = wpilib.DoubleSolenoid( + 0, constants.kHatchSolenoidModule, constants.kHatchSolenoidModuleType, - *constants.kHatchSolenoidPorts + *constants.kHatchSolenoidPorts, ) def grabHatch(self) -> commands2.Command: diff --git a/HatchbotTraditional/physics.py b/HatchbotTraditional/physics.py index ef783d49..0f9c86c8 100644 --- a/HatchbotTraditional/physics.py +++ b/HatchbotTraditional/physics.py @@ -40,10 +40,10 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): self.physics_controller = physics_controller # Motors - self.l_motor = wpilib.simulation.PWMSim( + self.l_motor = wpilib.simulation.PWMMotorControllerSim( robot.container.drive.left1.getChannel() ) - self.r_motor = wpilib.simulation.PWMSim( + self.r_motor = wpilib.simulation.PWMMotorControllerSim( robot.container.drive.right1.getChannel() ) diff --git a/HatchbotTraditional/subsystems/hatchsubsystem.py b/HatchbotTraditional/subsystems/hatchsubsystem.py index 9229d1c8..2ff4fcd4 100644 --- a/HatchbotTraditional/subsystems/hatchsubsystem.py +++ b/HatchbotTraditional/subsystems/hatchsubsystem.py @@ -15,9 +15,10 @@ def __init__(self) -> None: super().__init__() self.hatchSolenoid = wpilib.DoubleSolenoid( + 0, constants.kHatchSolenoidModule, constants.kHatchSolenoidModuleType, - *constants.kHatchSolenoidPorts + *constants.kHatchSolenoidPorts, ) def grabHatch(self) -> None: diff --git a/Physics/src/physics.py b/Physics/src/physics.py index a919936e..1564be8c 100644 --- a/Physics/src/physics.py +++ b/Physics/src/physics.py @@ -38,14 +38,14 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): self.physics_controller = physics_controller # Motors - self.l_motor = wpilib.simulation.PWMSim(robot.l_motor.getChannel()) - self.r_motor = wpilib.simulation.PWMSim(robot.r_motor.getChannel()) + self.l_motor = wpilib.simulation.PWMMotorControllerSim(robot.l_motor) + self.r_motor = wpilib.simulation.PWMMotorControllerSim(robot.r_motor) self.dio1 = wpilib.simulation.DIOSim(robot.limit1) self.dio2 = wpilib.simulation.DIOSim(robot.limit2) self.ain2 = wpilib.simulation.AnalogInputSim(robot.position) - self.motor = wpilib.simulation.PWMSim(robot.motor.getChannel()) + self.motor = wpilib.simulation.PWMMotorControllerSim(robot.motor.getChannel()) # Gyro self.gyro = wpilib.simulation.AnalogGyroSim(robot.gyro) diff --git a/Physics4Wheel/src/physics.py b/Physics4Wheel/src/physics.py index 46b1f50a..c31cdcc3 100644 --- a/Physics4Wheel/src/physics.py +++ b/Physics4Wheel/src/physics.py @@ -35,10 +35,10 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): self.physics_controller = physics_controller # Motors - self.lf_motor = wpilib.simulation.PWMSim(robot.lf_motor.getChannel()) - # self.lr_motor = wpilib.simulation.PWMSim(2) - self.rf_motor = wpilib.simulation.PWMSim(robot.rf_motor.getChannel()) - # self.rr_motor = wpilib.simulation.PWMSim(4) + self.lf_motor = wpilib.simulation.PWMMotorControllerSim(robot.lf_motor) + # self.lr_motor = wpilib.simulation.PWMMotorControllerSim(2) + self.rf_motor = wpilib.simulation.PWMMotorControllerSim(robot.rf_motor) + # self.rr_motor = wpilib.simulation.PWMMotorControllerSim(4) # Gyro self.gyro = wpilib.simulation.AnalogGyroSim(robot.gyro) diff --git a/PhysicsCamSim/src/physics.py b/PhysicsCamSim/src/physics.py index 7283fbe8..b440edca 100644 --- a/PhysicsCamSim/src/physics.py +++ b/PhysicsCamSim/src/physics.py @@ -16,7 +16,7 @@ # from wpilib.simulation import ( - PWMSim, + PWMMotorControllerSim, DifferentialDrivetrainSim, EncoderSim, AnalogGyroSim, @@ -79,8 +79,8 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): ) # Create the motors. - self.l_motor = PWMSim(robot.leftMotor.getChannel()) - self.r_motor = PWMSim(robot.rightMotor.getChannel()) + self.l_motor = PWMMotorControllerSim(robot.leftMotor.getChannel()) + self.r_motor = PWMMotorControllerSim(robot.rightMotor.getChannel()) self.gyroSim = AnalogGyroSim(robot.gyro) diff --git a/PhysicsMecanum/src/physics.py b/PhysicsMecanum/src/physics.py index 74112b1a..b8944277 100644 --- a/PhysicsMecanum/src/physics.py +++ b/PhysicsMecanum/src/physics.py @@ -33,10 +33,10 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): self.physics_controller = physics_controller # Motors - self.lf_motor = wpilib.simulation.PWMSim(robot.frontLeftMotor.getChannel()) - self.lr_motor = wpilib.simulation.PWMSim(robot.rearLeftMotor.getChannel()) - self.rf_motor = wpilib.simulation.PWMSim(robot.frontRightMotor.getChannel()) - self.rr_motor = wpilib.simulation.PWMSim(robot.rearRightMotor.getChannel()) + self.lf_motor = wpilib.simulation.PWMMotorControllerSim(robot.frontLeftMotor) + self.lr_motor = wpilib.simulation.PWMMotorControllerSim(robot.rearLeftMotor) + self.rf_motor = wpilib.simulation.PWMMotorControllerSim(robot.frontRightMotor) + self.rr_motor = wpilib.simulation.PWMMotorControllerSim(robot.rearRightMotor) # Gyro self.gyro = wpilib.simulation.AnalogGyroSim(robot.gyro) diff --git a/PhysicsSPI/src/physics.py b/PhysicsSPI/src/physics.py index ac8441aa..0f34704f 100644 --- a/PhysicsSPI/src/physics.py +++ b/PhysicsSPI/src/physics.py @@ -44,14 +44,14 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): self.physics_controller = physics_controller # Motors - self.l_motor = wpilib.simulation.PWMSim(robot.l_motor.getChannel()) - self.r_motor = wpilib.simulation.PWMSim(robot.r_motor.getChannel()) + self.l_motor = wpilib.simulation.PWMMotorControllerSim(robot.l_motor) + self.r_motor = wpilib.simulation.PWMMotorControllerSim(robot.r_motor) self.dio1 = wpilib.simulation.DIOSim(robot.limit1) self.dio2 = wpilib.simulation.DIOSim(robot.limit2) self.ain2 = wpilib.simulation.AnalogInputSim(robot.position) - self.motor = wpilib.simulation.PWMSim(robot.motor.getChannel()) + self.motor = wpilib.simulation.PWMMotorControllerSim(robot.motor.getChannel()) # Gyro self.gyro = wpilib.simulation.ADXRS450_GyroSim(robot.gyro) diff --git a/RamseteCommand/constants.py b/RamseteCommand/constants.py deleted file mode 100644 index 7dea7a8b..00000000 --- a/RamseteCommand/constants.py +++ /dev/null @@ -1,64 +0,0 @@ -# -# 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. -# - -""" -A place for the constant values in the code that may be used in more than one place. -This offers a convenient resources to teams who need to make both quick and universal -changes. -""" - -from wpimath.kinematics import DifferentialDriveKinematics - -import math - -# ID for the driver's joystick. -kDriverControllerPort = 0 - -# 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 - -# In meters, distance between wheels on each side of robot. -kTrackWidthMeters = 0.69 -kDriveKinematics = DifferentialDriveKinematics(kTrackWidthMeters) - -# Encoder counts per revolution/rotation. -kEncoderCPR = 1024 -kWheelDiameterMeters = 0.15 - -# Assumes the encoders are directly mounted on the wheel shafts -kEncoderDistancePerPulse = (kWheelDiameterMeters * math.pi) / kEncoderCPR - -# These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! -# These characterization values MUST be determined either experimentally or theoretically -# for *your* robot's drive. -# The Robot Characterization Toolsuite provides a convenient tool for obtaining these -# values for your robot. -ksVolts = 0.22 -kvVoltSecondsPerMeter = 1.98 -kaVoltSecondsSquaredPerMeter = 0.2 - -# Example value only - as above, this must be tuned for your drive! -kPDriveVel = 8.5 - -# The max velocity and acceleration for our autonomous. -kMaxSpeedMetersPerSecond = 3 -kMaxAccelerationMetersPerSecondSquared = 1 - -# Reasonable baseline values for a RAMSETE follower in units of meters and seconds. -kRamseteB = 2 -kRamseteZeta = 0.7 - -# The number of motors on the robot. -kDrivetrainMotorCount = 4 diff --git a/RamseteCommand/physics.py b/RamseteCommand/physics.py deleted file mode 100644 index 1d74a217..00000000 --- a/RamseteCommand/physics.py +++ /dev/null @@ -1,106 +0,0 @@ -# -# 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. -# - -# -# See the documentation for more details on how this works -# -# The idea here is you provide a simulation object that overrides specific -# pieces of WPILib, and modifies motors/sensors accordingly depending on the -# state of the simulation. An example of this would be measuring a motor -# moving for a set period of time, and then changing a limit switch to turn -# on after that period of time. This can help you do more complex simulations -# of your robot code without too much extra effort. -# - -from wpilib import RobotController, ADXRS450_Gyro -from wpilib.simulation import ( - PWMSim, - DifferentialDrivetrainSim, - EncoderSim, - AnalogGyroSim, -) -from wpimath.system.plant import DCMotor, LinearSystemId - -import constants - -from pyfrc.physics.core import PhysicsInterface - -import typing - -if typing.TYPE_CHECKING: - from robot import MyRobot - - -class PhysicsEngine: - """ - Simulates a motor moving something that strikes two limit switches, - one on each end of the track. Obviously, this is not particularly - realistic, but it's good enough to illustrate the point - """ - - def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): - self.physics_controller = physics_controller - - # Motor simulation definitions. Each correlates to a motor defined in - # the drivetrain subsystem. - self.frontLeftMotor = PWMSim(constants.kLeftMotor1Port) - self.backLeftMotor = PWMSim(constants.kLeftMotor2Port) - self.frontRightMotor = PWMSim(constants.kRightMotor1Port) - self.backRightMotor = PWMSim(constants.kRightMotor2Port) - - self.system = LinearSystemId.identifyDrivetrainSystem( - constants.kvVoltSecondsPerMeter, # The linear velocity gain in volt seconds per distance. - constants.kaVoltSecondsSquaredPerMeter, # The linear acceleration gain, in volt seconds^2 per distance. - 1.5, # The angular velocity gain, in volt seconds per angle. - 0.3, # The angular acceleration gain, in volt seconds^2 per angle. - ) - - # The simulation model of the drivetrain. - self.drivesim = DifferentialDrivetrainSim( - # The state-space model for a drivetrain. - self.system, - # The robot's trackwidth, which is the distance between the wheels on the left side - # and those on the right side. The units is meters. - constants.kTrackWidthMeters, - # Four NEO drivetrain setup. - DCMotor.NEO(constants.kDrivetrainMotorCount), - # One to one output gearing. - 1, - # The radius of the drivetrain wheels in meters. - (constants.kWheelDiameterMeters / 2), - ) - - self.leftEncoderSim = EncoderSim(robot.robotContainer.robotDrive.leftEncoder) - self.rightEncoderSim = EncoderSim(robot.robotContainer.robotDrive.rightEncoder) - - self.gyro = AnalogGyroSim(robot.robotContainer.robotDrive.gyro) - - def update_sim(self, now: float, tm_diff: float) -> None: - """ - Called when the simulation parameters for the program need to be - updated. - - :param now: The current time as a float - :param tm_diff: The amount of time that has passed since the last - time that this function was called - """ - - # Simulate the drivetrain - l_motor = self.frontLeftMotor.getSpeed() - r_motor = self.frontRightMotor.getSpeed() - - self.gyro.setAngle(-self.drivesim.getHeading().degrees()) - - voltage = RobotController.getInputVoltage() - self.drivesim.setInputs(l_motor * voltage, -r_motor * voltage) - self.drivesim.update(tm_diff) - - self.leftEncoderSim.setDistance(self.drivesim.getLeftPosition()) - self.leftEncoderSim.setRate(self.drivesim.getLeftVelocity()) - self.rightEncoderSim.setDistance(self.drivesim.getRightPosition()) - self.rightEncoderSim.setRate(self.drivesim.getRightVelocity()) - - self.physics_controller.field.setRobotPose(self.drivesim.getPose()) diff --git a/RamseteCommand/robot.py b/RamseteCommand/robot.py deleted file mode 100644 index 96b01dda..00000000 --- a/RamseteCommand/robot.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/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. -# - -import typing -import wpilib -import commands2 - -from robotcontainer import RobotContainer - - -class MyRobot(commands2.TimedCommandRobot): - """ - Command v2 robots are encouraged to inherit from TimedCommandRobot, which - has an implementation of robotPeriodic which runs the scheduler for you - """ - - autonomousCommand: typing.Optional[commands2.Command] = None - - def robotInit(self) -> None: - """ - This function is run when the robot is first started up and should be used for any - initialization code. - """ - - # Instantiate our RobotContainer. This will perform all our button bindings, and put our - # autonomous chooser on the dashboard. - self.robotContainer = RobotContainer() - - def disabledInit(self) -> None: - """This function is called once each time the robot enters Disabled mode.""" - - def disabledPeriodic(self) -> None: - """This function is called periodically when disabled""" - - def autonomousInit(self) -> None: - """This autonomous runs the autonomous command selected by your RobotContainer class.""" - self.autonomousCommand = self.robotContainer.getAutonomousCommand() - - # schedule the autonomous command (example) - if self.autonomousCommand: - self.autonomousCommand.schedule() - - def autonomousPeriodic(self) -> None: - """This function is called periodically during autonomous""" - - 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. - if self.autonomousCommand: - self.autonomousCommand.cancel() - - def teleopPeriodic(self) -> None: - """This function is called periodically during operator control""" - - def testInit(self) -> None: - # Cancels all running commands at the start of test mode - commands2.CommandScheduler.getInstance().cancelAll() - - def testPeriodic(self) -> None: - """This function is called periodically during test mode""" diff --git a/RamseteCommand/robotcontainer.py b/RamseteCommand/robotcontainer.py deleted file mode 100644 index 3eb752e7..00000000 --- a/RamseteCommand/robotcontainer.py +++ /dev/null @@ -1,134 +0,0 @@ -# -# 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 wpilib import XboxController -from wpimath.controller import ( - RamseteController, - PIDController, - SimpleMotorFeedforwardMeters, -) -from wpimath.geometry import Pose2d, Rotation2d, Translation2d -from wpimath.trajectory.constraint import DifferentialDriveVoltageConstraint -from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator - -from commands2 import InstantCommand, RunCommand, RamseteCommand, cmd -from commands2.button import JoystickButton - -from subsystems.driveSubsystem import DriveSubsystem -import constants - - -class RobotContainer: - """ - 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 {@link 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): - # The robot's subsystems - self.robotDrive = DriveSubsystem() - - # The driver's controller. - self.driverController = XboxController(constants.kDriverControllerPort) - - # Configure the button bindings - self.configureButtons() - - # Configure default commands - # Set the default drive command to split-stick arcade drive - self.robotDrive.setDefaultCommand( - # A split-stick arcade command, with forward/backward controlled by the left - # hand, and turning controlled by the right. - RunCommand( - lambda: self.robotDrive.arcadeDrive( - -self.driverController.getLeftY(), - -self.driverController.getRightX(), - ), - self.robotDrive, - ) - ) - - def configureButtons(self): - """ - Use this method to define your button->command mappings. Buttons can be created by - instantiating a GenericHID or one of its subclasses (Joystick or XboxController), - and then calling passing it to a JoystickButton. - """ - - # Drive at half speed when the right bumper is held - ( - JoystickButton(self.driverController, XboxController.Button.kRightBumper) - .onTrue(InstantCommand(lambda: self.robotDrive.setMaxOutput(0.5))) - .onFalse(InstantCommand(lambda: self.robotDrive.setMaxOutput(1))) - ) - - def getAutonomousCommand(self): - """Use this to pass the autonomous command to the main {@link Robot} class.""" - - # Create a voltage constraint to ensure we don't accelerate too fast. - autoVoltageConstraint = DifferentialDriveVoltageConstraint( - SimpleMotorFeedforwardMeters( - constants.ksVolts, - constants.kvVoltSecondsPerMeter, - constants.kaVoltSecondsSquaredPerMeter, - ), - constants.kDriveKinematics, - maxVoltage=10, # 10 volts max. - ) - - # Create config for trajectory - config = TrajectoryConfig( - constants.kMaxSpeedMetersPerSecond, - constants.kMaxAccelerationMetersPerSecondSquared, - ) - # Add kinematics to ensure max speed is actually obeyed - config.setKinematics(constants.kDriveKinematics) - # Apply the voltage constraint - config.addConstraint(autoVoltageConstraint) - - # An example trajectory to follow. All of these units are in meters. - self.exampleTrajectory = TrajectoryGenerator.generateTrajectory( - # Start at the origin facing the +x direction. - Pose2d(0, 0, Rotation2d(0)), - # Pass through these two interior waypoints, making an 's' curve path - [Translation2d(1, 1), Translation2d(2, -1)], - # End 3 meters straight ahead of where we started, facing forward - Pose2d(3, 0, Rotation2d(0)), - # Pass config - config, - ) - - ramseteCommand = RamseteCommand( - self.exampleTrajectory, - self.robotDrive.getPose, - RamseteController(constants.kRamseteB, constants.kRamseteZeta), - SimpleMotorFeedforwardMeters( - constants.ksVolts, - constants.kvVoltSecondsPerMeter, - constants.kaVoltSecondsSquaredPerMeter, - ), - constants.kDriveKinematics, - self.robotDrive.getWheelSpeeds, - PIDController(constants.kPDriveVel, 0, 0), - PIDController(constants.kPDriveVel, 0, 0), - # RamseteCommand passes volts to the callback - self.robotDrive.tankDriveVolts, - [self.robotDrive], - ) - - # Reset odometry to the initial pose of the trajectory, run path following - # command, then stop at the end. - ( - cmd.runOnce( - lambda: self.robotDrive.resetOdometry( - self.exampleTrajectory.initialPose() - ) - ) - .andThen(ramseteCommand) - .andThen(cmd.runOnce(lambda: self.robotDrive.tankDriveVolts(0, 0))) - ) diff --git a/RamseteCommand/subsystems/__init__.py b/RamseteCommand/subsystems/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/RamseteCommand/subsystems/driveSubsystem.py b/RamseteCommand/subsystems/driveSubsystem.py deleted file mode 100644 index f81486a9..00000000 --- a/RamseteCommand/subsystems/driveSubsystem.py +++ /dev/null @@ -1,138 +0,0 @@ -# -# 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 Subsystem - -from wpilib import PWMSparkMax, Encoder, ADXRS450_Gyro -from wpilib.drive import DifferentialDrive - -from wpimath.kinematics import DifferentialDriveOdometry, DifferentialDriveWheelSpeeds - -import constants - - -class DriveSubsystem(Subsystem): - def __init__(self): - super().__init__() - - # The motors on the left side of the drive. - self.left1 = PWMSparkMax(constants.kLeftMotor1Port) - self.left2 = PWMSparkMax(constants.kLeftMotor2Port) - - # The motors on the right side of the drive. - self.right1 = PWMSparkMax(constants.kRightMotor1Port) - self.right2 = PWMSparkMax(constants.kRightMotor2Port) - - self.left1.addFollower(self.left2) - self.right1.addFollower(self.right2) - - # The robot's drive - self.drive = DifferentialDrive(self.left1, self.right1) - - # We need to invert one side of the drivetrain so that positive voltages - # result in both sides moving forward. Depending on how your robot's - # gearbox is constructed, you might have to invert the left side instead. - self.right1.setInverted(True) - - # The left-side drive encoder - self.leftEncoder = Encoder( - constants.kLeftEncoderPorts[0], - constants.kLeftEncoderPorts[1], - constants.kLeftEncoderReversed, - ) - - # The right-side drive encoder - self.rightEncoder = Encoder( - constants.kRightEncoderPorts[0], - constants.kRightEncoderPorts[1], - constants.kRightEncoderReversed, - ) - - # The gyro sensor - self.gyro = ADXRS450_Gyro() - - # Sets the distance per pulse for the encoders - self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) - self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) - - self.resetEncoders() - - self.odometry = DifferentialDriveOdometry( - self.gyro.getRotation2d(), - self.leftEncoder.getDistance(), - self.rightEncoder.getDistance(), - ) - - def periodic(self): - # Update the odometry in the periodic block - self.odometry.update( - self.gyro.getRotation2d(), - self.leftEncoder.getDistance(), - self.rightEncoder.getDistance(), - ) - - def getPose(self): - """Returns the currently-estimated pose of the robot.""" - return self.odometry.getPose() - - def getWheelSpeeds(self): - """Returns the current wheel speeds of the robot.""" - return DifferentialDriveWheelSpeeds( - self.leftEncoder.getRate(), self.rightEncoder.getRate() - ) - - def resetOdometry(self, pose): - """Resets the odometry to the specified pose.""" - self.resetEncoders() - self.odometry.resetPosition( - self.gyro.getRotation2d(), - self.leftEncoder.getDistance(), - self.rightEncoder.getDistance(), - pose, - ) - - def arcadeDrive(self, fwd, rot): - """Drives the robot using arcade controls.""" - self.drive.arcadeDrive(fwd, rot) - - def tankDriveVolts(self, leftVolts, rightVolts): - """Controls the left and right sides of the drive directly with voltages.""" - self.left1.setVoltage(leftVolts) - self.right1.setVoltage(rightVolts) - self.drive.feed() - - def resetEncoders(self): - """Resets the drive encoders to currently read a position of 0.""" - self.leftEncoder.reset() - self.rightEncoder.reset() - - def getAverageEncoderDistance(self): - """Gets the average distance of the two encoders.""" - return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2 - - def getLeftEncoder(self): - """Gets the left drive encoder.""" - return self.leftEncoder - - def getRightEncoder(self): - """Gets the right drive encoder.""" - return self.rightEncoder - - def setMaxOutput(self, maxOutput): - """Sets the max output of the drive. Useful for scaling the drive to drive more slowly.""" - self.drive.setMaxOutput(maxOutput) - - def zeroHeading(self): - """Zeroes the heading of the robot.""" - self.gyro.reset() - - def getHeading(self): - """Returns the heading of the robot.""" - return self.gyro.getRotation2d().degrees() - - def getTurnRate(self): - """Returns the turn rate of the robot.""" - return -self.gyro.getRate() diff --git a/Solenoid/robot.py b/Solenoid/robot.py index 988e0374..c9142438 100644 --- a/Solenoid/robot.py +++ b/Solenoid/robot.py @@ -28,19 +28,20 @@ def robotInit(self): # Solenoid corresponds to a single solenoid. # In this case, it's connected to channel 0 of a PH with the default CAN ID. self.solenoid = wpilib.Solenoid( - moduleType=wpilib.PneumaticsModuleType.REVPH, channel=0 + busId=0, moduleType=wpilib.PneumaticsModuleType.REVPH, channel=0 ) # DoubleSolenoid corresponds to a double solenoid. # In this case, it's connected to channels 1 and 2 of a PH with the default CAN ID. self.doubleSolenoid = wpilib.DoubleSolenoid( + busId=0, moduleType=wpilib.PneumaticsModuleType.REVPH, forwardChannel=1, reverseChannel=2, ) # Compressor connected to a PH with a default CAN ID (1) - self.compressor = wpilib.Compressor(wpilib.PneumaticsModuleType.REVPH) + self.compressor = wpilib.Compressor(0, wpilib.PneumaticsModuleType.REVPH) self.kSolenoidButton = 1 self.kDoubleSolenoidForwardButton = 2 diff --git a/Timed/tests/basic_test.py b/Timed/tests/basic_test.py index 032978b0..222eea23 100644 --- a/Timed/tests/basic_test.py +++ b/Timed/tests/basic_test.py @@ -34,7 +34,7 @@ def test_operator_control(control, robot): # run_robot will cause the robot to be initialized and robotInit to be called with control.run_robot(): - motorsim = wpilib.simulation.PWMSim(robot.motor.getChannel()) + motorsim = wpilib.simulation.PWMMotorControllerSim(robot.motor.getChannel()) joysim = wpilib.simulation.JoystickSim(robot.lstick) # Set the joystick value to something diff --git a/run_tests.sh b/run_tests.sh index b713092d..0508479f 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -66,7 +66,6 @@ BASE_TESTS=" " IGNORED_TESTS=" - RamseteCommand PhysicsCamSim/src StateSpaceArm StateSpaceElevator @@ -99,7 +98,7 @@ fi for t in ${TESTS}; do pushd $t > /dev/null pwd - if ! python3 -m robotpy test --builtin "${@:2}"; then + if ! robotpy test --builtin "${@:2}"; then EC=$? echo "Test in $(pwd) failed" exit 1