From 5e6d0c5121cdb01fc94c8616f203c8ed38bdf1f1 Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Sat, 20 Apr 2024 19:04:19 -0400 Subject: [PATCH] Add XrpReference example - Tests don't pass because static variables --- XrpReference/commands/arcadedrive.py | 35 +++++ XrpReference/commands/autonomous_distance.py | 28 ++++ XrpReference/commands/autonomous_time.py | 30 ++++ XrpReference/commands/drivedistance.py | 44 ++++++ XrpReference/commands/drivetime.py | 47 +++++++ XrpReference/commands/turndegrees.py | 58 ++++++++ XrpReference/commands/turntime.py | 49 +++++++ XrpReference/robot.py | 97 +++++++++++++ XrpReference/robotcontainer.py | 95 +++++++++++++ XrpReference/subsystems/arm.py | 28 ++++ XrpReference/subsystems/drivetrain.py | 141 +++++++++++++++++++ run_tests.sh | 1 + 12 files changed, 653 insertions(+) create mode 100644 XrpReference/commands/arcadedrive.py create mode 100644 XrpReference/commands/autonomous_distance.py create mode 100644 XrpReference/commands/autonomous_time.py create mode 100644 XrpReference/commands/drivedistance.py create mode 100644 XrpReference/commands/drivetime.py create mode 100644 XrpReference/commands/turndegrees.py create mode 100644 XrpReference/commands/turntime.py create mode 100644 XrpReference/robot.py create mode 100644 XrpReference/robotcontainer.py create mode 100644 XrpReference/subsystems/arm.py create mode 100644 XrpReference/subsystems/drivetrain.py diff --git a/XrpReference/commands/arcadedrive.py b/XrpReference/commands/arcadedrive.py new file mode 100644 index 00000000..d04ba5e4 --- /dev/null +++ b/XrpReference/commands/arcadedrive.py @@ -0,0 +1,35 @@ +# +# 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 commands2 +from subsystems.drivetrain import Drivetrain + + +class ArcadeDrive(commands2.Command): + def __init__( + self, + drive: Drivetrain, + xaxisSpeedSupplier: typing.Callable[[], float], + zaxisRotateSupplier: typing.Callable[[], float], + ) -> None: + """Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier + lambdas. This command does not terminate. + + :param drivetrain: The drivetrain subsystem on which this command will run + :param xaxisSpeedSupplier: Callable supplier of forward/backward speed + :param zaxisRotateSupplier: Callable supplier of rotational speed + """ + super().__init__() + + self.drive = drive + self.xaxisSpeedSupplier = xaxisSpeedSupplier + self.zaxisRotateSupplier = zaxisRotateSupplier + + self.addRequirements(self.drive) + + def execute(self) -> None: + self.drive.arcadeDrive(self.xaxisSpeedSupplier(), self.zaxisRotateSupplier()) diff --git a/XrpReference/commands/autonomous_distance.py b/XrpReference/commands/autonomous_distance.py new file mode 100644 index 00000000..ad84366e --- /dev/null +++ b/XrpReference/commands/autonomous_distance.py @@ -0,0 +1,28 @@ +# +# 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 + +from commands.drivedistance import DriveDistance +from commands.turndegrees import TurnDegrees +from subsystems.drivetrain import Drivetrain + + +class AutonomousDistance(commands2.SequentialCommandGroup): + def __init__(self, drive: Drivetrain) -> None: + """Creates a new Autonomous Drive based on distance. This will drive out for a specified distance, + turn around and drive back. + + :param drivetrain: The drivetrain subsystem on which this command will run + """ + super().__init__() + + self.addCommands( + DriveDistance(-0.5, 10, drive), + TurnDegrees(-0.5, 180, drive), + DriveDistance(-0.5, 10, drive), + TurnDegrees(0.5, 180, drive), + ) diff --git a/XrpReference/commands/autonomous_time.py b/XrpReference/commands/autonomous_time.py new file mode 100644 index 00000000..1a48cf84 --- /dev/null +++ b/XrpReference/commands/autonomous_time.py @@ -0,0 +1,30 @@ +# +# 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 + +from commands.drivetime import DriveTime +from commands.turntime import TurnTime +from subsystems.drivetrain import Drivetrain + + +class AutonomousTime(commands2.SequentialCommandGroup): + def __init__(self, drive: Drivetrain) -> None: + """Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn + around for time (equivalent to time to turn around) and drive forward again. This should mimic + driving out, turning around and driving back. + + :param drivetrain: The drive subsystem on which this command will run + + """ + super().__init__() + + self.addCommands( + DriveTime(-0.6, 2.0, drive), + TurnTime(-0.5, 1.3, drive), + DriveTime(-0.6, 2.0, drive), + TurnTime(0.5, 1.3, drive), + ) diff --git a/XrpReference/commands/drivedistance.py b/XrpReference/commands/drivedistance.py new file mode 100644 index 00000000..bbe95b22 --- /dev/null +++ b/XrpReference/commands/drivedistance.py @@ -0,0 +1,44 @@ +# +# 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 + +from subsystems.drivetrain import Drivetrain + + +class DriveDistance(commands2.Command): + def __init__(self, speed: float, inches: float, drive: Drivetrain) -> None: + """Creates a new DriveDistance. This command will drive your your robot for a desired distance at + a desired speed. + + :param speed: The speed at which the robot will drive + :param inches: The number of inches the robot will drive + :param drive: The drivetrain subsystem on which this command will run + """ + super().__init__() + + self.distance = inches + self.speed = speed + self.drive = drive + self.addRequirements(drive) + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + self.drive.arcadeDrive(0, 0) + self.drive.resetEncoders() + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(self.speed, 0) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end.""" + # Compare distance travelled from start to desired distance + return abs(self.drive.getAverageDistanceInch()) >= self.distance diff --git a/XrpReference/commands/drivetime.py b/XrpReference/commands/drivetime.py new file mode 100644 index 00000000..fbae9b09 --- /dev/null +++ b/XrpReference/commands/drivetime.py @@ -0,0 +1,47 @@ +# +# 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 wpilib + +from subsystems.drivetrain import Drivetrain + + +class DriveTime(commands2.Command): + """Creates a new DriveTime. This command will drive your robot for a desired speed and time.""" + + def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: + """Creates a new DriveTime. This command will drive your robot for a desired speed and time. + + :param speed: The speed which the robot will drive. Negative is in reverse. + :param time: How much time to drive in seconds + :param drive: The drivetrain subsystem on which this command will run + """ + super().__init__() + + self.speed = speed + self.duration = time + self.drive = drive + self.addRequirements(drive) + + self.startTime = 0.0 + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + self.startTime = wpilib.Timer.getFPGATimestamp() + self.drive.arcadeDrive(0, 0) + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(self.speed, 0) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end""" + return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration diff --git a/XrpReference/commands/turndegrees.py b/XrpReference/commands/turndegrees.py new file mode 100644 index 00000000..6d094ee9 --- /dev/null +++ b/XrpReference/commands/turndegrees.py @@ -0,0 +1,58 @@ +# +# 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 math +import commands2 + +from subsystems.drivetrain import Drivetrain + + +class TurnDegrees(commands2.Command): + def __init__(self, speed: float, degrees: float, drive: Drivetrain) -> None: + """Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in + degrees) and rotational speed. + + :param speed: The speed which the robot will drive. Negative is in reverse. + :param degrees: Degrees to turn. Leverages encoders to compare distance. + :param drive: The drive subsystem on which this command will run + """ + super().__init__() + + self.degrees = degrees + self.speed = speed + self.drive = drive + self.addRequirements(drive) + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + # Set motors to stop, read encoder values for starting point + self.drive.arcadeDrive(0, 0) + self.drive.resetEncoders() + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(0, self.speed) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end.""" + + # Need to convert distance travelled to degrees. The Standard + # XRP Chassis found here, https://www.sparkfun.com/products/22230, + # has a wheel placement diameter (163 mm) - width of the wheel (8 mm) = 155 mm + # or 6.102 inches. We then take into consideration the width of the tires. + inchPerDegree = math.pi * 6.102 / 360 + + # Compare distance travelled from start to distance based on degree turn + return self._getAverageTurningDistance() >= inchPerDegree * self.degrees + + def _getAverageTurningDistance(self) -> float: + leftDistance = abs(self.drive.getLeftDistanceInch()) + rightDistance = abs(self.drive.getRightDistanceInch()) + return (leftDistance + rightDistance) / 2.0 diff --git a/XrpReference/commands/turntime.py b/XrpReference/commands/turntime.py new file mode 100644 index 00000000..691cc51d --- /dev/null +++ b/XrpReference/commands/turntime.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 commands2 +import wpilib + +from subsystems.drivetrain import Drivetrain + + +class TurnTime(commands2.Command): + """Creates a new TurnTime command. This command will turn your robot for a + desired rotational speed and time. + """ + + def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: + """Creates a new TurnTime. + + :param speed: The speed which the robot will turn. Negative is in reverse. + :param time: How much time to turn in seconds + :param drive: The drive subsystem on which this command will run + """ + super().__init__() + + self.rotationalSpeed = speed + self.duration = time + self.drive = drive + self.addRequirements(drive) + + self.startTime = 0.0 + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + self.startTime = wpilib.Timer.getFPGATimestamp() + self.drive.arcadeDrive(0, 0) + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(0, self.rotationalSpeed) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end""" + return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration diff --git a/XrpReference/robot.py b/XrpReference/robot.py new file mode 100644 index 00000000..afee311a --- /dev/null +++ b/XrpReference/robot.py @@ -0,0 +1,97 @@ +#!/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. +# + +# +# Example that shows how to connect to a XRP from RobotPy +# +# Requirements +# ------------ +# +# You must have the robotpy-xrp package installed. This is best done via: +# +# # Windows +# py -3 -m pip install robotpy[commands2,sim,xrp] +# +# # Linux/macOS +# pip3 install robotpy[commands2,sim,xrp] +# +# Run the program +# --------------- +# +# To run the program you will need to explicitly use the XRP option: +# +# # Windows +# py -3 robotpy sim --xrp +# +# # Linux/macOS +# python robotpy sim --xrp +# +# By default the WPILib simulation GUI will be displayed. To disable the display +# you can add the --nogui option +# + +import os +import typing + +import wpilib +import commands2 + +from robotcontainer import RobotContainer + +# If your ROMI isn't at the default address, set that here +os.environ["HALSIMXRP_HOST"] = "192.168.42.1" +os.environ["HALSIMXRP_PORT"] = "3540" + + +class MyRobot(commands2.TimedCommandRobot): + """ + Command v2 robots are encouraged to inherit from TimedCommandRobot, which + has an implementation of robotPeriodic which runs the scheduler for you + """ + + autonomousCommand: typing.Optional[commands2.Command] = None + + def robotInit(self) -> None: + """ + This function is run when the robot is first started up and should be used for any + initialization code. + """ + + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.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() diff --git a/XrpReference/robotcontainer.py b/XrpReference/robotcontainer.py new file mode 100644 index 00000000..22e4df8e --- /dev/null +++ b/XrpReference/robotcontainer.py @@ -0,0 +1,95 @@ +# +# 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 commands2 +import commands2.button +import wpilib +import xrp + +from commands.arcadedrive import ArcadeDrive +from commands.autonomous_distance import AutonomousDistance +from commands.autonomous_time import AutonomousTime + +from subsystems.arm import Arm +from subsystems.drivetrain import Drivetrain + + +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 robot's subsystems and commands are defined here... + self.drivetrain = Drivetrain() + self.onboardIO = xrp.XRPOnBoardIO() + self.arm = Arm() + + # Assumes a gamepad plugged into channnel 0 + self.controller = wpilib.Joystick(0) + + # Create SmartDashboard chooser for autonomous routines + self.chooser = wpilib.SendableChooser() + + self._configureButtonBindings() + + def _configureButtonBindings(self): + """Use this method to define your button->command mappings. Buttons can be created by + instantiating a :class:`.GenericHID` or one of its subclasses (:class`.Joystick or + :class:`.XboxController`), and then passing it to a :class:`.JoystickButton`. + """ + + # Default command is arcade drive. This will run unless another command + # is scheduled over it + self.drivetrain.setDefaultCommand(self.getArcadeDriveCommand()) + + # Example of how to use the onboard IO + onboardButtonA = commands2.button.Trigger(self.onboardIO.getUserButtonPressed) + onboardButtonA.onTrue(commands2.PrintCommand("USER Button Pressed")).onFalse( + commands2.PrintCommand("USER Button Released") + ) + + joystickAButton = commands2.button.JoystickButton(self.controller, 1) + joystickAButton.onTrue( + commands2.InstantCommand(lambda: self.arm.setAngle(45.0), self.arm) + ) + joystickAButton.onFalse( + commands2.InstantCommand(lambda: self.arm.setAngle(0.0), self.arm) + ) + + joystickBButton = commands2.button.JoystickButton(self.controller, 2) + joystickBButton.onTrue( + commands2.InstantCommand(lambda: self.arm.setAngle(90.0), self.arm) + ) + joystickBButton.onFalse( + commands2.InstantCommand(lambda: self.arm.setAngle(0.0), self.arm) + ) + + # Setup SmartDashboard options + self.chooser.setDefaultOption( + "Auto Routine Distance", AutonomousDistance(self.drivetrain) + ) + self.chooser.addOption("Auto Routine Time", AutonomousTime(self.drivetrain)) + wpilib.SmartDashboard.putData(self.chooser) + + def getAutonomousCommand(self) -> typing.Optional[commands2.Command]: + return self.chooser.getSelected() + + def getArcadeDriveCommand(self) -> ArcadeDrive: + """Use this to pass the teleop command to the main robot class. + + :returns: the command to run in teleop + """ + return ArcadeDrive( + self.drivetrain, + lambda: self.controller.getRawAxis(1), + lambda: self.controller.getRawAxis(2), + ) diff --git a/XrpReference/subsystems/arm.py b/XrpReference/subsystems/arm.py new file mode 100644 index 00000000..3306da11 --- /dev/null +++ b/XrpReference/subsystems/arm.py @@ -0,0 +1,28 @@ +# +# 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 xrp + + +class Arm(commands2.Subsystem): + + def __init__(self): + """Creates a new Arm.""" + + # Device number 4 maps to the physical Servo 1 port on the XRP + self.armServo = xrp.XRPServo(4) + + def periodic(self): + """This method will be called once per scheduler run""" + + def setAngle(self, angleDeg: float): + """ + Set the current angle of the arm (0 - 180 degrees). + + :param angleDeg: Desired arm angle in degrees + """ + self.armServo.setAngle(angleDeg) diff --git a/XrpReference/subsystems/drivetrain.py b/XrpReference/subsystems/drivetrain.py new file mode 100644 index 00000000..b1719472 --- /dev/null +++ b/XrpReference/subsystems/drivetrain.py @@ -0,0 +1,141 @@ +# +# 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 math + +import commands2 +import wpilib +import wpilib.drive +import wpiutil +import xrp + + +class Drivetrain(commands2.Subsystem): + kGearRatio = (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0) # 48.75:1 + kCountsPerMotorShaftRev = 12.0 + kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio # 585.0 + kWheelDiameterInch = 2.3622 # 60 mm + + def __init__(self) -> None: + super().__init__() + + # The XRP has the left and right motors set to + # PWM channels 0 and 1 respectively + self.leftMotor = xrp.XRPMotor(0) + self.rightMotor = xrp.XRPMotor(1) + + # The XRP has onboard encoders that are hardcoded + # to use DIO pins 4/5 and 6/7 for the left and right + self.leftEncoder = wpilib.Encoder(4, 5) + self.rightEncoder = wpilib.Encoder(6, 7) + + # Set up the differential drive controller + self.drive = wpilib.drive.DifferentialDrive( + self.leftMotor.set, self.rightMotor.set + ) + + # TODO: these don't work + # wpiutil.SendableRegistry.addChild(self.drive, self.leftMotor) + # wpiutil.SendableRegistry.addChild(self.drive, self.rightMotor) + + # Set up the XRPGyro + self.gyro = xrp.XRPGyro() + + # Set up the BuiltInAccelerometer + self.accelerometer = wpilib.BuiltInAccelerometer() + + # 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.rightMotor.setInverted(True) + + # Use inches as unit for encoder distances + self.leftEncoder.setDistancePerPulse( + (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution + ) + self.rightEncoder.setDistancePerPulse( + (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution + ) + self.resetEncoders() + + def arcadeDrive(self, xaxisSpeed: float, zaxisRotate: float) -> None: + """ + Drives the robot using arcade controls. + + :param xaxisSpeed: the commanded forward movement + :param zaxisRotate: the commanded rotation + """ + self.drive.arcadeDrive(xaxisSpeed, zaxisRotate) + + def resetEncoders(self) -> None: + """Resets the drive encoders to currently read a position of 0.""" + self.leftEncoder.reset() + self.rightEncoder.reset() + + def getLeftEncoderCount(self) -> int: + return self.leftEncoder.get() + + def getRightEncoderCount(self) -> int: + return self.rightEncoder.get() + + def getLeftDistanceInch(self) -> float: + return self.leftEncoder.getDistance() + + def getRightDistanceInch(self) -> float: + return self.rightEncoder.getDistance() + + def getAverageDistanceInch(self) -> float: + """Gets the average distance of the TWO encoders.""" + return (self.getLeftDistanceInch() + self.getRightDistanceInch()) / 2.0 + + def getAccelX(self) -> float: + """The acceleration in the X-axis. + + :returns: The acceleration of the Romi along the X-axis in Gs + """ + return self.accelerometer.getX() + + def getAccelY(self) -> float: + """The acceleration in the Y-axis. + + :returns: The acceleration of the Romi along the Y-axis in Gs + """ + return self.accelerometer.getY() + + def getAccelZ(self) -> float: + """The acceleration in the Z-axis. + + :returns: The acceleration of the Romi along the Z-axis in Gs + """ + return self.accelerometer.getZ() + + def getGyroAngleX(self) -> float: + """Current angle of the Romi around the X-axis. + + :returns: The current angle of the Romi in degrees + """ + return self.gyro.getAngleX() + + def getGyroAngleY(self) -> float: + """Current angle of the Romi around the Y-axis. + + :returns: The current angle of the Romi in degrees + """ + return self.gyro.getAngleY() + + def getGyroAngleZ(self) -> float: + """Current angle of the Romi around the Z-axis. + + :returns: The current angle of the Romi in degrees + """ + return self.gyro.getAngleZ() + + def resetGyro(self) -> None: + """Reset the gyro""" + self.gyro.reset() + + def periodic(self) -> None: + """This method will be called once per scheduler run""" diff --git a/run_tests.sh b/run_tests.sh index 6ef33385..11fcf4be 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -70,6 +70,7 @@ BASE_TESTS=" IGNORED_TESTS=" RamseteCommand PhysicsCamSim/src + XrpReference " ALL_TESTS="${BASE_TESTS}"