From a6bc3a6b49f5b8fd00ad11164da884e0a7ae070d Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Tue, 20 Dec 2022 23:39:04 +0300 Subject: [PATCH 01/17] added elevator trapezoid profile example --- .../examplesmartmotorcontroller.py | 93 +++++++++++++++++++ elevator-trapezoid-profile/robot.py | 51 ++++++++++ 2 files changed, 144 insertions(+) create mode 100644 elevator-trapezoid-profile/examplesmartmotorcontroller.py create mode 100644 elevator-trapezoid-profile/robot.py diff --git a/elevator-trapezoid-profile/examplesmartmotorcontroller.py b/elevator-trapezoid-profile/examplesmartmotorcontroller.py new file mode 100644 index 0000000..3d82aac --- /dev/null +++ b/elevator-trapezoid-profile/examplesmartmotorcontroller.py @@ -0,0 +1,93 @@ +# 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.interfaces +import enum + + +class ExampleSmartMotorController(wpilib.interfaces.MotorController): + """A simplified stub class that simulates the API of a common "smart" motor controller. + Has no actual functionality. + """ + + class PIDMode(enum.Enum): + kPosition = enum.auto() + kVelocity = enum.auto() + kMovementWitchcraft = enum.auto() + + def __init__(self, port: int) -> None: + """Creates a new ExampleSmartMotorController. + + Args: + port: The port for the controller. + """ + super().__init__() + + def setPID(self, kp: float, ki: float, kd: float) -> None: + """Example method for setting the PID gains of the smart controller. + + Args: + kp: The proportional gain. + ki: The integral gain. + kd: The derivative gain. + """ + pass + + def setSetPoint( + self, mode: PIDMode, setpoint: float, arbfeedforward: float + ) -> None: + """Example method for setting the setpoint of the smart controller in PID mode. + + Args: + mode: The mode of the PID controller. + setpoint: The controller setpoint. + arbfeedforward: An arbitrary feedforward output (from -1 to 1). + """ + pass + + def follow(self, leader: "ExampleSmartMotorController") -> None: + """Places this motor controller in follower mode. + + Args: + leader: The leader to follow. + """ + pass + + def getEncoderDistance(self) -> float: + """Returns the encoder distance. + + Returns: + The current encoder distance. + """ + return 0 + + def getEncoderRate(self) -> float: + """Returns the encoder rate. + + Returns: + The current encoder rate. + """ + return 0 + + def resetEncoder(self) -> None: + """Resets the encoder to zero distance.""" + pass + + def set(self, speed: float) -> None: + pass + + def get(self) -> float: + pass + + def setInverted(isInverted: bool) -> None: + pass + + def getInverted(self) -> bool: + pass + + def disable(self) -> None: + pass + + def stopMotor(self) -> None: + pass diff --git a/elevator-trapezoid-profile/robot.py b/elevator-trapezoid-profile/robot.py new file mode 100644 index 0000000..8df0fa1 --- /dev/null +++ b/elevator-trapezoid-profile/robot.py @@ -0,0 +1,51 @@ +import wpilib +import wpimath.controller +import wpimath.trajectory +import examplesmartmotorcontroller + + +class MyRobot(wpilib.TimedRobot): + + kDt = 0.02 + + def robotInit(self): + self.joystick = wpilib.Joystick(1) + self.motor = examplesmartmotorcontroller.ExampleSmartMotorController(1) + # Note: These gains are fake, and will have to be tuned for your robot. + self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 1.5) + + self.constraints = wpimath.trajectory.TrapezoidProfile.Constraints(1.75, 0.75) + + self.goal = wpimath.trajectory.TrapezoidProfile.State() + self.setpoint = wpimath.trajectory.TrapezoidProfile.State() + + # Note: These gains are fake, and will have to be tuned for your robot. + self.motor.setPID(1.3, 0.0, 0.7) + + def teleopPeriodic(self): + if self.joystick.getRawButtonPressed(2): + self.goal = wpimath.trajectory.TrapezoidProfile.State(5, 0) + elif self.joystick.getRawButtonPressed(3): + self.goal = wpimath.trajectory.TrapezoidProfile.State(0, 0) + + # Create a motion profile with the given maximum velocity and maximum + # acceleration constraints for the next setpoint, the desired goal, and the + # current setpoint. + profile = wpimath.trajectory.TrapezoidProfile( + self.constraints, self.goal, self.setpoint + ) + + # Retrieve the profiled setpoint for the next timestep. This setpoint moves + # toward the goal while obeying the constraints. + self.setpoint = profile.calculate(self.kDt) + + # Send setpoint to offboard controller PID + self.motor.setSetPoint( + examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition, + self.setpoint.position, + self.feedforward.calculate(self.setpoint.velocity) / 12, + ) + + +if __name__ == "__main__": + wpilib.run(MyRobot) From 2c842977834fb5624ffa623545ae274ab870f8fd Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Tue, 20 Dec 2022 23:39:42 +0300 Subject: [PATCH 02/17] updated tests --- run_tests.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/run_tests.sh b/run_tests.sh index 90410a3..92e702c 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -27,6 +27,7 @@ BASE_TESTS=" tank-drive timed/src elevator-profiled-pid + elevator-trapezoid-profile " IGNORED_TESTS=" From 0e9b86598daf579e7eb6e43696005a6ea7554114 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Tue, 20 Dec 2022 23:42:32 +0300 Subject: [PATCH 03/17] No self parameter in setSetPoint function --- commands-v2/armbotoffboard/examplesmartmotorcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/commands-v2/armbotoffboard/examplesmartmotorcontroller.py b/commands-v2/armbotoffboard/examplesmartmotorcontroller.py index 5974f7c..8d1417d 100644 --- a/commands-v2/armbotoffboard/examplesmartmotorcontroller.py +++ b/commands-v2/armbotoffboard/examplesmartmotorcontroller.py @@ -34,7 +34,7 @@ def setPID(self, kp: float, ki: float, kd: float) -> None: """ pass - def setSetPoint(mode: PIDMode, setpoint: float, arbfeedforward: float) -> None: + def setSetPoint(self, mode: PIDMode, setpoint: float, arbfeedforward: float) -> None: """Example method for setting the setpoint of the smart controller in PID mode. Args: From 98588f71c90a368dd2a43b30573f7896f31685cf Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Tue, 20 Dec 2022 23:45:45 +0300 Subject: [PATCH 04/17] format with black --- commands-v2/armbotoffboard/examplesmartmotorcontroller.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/commands-v2/armbotoffboard/examplesmartmotorcontroller.py b/commands-v2/armbotoffboard/examplesmartmotorcontroller.py index 8d1417d..3d82aac 100644 --- a/commands-v2/armbotoffboard/examplesmartmotorcontroller.py +++ b/commands-v2/armbotoffboard/examplesmartmotorcontroller.py @@ -34,7 +34,9 @@ def setPID(self, kp: float, ki: float, kd: float) -> None: """ pass - def setSetPoint(self, mode: PIDMode, setpoint: float, arbfeedforward: float) -> None: + def setSetPoint( + self, mode: PIDMode, setpoint: float, arbfeedforward: float + ) -> None: """Example method for setting the setpoint of the smart controller in PID mode. Args: From c2c05baff3308de1a4942b14a03addf693a656bb Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Tue, 20 Dec 2022 23:59:17 +0300 Subject: [PATCH 05/17] license and enviroment stuff --- elevator-trapezoid-profile/robot.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/elevator-trapezoid-profile/robot.py b/elevator-trapezoid-profile/robot.py index 8df0fa1..5d7c2ce 100644 --- a/elevator-trapezoid-profile/robot.py +++ b/elevator-trapezoid-profile/robot.py @@ -1,3 +1,10 @@ +#!/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 wpimath.controller import wpimath.trajectory From ae0b4362392b73896eb7470e42773533b2cdc91e Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Sat, 24 Dec 2022 23:09:55 +0300 Subject: [PATCH 06/17] Add gyro-drive-commands example --- .../commands/turntoangle.py | 49 +++++++ .../commands/turntoangleprofiled.py | 54 ++++++++ commands-v2/gyro-drive-commands/constants.py | 48 +++++++ commands-v2/gyro-drive-commands/robot.py | 78 +++++++++++ .../gyro-drive-commands/robotcontainer.py | 109 +++++++++++++++ .../subsystems/drivesubsystem.py | 129 ++++++++++++++++++ run_tests.sh | 1 + 7 files changed, 468 insertions(+) create mode 100644 commands-v2/gyro-drive-commands/commands/turntoangle.py create mode 100644 commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py create mode 100644 commands-v2/gyro-drive-commands/constants.py create mode 100755 commands-v2/gyro-drive-commands/robot.py create mode 100644 commands-v2/gyro-drive-commands/robotcontainer.py create mode 100644 commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py diff --git a/commands-v2/gyro-drive-commands/commands/turntoangle.py b/commands-v2/gyro-drive-commands/commands/turntoangle.py new file mode 100644 index 0000000..57cf56d --- /dev/null +++ b/commands-v2/gyro-drive-commands/commands/turntoangle.py @@ -0,0 +1,49 @@ +# 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 wpimath.controller + +from subsystems.drivesubsystem import DriveSubsystem + +import constants + +class TurnToAngle(commands2.PIDCommand): + """A command that will turn the robot to the specified angle.""" + + def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: + """ + Turns to robot to the specified angle. + + :param: targetAngleDegrees The angle to turn to + :param: drive The drive subsystem to + """ + super().__init__( + wpimath.controller.PIDController( + constants.DriveConstants.kTurnP, + constants.DriveConstants.kTurnI, + constants.DriveConstants.kTurnD + ), + # Close loop on heading + drive.getHeading, + # Set reference to target + targetAngleDegrees, + # Pipe output to turn robot + lambda output: drive.arcadeDrive(0, output), + # Require the drive + [drive] + ) + + # Set the controller to be continuous (because it is an angle controller) + self.getController().enableContinuousInput(-180, 180) + + # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the + # setpoint before it is considered as having reached the reference + self.getController().setTolerance(constants.DriveConstants.kTurnToleranceDeg, constants.DriveConstants.kTurnRateToleranceDegPerS) + + def isFinished(self) -> bool: + # End when the controller is at the reference. + return self.getController().atSetpoint() \ No newline at end of file diff --git a/commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py b/commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py new file mode 100644 index 0000000..4905b53 --- /dev/null +++ b/commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py @@ -0,0 +1,54 @@ +# 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 wpimath.controller +import wpimath.trajectory + +from subsystems.drivesubsystem import DriveSubsystem + +import constants + +class TurnToAngleProfiled(commands2.ProfiledPIDCommand): + """A command that will turn the robot to the specified angle using a motion profile.""" + + def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: + """ + Turns to robot to the specified angle. + + :param: targetAngleDegrees The angle to turn to + :param: drive The drive subsystem to + """ + super().__init__( + wpimath.controller.ProfiledPIDController( + constants.DriveConstants.kTurnP, + constants.DriveConstants.kTurnI, + constants.DriveConstants.kTurnD, + wpimath.trajectory.TrapezoidProfile.Constraints( + constants.DriveConstants.kMaxTurnRateDegPerS, + constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared + ) + ), + # Close loop on heading + drive.getHeading, + # Set reference to target + targetAngleDegrees, + # Pipe output to turn robot + lambda output, setpoint: drive.arcadeDrive(0, output), + # Require the drive + [drive] + ) + + # Set the controller to be continuous (because it is an angle controller) + self.getController().enableContinuousInput(-180, 180) + + # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the + # setpoint before it is considered as having reached the reference + self.getController().setTolerance(constants.DriveConstants.kTurnToleranceDeg, constants.DriveConstants.kTurnRateToleranceDegPerS) + + def isFinished(self) -> bool: + # End when the controller is at the reference. + return self.getController().atSetpoint() \ No newline at end of file diff --git a/commands-v2/gyro-drive-commands/constants.py b/commands-v2/gyro-drive-commands/constants.py new file mode 100644 index 0000000..b24ee21 --- /dev/null +++ b/commands-v2/gyro-drive-commands/constants.py @@ -0,0 +1,48 @@ +# 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. +""" + +import math + + +class DriveConstants: + kLeftMotor1Port = 0 + kLeftMotor2Port = 1 + kRightMotor1Port = 2 + kRightMotor2Port = 3 + + kLeftEncoderPorts = (0, 1) + kRightEncoderPorts = (2, 3) + kLeftEncoderReversed = False + kRightEncoderReversed = True + + kEncoderCPR = 1024 + kWheelDiameterInches = 6 + + # Assumes the encoders are directly mounted on the wheel shafts + kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR + + kGyroReversed = False + + kStabilizationP = 1 + kStabilizationI = 0.5 + kStabilizationD = 0 + + kTurnP = 1 + kTurnI = 0 + kTurnD = 0 + + kMaxTurnRateDegPerS = 100 + kMaxTurnAccelerationDegPerSSquared = 300 + + kTurnToleranceDeg = 5 + kTurnRateToleranceDegPerS = 10 # degrees per second + +class OIConstants: + kDriverControllerPort = 0 diff --git a/commands-v2/gyro-drive-commands/robot.py b/commands-v2/gyro-drive-commands/robot.py new file mode 100755 index 0000000..0b7561a --- /dev/null +++ b/commands-v2/gyro-drive-commands/robot.py @@ -0,0 +1,78 @@ +#!/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 +import commands2.cmd + +import robotcontainer + +""" +The VM is configured to automatically run this class, and to call the functions corresponding to +each mode, as described in the TimedRobot documentation. If you change the name of this class or +the package after creating this project, you must also update the build.gradle file in the +project. +""" + + +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 + """ + + def robotInit(self) -> None: + """ + This function is run when the robot is first started up and should be used for any + initialization code. + """ + self.autonomousCommand: typing.Optional[commands2.Command] = None + + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.container = 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.container.getAutonomousCommand() + + # schedule the autonomous command (example) + if self.autonomousCommand is not None: + self.autonomousCommand.schedule() + else: + print("no auto command?") + + 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 is not None: + 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/gyro-drive-commands/robotcontainer.py b/commands-v2/gyro-drive-commands/robotcontainer.py new file mode 100644 index 0000000..439a3b9 --- /dev/null +++ b/commands-v2/gyro-drive-commands/robotcontainer.py @@ -0,0 +1,109 @@ +# 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 wpimath.controller + +import commands2 +import commands2.cmd +import commands2.button + +import constants +import subsystems.drivesubsystem +import commands.turntoangle +import commands.turntoangleprofiled + +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): + """The container for the robot. Contains subsystems, OI devices, and commands.""" + # The robot's subsystems + self.robotDrive = subsystems.drivesubsystem.DriveSubsystem() + + # The driver's controller + self.driverController = commands2.button.CommandPS4Controller(constants.OIConstants.kDriverControllerPort) + + # Configure the button bindings + self.configureButtonBindings() + + # 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. + commands2.RunCommand( + lambda: self.robotDrive.arcadeDrive( + -self.driverController.getLeftY(), -self.driverController.getRightX() + ), + [self.robotDrive] + ) + ) + + + def configureButtonBindings(self): + """ + Use this method to define your button->command mappings. Buttons can be created via the button + factories on commands2.button.CommandGenericHID or one of its + subclasses (commands2.button.CommandJoystick or command2.button.CommandXboxController). + """ + # Drive at half speed when the right bumper is held + commands2.button.JoystickButton( + self.driverController, + wpilib.PS4Controller.Button.kR1 + ).onTrue( + commands2.InstantCommand((lambda: self.robotDrive.setMaxOutput(0.5)), [self.robotDrive]) + ).onFalse( + commands2.InstantCommand((lambda: self.robotDrive.setMaxOutput(1)), [self.robotDrive]) + ) + + # Stabilize robot to drive straight with gyro when left bumper is held + commands2.button.JoystickButton( + self.driverController, + wpilib.PS4Controller.Button.kL1 + ).whileTrue( + commands2.PIDCommand( + wpimath.controller.PIDController( + constants.DriveConstants.kStabilizationP, + constants.DriveConstants.kStabilizationI, + constants.DriveConstants.kStabilizationD + ), + # Close the loop on the turn rate + self.robotDrive.getTurnRate, + # Setpoint is 0 + 0, + # Pipe the output to the turning controls + lambda output: self.robotDrive.arcadeDrive(-self.driverController.getLeftY(), output), + # Require the robot drive + [self.robotDrive] + ) + ) + + # Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout + commands2.button.JoystickButton( + self.driverController, wpilib.PS4Controller.Button.kCross + ).onTrue( + commands.turntoangle.TurnToAngle(90, self.robotDrive).withTimeout(5) + ) + + # Turn to -90 degrees with a profile when the Circle button is pressed, with a 5 second timeout + commands2.button.JoystickButton( + self.driverController, wpilib.PS4Controller.Button.kCircle + ).onTrue( + commands.turntoangleprofiled.TurnToAngleProfiled(-90, self.robotDrive).withTimeout(5) + ) + + def getAutonomousCommand(self) -> commands2.Command: + """ + Use this to pass the autonomous command to the main :class:`.Robot` class. + + :returns: the command to run in autonomous + """ + return commands2.InstantCommand() diff --git a/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py b/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py new file mode 100644 index 0000000..1fcd20b --- /dev/null +++ b/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py @@ -0,0 +1,129 @@ +# 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 math + +import constants + + +class DriveSubsystem(commands2.SubsystemBase): + def __init__(self) -> None: + """Creates a new DriveSubsystem""" + 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[0], + constants.DriveConstants.kLeftEncoderPorts[1], + constants.DriveConstants.kLeftEncoderReversed, + ) + + # The right-side drive encoder + self.rightEncoder = wpilib.Encoder( + constants.DriveConstants.kRightEncoderPorts[0], + constants.DriveConstants.kRightEncoderPorts[1], + 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 + ) + + self.gyro = wpilib.ADXRS450_Gyro() + + def arcadeDrive(self, fwd: float, rot: float): + """ + Drives the robot using arcade controls. + + :param fwd: the commanded forward movement + :param rot: the commanded rotation + """ + self.drive.arcadeDrive(fwd, rot) + + 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. + + :returns: the average of the two encoder readings + """ + return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 + + def getLeftEncoder(self) -> wpilib.Encoder: + """ + Gets the left drive encoder. + + :returns: the left drive encoder + """ + return self.leftEncoder + + def getRightEncoder(self) -> wpilib.Encoder: + """ + Gets the right drive encoder. + + :returns: the right drive encoder + """ + return self.rightEncoder + + def setMaxOutput(self, maxOutput: float): + """ + Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + + :param maxOutput: the maximum output to which the drive will be constrained + """ + 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. + + :returns: the robot's heading in degrees, from 180 to 180 + """ + return math.remainder(self.gyro.getAngle(), 180) * (-1 if constants.DriveConstants.kGyroReversed else 1) + + def getTurnRate(self): + """ + Returns the turn rate of the robot. + + :returns: The turn rate of the robot, in degrees per second + """ + return self.gyro.getRate() * (-1 if constants.DriveConstants.kGyroReversed else 1) diff --git a/run_tests.sh b/run_tests.sh index 92e702c..664a493 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -28,6 +28,7 @@ BASE_TESTS=" timed/src elevator-profiled-pid elevator-trapezoid-profile + commands-v2/gyro-drive-commands " IGNORED_TESTS=" From 30d426dab5d5d1cefb4bbe9134fba8dfe3ca2e6c Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Sat, 24 Dec 2022 23:10:54 +0300 Subject: [PATCH 07/17] format with black --- .../commands/turntoangle.py | 12 ++++-- .../commands/turntoangleprofiled.py | 14 +++--- commands-v2/gyro-drive-commands/constants.py | 5 ++- .../gyro-drive-commands/robotcontainer.py | 43 +++++++++++-------- .../subsystems/drivesubsystem.py | 10 +++-- 5 files changed, 52 insertions(+), 32 deletions(-) diff --git a/commands-v2/gyro-drive-commands/commands/turntoangle.py b/commands-v2/gyro-drive-commands/commands/turntoangle.py index 57cf56d..38b5416 100644 --- a/commands-v2/gyro-drive-commands/commands/turntoangle.py +++ b/commands-v2/gyro-drive-commands/commands/turntoangle.py @@ -11,6 +11,7 @@ import constants + class TurnToAngle(commands2.PIDCommand): """A command that will turn the robot to the specified angle.""" @@ -25,7 +26,7 @@ def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: wpimath.controller.PIDController( constants.DriveConstants.kTurnP, constants.DriveConstants.kTurnI, - constants.DriveConstants.kTurnD + constants.DriveConstants.kTurnD, ), # Close loop on heading drive.getHeading, @@ -34,7 +35,7 @@ def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: # Pipe output to turn robot lambda output: drive.arcadeDrive(0, output), # Require the drive - [drive] + [drive], ) # Set the controller to be continuous (because it is an angle controller) @@ -42,8 +43,11 @@ def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the # setpoint before it is considered as having reached the reference - self.getController().setTolerance(constants.DriveConstants.kTurnToleranceDeg, constants.DriveConstants.kTurnRateToleranceDegPerS) + self.getController().setTolerance( + constants.DriveConstants.kTurnToleranceDeg, + constants.DriveConstants.kTurnRateToleranceDegPerS, + ) def isFinished(self) -> bool: # End when the controller is at the reference. - return self.getController().atSetpoint() \ No newline at end of file + return self.getController().atSetpoint() diff --git a/commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py b/commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py index 4905b53..0e1a55f 100644 --- a/commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py +++ b/commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py @@ -12,6 +12,7 @@ import constants + class TurnToAngleProfiled(commands2.ProfiledPIDCommand): """A command that will turn the robot to the specified angle using a motion profile.""" @@ -29,8 +30,8 @@ def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: constants.DriveConstants.kTurnD, wpimath.trajectory.TrapezoidProfile.Constraints( constants.DriveConstants.kMaxTurnRateDegPerS, - constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared - ) + constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared, + ), ), # Close loop on heading drive.getHeading, @@ -39,7 +40,7 @@ def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: # Pipe output to turn robot lambda output, setpoint: drive.arcadeDrive(0, output), # Require the drive - [drive] + [drive], ) # Set the controller to be continuous (because it is an angle controller) @@ -47,8 +48,11 @@ def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the # setpoint before it is considered as having reached the reference - self.getController().setTolerance(constants.DriveConstants.kTurnToleranceDeg, constants.DriveConstants.kTurnRateToleranceDegPerS) + self.getController().setTolerance( + constants.DriveConstants.kTurnToleranceDeg, + constants.DriveConstants.kTurnRateToleranceDegPerS, + ) def isFinished(self) -> bool: # End when the controller is at the reference. - return self.getController().atSetpoint() \ No newline at end of file + return self.getController().atSetpoint() diff --git a/commands-v2/gyro-drive-commands/constants.py b/commands-v2/gyro-drive-commands/constants.py index b24ee21..7f8546e 100644 --- a/commands-v2/gyro-drive-commands/constants.py +++ b/commands-v2/gyro-drive-commands/constants.py @@ -40,9 +40,10 @@ class DriveConstants: kMaxTurnRateDegPerS = 100 kMaxTurnAccelerationDegPerSSquared = 300 - + kTurnToleranceDeg = 5 - kTurnRateToleranceDegPerS = 10 # degrees per second + kTurnRateToleranceDegPerS = 10 # degrees per second + class OIConstants: kDriverControllerPort = 0 diff --git a/commands-v2/gyro-drive-commands/robotcontainer.py b/commands-v2/gyro-drive-commands/robotcontainer.py index 439a3b9..9929108 100644 --- a/commands-v2/gyro-drive-commands/robotcontainer.py +++ b/commands-v2/gyro-drive-commands/robotcontainer.py @@ -14,6 +14,7 @@ import commands.turntoangle import commands.turntoangleprofiled + class RobotContainer: """ This class is where the bulk of the robot should be declared. Since Command-based is a @@ -29,25 +30,27 @@ def __init__(self): self.robotDrive = subsystems.drivesubsystem.DriveSubsystem() # The driver's controller - self.driverController = commands2.button.CommandPS4Controller(constants.OIConstants.kDriverControllerPort) + self.driverController = commands2.button.CommandPS4Controller( + constants.OIConstants.kDriverControllerPort + ) # Configure the button bindings self.configureButtonBindings() # Configure default commands - # Set the default drive command to split-stick arcade drive + # 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. commands2.RunCommand( lambda: self.robotDrive.arcadeDrive( - -self.driverController.getLeftY(), -self.driverController.getRightX() + -self.driverController.getLeftY(), + -self.driverController.getRightX(), ), - [self.robotDrive] + [self.robotDrive], ) ) - def configureButtonBindings(self): """ Use this method to define your button->command mappings. Buttons can be created via the button @@ -56,48 +59,52 @@ def configureButtonBindings(self): """ # Drive at half speed when the right bumper is held commands2.button.JoystickButton( - self.driverController, - wpilib.PS4Controller.Button.kR1 + self.driverController, wpilib.PS4Controller.Button.kR1 ).onTrue( - commands2.InstantCommand((lambda: self.robotDrive.setMaxOutput(0.5)), [self.robotDrive]) + commands2.InstantCommand( + (lambda: self.robotDrive.setMaxOutput(0.5)), [self.robotDrive] + ) ).onFalse( - commands2.InstantCommand((lambda: self.robotDrive.setMaxOutput(1)), [self.robotDrive]) + commands2.InstantCommand( + (lambda: self.robotDrive.setMaxOutput(1)), [self.robotDrive] + ) ) # Stabilize robot to drive straight with gyro when left bumper is held commands2.button.JoystickButton( - self.driverController, - wpilib.PS4Controller.Button.kL1 + self.driverController, wpilib.PS4Controller.Button.kL1 ).whileTrue( commands2.PIDCommand( wpimath.controller.PIDController( constants.DriveConstants.kStabilizationP, constants.DriveConstants.kStabilizationI, - constants.DriveConstants.kStabilizationD + constants.DriveConstants.kStabilizationD, ), # Close the loop on the turn rate self.robotDrive.getTurnRate, # Setpoint is 0 0, # Pipe the output to the turning controls - lambda output: self.robotDrive.arcadeDrive(-self.driverController.getLeftY(), output), + lambda output: self.robotDrive.arcadeDrive( + -self.driverController.getLeftY(), output + ), # Require the robot drive - [self.robotDrive] + [self.robotDrive], ) ) # Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout commands2.button.JoystickButton( self.driverController, wpilib.PS4Controller.Button.kCross - ).onTrue( - commands.turntoangle.TurnToAngle(90, self.robotDrive).withTimeout(5) - ) + ).onTrue(commands.turntoangle.TurnToAngle(90, self.robotDrive).withTimeout(5)) # Turn to -90 degrees with a profile when the Circle button is pressed, with a 5 second timeout commands2.button.JoystickButton( self.driverController, wpilib.PS4Controller.Button.kCircle ).onTrue( - commands.turntoangleprofiled.TurnToAngleProfiled(-90, self.robotDrive).withTimeout(5) + commands.turntoangleprofiled.TurnToAngleProfiled( + -90, self.robotDrive + ).withTimeout(5) ) def getAutonomousCommand(self) -> commands2.Command: diff --git a/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py b/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py index 1fcd20b..518479f 100644 --- a/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py +++ b/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py @@ -118,12 +118,16 @@ def getHeading(self): :returns: the robot's heading in degrees, from 180 to 180 """ - return math.remainder(self.gyro.getAngle(), 180) * (-1 if constants.DriveConstants.kGyroReversed else 1) - + return math.remainder(self.gyro.getAngle(), 180) * ( + -1 if constants.DriveConstants.kGyroReversed else 1 + ) + def getTurnRate(self): """ Returns the turn rate of the robot. :returns: The turn rate of the robot, in degrees per second """ - return self.gyro.getRate() * (-1 if constants.DriveConstants.kGyroReversed else 1) + return self.gyro.getRate() * ( + -1 if constants.DriveConstants.kGyroReversed else 1 + ) From edd072ba1738581ba36047d09aff2e441346b4e1 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Thu, 29 Dec 2022 23:45:24 +0300 Subject: [PATCH 08/17] Added most of the code, only thing that is left is the commands folder. --- .../drive-distance-offboard/constants.py | 37 ++++++ .../examplesmartmotorcontroller.py | 93 ++++++++++++++ commands-v2/drive-distance-offboard/robot.py | 78 +++++++++++ .../drive-distance-offboard/robotcontainer.py | 107 ++++++++++++++++ .../subsystems/drivesubsystem.py | 121 ++++++++++++++++++ 5 files changed, 436 insertions(+) create mode 100644 commands-v2/drive-distance-offboard/constants.py create mode 100644 commands-v2/drive-distance-offboard/examplesmartmotorcontroller.py create mode 100755 commands-v2/drive-distance-offboard/robot.py create mode 100644 commands-v2/drive-distance-offboard/robotcontainer.py create mode 100644 commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py diff --git a/commands-v2/drive-distance-offboard/constants.py b/commands-v2/drive-distance-offboard/constants.py new file mode 100644 index 0000000..6bd1f9d --- /dev/null +++ b/commands-v2/drive-distance-offboard/constants.py @@ -0,0 +1,37 @@ +# 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. +""" + +import math + + +class DriveConstants: + kLeftMotor1Port = 0 + kLeftMotor2Port = 1 + kRightMotor1Port = 2 + kRightMotor2Port = 3 + + """ + 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 SysId tool provides a convenient method for obtaining these values for your robot. + """ + ksVolts = 1 + kvVoltSecondsPerMeter = 0.8 + kaVoltSecondsSquaredPerMeter = 0.15 + + kp = 1 + + kMaxSpeedMetersPerSecond = 3 + kMaxAccelerationMetersPerSecondSquared = 3 + + +class OIConstants: + kDriverControllerPort = 0 diff --git a/commands-v2/drive-distance-offboard/examplesmartmotorcontroller.py b/commands-v2/drive-distance-offboard/examplesmartmotorcontroller.py new file mode 100644 index 0000000..3d82aac --- /dev/null +++ b/commands-v2/drive-distance-offboard/examplesmartmotorcontroller.py @@ -0,0 +1,93 @@ +# 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.interfaces +import enum + + +class ExampleSmartMotorController(wpilib.interfaces.MotorController): + """A simplified stub class that simulates the API of a common "smart" motor controller. + Has no actual functionality. + """ + + class PIDMode(enum.Enum): + kPosition = enum.auto() + kVelocity = enum.auto() + kMovementWitchcraft = enum.auto() + + def __init__(self, port: int) -> None: + """Creates a new ExampleSmartMotorController. + + Args: + port: The port for the controller. + """ + super().__init__() + + def setPID(self, kp: float, ki: float, kd: float) -> None: + """Example method for setting the PID gains of the smart controller. + + Args: + kp: The proportional gain. + ki: The integral gain. + kd: The derivative gain. + """ + pass + + def setSetPoint( + self, mode: PIDMode, setpoint: float, arbfeedforward: float + ) -> None: + """Example method for setting the setpoint of the smart controller in PID mode. + + Args: + mode: The mode of the PID controller. + setpoint: The controller setpoint. + arbfeedforward: An arbitrary feedforward output (from -1 to 1). + """ + pass + + def follow(self, leader: "ExampleSmartMotorController") -> None: + """Places this motor controller in follower mode. + + Args: + leader: The leader to follow. + """ + pass + + def getEncoderDistance(self) -> float: + """Returns the encoder distance. + + Returns: + The current encoder distance. + """ + return 0 + + def getEncoderRate(self) -> float: + """Returns the encoder rate. + + Returns: + The current encoder rate. + """ + return 0 + + def resetEncoder(self) -> None: + """Resets the encoder to zero distance.""" + pass + + def set(self, speed: float) -> None: + pass + + def get(self) -> float: + pass + + def setInverted(isInverted: bool) -> None: + pass + + def getInverted(self) -> bool: + pass + + def disable(self) -> None: + pass + + def stopMotor(self) -> None: + pass diff --git a/commands-v2/drive-distance-offboard/robot.py b/commands-v2/drive-distance-offboard/robot.py new file mode 100755 index 0000000..0b7561a --- /dev/null +++ b/commands-v2/drive-distance-offboard/robot.py @@ -0,0 +1,78 @@ +#!/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 +import commands2.cmd + +import robotcontainer + +""" +The VM is configured to automatically run this class, and to call the functions corresponding to +each mode, as described in the TimedRobot documentation. If you change the name of this class or +the package after creating this project, you must also update the build.gradle file in the +project. +""" + + +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 + """ + + def robotInit(self) -> None: + """ + This function is run when the robot is first started up and should be used for any + initialization code. + """ + self.autonomousCommand: typing.Optional[commands2.Command] = None + + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.container = 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.container.getAutonomousCommand() + + # schedule the autonomous command (example) + if self.autonomousCommand is not None: + self.autonomousCommand.schedule() + else: + print("no auto command?") + + 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 is not None: + 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/drive-distance-offboard/robotcontainer.py b/commands-v2/drive-distance-offboard/robotcontainer.py new file mode 100644 index 0000000..18fc7a0 --- /dev/null +++ b/commands-v2/drive-distance-offboard/robotcontainer.py @@ -0,0 +1,107 @@ +# 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 commands2.button +import wpimath.trajectory + +import constants +import subsystems.drivesubsystem + + +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): + # The robot's subsystems + self.robotDrive = subsystems.drivesubsystem.DriveSubsystem() + + # Retained command references + self.driveFullSpeed = commands2.cmd.runOnce( + lambda: self.robotDrive.setMaxOutput(1), [self.robotDrive] + ) + self.driveHalfSpeed = commands2.cmd.runOnce( + lambda: self.robotDrive.setMaxOutput(0.5), [self.robotDrive] + ) + + # The driver's controller + self.driverController = commands2.button.CommandXboxController( + constants.OIConstants.kDriverControllerPort + ) + + # Configure the button bindings + self.configureButtonBindings() + + # 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. + commands2.cmd.run( + lambda: self.robotDrive.arcadeDrive( + -self.driverController.getLeftY(), + -self.driverController.getRightX(), + ), + [self.robotDrive], + ) + ) + + def configureButtonBindings(self): + """ + Use this method to define your button->command mappings. Buttons can be created via the button + factories on commands2.button.CommandGenericHID or one of its + subclasses (commands2.button.CommandJoystick or command2.button.CommandXboxController). + """ + + # Configure your button bindings here + + # We can bind commands while retaining references to them in RobotContainer + + # Drive at half speed when the bumper is held + self.driverController.rightBumper().onTrue(self.driveHalfSpeed).onFalse( + self.driveFullSpeed + ) + + # Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds + self.driverController.A().onTrue( + commands.drivedistanceprofiled.DriveDistanceProfiled( + 3, self.robotDrive + ).withTimeout(10) + ) + + # Do the same thing as above when the 'B' button is pressed, but defined inline + self.driverController.B().onTrue( + commands2.TrapezoidProfileCommand( + # Limit the max acceleration and velocity + wpimath.trajectory.TrapezoidProfile.Constraints( + constants.DriveConstants.kMaxSpeedMetersPerSecond, + constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared, + ), + # End at desired position in meters; implicitly starts at 0 + wpimath.trajectory.TrapezoidProfile.State(3, 0), + # Pipe the profile state to the drive + lambda setpointState: self.robotDrive.setDriveStates( + setpointState, setpointState + ), + [self.robotDrive], + ) + .beforeStarting(self.robotDrive.resetEncoders()) + .withTimeout(10) + ) + + def getAutonomousCommand(self) -> commands2.Command: + """ + Use this to pass the autonomous command to the main :class:`.Robot` class. + + :returns: the command to run in autonomous + """ + return commands2.cmd.nothing() diff --git a/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py b/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py new file mode 100644 index 0000000..6ae5e95 --- /dev/null +++ b/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py @@ -0,0 +1,121 @@ +# 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 wpimath.controller +import wpimath.trajectory + +import constants +import examplesmartmotorcontroller + + +class DriveSubsystem(commands2.SubsystemBase): + def __init__(self) -> None: + """Creates a new DriveSubsystem""" + super().__init__() + + # The motors on the left side of the drive. + self.leftLeader = examplesmartmotorcontroller.ExampleSmartMotorController( + constants.DriveConstants.kLeftMotor1Port + ) + + self.leftFollower = examplesmartmotorcontroller.ExampleSmartMotorController( + constants.DriveConstants.kLeftMotor2Port + ) + + # The motors on the right side of the drive. + self.rightLeader = examplesmartmotorcontroller.ExampleSmartMotorController( + constants.DriveConstants.kRightMotor1Port + ) + + self.rightFollower = examplesmartmotorcontroller.ExampleSmartMotorController( + constants.DriveConstants.kRightMotor1Port + ) + + # 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.rightLeader.setInverted(True) + + # You might need to not do this depending on the specific motor controller + # that you are using -- contact the respective vendor's documentation for + # more details. + self.rightFollower.setInverted(True) + + self.leftFollower.follow(self.leftLeader) + self.rightFollower.follow(self.rightLeader) + + self.leftLeader.setPID(constants.DriveConstants.kp, 0, 0) + self.rightLeader.setPID(constants.DriveConstants.kp, 0, 0) + + # The feedforward controller (note that these are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!) + # check DriveConstants for more information. + self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters( + constants.DriveConstants.ksVolts, + constants.DriveConstants.kvVoltSecondsPerMeter, + constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared + ) + + # The robot's drive + self.drive = wpilib.drive.DifferentialDrive(self.leftLeader, self.rightLeader) + + def arcadeDrive(self, fwd: float, rot: float): + """ + Drives the robot using arcade controls. + + :param fwd: the commanded forward movement + :param rot: the commanded rotation + """ + self.drive.arcadeDrive(fwd, rot) + + def setDriveStates(self, left: wpimath.trajectory.TrapezoidProfile.State, right: wpimath.trajectory.TrapezoidProfile.State): + """ + Attempts to follow the given drive states using offboard PID. + + :param left: The left wheel state. + :param right: The right wheel state. + """ + self.leftLeader.setSetPoint( + examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition, + left.position, + self.feedforward.calculate(left.velocity) + ) + + self.rightLeader.setSetPoint( + examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition, + right.position, + self.feedforward.calculate(right.velocity) + ) + + def getLeftEncoderDistance(self) -> wpilib.Encoder: + """ + Returns the left drive encoder distance. + + :returns: the left drive encoder distance + """ + return self.leftLeader.getEncoderDistance() + + def getRightEncoderDistance(self) -> wpilib.Encoder: + """ + Returns the right drive encoder distance. + + :returns: the right drive encoder distance + """ + return self.rightLeader.getEncoderDistance() + + def resetEncoders(self): + """Resets the drive encoders""" + self.leftLeader.resetEncoder() + self.rightLeader.resetEncoder() + + def setMaxOutput(self, maxOutput: float): + """ + Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + + :param maxOutput: the maximum output to which the drive will be constrained + """ + self.drive.setMaxOutput(maxOutput) From 8ed82eb43f5ae053f6ffb0d96fcd8d2f27df7fa5 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Fri, 30 Dec 2022 22:48:09 +0300 Subject: [PATCH 09/17] add commands.DriveDistanceProfiled and fix examplesmartmotorcontroller. --- .../commands/drivedistanceprofiled.py | 44 +++++++++++++++++++ .../examplesmartmotorcontroller.py | 2 +- .../drive-distance-offboard/robotcontainer.py | 15 ++++--- .../subsystems/drivesubsystem.py | 16 ++++--- 4 files changed, 64 insertions(+), 13 deletions(-) create mode 100644 commands-v2/drive-distance-offboard/commands/drivedistanceprofiled.py diff --git a/commands-v2/drive-distance-offboard/commands/drivedistanceprofiled.py b/commands-v2/drive-distance-offboard/commands/drivedistanceprofiled.py new file mode 100644 index 0000000..2fb2f4a --- /dev/null +++ b/commands-v2/drive-distance-offboard/commands/drivedistanceprofiled.py @@ -0,0 +1,44 @@ +#!/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 wpimath.trajectory + +import subsystems.drivesubsystem +import constants + + +class DriveDistanceProfiled(commands2.TrapezoidProfileCommand): + """Drives a set distance using a motion profile.""" + + def __init__( + self, meters: float, drive: subsystems.drivesubsystem.DriveSubsystem + ) -> None: + """ + Creates a new DriveDistanceProfiled command. + + :param: meters The distance to drive. + :param: drive The drive subsystem to use. + """ + super().__init__( + wpimath.trajectory.TrapezoidProfile( + # Limit the max acceleration and velocity + wpimath.trajectory.TrapezoidProfile.Constraints( + constants.DriveConstants.kMaxSpeedMetersPerSecond, + constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared, + ), + # End at desired position in meters; implicitly starts at 0 + wpimath.trajectory.TrapezoidProfile.State(meters, 0), + ), + # Pipe the profile state to the drive + lambda setpointState: drive.setDriveStates(setpointState, setpointState), + # Require the drive + [drive], + ) + # Reset drive encoders since we're starting at 0 + drive.resetEncoders() diff --git a/commands-v2/drive-distance-offboard/examplesmartmotorcontroller.py b/commands-v2/drive-distance-offboard/examplesmartmotorcontroller.py index 3d82aac..2b7d60a 100644 --- a/commands-v2/drive-distance-offboard/examplesmartmotorcontroller.py +++ b/commands-v2/drive-distance-offboard/examplesmartmotorcontroller.py @@ -80,7 +80,7 @@ def set(self, speed: float) -> None: def get(self) -> float: pass - def setInverted(isInverted: bool) -> None: + def setInverted(self, isInverted: bool) -> None: pass def getInverted(self) -> bool: diff --git a/commands-v2/drive-distance-offboard/robotcontainer.py b/commands-v2/drive-distance-offboard/robotcontainer.py index 18fc7a0..092b854 100644 --- a/commands-v2/drive-distance-offboard/robotcontainer.py +++ b/commands-v2/drive-distance-offboard/robotcontainer.py @@ -10,6 +10,7 @@ import constants import subsystems.drivesubsystem +import commands.drivedistanceprofiled class RobotContainer: @@ -81,13 +82,15 @@ def configureButtonBindings(self): # Do the same thing as above when the 'B' button is pressed, but defined inline self.driverController.B().onTrue( commands2.TrapezoidProfileCommand( - # Limit the max acceleration and velocity - wpimath.trajectory.TrapezoidProfile.Constraints( - constants.DriveConstants.kMaxSpeedMetersPerSecond, - constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared, + wpimath.trajectory.TrapezoidProfile( + # Limit the max acceleration and velocity + wpimath.trajectory.TrapezoidProfile.Constraints( + constants.DriveConstants.kMaxSpeedMetersPerSecond, + constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared, + ), + # End at desired position in meters; implicitly starts at 0 + wpimath.trajectory.TrapezoidProfile.State(3, 0), ), - # End at desired position in meters; implicitly starts at 0 - wpimath.trajectory.TrapezoidProfile.State(3, 0), # Pipe the profile state to the drive lambda setpointState: self.robotDrive.setDriveStates( setpointState, setpointState diff --git a/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py b/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py index 6ae5e95..65423ee 100644 --- a/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py +++ b/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py @@ -40,7 +40,7 @@ def __init__(self) -> None: # 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.rightLeader.setInverted(True) - + # You might need to not do this depending on the specific motor controller # that you are using -- contact the respective vendor's documentation for # more details. @@ -57,7 +57,7 @@ def __init__(self) -> None: self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters( constants.DriveConstants.ksVolts, constants.DriveConstants.kvVoltSecondsPerMeter, - constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared + constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared, ) # The robot's drive @@ -71,8 +71,12 @@ def arcadeDrive(self, fwd: float, rot: float): :param rot: the commanded rotation """ self.drive.arcadeDrive(fwd, rot) - - def setDriveStates(self, left: wpimath.trajectory.TrapezoidProfile.State, right: wpimath.trajectory.TrapezoidProfile.State): + + def setDriveStates( + self, + left: wpimath.trajectory.TrapezoidProfile.State, + right: wpimath.trajectory.TrapezoidProfile.State, + ): """ Attempts to follow the given drive states using offboard PID. @@ -82,13 +86,13 @@ def setDriveStates(self, left: wpimath.trajectory.TrapezoidProfile.State, right: self.leftLeader.setSetPoint( examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition, left.position, - self.feedforward.calculate(left.velocity) + self.feedforward.calculate(left.velocity), ) self.rightLeader.setSetPoint( examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition, right.position, - self.feedforward.calculate(right.velocity) + self.feedforward.calculate(right.velocity), ) def getLeftEncoderDistance(self) -> wpilib.Encoder: From b0228834c9bff8f7df4781839a53d165c5ec43d3 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Fri, 30 Dec 2022 22:53:31 +0300 Subject: [PATCH 10/17] add to tests and update all examplesmartmotorcontroller modules --- commands-v2/armbotoffboard/examplesmartmotorcontroller.py | 2 +- elevator-trapezoid-profile/examplesmartmotorcontroller.py | 2 +- run_tests.sh | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/commands-v2/armbotoffboard/examplesmartmotorcontroller.py b/commands-v2/armbotoffboard/examplesmartmotorcontroller.py index 3d82aac..2b7d60a 100644 --- a/commands-v2/armbotoffboard/examplesmartmotorcontroller.py +++ b/commands-v2/armbotoffboard/examplesmartmotorcontroller.py @@ -80,7 +80,7 @@ def set(self, speed: float) -> None: def get(self) -> float: pass - def setInverted(isInverted: bool) -> None: + def setInverted(self, isInverted: bool) -> None: pass def getInverted(self) -> bool: diff --git a/elevator-trapezoid-profile/examplesmartmotorcontroller.py b/elevator-trapezoid-profile/examplesmartmotorcontroller.py index 3d82aac..2b7d60a 100644 --- a/elevator-trapezoid-profile/examplesmartmotorcontroller.py +++ b/elevator-trapezoid-profile/examplesmartmotorcontroller.py @@ -80,7 +80,7 @@ def set(self, speed: float) -> None: def get(self) -> float: pass - def setInverted(isInverted: bool) -> None: + def setInverted(self, isInverted: bool) -> None: pass def getInverted(self) -> bool: diff --git a/run_tests.sh b/run_tests.sh index 664a493..ceb39de 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -29,6 +29,7 @@ BASE_TESTS=" elevator-profiled-pid elevator-trapezoid-profile commands-v2/gyro-drive-commands + commands-v2/drive-distance-offboard " IGNORED_TESTS=" From c123a1f41c483542a754ad21854268938bffc21e Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Sat, 31 Dec 2022 18:21:52 +0300 Subject: [PATCH 11/17] fix docstring --- .../commands/drivedistanceprofiled.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/commands-v2/drive-distance-offboard/commands/drivedistanceprofiled.py b/commands-v2/drive-distance-offboard/commands/drivedistanceprofiled.py index 2fb2f4a..6c810eb 100644 --- a/commands-v2/drive-distance-offboard/commands/drivedistanceprofiled.py +++ b/commands-v2/drive-distance-offboard/commands/drivedistanceprofiled.py @@ -19,11 +19,10 @@ class DriveDistanceProfiled(commands2.TrapezoidProfileCommand): def __init__( self, meters: float, drive: subsystems.drivesubsystem.DriveSubsystem ) -> None: - """ - Creates a new DriveDistanceProfiled command. + """Creates a new DriveDistanceProfiled command. - :param: meters The distance to drive. - :param: drive The drive subsystem to use. + :param meters: The distance to drive. + :param drive: The drive subsystem to use. """ super().__init__( wpimath.trajectory.TrapezoidProfile( From 673407969d0719043adc6d99aa884353ec5d02c9 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Sat, 31 Dec 2022 22:23:08 +0300 Subject: [PATCH 12/17] copy hatchbot to hatchbot-inlined, rewrite the commands module and rename old hatchbot accordingly. --- .../hatchbot-inlined/commands/autos.py | 69 ++++++++++++++ commands-v2/hatchbot-inlined/constants.py | 45 ++++++++++ commands-v2/hatchbot-inlined/physics.py | 85 ++++++++++++++++++ commands-v2/hatchbot-inlined/robot.py | 63 +++++++++++++ .../hatchbot-inlined/robotcontainer.py | 89 +++++++++++++++++++ .../hatchbot-inlined/subsystems/__init__.py | 0 .../subsystems/drivesubsystem.py | 60 +++++++++++++ .../subsystems/hatchsubsystem.py | 23 +++++ .../commands/complexauto.py | 29 ++++++ .../commands/defaultdrive.py | 22 +++++ .../commands/drivedistance.py | 25 ++++++ .../commands/grabhatch.py | 15 ++++ .../commands/halvedrivespeed.py | 15 ++++ .../commands/releasehatch.py | 8 ++ commands-v2/hatchbot-traditional/constants.py | 45 ++++++++++ commands-v2/hatchbot-traditional/physics.py | 85 ++++++++++++++++++ commands-v2/hatchbot-traditional/robot.py | 63 +++++++++++++ .../hatchbot-traditional/robotcontainer.py | 89 +++++++++++++++++++ .../subsystems/__init__.py | 0 .../subsystems/drivesubsystem.py | 60 +++++++++++++ .../subsystems/hatchsubsystem.py | 23 +++++ 21 files changed, 913 insertions(+) create mode 100644 commands-v2/hatchbot-inlined/commands/autos.py create mode 100644 commands-v2/hatchbot-inlined/constants.py create mode 100644 commands-v2/hatchbot-inlined/physics.py create mode 100755 commands-v2/hatchbot-inlined/robot.py create mode 100644 commands-v2/hatchbot-inlined/robotcontainer.py create mode 100644 commands-v2/hatchbot-inlined/subsystems/__init__.py create mode 100644 commands-v2/hatchbot-inlined/subsystems/drivesubsystem.py create mode 100644 commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py create mode 100644 commands-v2/hatchbot-traditional/commands/complexauto.py create mode 100644 commands-v2/hatchbot-traditional/commands/defaultdrive.py create mode 100644 commands-v2/hatchbot-traditional/commands/drivedistance.py create mode 100644 commands-v2/hatchbot-traditional/commands/grabhatch.py create mode 100644 commands-v2/hatchbot-traditional/commands/halvedrivespeed.py create mode 100644 commands-v2/hatchbot-traditional/commands/releasehatch.py create mode 100644 commands-v2/hatchbot-traditional/constants.py create mode 100644 commands-v2/hatchbot-traditional/physics.py create mode 100755 commands-v2/hatchbot-traditional/robot.py create mode 100644 commands-v2/hatchbot-traditional/robotcontainer.py create mode 100644 commands-v2/hatchbot-traditional/subsystems/__init__.py create mode 100644 commands-v2/hatchbot-traditional/subsystems/drivesubsystem.py create mode 100644 commands-v2/hatchbot-traditional/subsystems/hatchsubsystem.py diff --git a/commands-v2/hatchbot-inlined/commands/autos.py b/commands-v2/hatchbot-inlined/commands/autos.py new file mode 100644 index 0000000..3e71991 --- /dev/null +++ b/commands-v2/hatchbot-inlined/commands/autos.py @@ -0,0 +1,69 @@ +#!/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 subsystems.drivesubsystem +import subsystems.hatchsubsystem + +import constants + +class Autos(): + """Container for auto command factories.""" + + def __init__(self) -> None: + raise Exception("This is a utility class!") + + def simpleAuto(self, drive: subsystems.drivesubsystem.DriveSubsystem): + """A simple auto routine that drives forward a specified distance, and then stops.""" + return commands2.FunctionalCommand( + # Reset encoders on command start + drive.resetEncoders(), + # Drive forward while the command is executing, + lambda: drive.arcadeDrive(constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: drive.arcadeDrive(0, 0), + # End the command when the robot's driven distance exceeds the desired value + lambda: drive.getAverageEncoderDistance() >= constants.kAutoDriveDistanceInches, + # Require the drive subsystem + ) + + def complexAuto(self, driveSubsystem: subsystems.drivesubsystem.DriveSubsystem, hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem): + """A complex auto routine that drives forward, drops a hatch, and then drives backward.""" + return commands2.cmd.sequence([ + # Drive forward up to the front of the cargo ship + commands2.FunctionalCommand( + # Reset encoders on command start + driveSubsystem.resetEncoders(), + # Drive forward while the command is executing + lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: driveSubsystem.arcadeDrive(0,0), + # End the command when the robot's driven distance exceeds the desired value + lambda: driveSubsystem.getAverageEncoderDistance() >= constants.kAutoDriveDistanceInches, + # Require the drive subsystem + [driveSubsystem] + ), + + # Release the hatch + hatchSubsystem.releaseHatch(), + + # Drive backward the specified distance + commands2.FunctionalCommand( + # Reset encoders on command start + driveSubsystem.resetEncoders(), + # Drive backwards while the command is executing + lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: driveSubsystem.arcadeDrive(0,0), + # End the command when the robot's driven distance exceeds the desired value + lambda: driveSubsystem.getAverageEncoderDistance() <= constants.kAutoBackupDistanceInches, + # Require the drive subsystem + [driveSubsystem] + ) + ]) \ No newline at end of file diff --git a/commands-v2/hatchbot-inlined/constants.py b/commands-v2/hatchbot-inlined/constants.py new file mode 100644 index 0000000..4f6b247 --- /dev/null +++ b/commands-v2/hatchbot-inlined/constants.py @@ -0,0 +1,45 @@ +# +# The constants module is a convenience place for teams to hold robot-wide +# numerical or boolean constants. Don't use this for any other purpose! +# + +import math +import wpilib + +# Motors +kLeftMotor1Port = 0 +kLeftMotor2Port = 1 +kRightMotor1Port = 2 +kRightMotor2Port = 3 + +# Encoders +kLeftEncoderPorts = (0, 1) +kRightEncoderPorts = (2, 3) +kLeftEncoderReversed = False +kRightEncoderReversed = True + +kEncoderCPR = 1024 +kWheelDiameterInches = 6 +# Assumes the encoders are directly mounted on the wheel shafts +kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR + +# Hatch +kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTREPCM +kHatchSolenoidModule = 0 +kHatchSolenoidPorts = (0, 1) + +# Autonomous +kAutoDriveDistanceInches = 60 +kAutoBackupDistanceInches = 20 +kAutoDriveSpeed = 0.5 + +# Operator Interface +kDriverControllerPort = 0 + +# Physical parameters +kDriveTrainMotorCount = 2 +kTrackWidth = 0.381 * 2 +kGearingRatio = 8 +kWheelRadius = 0.0508 + +# kEncoderResolution = - diff --git a/commands-v2/hatchbot-inlined/physics.py b/commands-v2/hatchbot-inlined/physics.py new file mode 100644 index 0000000..f6fc74d --- /dev/null +++ b/commands-v2/hatchbot-inlined/physics.py @@ -0,0 +1,85 @@ +# +# 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. +# + +import wpilib +import wpilib.simulation +from wpimath.system import LinearSystemId +from wpimath.system.plant import DCMotor + +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 + + # Motors + self.l_motor = wpilib.simulation.PWMSim( + robot.container.drive.left1.getChannel() + ) + self.r_motor = wpilib.simulation.PWMSim( + robot.container.drive.right1.getChannel() + ) + + self.system = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3) + self.drivesim = wpilib.simulation.DifferentialDrivetrainSim( + self.system, + constants.kTrackWidth, + DCMotor.CIM(constants.kDriveTrainMotorCount), + constants.kGearingRatio, + constants.kWheelRadius, + ) + + self.leftEncoderSim = wpilib.simulation.EncoderSim( + robot.container.drive.leftEncoder + ) + self.rightEncoderSim = wpilib.simulation.EncoderSim( + robot.container.drive.rightEncoder + ) + + 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.l_motor.getSpeed() + r_motor = self.r_motor.getSpeed() + + voltage = wpilib.RobotController.getInputVoltage() + self.drivesim.setInputs(l_motor * voltage, r_motor * voltage) + self.drivesim.update(tm_diff) + + self.leftEncoderSim.setDistance(self.drivesim.getLeftPosition() * 39.37) + self.leftEncoderSim.setRate(self.drivesim.getLeftVelocity() * 39.37) + self.rightEncoderSim.setDistance(self.drivesim.getRightPosition() * 39.37) + self.rightEncoderSim.setRate(self.drivesim.getRightVelocity() * 39.37) + + self.physics_controller.field.setRobotPose(self.drivesim.getPose()) diff --git a/commands-v2/hatchbot-inlined/robot.py b/commands-v2/hatchbot-inlined/robot.py new file mode 100755 index 0000000..a0adb13 --- /dev/null +++ b/commands-v2/hatchbot-inlined/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/hatchbot-inlined/robotcontainer.py b/commands-v2/hatchbot-inlined/robotcontainer.py new file mode 100644 index 0000000..c2ba97d --- /dev/null +++ b/commands-v2/hatchbot-inlined/robotcontainer.py @@ -0,0 +1,89 @@ +import wpilib +from wpilib.interfaces import GenericHID + +import commands2 +import commands2.button + +import constants + +from commands.complexauto import ComplexAuto +from commands.drivedistance import DriveDistance +from commands.defaultdrive import DefaultDrive +from commands.grabhatch import GrabHatch +from commands.halvedrivespeed import HalveDriveSpeed +from commands.releasehatch import ReleaseHatch + +from subsystems.drivesubsystem import DriveSubsystem +from subsystems.hatchsubsystem import HatchSubsystem + + +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: + + # The driver's controller + # self.driverController = wpilib.XboxController(constants.kDriverControllerPort) + self.driverController = wpilib.Joystick(constants.kDriverControllerPort) + + # The robot's subsystems + self.drive = DriveSubsystem() + self.hatch = HatchSubsystem() + + # Autonomous routines + + # A simple auto routine that drives forward a specified distance, and then stops. + self.simpleAuto = DriveDistance( + constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, self.drive + ) + + # A complex auto routine that drives forward, drops a hatch, and then drives backward. + self.complexAuto = ComplexAuto(self.drive, self.hatch) + + # Chooser + self.chooser = wpilib.SendableChooser() + + # Add commands to the autonomous command chooser + self.chooser.setDefaultOption("Simple Auto", self.simpleAuto) + self.chooser.addOption("Complex Auto", self.complexAuto) + + # Put the chooser on the dashboard + wpilib.SmartDashboard.putData("Autonomous", self.chooser) + + self.configureButtonBindings() + + # set up default drive command + self.drive.setDefaultCommand( + DefaultDrive( + self.drive, + lambda: -self.driverController.getY(), + lambda: self.driverController.getX(), + ) + ) + + def configureButtonBindings(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 passing it to a JoystickButton. + """ + + commands2.button.JoystickButton(self.driverController, 1).whenPressed( + GrabHatch(self.hatch) + ) + + commands2.button.JoystickButton(self.driverController, 2).whenPressed( + ReleaseHatch(self.hatch) + ) + + commands2.button.JoystickButton(self.driverController, 3).whenHeld( + HalveDriveSpeed(self.drive) + ) + + def getAutonomousCommand(self) -> commands2.Command: + return self.chooser.getSelected() diff --git a/commands-v2/hatchbot-inlined/subsystems/__init__.py b/commands-v2/hatchbot-inlined/subsystems/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/commands-v2/hatchbot-inlined/subsystems/drivesubsystem.py b/commands-v2/hatchbot-inlined/subsystems/drivesubsystem.py new file mode 100644 index 0000000..927e536 --- /dev/null +++ b/commands-v2/hatchbot-inlined/subsystems/drivesubsystem.py @@ -0,0 +1,60 @@ +import commands2 +import wpilib +import wpilib.drive + +import constants + + +class DriveSubsystem(commands2.SubsystemBase): + def __init__(self) -> None: + super().__init__() + + self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port) + self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port) + self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) + self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) + + # The robot's drive + self.drive = wpilib.drive.DifferentialDrive( + wpilib.MotorControllerGroup(self.left1, self.left2), + wpilib.MotorControllerGroup(self.right1, self.right2), + ) + + # The left-side drive encoder + self.leftEncoder = wpilib.Encoder( + *constants.kLeftEncoderPorts, + reverseDirection=constants.kLeftEncoderReversed + ) + + # The right-side drive encoder + self.rightEncoder = wpilib.Encoder( + *constants.kRightEncoderPorts, + reverseDirection=constants.kRightEncoderReversed + ) + + # Sets the distance per pulse for the encoders + self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) + self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) + + def arcadeDrive(self, fwd: float, rot: float) -> None: + """ + Drives the robot using arcade controls. + + :param fwd: the commanded forward movement + :param rot: the commanded rotation + """ + self.drive.arcadeDrive(fwd, rot) + + def resetEncoders(self) -> None: + """Resets the drive encoders to currently read a position of 0.""" + + def getAverageEncoderDistance(self) -> float: + """Gets the average distance of the TWO encoders.""" + return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 + + def setMaxOutput(self, maxOutput: float): + """ + Sets the max output of the drive. Useful for scaling the + drive to drive more slowly. + """ + self.drive.setMaxOutput(maxOutput) diff --git a/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py b/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py new file mode 100644 index 0000000..7893658 --- /dev/null +++ b/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py @@ -0,0 +1,23 @@ +import wpilib +import commands2 + +import constants + + +class HatchSubsystem(commands2.SubsystemBase): + def __init__(self) -> None: + super().__init__() + + self.hatchSolenoid = wpilib.DoubleSolenoid( + constants.kHatchSolenoidModule, + constants.kHatchSolenoidModuleType, + *constants.kHatchSolenoidPorts + ) + + def grabHatch(self) -> None: + """Grabs the hatch""" + self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward) + + def releaseHatch(self) -> None: + """Releases the hatch""" + self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse) diff --git a/commands-v2/hatchbot-traditional/commands/complexauto.py b/commands-v2/hatchbot-traditional/commands/complexauto.py new file mode 100644 index 0000000..eed91f8 --- /dev/null +++ b/commands-v2/hatchbot-traditional/commands/complexauto.py @@ -0,0 +1,29 @@ +import commands2 + +import constants + +from .drivedistance import DriveDistance +from .releasehatch import ReleaseHatch + +from subsystems.drivesubsystem import DriveSubsystem +from subsystems.hatchsubsystem import HatchSubsystem + + +class ComplexAuto(commands2.SequentialCommandGroup): + """ + A complex auto command that drives forward, releases a hatch, and then drives backward. + """ + + def __init__(self, drive: DriveSubsystem, hatch: HatchSubsystem): + super().__init__( + # Drive forward the specified distance + DriveDistance( + constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, drive + ), + # Release the hatch + ReleaseHatch(hatch), + # Drive backward the specified distance + DriveDistance( + constants.kAutoBackupDistanceInches, -constants.kAutoDriveSpeed, drive + ), + ) diff --git a/commands-v2/hatchbot-traditional/commands/defaultdrive.py b/commands-v2/hatchbot-traditional/commands/defaultdrive.py new file mode 100644 index 0000000..5a2a3aa --- /dev/null +++ b/commands-v2/hatchbot-traditional/commands/defaultdrive.py @@ -0,0 +1,22 @@ +import typing +import commands2 +from subsystems.drivesubsystem import DriveSubsystem + + +class DefaultDrive(commands2.CommandBase): + def __init__( + self, + drive: DriveSubsystem, + forward: typing.Callable[[], float], + rotation: typing.Callable[[], float], + ) -> None: + super().__init__() + + self.drive = drive + self.forward = forward + self.rotation = rotation + + self.addRequirements([self.drive]) + + def execute(self) -> None: + self.drive.arcadeDrive(self.forward(), self.rotation()) diff --git a/commands-v2/hatchbot-traditional/commands/drivedistance.py b/commands-v2/hatchbot-traditional/commands/drivedistance.py new file mode 100644 index 0000000..f5d7fa7 --- /dev/null +++ b/commands-v2/hatchbot-traditional/commands/drivedistance.py @@ -0,0 +1,25 @@ +import commands2 + +from subsystems.drivesubsystem import DriveSubsystem + + +class DriveDistance(commands2.CommandBase): + def __init__(self, inches: float, speed: float, drive: DriveSubsystem) -> None: + super().__init__() + self.distance = inches + self.speed = speed + self.drive = drive + self.addRequirements(drive) + + def initialize(self) -> None: + self.drive.resetEncoders() + self.drive.arcadeDrive(self.speed, 0) + + def execute(self) -> None: + self.drive.arcadeDrive(self.speed, 0) + + def end(self, interrupted: bool) -> None: + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + return abs(self.drive.getAverageEncoderDistance()) >= self.distance diff --git a/commands-v2/hatchbot-traditional/commands/grabhatch.py b/commands-v2/hatchbot-traditional/commands/grabhatch.py new file mode 100644 index 0000000..c87a127 --- /dev/null +++ b/commands-v2/hatchbot-traditional/commands/grabhatch.py @@ -0,0 +1,15 @@ +import commands2 +from subsystems.hatchsubsystem import HatchSubsystem + + +class GrabHatch(commands2.CommandBase): + def __init__(self, hatch: HatchSubsystem) -> None: + super().__init__() + self.hatch = hatch + self.addRequirements(hatch) + + def initialize(self) -> None: + self.hatch.grabHatch() + + def isFinished(self) -> bool: + return True diff --git a/commands-v2/hatchbot-traditional/commands/halvedrivespeed.py b/commands-v2/hatchbot-traditional/commands/halvedrivespeed.py new file mode 100644 index 0000000..1d77b0a --- /dev/null +++ b/commands-v2/hatchbot-traditional/commands/halvedrivespeed.py @@ -0,0 +1,15 @@ +import commands2 + +from subsystems.drivesubsystem import DriveSubsystem + + +class HalveDriveSpeed(commands2.CommandBase): + def __init__(self, drive: DriveSubsystem) -> None: + super().__init__() + self.drive = drive + + def initialize(self) -> None: + self.drive.setMaxOutput(0.5) + + def end(self, interrupted: bool) -> None: + self.drive.setMaxOutput(1.0) diff --git a/commands-v2/hatchbot-traditional/commands/releasehatch.py b/commands-v2/hatchbot-traditional/commands/releasehatch.py new file mode 100644 index 0000000..ffea787 --- /dev/null +++ b/commands-v2/hatchbot-traditional/commands/releasehatch.py @@ -0,0 +1,8 @@ +import commands2 + +from subsystems.hatchsubsystem import HatchSubsystem + + +class ReleaseHatch(commands2.InstantCommand): + def __init__(self, hatch: HatchSubsystem) -> None: + super().__init__(hatch.releaseHatch, hatch) diff --git a/commands-v2/hatchbot-traditional/constants.py b/commands-v2/hatchbot-traditional/constants.py new file mode 100644 index 0000000..4f6b247 --- /dev/null +++ b/commands-v2/hatchbot-traditional/constants.py @@ -0,0 +1,45 @@ +# +# The constants module is a convenience place for teams to hold robot-wide +# numerical or boolean constants. Don't use this for any other purpose! +# + +import math +import wpilib + +# Motors +kLeftMotor1Port = 0 +kLeftMotor2Port = 1 +kRightMotor1Port = 2 +kRightMotor2Port = 3 + +# Encoders +kLeftEncoderPorts = (0, 1) +kRightEncoderPorts = (2, 3) +kLeftEncoderReversed = False +kRightEncoderReversed = True + +kEncoderCPR = 1024 +kWheelDiameterInches = 6 +# Assumes the encoders are directly mounted on the wheel shafts +kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR + +# Hatch +kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTREPCM +kHatchSolenoidModule = 0 +kHatchSolenoidPorts = (0, 1) + +# Autonomous +kAutoDriveDistanceInches = 60 +kAutoBackupDistanceInches = 20 +kAutoDriveSpeed = 0.5 + +# Operator Interface +kDriverControllerPort = 0 + +# Physical parameters +kDriveTrainMotorCount = 2 +kTrackWidth = 0.381 * 2 +kGearingRatio = 8 +kWheelRadius = 0.0508 + +# kEncoderResolution = - diff --git a/commands-v2/hatchbot-traditional/physics.py b/commands-v2/hatchbot-traditional/physics.py new file mode 100644 index 0000000..f6fc74d --- /dev/null +++ b/commands-v2/hatchbot-traditional/physics.py @@ -0,0 +1,85 @@ +# +# 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. +# + +import wpilib +import wpilib.simulation +from wpimath.system import LinearSystemId +from wpimath.system.plant import DCMotor + +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 + + # Motors + self.l_motor = wpilib.simulation.PWMSim( + robot.container.drive.left1.getChannel() + ) + self.r_motor = wpilib.simulation.PWMSim( + robot.container.drive.right1.getChannel() + ) + + self.system = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3) + self.drivesim = wpilib.simulation.DifferentialDrivetrainSim( + self.system, + constants.kTrackWidth, + DCMotor.CIM(constants.kDriveTrainMotorCount), + constants.kGearingRatio, + constants.kWheelRadius, + ) + + self.leftEncoderSim = wpilib.simulation.EncoderSim( + robot.container.drive.leftEncoder + ) + self.rightEncoderSim = wpilib.simulation.EncoderSim( + robot.container.drive.rightEncoder + ) + + 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.l_motor.getSpeed() + r_motor = self.r_motor.getSpeed() + + voltage = wpilib.RobotController.getInputVoltage() + self.drivesim.setInputs(l_motor * voltage, r_motor * voltage) + self.drivesim.update(tm_diff) + + self.leftEncoderSim.setDistance(self.drivesim.getLeftPosition() * 39.37) + self.leftEncoderSim.setRate(self.drivesim.getLeftVelocity() * 39.37) + self.rightEncoderSim.setDistance(self.drivesim.getRightPosition() * 39.37) + self.rightEncoderSim.setRate(self.drivesim.getRightVelocity() * 39.37) + + self.physics_controller.field.setRobotPose(self.drivesim.getPose()) diff --git a/commands-v2/hatchbot-traditional/robot.py b/commands-v2/hatchbot-traditional/robot.py new file mode 100755 index 0000000..a0adb13 --- /dev/null +++ b/commands-v2/hatchbot-traditional/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/hatchbot-traditional/robotcontainer.py b/commands-v2/hatchbot-traditional/robotcontainer.py new file mode 100644 index 0000000..c2ba97d --- /dev/null +++ b/commands-v2/hatchbot-traditional/robotcontainer.py @@ -0,0 +1,89 @@ +import wpilib +from wpilib.interfaces import GenericHID + +import commands2 +import commands2.button + +import constants + +from commands.complexauto import ComplexAuto +from commands.drivedistance import DriveDistance +from commands.defaultdrive import DefaultDrive +from commands.grabhatch import GrabHatch +from commands.halvedrivespeed import HalveDriveSpeed +from commands.releasehatch import ReleaseHatch + +from subsystems.drivesubsystem import DriveSubsystem +from subsystems.hatchsubsystem import HatchSubsystem + + +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: + + # The driver's controller + # self.driverController = wpilib.XboxController(constants.kDriverControllerPort) + self.driverController = wpilib.Joystick(constants.kDriverControllerPort) + + # The robot's subsystems + self.drive = DriveSubsystem() + self.hatch = HatchSubsystem() + + # Autonomous routines + + # A simple auto routine that drives forward a specified distance, and then stops. + self.simpleAuto = DriveDistance( + constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, self.drive + ) + + # A complex auto routine that drives forward, drops a hatch, and then drives backward. + self.complexAuto = ComplexAuto(self.drive, self.hatch) + + # Chooser + self.chooser = wpilib.SendableChooser() + + # Add commands to the autonomous command chooser + self.chooser.setDefaultOption("Simple Auto", self.simpleAuto) + self.chooser.addOption("Complex Auto", self.complexAuto) + + # Put the chooser on the dashboard + wpilib.SmartDashboard.putData("Autonomous", self.chooser) + + self.configureButtonBindings() + + # set up default drive command + self.drive.setDefaultCommand( + DefaultDrive( + self.drive, + lambda: -self.driverController.getY(), + lambda: self.driverController.getX(), + ) + ) + + def configureButtonBindings(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 passing it to a JoystickButton. + """ + + commands2.button.JoystickButton(self.driverController, 1).whenPressed( + GrabHatch(self.hatch) + ) + + commands2.button.JoystickButton(self.driverController, 2).whenPressed( + ReleaseHatch(self.hatch) + ) + + commands2.button.JoystickButton(self.driverController, 3).whenHeld( + HalveDriveSpeed(self.drive) + ) + + def getAutonomousCommand(self) -> commands2.Command: + return self.chooser.getSelected() diff --git a/commands-v2/hatchbot-traditional/subsystems/__init__.py b/commands-v2/hatchbot-traditional/subsystems/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/commands-v2/hatchbot-traditional/subsystems/drivesubsystem.py b/commands-v2/hatchbot-traditional/subsystems/drivesubsystem.py new file mode 100644 index 0000000..927e536 --- /dev/null +++ b/commands-v2/hatchbot-traditional/subsystems/drivesubsystem.py @@ -0,0 +1,60 @@ +import commands2 +import wpilib +import wpilib.drive + +import constants + + +class DriveSubsystem(commands2.SubsystemBase): + def __init__(self) -> None: + super().__init__() + + self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port) + self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port) + self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) + self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) + + # The robot's drive + self.drive = wpilib.drive.DifferentialDrive( + wpilib.MotorControllerGroup(self.left1, self.left2), + wpilib.MotorControllerGroup(self.right1, self.right2), + ) + + # The left-side drive encoder + self.leftEncoder = wpilib.Encoder( + *constants.kLeftEncoderPorts, + reverseDirection=constants.kLeftEncoderReversed + ) + + # The right-side drive encoder + self.rightEncoder = wpilib.Encoder( + *constants.kRightEncoderPorts, + reverseDirection=constants.kRightEncoderReversed + ) + + # Sets the distance per pulse for the encoders + self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) + self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) + + def arcadeDrive(self, fwd: float, rot: float) -> None: + """ + Drives the robot using arcade controls. + + :param fwd: the commanded forward movement + :param rot: the commanded rotation + """ + self.drive.arcadeDrive(fwd, rot) + + def resetEncoders(self) -> None: + """Resets the drive encoders to currently read a position of 0.""" + + def getAverageEncoderDistance(self) -> float: + """Gets the average distance of the TWO encoders.""" + return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 + + def setMaxOutput(self, maxOutput: float): + """ + Sets the max output of the drive. Useful for scaling the + drive to drive more slowly. + """ + self.drive.setMaxOutput(maxOutput) diff --git a/commands-v2/hatchbot-traditional/subsystems/hatchsubsystem.py b/commands-v2/hatchbot-traditional/subsystems/hatchsubsystem.py new file mode 100644 index 0000000..7893658 --- /dev/null +++ b/commands-v2/hatchbot-traditional/subsystems/hatchsubsystem.py @@ -0,0 +1,23 @@ +import wpilib +import commands2 + +import constants + + +class HatchSubsystem(commands2.SubsystemBase): + def __init__(self) -> None: + super().__init__() + + self.hatchSolenoid = wpilib.DoubleSolenoid( + constants.kHatchSolenoidModule, + constants.kHatchSolenoidModuleType, + *constants.kHatchSolenoidPorts + ) + + def grabHatch(self) -> None: + """Grabs the hatch""" + self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward) + + def releaseHatch(self) -> None: + """Releases the hatch""" + self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse) From 3d86942d71a940666fd5a99c0d4b50bb7e1a16b2 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Sun, 1 Jan 2023 10:23:55 +0300 Subject: [PATCH 13/17] remove wrongfully copied hatchbot folder, finish the inlined example. --- .../hatchbot-inlined/commands/autos.py | 12 +-- commands-v2/hatchbot-inlined/physics.py | 8 +- .../hatchbot-inlined/robotcontainer.py | 77 ++++++++++--------- .../subsystems/hatchsubsystem.py | 9 ++- 4 files changed, 54 insertions(+), 52 deletions(-) diff --git a/commands-v2/hatchbot-inlined/commands/autos.py b/commands-v2/hatchbot-inlined/commands/autos.py index 3e71991..03751a6 100644 --- a/commands-v2/hatchbot-inlined/commands/autos.py +++ b/commands-v2/hatchbot-inlined/commands/autos.py @@ -19,11 +19,11 @@ class Autos(): def __init__(self) -> None: raise Exception("This is a utility class!") - def simpleAuto(self, drive: subsystems.drivesubsystem.DriveSubsystem): + def simpleAuto(drive: subsystems.drivesubsystem.DriveSubsystem): """A simple auto routine that drives forward a specified distance, and then stops.""" return commands2.FunctionalCommand( # Reset encoders on command start - drive.resetEncoders(), + drive.resetEncoders, # Drive forward while the command is executing, lambda: drive.arcadeDrive(constants.kAutoDriveSpeed, 0), # Stop driving at the end of the command @@ -33,13 +33,13 @@ def simpleAuto(self, drive: subsystems.drivesubsystem.DriveSubsystem): # Require the drive subsystem ) - def complexAuto(self, driveSubsystem: subsystems.drivesubsystem.DriveSubsystem, hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem): + def complexAuto(driveSubsystem: subsystems.drivesubsystem.DriveSubsystem, hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem): """A complex auto routine that drives forward, drops a hatch, and then drives backward.""" return commands2.cmd.sequence([ # Drive forward up to the front of the cargo ship commands2.FunctionalCommand( # Reset encoders on command start - driveSubsystem.resetEncoders(), + driveSubsystem.resetEncoders, # Drive forward while the command is executing lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveSpeed, 0), # Stop driving at the end of the command @@ -56,13 +56,13 @@ def complexAuto(self, driveSubsystem: subsystems.drivesubsystem.DriveSubsystem, # Drive backward the specified distance commands2.FunctionalCommand( # Reset encoders on command start - driveSubsystem.resetEncoders(), + driveSubsystem.resetEncoders, # Drive backwards while the command is executing lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveSpeed, 0), # Stop driving at the end of the command lambda interrupt: driveSubsystem.arcadeDrive(0,0), # End the command when the robot's driven distance exceeds the desired value - lambda: driveSubsystem.getAverageEncoderDistance() <= constants.kAutoBackupDistanceInches, + lambda: abs(driveSubsystem.getAverageEncoderDistance()) >= constants.kAutoBackupDistanceInches, # Require the drive subsystem [driveSubsystem] ) diff --git a/commands-v2/hatchbot-inlined/physics.py b/commands-v2/hatchbot-inlined/physics.py index f6fc74d..26ba33f 100644 --- a/commands-v2/hatchbot-inlined/physics.py +++ b/commands-v2/hatchbot-inlined/physics.py @@ -37,10 +37,10 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): # Motors self.l_motor = wpilib.simulation.PWMSim( - robot.container.drive.left1.getChannel() + robot.container.driveSubsystem.left1.getChannel() ) self.r_motor = wpilib.simulation.PWMSim( - robot.container.drive.right1.getChannel() + robot.container.driveSubsystem.right1.getChannel() ) self.system = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3) @@ -53,10 +53,10 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): ) self.leftEncoderSim = wpilib.simulation.EncoderSim( - robot.container.drive.leftEncoder + robot.container.driveSubsystem.leftEncoder ) self.rightEncoderSim = wpilib.simulation.EncoderSim( - robot.container.drive.rightEncoder + robot.container.driveSubsystem.rightEncoder ) def update_sim(self, now: float, tm_diff: float) -> None: diff --git a/commands-v2/hatchbot-inlined/robotcontainer.py b/commands-v2/hatchbot-inlined/robotcontainer.py index c2ba97d..eb6f8d6 100644 --- a/commands-v2/hatchbot-inlined/robotcontainer.py +++ b/commands-v2/hatchbot-inlined/robotcontainer.py @@ -3,15 +3,11 @@ import commands2 import commands2.button +import commands2.cmd import constants -from commands.complexauto import ComplexAuto -from commands.drivedistance import DriveDistance -from commands.defaultdrive import DefaultDrive -from commands.grabhatch import GrabHatch -from commands.halvedrivespeed import HalveDriveSpeed -from commands.releasehatch import ReleaseHatch +import commands.autos from subsystems.drivesubsystem import DriveSubsystem from subsystems.hatchsubsystem import HatchSubsystem @@ -26,24 +22,41 @@ class RobotContainer: """ def __init__(self) -> None: - - # The driver's controller - # self.driverController = wpilib.XboxController(constants.kDriverControllerPort) - self.driverController = wpilib.Joystick(constants.kDriverControllerPort) - # The robot's subsystems - self.drive = DriveSubsystem() - self.hatch = HatchSubsystem() + self.driveSubsystem = DriveSubsystem() + self.hatchSubsystem = HatchSubsystem() - # Autonomous routines + # Retained command handles # A simple auto routine that drives forward a specified distance, and then stops. - self.simpleAuto = DriveDistance( - constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, self.drive - ) + self.simpleAuto = commands.autos.Autos.simpleAuto(self.driveSubsystem) # A complex auto routine that drives forward, drops a hatch, and then drives backward. - self.complexAuto = ComplexAuto(self.drive, self.hatch) + self.complexAuto = commands.autos.Autos.complexAuto( + self.driveSubsystem, self.hatchSubsystem + ) + + # The driver's controller + self.driverController = commands2.button.CommandPS4Controller( + constants.kDriverControllerPort + ) + + # Configure the button bindings + self.configureButtonBindings() + + # Configure default commands + # Set the default drive command to split-stick arcade drive + self.driveSubsystem.setDefaultCommand( + # A split-stick arcade command, with forward/backward controlled by the left + # hand, and turning controlled by the right. + commands2.cmd.run( + lambda: self.driveSubsystem.arcadeDrive( + -self.driverController.getLeftY(), + -self.driverController.getRightX(), + ), + [self.driveSubsystem], + ) + ) # Chooser self.chooser = wpilib.SendableChooser() @@ -55,17 +68,6 @@ def __init__(self) -> None: # Put the chooser on the dashboard wpilib.SmartDashboard.putData("Autonomous", self.chooser) - self.configureButtonBindings() - - # set up default drive command - self.drive.setDefaultCommand( - DefaultDrive( - self.drive, - lambda: -self.driverController.getY(), - lambda: self.driverController.getX(), - ) - ) - def configureButtonBindings(self): """ Use this method to define your button->command mappings. Buttons can be created by @@ -73,17 +75,16 @@ def configureButtonBindings(self): and then passing it to a JoystickButton. """ - commands2.button.JoystickButton(self.driverController, 1).whenPressed( - GrabHatch(self.hatch) - ) + # Grab the hatch when the Circle button is pressed. + self.driverController.circle().onTrue(self.hatchSubsystem.grabHatch()) - commands2.button.JoystickButton(self.driverController, 2).whenPressed( - ReleaseHatch(self.hatch) - ) + # Release the hatch when the Square button is pressed. + self.driverController.square().onTrue(self.hatchSubsystem.releaseHatch()) - commands2.button.JoystickButton(self.driverController, 3).whenHeld( - HalveDriveSpeed(self.drive) - ) + # While holding R1, drive at half speed + self.driverController.R1().onTrue( + commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(0.5)) + ).onFalse(commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(1))) def getAutonomousCommand(self) -> commands2.Command: return self.chooser.getSelected() diff --git a/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py b/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py index 7893658..a2ee19d 100644 --- a/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py +++ b/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py @@ -1,5 +1,6 @@ import wpilib import commands2 +import commands2.cmd import constants @@ -14,10 +15,10 @@ def __init__(self) -> None: *constants.kHatchSolenoidPorts ) - def grabHatch(self) -> None: + def grabHatch(self) -> commands2.Command: """Grabs the hatch""" - self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward) + return commands2.cmd.runOnce(lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward), [self]) - def releaseHatch(self) -> None: + def releaseHatch(self) -> commands2.Command: """Releases the hatch""" - self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse) + return commands2.cmd.runOnce(lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse), [self]) From c245e88be7c44d1bec7ee430f659578696ef518f Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Sun, 1 Jan 2023 10:25:38 +0300 Subject: [PATCH 14/17] remove wrongfully copied hatchbot folder, finish the inlined example. --- .../commands/complexauto.py | 29 ------ .../commands/defaultdrive.py | 22 ----- .../commands/drivedistance.py | 25 ------ .../commands/grabhatch.py | 15 ---- .../commands/halvedrivespeed.py | 15 ---- .../commands/releasehatch.py | 8 -- commands-v2/hatchbot-traditional/constants.py | 45 ---------- commands-v2/hatchbot-traditional/physics.py | 85 ------------------ commands-v2/hatchbot-traditional/robot.py | 63 ------------- .../hatchbot-traditional/robotcontainer.py | 89 ------------------- .../subsystems/__init__.py | 0 .../subsystems/drivesubsystem.py | 60 ------------- .../subsystems/hatchsubsystem.py | 23 ----- run_tests.sh | 1 + 14 files changed, 1 insertion(+), 479 deletions(-) delete mode 100644 commands-v2/hatchbot-traditional/commands/complexauto.py delete mode 100644 commands-v2/hatchbot-traditional/commands/defaultdrive.py delete mode 100644 commands-v2/hatchbot-traditional/commands/drivedistance.py delete mode 100644 commands-v2/hatchbot-traditional/commands/grabhatch.py delete mode 100644 commands-v2/hatchbot-traditional/commands/halvedrivespeed.py delete mode 100644 commands-v2/hatchbot-traditional/commands/releasehatch.py delete mode 100644 commands-v2/hatchbot-traditional/constants.py delete mode 100644 commands-v2/hatchbot-traditional/physics.py delete mode 100755 commands-v2/hatchbot-traditional/robot.py delete mode 100644 commands-v2/hatchbot-traditional/robotcontainer.py delete mode 100644 commands-v2/hatchbot-traditional/subsystems/__init__.py delete mode 100644 commands-v2/hatchbot-traditional/subsystems/drivesubsystem.py delete mode 100644 commands-v2/hatchbot-traditional/subsystems/hatchsubsystem.py diff --git a/commands-v2/hatchbot-traditional/commands/complexauto.py b/commands-v2/hatchbot-traditional/commands/complexauto.py deleted file mode 100644 index eed91f8..0000000 --- a/commands-v2/hatchbot-traditional/commands/complexauto.py +++ /dev/null @@ -1,29 +0,0 @@ -import commands2 - -import constants - -from .drivedistance import DriveDistance -from .releasehatch import ReleaseHatch - -from subsystems.drivesubsystem import DriveSubsystem -from subsystems.hatchsubsystem import HatchSubsystem - - -class ComplexAuto(commands2.SequentialCommandGroup): - """ - A complex auto command that drives forward, releases a hatch, and then drives backward. - """ - - def __init__(self, drive: DriveSubsystem, hatch: HatchSubsystem): - super().__init__( - # Drive forward the specified distance - DriveDistance( - constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, drive - ), - # Release the hatch - ReleaseHatch(hatch), - # Drive backward the specified distance - DriveDistance( - constants.kAutoBackupDistanceInches, -constants.kAutoDriveSpeed, drive - ), - ) diff --git a/commands-v2/hatchbot-traditional/commands/defaultdrive.py b/commands-v2/hatchbot-traditional/commands/defaultdrive.py deleted file mode 100644 index 5a2a3aa..0000000 --- a/commands-v2/hatchbot-traditional/commands/defaultdrive.py +++ /dev/null @@ -1,22 +0,0 @@ -import typing -import commands2 -from subsystems.drivesubsystem import DriveSubsystem - - -class DefaultDrive(commands2.CommandBase): - def __init__( - self, - drive: DriveSubsystem, - forward: typing.Callable[[], float], - rotation: typing.Callable[[], float], - ) -> None: - super().__init__() - - self.drive = drive - self.forward = forward - self.rotation = rotation - - self.addRequirements([self.drive]) - - def execute(self) -> None: - self.drive.arcadeDrive(self.forward(), self.rotation()) diff --git a/commands-v2/hatchbot-traditional/commands/drivedistance.py b/commands-v2/hatchbot-traditional/commands/drivedistance.py deleted file mode 100644 index f5d7fa7..0000000 --- a/commands-v2/hatchbot-traditional/commands/drivedistance.py +++ /dev/null @@ -1,25 +0,0 @@ -import commands2 - -from subsystems.drivesubsystem import DriveSubsystem - - -class DriveDistance(commands2.CommandBase): - def __init__(self, inches: float, speed: float, drive: DriveSubsystem) -> None: - super().__init__() - self.distance = inches - self.speed = speed - self.drive = drive - self.addRequirements(drive) - - def initialize(self) -> None: - self.drive.resetEncoders() - self.drive.arcadeDrive(self.speed, 0) - - def execute(self) -> None: - self.drive.arcadeDrive(self.speed, 0) - - def end(self, interrupted: bool) -> None: - self.drive.arcadeDrive(0, 0) - - def isFinished(self) -> bool: - return abs(self.drive.getAverageEncoderDistance()) >= self.distance diff --git a/commands-v2/hatchbot-traditional/commands/grabhatch.py b/commands-v2/hatchbot-traditional/commands/grabhatch.py deleted file mode 100644 index c87a127..0000000 --- a/commands-v2/hatchbot-traditional/commands/grabhatch.py +++ /dev/null @@ -1,15 +0,0 @@ -import commands2 -from subsystems.hatchsubsystem import HatchSubsystem - - -class GrabHatch(commands2.CommandBase): - def __init__(self, hatch: HatchSubsystem) -> None: - super().__init__() - self.hatch = hatch - self.addRequirements(hatch) - - def initialize(self) -> None: - self.hatch.grabHatch() - - def isFinished(self) -> bool: - return True diff --git a/commands-v2/hatchbot-traditional/commands/halvedrivespeed.py b/commands-v2/hatchbot-traditional/commands/halvedrivespeed.py deleted file mode 100644 index 1d77b0a..0000000 --- a/commands-v2/hatchbot-traditional/commands/halvedrivespeed.py +++ /dev/null @@ -1,15 +0,0 @@ -import commands2 - -from subsystems.drivesubsystem import DriveSubsystem - - -class HalveDriveSpeed(commands2.CommandBase): - def __init__(self, drive: DriveSubsystem) -> None: - super().__init__() - self.drive = drive - - def initialize(self) -> None: - self.drive.setMaxOutput(0.5) - - def end(self, interrupted: bool) -> None: - self.drive.setMaxOutput(1.0) diff --git a/commands-v2/hatchbot-traditional/commands/releasehatch.py b/commands-v2/hatchbot-traditional/commands/releasehatch.py deleted file mode 100644 index ffea787..0000000 --- a/commands-v2/hatchbot-traditional/commands/releasehatch.py +++ /dev/null @@ -1,8 +0,0 @@ -import commands2 - -from subsystems.hatchsubsystem import HatchSubsystem - - -class ReleaseHatch(commands2.InstantCommand): - def __init__(self, hatch: HatchSubsystem) -> None: - super().__init__(hatch.releaseHatch, hatch) diff --git a/commands-v2/hatchbot-traditional/constants.py b/commands-v2/hatchbot-traditional/constants.py deleted file mode 100644 index 4f6b247..0000000 --- a/commands-v2/hatchbot-traditional/constants.py +++ /dev/null @@ -1,45 +0,0 @@ -# -# The constants module is a convenience place for teams to hold robot-wide -# numerical or boolean constants. Don't use this for any other purpose! -# - -import math -import wpilib - -# Motors -kLeftMotor1Port = 0 -kLeftMotor2Port = 1 -kRightMotor1Port = 2 -kRightMotor2Port = 3 - -# Encoders -kLeftEncoderPorts = (0, 1) -kRightEncoderPorts = (2, 3) -kLeftEncoderReversed = False -kRightEncoderReversed = True - -kEncoderCPR = 1024 -kWheelDiameterInches = 6 -# Assumes the encoders are directly mounted on the wheel shafts -kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR - -# Hatch -kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTREPCM -kHatchSolenoidModule = 0 -kHatchSolenoidPorts = (0, 1) - -# Autonomous -kAutoDriveDistanceInches = 60 -kAutoBackupDistanceInches = 20 -kAutoDriveSpeed = 0.5 - -# Operator Interface -kDriverControllerPort = 0 - -# Physical parameters -kDriveTrainMotorCount = 2 -kTrackWidth = 0.381 * 2 -kGearingRatio = 8 -kWheelRadius = 0.0508 - -# kEncoderResolution = - diff --git a/commands-v2/hatchbot-traditional/physics.py b/commands-v2/hatchbot-traditional/physics.py deleted file mode 100644 index f6fc74d..0000000 --- a/commands-v2/hatchbot-traditional/physics.py +++ /dev/null @@ -1,85 +0,0 @@ -# -# 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. -# - -import wpilib -import wpilib.simulation -from wpimath.system import LinearSystemId -from wpimath.system.plant import DCMotor - -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 - - # Motors - self.l_motor = wpilib.simulation.PWMSim( - robot.container.drive.left1.getChannel() - ) - self.r_motor = wpilib.simulation.PWMSim( - robot.container.drive.right1.getChannel() - ) - - self.system = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3) - self.drivesim = wpilib.simulation.DifferentialDrivetrainSim( - self.system, - constants.kTrackWidth, - DCMotor.CIM(constants.kDriveTrainMotorCount), - constants.kGearingRatio, - constants.kWheelRadius, - ) - - self.leftEncoderSim = wpilib.simulation.EncoderSim( - robot.container.drive.leftEncoder - ) - self.rightEncoderSim = wpilib.simulation.EncoderSim( - robot.container.drive.rightEncoder - ) - - 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.l_motor.getSpeed() - r_motor = self.r_motor.getSpeed() - - voltage = wpilib.RobotController.getInputVoltage() - self.drivesim.setInputs(l_motor * voltage, r_motor * voltage) - self.drivesim.update(tm_diff) - - self.leftEncoderSim.setDistance(self.drivesim.getLeftPosition() * 39.37) - self.leftEncoderSim.setRate(self.drivesim.getLeftVelocity() * 39.37) - self.rightEncoderSim.setDistance(self.drivesim.getRightPosition() * 39.37) - self.rightEncoderSim.setRate(self.drivesim.getRightVelocity() * 39.37) - - self.physics_controller.field.setRobotPose(self.drivesim.getPose()) diff --git a/commands-v2/hatchbot-traditional/robot.py b/commands-v2/hatchbot-traditional/robot.py deleted file mode 100755 index a0adb13..0000000 --- a/commands-v2/hatchbot-traditional/robot.py +++ /dev/null @@ -1,63 +0,0 @@ -#!/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/hatchbot-traditional/robotcontainer.py b/commands-v2/hatchbot-traditional/robotcontainer.py deleted file mode 100644 index c2ba97d..0000000 --- a/commands-v2/hatchbot-traditional/robotcontainer.py +++ /dev/null @@ -1,89 +0,0 @@ -import wpilib -from wpilib.interfaces import GenericHID - -import commands2 -import commands2.button - -import constants - -from commands.complexauto import ComplexAuto -from commands.drivedistance import DriveDistance -from commands.defaultdrive import DefaultDrive -from commands.grabhatch import GrabHatch -from commands.halvedrivespeed import HalveDriveSpeed -from commands.releasehatch import ReleaseHatch - -from subsystems.drivesubsystem import DriveSubsystem -from subsystems.hatchsubsystem import HatchSubsystem - - -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: - - # The driver's controller - # self.driverController = wpilib.XboxController(constants.kDriverControllerPort) - self.driverController = wpilib.Joystick(constants.kDriverControllerPort) - - # The robot's subsystems - self.drive = DriveSubsystem() - self.hatch = HatchSubsystem() - - # Autonomous routines - - # A simple auto routine that drives forward a specified distance, and then stops. - self.simpleAuto = DriveDistance( - constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, self.drive - ) - - # A complex auto routine that drives forward, drops a hatch, and then drives backward. - self.complexAuto = ComplexAuto(self.drive, self.hatch) - - # Chooser - self.chooser = wpilib.SendableChooser() - - # Add commands to the autonomous command chooser - self.chooser.setDefaultOption("Simple Auto", self.simpleAuto) - self.chooser.addOption("Complex Auto", self.complexAuto) - - # Put the chooser on the dashboard - wpilib.SmartDashboard.putData("Autonomous", self.chooser) - - self.configureButtonBindings() - - # set up default drive command - self.drive.setDefaultCommand( - DefaultDrive( - self.drive, - lambda: -self.driverController.getY(), - lambda: self.driverController.getX(), - ) - ) - - def configureButtonBindings(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 passing it to a JoystickButton. - """ - - commands2.button.JoystickButton(self.driverController, 1).whenPressed( - GrabHatch(self.hatch) - ) - - commands2.button.JoystickButton(self.driverController, 2).whenPressed( - ReleaseHatch(self.hatch) - ) - - commands2.button.JoystickButton(self.driverController, 3).whenHeld( - HalveDriveSpeed(self.drive) - ) - - def getAutonomousCommand(self) -> commands2.Command: - return self.chooser.getSelected() diff --git a/commands-v2/hatchbot-traditional/subsystems/__init__.py b/commands-v2/hatchbot-traditional/subsystems/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/commands-v2/hatchbot-traditional/subsystems/drivesubsystem.py b/commands-v2/hatchbot-traditional/subsystems/drivesubsystem.py deleted file mode 100644 index 927e536..0000000 --- a/commands-v2/hatchbot-traditional/subsystems/drivesubsystem.py +++ /dev/null @@ -1,60 +0,0 @@ -import commands2 -import wpilib -import wpilib.drive - -import constants - - -class DriveSubsystem(commands2.SubsystemBase): - def __init__(self) -> None: - super().__init__() - - self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port) - self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port) - self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) - self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) - - # The robot's drive - self.drive = wpilib.drive.DifferentialDrive( - wpilib.MotorControllerGroup(self.left1, self.left2), - wpilib.MotorControllerGroup(self.right1, self.right2), - ) - - # The left-side drive encoder - self.leftEncoder = wpilib.Encoder( - *constants.kLeftEncoderPorts, - reverseDirection=constants.kLeftEncoderReversed - ) - - # The right-side drive encoder - self.rightEncoder = wpilib.Encoder( - *constants.kRightEncoderPorts, - reverseDirection=constants.kRightEncoderReversed - ) - - # Sets the distance per pulse for the encoders - self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) - self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) - - def arcadeDrive(self, fwd: float, rot: float) -> None: - """ - Drives the robot using arcade controls. - - :param fwd: the commanded forward movement - :param rot: the commanded rotation - """ - self.drive.arcadeDrive(fwd, rot) - - def resetEncoders(self) -> None: - """Resets the drive encoders to currently read a position of 0.""" - - def getAverageEncoderDistance(self) -> float: - """Gets the average distance of the TWO encoders.""" - return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 - - def setMaxOutput(self, maxOutput: float): - """ - Sets the max output of the drive. Useful for scaling the - drive to drive more slowly. - """ - self.drive.setMaxOutput(maxOutput) diff --git a/commands-v2/hatchbot-traditional/subsystems/hatchsubsystem.py b/commands-v2/hatchbot-traditional/subsystems/hatchsubsystem.py deleted file mode 100644 index 7893658..0000000 --- a/commands-v2/hatchbot-traditional/subsystems/hatchsubsystem.py +++ /dev/null @@ -1,23 +0,0 @@ -import wpilib -import commands2 - -import constants - - -class HatchSubsystem(commands2.SubsystemBase): - def __init__(self) -> None: - super().__init__() - - self.hatchSolenoid = wpilib.DoubleSolenoid( - constants.kHatchSolenoidModule, - constants.kHatchSolenoidModuleType, - *constants.kHatchSolenoidPorts - ) - - def grabHatch(self) -> None: - """Grabs the hatch""" - self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward) - - def releaseHatch(self) -> None: - """Releases the hatch""" - self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse) diff --git a/run_tests.sh b/run_tests.sh index cef661c..fc8bf09 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -32,6 +32,7 @@ BASE_TESTS=" elevator-trapezoid-profile commands-v2/gyro-drive-commands commands-v2/drive-distance-offboard + commands-v2/hatchbot-inlined " IGNORED_TESTS=" From db26507878e4664af85ed932c91dfcffcb8cab73 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Sun, 1 Jan 2023 11:06:14 +0300 Subject: [PATCH 15/17] format with black --- .../hatchbot-inlined/commands/autos.py | 81 ++++++++++--------- .../subsystems/hatchsubsystem.py | 8 +- 2 files changed, 50 insertions(+), 39 deletions(-) diff --git a/commands-v2/hatchbot-inlined/commands/autos.py b/commands-v2/hatchbot-inlined/commands/autos.py index 03751a6..c46d544 100644 --- a/commands-v2/hatchbot-inlined/commands/autos.py +++ b/commands-v2/hatchbot-inlined/commands/autos.py @@ -13,12 +13,13 @@ import constants -class Autos(): + +class Autos: """Container for auto command factories.""" def __init__(self) -> None: raise Exception("This is a utility class!") - + def simpleAuto(drive: subsystems.drivesubsystem.DriveSubsystem): """A simple auto routine that drives forward a specified distance, and then stops.""" return commands2.FunctionalCommand( @@ -29,41 +30,47 @@ def simpleAuto(drive: subsystems.drivesubsystem.DriveSubsystem): # Stop driving at the end of the command lambda interrupt: drive.arcadeDrive(0, 0), # End the command when the robot's driven distance exceeds the desired value - lambda: drive.getAverageEncoderDistance() >= constants.kAutoDriveDistanceInches, + lambda: drive.getAverageEncoderDistance() + >= constants.kAutoDriveDistanceInches, # Require the drive subsystem ) - - def complexAuto(driveSubsystem: subsystems.drivesubsystem.DriveSubsystem, hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem): - """A complex auto routine that drives forward, drops a hatch, and then drives backward.""" - return commands2.cmd.sequence([ - # Drive forward up to the front of the cargo ship - commands2.FunctionalCommand( - # Reset encoders on command start - driveSubsystem.resetEncoders, - # Drive forward while the command is executing - lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveSpeed, 0), - # Stop driving at the end of the command - lambda interrupt: driveSubsystem.arcadeDrive(0,0), - # End the command when the robot's driven distance exceeds the desired value - lambda: driveSubsystem.getAverageEncoderDistance() >= constants.kAutoDriveDistanceInches, - # Require the drive subsystem - [driveSubsystem] - ), - # Release the hatch - hatchSubsystem.releaseHatch(), - - # Drive backward the specified distance - commands2.FunctionalCommand( - # Reset encoders on command start - driveSubsystem.resetEncoders, - # Drive backwards while the command is executing - lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveSpeed, 0), - # Stop driving at the end of the command - lambda interrupt: driveSubsystem.arcadeDrive(0,0), - # End the command when the robot's driven distance exceeds the desired value - lambda: abs(driveSubsystem.getAverageEncoderDistance()) >= constants.kAutoBackupDistanceInches, - # Require the drive subsystem - [driveSubsystem] - ) - ]) \ No newline at end of file + def complexAuto( + driveSubsystem: subsystems.drivesubsystem.DriveSubsystem, + hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem, + ): + """A complex auto routine that drives forward, drops a hatch, and then drives backward.""" + return commands2.cmd.sequence( + [ + # Drive forward up to the front of the cargo ship + commands2.FunctionalCommand( + # Reset encoders on command start + driveSubsystem.resetEncoders, + # Drive forward while the command is executing + lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: driveSubsystem.arcadeDrive(0, 0), + # End the command when the robot's driven distance exceeds the desired value + lambda: driveSubsystem.getAverageEncoderDistance() + >= constants.kAutoDriveDistanceInches, + # Require the drive subsystem + [driveSubsystem], + ), + # Release the hatch + hatchSubsystem.releaseHatch(), + # Drive backward the specified distance + commands2.FunctionalCommand( + # Reset encoders on command start + driveSubsystem.resetEncoders, + # Drive backwards while the command is executing + lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: driveSubsystem.arcadeDrive(0, 0), + # End the command when the robot's driven distance exceeds the desired value + lambda: abs(driveSubsystem.getAverageEncoderDistance()) + >= constants.kAutoBackupDistanceInches, + # Require the drive subsystem + [driveSubsystem], + ), + ] + ) diff --git a/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py b/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py index a2ee19d..433be28 100644 --- a/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py +++ b/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py @@ -17,8 +17,12 @@ def __init__(self) -> None: def grabHatch(self) -> commands2.Command: """Grabs the hatch""" - return commands2.cmd.runOnce(lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward), [self]) + return commands2.cmd.runOnce( + lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward), [self] + ) def releaseHatch(self) -> commands2.Command: """Releases the hatch""" - return commands2.cmd.runOnce(lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse), [self]) + return commands2.cmd.runOnce( + lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse), [self] + ) From 4a73e80660063fed93a5c2e4e08ab832365bbd65 Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Mon, 2 Jan 2023 19:39:59 +0300 Subject: [PATCH 16/17] delete duplicate entries --- run_tests.sh | 2 -- 1 file changed, 2 deletions(-) mode change 100644 => 100755 run_tests.sh diff --git a/run_tests.sh b/run_tests.sh old mode 100644 new mode 100755 index 203fc4e..3777f84 --- a/run_tests.sh +++ b/run_tests.sh @@ -15,8 +15,6 @@ BASE_TESTS=" commands-v2/hatchbot-inlined commands-v2/ramsete commands-v2/selectcommand - commands-v2/gyro-drive-commands - commands-v2/drive-distance-offboard cscore-intermediate-vision cscore-quick-vision elevator-profiled-pid From ab0b8d15ed677e43ae6071a45b8b0d1e19b3e131 Mon Sep 17 00:00:00 2001 From: Berke Sinan Yetkin Date: Tue, 3 Jan 2023 22:57:43 +0300 Subject: [PATCH 17/17] Update physics.py --- commands-v2/hatchbot-inlined/physics.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/commands-v2/hatchbot-inlined/physics.py b/commands-v2/hatchbot-inlined/physics.py index 26ba33f..782be77 100644 --- a/commands-v2/hatchbot-inlined/physics.py +++ b/commands-v2/hatchbot-inlined/physics.py @@ -26,9 +26,7 @@ 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 + Simulates a Differential Drivetrain using system identification data for the space-state model. """ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"):