From a6bc3a6b49f5b8fd00ad11164da884e0a7ae070d Mon Sep 17 00:00:00 2001 From: BerkeSinanYetkin Date: Tue, 20 Dec 2022 23:39:04 +0300 Subject: [PATCH 1/7] 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 00000000..3d82aac9 --- /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 00000000..8df0fa1c --- /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 2/7] updated tests --- run_tests.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/run_tests.sh b/run_tests.sh index 90410a3e..92e702cc 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 3/7] 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 5974f7cc..8d1417de 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 4/7] 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 8d1417de..3d82aac9 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 5/7] 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 8df0fa1c..5d7c2cec 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 6/7] 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 00000000..57cf56d7 --- /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 00000000..4905b530 --- /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 00000000..b24ee219 --- /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 00000000..0b7561ab --- /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 00000000..439a3b99 --- /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 00000000..1fcd20b4 --- /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 92e702cc..664a493b 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 7/7] 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 57cf56d7..38b5416c 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 4905b530..0e1a55f4 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 b24ee219..7f8546e6 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 439a3b99..99291084 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 1fcd20b4..518479fd 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 + )