From 89202cb4f81534e145ca54997f2fc3c4a3511c24 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Sun, 2 Apr 2023 12:13:54 +0300 Subject: [PATCH 1/5] initial commit --- commands-v2/rapid-react-command-bot/robot.py | 63 +++++++++++++++++++ .../rapid-react-command-bot/robotcontainer.py | 28 +++++++++ 2 files changed, 91 insertions(+) create mode 100755 commands-v2/rapid-react-command-bot/robot.py create mode 100644 commands-v2/rapid-react-command-bot/robotcontainer.py diff --git a/commands-v2/rapid-react-command-bot/robot.py b/commands-v2/rapid-react-command-bot/robot.py new file mode 100755 index 0000000..a0adb13 --- /dev/null +++ b/commands-v2/rapid-react-command-bot/robot.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 + +import typing +import wpilib +import commands2 + +from robotcontainer import RobotContainer + + +class MyRobot(commands2.TimedCommandRobot): + """ + Our default robot class, pass it to wpilib.run + + 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.container = 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.container.getAutonomousCommand() + + 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() + + +if __name__ == "__main__": + wpilib.run(MyRobot) diff --git a/commands-v2/rapid-react-command-bot/robotcontainer.py b/commands-v2/rapid-react-command-bot/robotcontainer.py new file mode 100644 index 0000000..185b5cc --- /dev/null +++ b/commands-v2/rapid-react-command-bot/robotcontainer.py @@ -0,0 +1,28 @@ +#!/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 commands2 +import commands2.button +import commands2.cmd + + +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 :class:`.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) -> None: + self.configureButtonBindings() + + def configureButtonBindings(self): + pass + + def getAutonomousCommand(self) -> commands2.Command: + return commands2.cmd.nothing() From fb4848b96eee584df19b13b5ac6c9b6ab2aa6020 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Sun, 2 Apr 2023 13:04:01 +0300 Subject: [PATCH 2/5] add constants and drivesubsystem seems like finallyDo doesn't exist in RobotPY, so I've done it with andThen and runOnce --- .../rapid-react-command-bot/constants.py | 74 +++++++++++++++++ .../subsystems/drive.py | 82 +++++++++++++++++++ 2 files changed, 156 insertions(+) create mode 100644 commands-v2/rapid-react-command-bot/constants.py create mode 100644 commands-v2/rapid-react-command-bot/subsystems/drive.py diff --git a/commands-v2/rapid-react-command-bot/constants.py b/commands-v2/rapid-react-command-bot/constants.py new file mode 100644 index 0000000..8be5256 --- /dev/null +++ b/commands-v2/rapid-react-command-bot/constants.py @@ -0,0 +1,74 @@ +#!/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 wpimath.units +import math + +"""The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean +constants. This class should not be used for any other purpose. All constants should be declared +globally (i.e. public static). Do not put anything functional in this class. + +It is advised to statically import this class (or one of its inner classes) wherever the +constants are needed, to reduce verbosity. +""" + +class DriveConstants: + kLeftMotor1Port = 0 + kLeftMotor2Port = 1 + kRightMotor1Port = 2 + kRightMotor2Port = 3 + + kLeftEncoderPorts = (0, 1) + kRightEncoderPorts = (2, 3) + kLeftEncoderReversed = False + kRightEncoderReversed = True + + kEncoderCPR = 1024 + kWheelDiameterMeters = wpimath.units.inchesToMeters(6) + # Assumes the encoders are directly mounted on the wheel shafts + kEncoderDistancePerPulse = (kWheelDiameterMeters * math.pi) / kEncoderCPR + +class ShooterConstants: + kEncoderPorts = (4, 5) + kEncoderReversed = False + kEncoderCPR = 1024 + # Distance units will be rotations + kEncoderDistancePerPulse = 1 / kEncoderCPR + + kShooterMotorPort = 4 + kFeederMotorPort = 5 + + kShooterFreeRPS = 5300 + kShooterTargetRPS = 4000 + kShooterToleranceRPS = 50 + + # These are not real PID gains, and will have to be tuned for your specific robot. + kP = 1 + + # On a real robot the feedforward constants should be empirically determined; these are + # reasonable guesses. + kSVolts = 0.05 + #Should have value 12V at free speed... + kVVoltSecondsPerRotation = 12.0 / kShooterFreeRPS + + kFeederSpeed = 0.5 + +class IntakeConstants: + kMotorPort = 6 + kSolenoidPorts = (0, 1) + +class StorageConstants: + kMotorPort = 7 + kBallSensorPort = 6 + +class AutoConstants: + kTimeoutSeconds = 3 + kDriveDistanceMeters = 2 + kDriveSpeed = 0.5 + +class OIConstants: + kDriverControllerPort = 0 \ No newline at end of file diff --git a/commands-v2/rapid-react-command-bot/subsystems/drive.py b/commands-v2/rapid-react-command-bot/subsystems/drive.py new file mode 100644 index 0000000..0fe0b8a --- /dev/null +++ b/commands-v2/rapid-react-command-bot/subsystems/drive.py @@ -0,0 +1,82 @@ +#!/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 wpilib +import wpilib.drive +import commands2 +import commands2.cmd + +import constants + +class Drive(commands2.SubsystemBase): + + def __init__(self) -> None: + """Creates a new Drive subsystem.""" + + super().__init__() + + # The motors on the left side of the drive. + self.leftMotors = wpilib.MotorControllerGroup( + wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor1Port), + wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor2Port) + ) + + # The motors on the right side of the drive. + self.rightMotors = wpilib.MotorControllerGroup( + wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor1Port), + wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port) + ) + + # The robot's drive + self.drive = wpilib.drive.DifferentialDrive( + self.leftMotors, self.rightMotors + ) + + # The left-side drive encoder + self.leftEncoder = wpilib.Encoder( + *constants.DriveConstants.kLeftEncoderPorts, + constants.DriveConstants.kLeftEncoderReversed + ) + + # The right-side drive encoder + self.rightEncoder = wpilib.Encoder( + *constants.DriveConstants.kRightEncoderPorts, + constants.DriveConstants.kRightEncoderReversed + ) + + # 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.rightMotors.setInverted(True) + + # Sets the distance per pulse for the encoders + self.leftEncoder.setDistancePerPulse(constants.DriveConstants.kEncoderDistancePerPulse) + self.rightEncoder.setDistancePerPulse(constants.DriveConstants.kEncoderDistancePerPulse) + + def arcadeDriveCommand(self, fwd: float, rot: float) -> commands2.Command: + # A split-stick arcade command, with forward/backward controlled by the left + # hand, and turning controlled by the right. + return self.run( + lambda: self.drive.arcadeDrive(fwd, rot) + ).setName("arcadeDrive") + + def driveDistanceCommand(self, distanceMeters: float, speed: float) -> commands2.SequentialCommandGroup: + return self.runOnce( + # Reset encoders at the start of the command + lambda: [self.leftEncoder.reset(), self.rightEncoder.reset()] + ).andThen( + # Drive forward at specified speed + self.run( + lambda: self.drive.arcadeDrive(speed, 0) + ) + ).until( + # End command when we've traveled the specified distance + lambda: max(self.leftEncoder.getDistance(), self.rightEncoder.getDistance()) >= distanceMeters + ).andThen( + # Stop the drive when the command ends + commands2.cmd.runOnce(lambda: self.drive.stopMotor(), [self]) + ) \ No newline at end of file From 72387e965cf77dc07057171fcda84b27f5f94254 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Fri, 21 Apr 2023 21:21:34 +0300 Subject: [PATCH 3/5] add rapidreactcommandbot --- .../rapid-react-command-bot/constants.py | 12 ++- .../rapidreactcommandbot.py | 92 +++++++++++++++++++ commands-v2/rapid-react-command-bot/robot.py | 11 ++- .../subsystems/drive.py | 64 +++++++------ .../subsystems/intake.py | 41 +++++++++ .../subsystems/shooter.py | 79 ++++++++++++++++ .../subsystems/storage.py | 42 +++++++++ run_tests.sh | 1 + 8 files changed, 307 insertions(+), 35 deletions(-) create mode 100644 commands-v2/rapid-react-command-bot/rapidreactcommandbot.py create mode 100644 commands-v2/rapid-react-command-bot/subsystems/intake.py create mode 100644 commands-v2/rapid-react-command-bot/subsystems/shooter.py create mode 100644 commands-v2/rapid-react-command-bot/subsystems/storage.py diff --git a/commands-v2/rapid-react-command-bot/constants.py b/commands-v2/rapid-react-command-bot/constants.py index 8be5256..3cad470 100644 --- a/commands-v2/rapid-react-command-bot/constants.py +++ b/commands-v2/rapid-react-command-bot/constants.py @@ -16,6 +16,7 @@ constants are needed, to reduce verbosity. """ + class DriveConstants: kLeftMotor1Port = 0 kLeftMotor2Port = 1 @@ -32,12 +33,13 @@ class DriveConstants: # Assumes the encoders are directly mounted on the wheel shafts kEncoderDistancePerPulse = (kWheelDiameterMeters * math.pi) / kEncoderCPR + class ShooterConstants: kEncoderPorts = (4, 5) kEncoderReversed = False kEncoderCPR = 1024 # Distance units will be rotations - kEncoderDistancePerPulse = 1 / kEncoderCPR + kEncoderDistancePerPulse = 1 / kEncoderCPR kShooterMotorPort = 4 kFeederMotorPort = 5 @@ -52,23 +54,27 @@ class ShooterConstants: # On a real robot the feedforward constants should be empirically determined; these are # reasonable guesses. kSVolts = 0.05 - #Should have value 12V at free speed... + # Should have value 12V at free speed... kVVoltSecondsPerRotation = 12.0 / kShooterFreeRPS kFeederSpeed = 0.5 + class IntakeConstants: kMotorPort = 6 kSolenoidPorts = (0, 1) + class StorageConstants: kMotorPort = 7 kBallSensorPort = 6 + class AutoConstants: kTimeoutSeconds = 3 kDriveDistanceMeters = 2 kDriveSpeed = 0.5 + class OIConstants: - kDriverControllerPort = 0 \ No newline at end of file + kDriverControllerPort = 0 diff --git a/commands-v2/rapid-react-command-bot/rapidreactcommandbot.py b/commands-v2/rapid-react-command-bot/rapidreactcommandbot.py new file mode 100644 index 0000000..4440838 --- /dev/null +++ b/commands-v2/rapid-react-command-bot/rapidreactcommandbot.py @@ -0,0 +1,92 @@ +#!/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 commands2 +import commands2.button +import commands2.cmd + +import subsystems.intake +import subsystems.drive +import subsystems.shooter +import subsystems.storage + +import constants + + +class RapidReactCommandBot: + """ + 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 :class:`.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) -> None: + # The robot's subsystems + self.drive = subsystems.drive.Drive() + self.intake = subsystems.intake.Intake() + self.storage = subsystems.storage.Storage() + self.shooter = subsystems.shooter.Shooter() + + # The driver's controller + self.driverController = commands2.button.CommandXboxController( + constants.OIConstants.kDriverControllerPort + ) + + self.configureButtonBindings() + + def configureButtonBindings(self): + """Use this method to define bindings between conditions and commands. These are useful for + automating robot behaviors based on button and sensor input. + + Should be called during :meth:`.Robot.robotInit`. + + Event binding methods are available on the {@link Trigger} class. + """ + # Automatically run the storage motor whenever the ball storage is not full, + # and turn it off whenever it fills. + commands2.Trigger(lambda: self.storage.isFull()).whileFalse( + self.storage.runCommand() + ) + + # Automatically disable and retract the intake whenever the ball storage is full. + commands2.Trigger(lambda: self.storage.isFull()).onTrue( + self.intake.retractCommand() + ) + + # Control the drive with split-stick arcade controls + self.drive.setDefaultCommand( + self.drive.arcadeDriveCommand( + lambda: self.driverController.getLeftY(), + lambda: self.driverController.getRightX(), + ) + ) + + # Deploy the intake with the X button + self.driverController.X().onTrue(self.intake.intakeCommand()) + # Retract the intake with the Y button + self.driverController.Y().onTrue(self.intake.retractCommand()) + + # Compose master shoot command + self.masterShootCommand = commands2.cmd.parallel( + self.shooter.shootCommand(constants.ShooterConstants.kShooterTargetRPS), + self.storage.runCommand(), + ) + # Since we composed this inline we should give it a name + self.masterShootCommand.setName("Shoot") + # Fire the shooter with the A button + self.driverController.A().onTrue(self.masterShootCommand) + + def getAutonomousCommand(self) -> commands2.Command: + """Use this to define the command that runs during autonomous. + + Scheduled during :meth:`.Robot.autonomousInit`. + """ + return self.drive.driveDistanceCommand( + constants.AutoConstants.kDriveDistanceMeters, + constants.AutoConstants.kDriveSpeed, + ).withTimeout(constants.AutoConstants.kTimeoutSeconds) diff --git a/commands-v2/rapid-react-command-bot/robot.py b/commands-v2/rapid-react-command-bot/robot.py index a0adb13..2120207 100755 --- a/commands-v2/rapid-react-command-bot/robot.py +++ b/commands-v2/rapid-react-command-bot/robot.py @@ -4,7 +4,7 @@ import wpilib import commands2 -from robotcontainer import RobotContainer +from rapidreactcommandbot import RapidReactCommandBot class MyRobot(commands2.TimedCommandRobot): @@ -23,9 +23,10 @@ def robotInit(self) -> None: initialization code. """ - # Instantiate our RobotContainer. This will perform all our button bindings, and put our - # autonomous chooser on the dashboard. - self.container = RobotContainer() + self.robot = RapidReactCommandBot() + + # Configure default commands and condition bindings on robot startup + self.robot.configureButtonBindings() def disabledInit(self) -> None: """This function is called once each time the robot enters Disabled mode.""" @@ -35,7 +36,7 @@ def disabledPeriodic(self) -> None: def autonomousInit(self) -> None: """This autonomous runs the autonomous command selected by your RobotContainer class.""" - self.autonomousCommand = self.container.getAutonomousCommand() + self.autonomousCommand = self.robot.getAutonomousCommand() if self.autonomousCommand: self.autonomousCommand.schedule() diff --git a/commands-v2/rapid-react-command-bot/subsystems/drive.py b/commands-v2/rapid-react-command-bot/subsystems/drive.py index 0fe0b8a..1ec7bfb 100644 --- a/commands-v2/rapid-react-command-bot/subsystems/drive.py +++ b/commands-v2/rapid-react-command-bot/subsystems/drive.py @@ -12,8 +12,8 @@ import constants -class Drive(commands2.SubsystemBase): +class Drive(commands2.SubsystemBase): def __init__(self) -> None: """Creates a new Drive subsystem.""" @@ -22,19 +22,17 @@ def __init__(self) -> None: # The motors on the left side of the drive. self.leftMotors = wpilib.MotorControllerGroup( wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor1Port), - wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor2Port) + wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor2Port), ) # The motors on the right side of the drive. self.rightMotors = wpilib.MotorControllerGroup( wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor1Port), - wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port) + wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port), ) # The robot's drive - self.drive = wpilib.drive.DifferentialDrive( - self.leftMotors, self.rightMotors - ) + self.drive = wpilib.drive.DifferentialDrive(self.leftMotors, self.rightMotors) # The left-side drive encoder self.leftEncoder = wpilib.Encoder( @@ -54,29 +52,41 @@ def __init__(self) -> None: self.rightMotors.setInverted(True) # Sets the distance per pulse for the encoders - self.leftEncoder.setDistancePerPulse(constants.DriveConstants.kEncoderDistancePerPulse) - self.rightEncoder.setDistancePerPulse(constants.DriveConstants.kEncoderDistancePerPulse) + self.leftEncoder.setDistancePerPulse( + constants.DriveConstants.kEncoderDistancePerPulse + ) + self.rightEncoder.setDistancePerPulse( + constants.DriveConstants.kEncoderDistancePerPulse + ) - def arcadeDriveCommand(self, fwd: float, rot: float) -> commands2.Command: + def arcadeDriveCommand(self, fwd, rot) -> commands2.Command: # A split-stick arcade command, with forward/backward controlled by the left # hand, and turning controlled by the right. - return self.run( - lambda: self.drive.arcadeDrive(fwd, rot) - ).setName("arcadeDrive") + command = self.run(lambda: self.drive.arcadeDrive(fwd(), rot())) + command.setName("arcadeDrive") + return command - def driveDistanceCommand(self, distanceMeters: float, speed: float) -> commands2.SequentialCommandGroup: - return self.runOnce( - # Reset encoders at the start of the command - lambda: [self.leftEncoder.reset(), self.rightEncoder.reset()] - ).andThen( - # Drive forward at specified speed - self.run( - lambda: self.drive.arcadeDrive(speed, 0) + def driveDistanceCommand( + self, distanceMeters: float, speed: float + ) -> commands2.SequentialCommandGroup: + return ( + self.runOnce( + # Reset encoders at the start of the command + lambda: [self.leftEncoder.reset(), self.rightEncoder.reset()] ) - ).until( - # End command when we've traveled the specified distance - lambda: max(self.leftEncoder.getDistance(), self.rightEncoder.getDistance()) >= distanceMeters - ).andThen( - # Stop the drive when the command ends - commands2.cmd.runOnce(lambda: self.drive.stopMotor(), [self]) - ) \ No newline at end of file + .andThen( + # Drive forward at specified speed + self.run(lambda: self.drive.arcadeDrive(speed, 0)) + ) + .until( + # End command when we've traveled the specified distance + lambda: max( + self.leftEncoder.getDistance(), self.rightEncoder.getDistance() + ) + >= distanceMeters + ) + .andThen( + # Stop the drive when the command ends + commands2.cmd.runOnce(lambda: self.drive.stopMotor(), [self]) + ) + ) diff --git a/commands-v2/rapid-react-command-bot/subsystems/intake.py b/commands-v2/rapid-react-command-bot/subsystems/intake.py new file mode 100644 index 0000000..fc01b46 --- /dev/null +++ b/commands-v2/rapid-react-command-bot/subsystems/intake.py @@ -0,0 +1,41 @@ +#!/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 wpilib +import commands2 +import commands2.cmd + +import constants + +class Intake(commands2.SubsystemBase): + + def __init__(self) -> None: + super().__init__() + + self.motor = wpilib.PWMSparkMax(constants.IntakeConstants.kMotorPort) + self.pistons = wpilib.DoubleSolenoid( + wpilib.PneumaticsModuleType.REVPH, + *constants.IntakeConstants.kSolenoidPorts + ) + + # Returns a command that deploys the intake, and then runs the intake motor indefinitely. + def intakeCommand(self) -> commands2.Command: + self.motorSetCommand = self.run(lambda: self.motor.set(1)) + self.motorSetCommand.setName("Intake") + return self.runOnce( + lambda: self.pistons.set(wpilib.DoubleSolenoid.Value.kForward) + ).andThen( + self.motorSetCommand + ) + + # Returns a command that turns off and retracts the intake. + def retractCommand(self) -> commands2.Command: + command = self.runOnce( + lambda: [self.motor.disable(), self.pistons.set(wpilib.DoubleSolenoid.Value.kReverse)] + ) + command.setName("Retract") + return command \ No newline at end of file diff --git a/commands-v2/rapid-react-command-bot/subsystems/shooter.py b/commands-v2/rapid-react-command-bot/subsystems/shooter.py new file mode 100644 index 0000000..b1e5af1 --- /dev/null +++ b/commands-v2/rapid-react-command-bot/subsystems/shooter.py @@ -0,0 +1,79 @@ +#!/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 commands2 +import commands2.cmd +import wpilib +import wpimath.controller + +import constants + + +class Shooter(commands2.SubsystemBase): + def __init__(self) -> None: + "The shooter subsystem for the robot." + + super().__init__() + + self.shooterMotor = wpilib.PWMSparkMax( + constants.ShooterConstants.kShooterMotorPort + ) + self.feederMotor = wpilib.PWMSparkMax( + constants.ShooterConstants.kFeederMotorPort + ) + self.shooterEncoder = wpilib.Encoder( + *constants.ShooterConstants.kEncoderPorts, + constants.ShooterConstants.kEncoderReversed + ) + self.shooterFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters( + constants.ShooterConstants.kSVolts, + constants.ShooterConstants.kVVoltSecondsPerRotation, + ) + self.shooterFeedback = wpimath.controller.PIDController( + constants.ShooterConstants.kP, 0, 0 + ) + + self.shooterFeedback.setTolerance( + constants.ShooterConstants.kShooterToleranceRPS + ) + self.shooterEncoder.setDistancePerPulse( + constants.ShooterConstants.kEncoderDistancePerPulse + ) + + # Set default command to turn off both the shooter and feeder motors, and then idle + self.defaultCommand = self.runOnce( + lambda: [self.shooterMotor.disable(), self.feederMotor.disable()] + ) + self.defaultCommand.setName("Idle") + self.setDefaultCommand( + self.defaultCommand.andThen(self.run(lambda: commands2.cmd.nothing())) + ) + + def shootCommand(self, setpointRotationsPerSecond: float): + """Returns a command to shoot the balls currently stored in the robot. Spins the shooter flywheel + up to the specified setpoint, and then runs the feeder motor. + + :param setpointRotationsPerSecond: The desired shooter velocity + """ + command = commands2.cmd.parallel( + # Run the shooter flywheel at the desired setpoint using feedforward and feedback + self.run( + lambda: self.shooterMotor.set( + self.shooterFeedforward.calculate(setpointRotationsPerSecond) + + self.shooterFeedback.calculate( + self.shooterEncoder.getRate(), setpointRotationsPerSecond + ) + ) + ), + # Wait until the shooter has reached the setpoint, and then run the feeder + commands2.cmd.waitUntil(lambda: self.shooterFeedback.atSetpoint()).andThen( + lambda: self.feederMotor.set(1) + ), + ) + command.setName("Shoot") + + return command diff --git a/commands-v2/rapid-react-command-bot/subsystems/storage.py b/commands-v2/rapid-react-command-bot/subsystems/storage.py new file mode 100644 index 0000000..f80cbc2 --- /dev/null +++ b/commands-v2/rapid-react-command-bot/subsystems/storage.py @@ -0,0 +1,42 @@ +#!/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 wpilib +import commands2 +import commands2.cmd + +import constants + + +class Storage(commands2.SubsystemBase): + def __init__(self) -> None: + "Create a new Storage subsystem." + + super().__init__() + + self.motor = wpilib.PWMSparkMax(constants.StorageConstants.kMotorPort) + self.ballSensor = wpilib.DigitalInput( + constants.StorageConstants.kBallSensorPort + ) + + # Set default command to turn off the storage motor and then idle + self.defaultCommand = commands2.cmd.nothing() + self.defaultCommand.setName("Idle") + self.setDefaultCommand( + self.runOnce(lambda: self.motor.disable()).andThen(self.defaultCommand) + ) + + def isFull(self) -> bool: + "Whether the ball storage is full." + return self.ballSensor.get() + + def runCommand(self) -> commands2.CommandBase: + "Returns a command that runs the storage motor indefinitely." + command = self.run(lambda: self.motor.set(1)) + command.setName("Run") + + return command diff --git a/run_tests.sh b/run_tests.sh index b311a55..8ce5d1b 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -15,6 +15,7 @@ BASE_TESTS=" commands-v2/hatchbot commands-v2/hatchbot-inlined commands-v2/ramsete + commands-v2/rapid-react-command-bot commands-v2/scheduler-event-logging commands-v2/selectcommand cscore-intermediate-vision From 22af60164a683ae1a916d750659cb2b44ba60c5b Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Fri, 21 Apr 2023 21:23:22 +0300 Subject: [PATCH 4/5] formatting fix --- .../subsystems/intake.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/commands-v2/rapid-react-command-bot/subsystems/intake.py b/commands-v2/rapid-react-command-bot/subsystems/intake.py index fc01b46..021e614 100644 --- a/commands-v2/rapid-react-command-bot/subsystems/intake.py +++ b/commands-v2/rapid-react-command-bot/subsystems/intake.py @@ -11,31 +11,31 @@ import constants -class Intake(commands2.SubsystemBase): +class Intake(commands2.SubsystemBase): def __init__(self) -> None: super().__init__() self.motor = wpilib.PWMSparkMax(constants.IntakeConstants.kMotorPort) self.pistons = wpilib.DoubleSolenoid( - wpilib.PneumaticsModuleType.REVPH, - *constants.IntakeConstants.kSolenoidPorts + wpilib.PneumaticsModuleType.REVPH, *constants.IntakeConstants.kSolenoidPorts ) - + # Returns a command that deploys the intake, and then runs the intake motor indefinitely. def intakeCommand(self) -> commands2.Command: self.motorSetCommand = self.run(lambda: self.motor.set(1)) self.motorSetCommand.setName("Intake") return self.runOnce( lambda: self.pistons.set(wpilib.DoubleSolenoid.Value.kForward) - ).andThen( - self.motorSetCommand - ) + ).andThen(self.motorSetCommand) # Returns a command that turns off and retracts the intake. def retractCommand(self) -> commands2.Command: command = self.runOnce( - lambda: [self.motor.disable(), self.pistons.set(wpilib.DoubleSolenoid.Value.kReverse)] + lambda: [ + self.motor.disable(), + self.pistons.set(wpilib.DoubleSolenoid.Value.kReverse), + ] ) command.setName("Retract") - return command \ No newline at end of file + return command From 1c76687d909572ac691ce1fb075c4e8da6d345f2 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Fri, 21 Apr 2023 21:25:17 +0300 Subject: [PATCH 5/5] fix armbot formatting and delete old robotcontainer --- commands-v2/armbot/robotcontainer.py | 3 +- .../rapid-react-command-bot/robotcontainer.py | 28 ------------------- 2 files changed, 2 insertions(+), 29 deletions(-) delete mode 100644 commands-v2/rapid-react-command-bot/robotcontainer.py diff --git a/commands-v2/armbot/robotcontainer.py b/commands-v2/armbot/robotcontainer.py index d507c5d..ba3c9bb 100644 --- a/commands-v2/armbot/robotcontainer.py +++ b/commands-v2/armbot/robotcontainer.py @@ -62,7 +62,8 @@ def configureButtonBindings(self) -> None: # Move the arm to neutral position when the 'B' button is pressed self.driver_controller.B().onTrue( commands2.cmd.run( - lambda: self.moveArm(constants.ArmConstants.kArmOffsetRads), [self.robot_arm] + lambda: self.moveArm(constants.ArmConstants.kArmOffsetRads), + [self.robot_arm], ) ) diff --git a/commands-v2/rapid-react-command-bot/robotcontainer.py b/commands-v2/rapid-react-command-bot/robotcontainer.py deleted file mode 100644 index 185b5cc..0000000 --- a/commands-v2/rapid-react-command-bot/robotcontainer.py +++ /dev/null @@ -1,28 +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 commands2 -import commands2.button -import commands2.cmd - - -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 :class:`.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) -> None: - self.configureButtonBindings() - - def configureButtonBindings(self): - pass - - def getAutonomousCommand(self) -> commands2.Command: - return commands2.cmd.nothing()