diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml new file mode 100644 index 0000000..d3f72be --- /dev/null +++ b/.github/workflows/test.yml @@ -0,0 +1,43 @@ +--- +name: dist + +on: + pull_request: + push: + branches: + - main + - '2027' + tags: + - '*' + workflow_dispatch: + +jobs: + check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v5 + - uses: pre-commit/action@v3.0.1 + + test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: ["ubuntu-22.04", "macos-14", "windows-2022"] + python_version: + - '3.10' + - '3.11' + - '3.12' + - '3.13' + - '3.14' + + steps: + - uses: actions/checkout@v5 + - uses: actions/setup-python@v5 + with: + python-version: ${{ matrix.python_version }} + - name: Install deps + run: | + pip install 'robotpy[commands2,navx,pykit,pathplannerlib,ctre,rev]<2027,>=2026.1.1' numpy pytest photonlibpy limelightlib-python + - name: Run tests + run: bash run_tests.sh + shell: bash diff --git a/.gitignore b/.gitignore index d178734..b6423d0 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,13 @@ +# Vim temporarily files +*.swp + # WPILib default config tests/ .wpilib/ +# WPILib default simulator +simgui-ds.json + # CTRE Simulator ctre_sim diff --git a/autonomous/__init__.py b/autonomous/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/autonomous/basicauto.py b/autonomous/basicauto.py new file mode 100644 index 0000000..137c719 --- /dev/null +++ b/autonomous/basicauto.py @@ -0,0 +1,34 @@ +import constants +from commands2 import Command +from wpimath.controller import LTVUnicycleController +from drivetrain import Drivetrain +from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator +from wpimath.geometry import Pose2d, Rotation2d, Translation2d + + +class BasicAuto(Command): + def __init__(self, drivetrain: Drivetrain) -> None: + super().__init__() + self.trajectory = TrajectoryGenerator.generateTrajectory( + Pose2d(0, 0, Rotation2d(0)), + [Translation2d(1, 1), Translation2d(2, -1)], + Pose2d(3, 0, Rotation2d(0)), + TrajectoryConfig(0.5, 0.5) + ) + self.reference = self.trajectory.sample(3) + self.controller = LTVUnicycleController(0.020) + + self.drivetrain = drivetrain + self.addRequirements(drivetrain) + + def execute(self) -> None: + adjustedSpeeds = self.controller.calculate( + self.drivetrain.getPose(), self.reference + ) + + wheelSpeeds = constants.kDrivetrainKinematics.toWheelSpeeds(adjustedSpeeds) + + self.drivetrain.driveWithWheelSpeeds(wheelSpeeds) + + def end(self, interrupted: bool) -> None: + self.drivetrain.stop() diff --git a/buttons.py b/buttons.py index e5618a7..d664c81 100644 --- a/buttons.py +++ b/buttons.py @@ -20,15 +20,15 @@ "pov-down": 180, "pov-left": 270, "pov-right": 90, - "left-x-axis": 0, - "left-y-axis": 1, - "right-x-axis": 2, - "right-y-axis": 5, - "left-trigger-axis": 3, - "right-trigger-axis": 4 + "left-x-stick": 0, + "left-y-stick": 1, + "right-x-stick": 2, + "right-y-stick": 5, + "left-trigger-axis": 3, + "right-trigger-axis": 4, } -#Generic dualshock4 +# Generic dualshock4 g_ps4_controller = { "square": 4, @@ -49,12 +49,12 @@ "pov-down": 180, "pov-left": 270, "pov-right": 90, - "left-x-axis": 0, - "left-y-axis": 1, - "right-x-axis": 2, - "right-y-axis": 3, - "left-trigger-axis": 2, - "right-trigger-axis": 3 + "left-x-stick": 0, + "left-y-stick": 1, + "right-x-stick": 2, + "right-y-stick": 3, + "left-trigger-axis": 2, + "right-trigger-axis": 3, } # Generic Nintendo Switch Pro Controller @@ -81,16 +81,16 @@ "rb": 6, "back": 7, "start": 8, - "press_left_stick": 9, - "press_right_stick": 10, + "press-left-stick": 9, + "press-right-stick": 10, "pov-up": 0, "pov-down": 180, "pov-left": 270, "pov-right": 90, "left-x-stick": 0, "left-y-stick": 1, - "left-trigger-axis": 2, - "right-trigger-axis": 3, + "left-trigger-axis": 2, + "right-trigger-axis": 3, "right-y-stick": 5, "right-x-stick": 4, } @@ -98,15 +98,15 @@ # Generic steering wheel controller (generic Xbox 360 controller) steering_wheel = { "turn-axis": 0, - "a" : 3, - "b" : 2, - "y" : 1, - "x" : 4, - "lb" : 5, - "rb" : 6, - "rt" : 8, - "lt" : 7, - "r3" : 12, - "l3" : 11, - "back" : 9, -} \ No newline at end of file + "a": 3, + "b": 2, + "y": 1, + "x": 4, + "lb": 5, + "rb": 6, + "rt": 8, + "lt": 7, + "r3": 12, + "l3": 11, + "back": 9, +} diff --git a/camera.py b/camera.py index c256eed..2530abc 100644 --- a/camera.py +++ b/camera.py @@ -1,43 +1,91 @@ -from photonlibpy import PhotonCamera -import wpimath.units -import constants -from typing import Optional, Tuple -from utils import Utils - -from photonlibpy.targeting.photonTrackedTarget import PhotonTrackedTarget - -class AprilTagCamera(PhotonCamera): - def __init__(self, camera: str) -> None: - self.camera = PhotonCamera(camera) - - def getBestTarget(self) -> Optional[PhotonTrackedTarget]: - result = self.camera.getLatestResult() - if result.hasTargets(): - target = result.getBestTarget() - return target - return None - - def getYaw(self, tag: int) -> float: - results = self.camera.getAllUnreadResults() - if len(results) > 0: - result = results[-1] - for target in result.getTargets(): - if target.getFiducialId() == tag: - return target.getYaw() - return -1 - - def getYawWithRange(self, tag: int) -> Tuple[float, float]: - results = self.camera.getAllUnreadResults() - target_range = 0 - if len(results) > 0: - result = results[-1] - for target in result.getTargets(): - if target.getFiducialId() == tag: - target_range = Utils.calculateDistanceToTargetMeters( - constants.kCameraHeightMeters, - constants.kTargetHeightMeters, - constants.kCameraPitchRadians, - wpimath.units.degreesToRadians(target.getPitch()) - ) - return target.getYaw(), target_range - return -1, -1 \ No newline at end of file +import constants +from photonlibpy import PhotonCamera +from typing import Optional, Tuple +from utils import Utils +from abc import ABC, abstractmethod +from limelight import Limelight +from limelightresults import parse_results +from wpinet import PortForwarder +from photonlibpy.targeting.photonTrackedTarget import PhotonTrackedTarget +from wpimath.units import degreesToRadians +from wpilib import RobotBase + +class Camera(ABC): + @abstractmethod + def getYawFromTag(self, tag: int) -> float: + pass + + @abstractmethod + def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]: + pass + + +class PhotonVisionCamera(Camera): + def __init__(self, camera: str) -> None: + self.camera = PhotonCamera(camera) + + def getBestTarget(self) -> Optional[PhotonTrackedTarget]: + result = self.camera.getLatestResult() + if result.hasTargets(): + target = result.getBestTarget() + return target + return None + + def getYawFromTag(self, tag: int) -> float: + results = self.camera.getAllUnreadResults() + if len(results) > 0: + result = results[-1] + for target in result.getTargets(): + if target.getFiducialId() == tag: + return target.getYaw() + return -1 + + def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]: + results = self.camera.getAllUnreadResults() + target_range = 0 + if len(results) > 0: + result = results[-1] + for target in result.getTargets(): + if target.getFiducialId() == tag: + target_range = Utils.calculateDistanceToTargetMeters( + constants.kCameraHeightMeters, + constants.kTargetHeightMeters, + constants.kCameraPitchRadians, + degreesToRadians(target.getPitch()), + ) + return target.getYaw(), target_range + return -1, -1 + + +class LimelightCamera(Camera): + def __init__(self, camera: str) -> None: + self.limelight = None + + if not RobotBase.isSimulation(): + self.limelight = Limelight(camera) + self.limelight.pipeline_switch(0) + + PortForwarder.getInstance().add(*constants.kLimelightPortForwarder) + + def getYawFromTag(self, tag: int) -> float: + if self.limelight is None: + return -1 + + result = self.limelight.get_results() + parsed_result = parse_results(result) + + for target in parsed_result.fiducialResults: + if target.fiducial_id == tag: + return target.target_x_degrees # yaw + + return -1 + + def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]: + return 0, 0 + +class Pixy2(Camera): + pass + + +class DriverCamera(Camera): + pass diff --git a/climber.py b/climber.py index eae0e86..2d65863 100644 --- a/climber.py +++ b/climber.py @@ -3,6 +3,7 @@ import phoenix6 import phoenix5 + class Climber: def __init__(self): self.left_motor = phoenix5.WPI_VictorSPX(1) @@ -16,4 +17,4 @@ def stop(self): self.climber.set(0) def down(self): - self.climber.set(-1) \ No newline at end of file + self.climber.set(-1) diff --git a/constants.py b/constants.py index 587aabb..82107ae 100644 --- a/constants.py +++ b/constants.py @@ -1,18 +1,24 @@ from math import pi from wpilib import SerialPort -from pathplannerlib.config import RobotConfig, ModuleConfig +from wpimath.kinematics import DifferentialDriveKinematics +from rev import SparkLowLevel, FeedbackSensor, SparkBaseConfig # Joystick kJoystickDriverPort = 0 kJoystickCoDriverPort = 1 -kXboxController = "Controller (XBOX 360 For Windows)" +kRealXboxController = "Controller (XBOX 360 For Windows)" +kSimXboxController = "Xbox Controller" kGenericPS4Controller = "Wire PS4 Controller" -# Drivetrain -kLeftFrontId = 1 -kLeftBackId = 2 -kRightFrontId = 3 -kRightBackId = 4 +# Drivetrain Motor Controllers +kLeftFrontId = 50 +kLeftBackId = 52 +kRightFrontId = 55 +kRightBackId = 54 +kDrivetrainSmartCurrentLimit = 40 +kDrivetrainMotorType = SparkLowLevel.MotorType.kBrushless +kDrivetrainIdleMode = SparkBaseConfig.IdleMode.kBrake +kDrivetrainPID = (0.2, 0, 0) # PhotonVision kCameraName = "Camera7459" @@ -21,29 +27,41 @@ kCameraPitchRadians = 0 kGoalRangeMeters = 1 +# Limelight 3A +kLimelightRemoteHost = "172.29.0.1" +kLimelightPortForwarder = (5807, kLimelightRemoteHost, 5807) # port, remoteHost, remotePort + # Drivetrain Odometry kInitialPose = (0, 0, 0) # Drivetrain Kinematics -kTrackWidthInMeters = 0.5 - -# Drivetrain PID Controller -kPIDAngularDrivetrain = (0.1, 0, 0) -kPIDForwardDrivetrain = (0.1, 0, 0) +kTrackWidthMeters = 0.5 +kDrivetrainKinematics = DifferentialDriveKinematics(kTrackWidthMeters) +kMaxVelocityMetersPerSecond = 3 +kMaxAccelerationMetersPerSecondSquared = 1 # Drivetrain Encoders -kLeftEncoder = (1, 2) -kRightEncoder = (3, 4, True) -kWheelDiameter = 0.152 # HiGrip -kGearReduction = 10.7 # Toughbox Mini -kWheelCircumference = kWheelDiameter * pi -kRotationsToMeters = kWheelCircumference / kGearReduction -kEncoderPPR = 2048 -kDistancePerPulse = kRotationsToMeters / kEncoderPPR -kRotationsPerMinuteToMetersPerSeconds = kRotationsToMeters / 60 +kLeftMotorsInverted = False +kRightMotorsInverted = True +kWheelDiameter = 0.152 # HiGrip +kGearReduction = 10.7 # Toughbox Mini +kRotationsToMeters = (kWheelDiameter * pi) / kGearReduction +kRotationsPerMinuteToMetersPerSecond = kRotationsToMeters / 60 +kFeedbackSensor = FeedbackSensor.kPrimaryEncoder + +# Drivetrain Feedforward +ksVolts = 0.30329 +kvVoltSecondsPerMeter = 2.9096 +kaVoltSecondsSquaredPerMeter = 0.35543 # Arduino kBaudRate = 9600 # WS2812b LEDs kLEDUSBPort = SerialPort.Port.kUSB1 + +# Intake +kIntakeAngleMotor = 1 +kIntakeTrackMotor = 12 +kPivotTimeDown = 0.35 +kPivotTimeUp = 0.5 \ No newline at end of file diff --git a/deploy/pathplanner/autos/New Auto.auto b/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..d158106 --- /dev/null +++ b/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "New New Path" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/deploy/pathplanner/paths/Example Path.path b/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..725a8c8 --- /dev/null +++ b/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.9330065398951426, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.95947218259629, + "y": 6.719771754636234 + }, + "prevControl": { + "x": 7.106880984323696, + "y": 8.083917671872381 + }, + "nextControl": { + "x": 8.865178316690441, + "y": 5.270641940085592 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.735791726105562, + "y": 4.300242510699002 + }, + "prevControl": { + "x": 10.171982881597717, + "y": 5.969329529243937 + }, + "nextControl": { + "x": 7.409370043302189, + "y": 2.7587254198734623 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.53798858773181, + "y": 1.2079029957203988 + }, + "prevControl": { + "x": 9.698104493580598, + "y": 1.6329540284587654 + }, + "nextControl": { + "x": 9.377872681883023, + "y": 0.7828519629820323 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.109243937232525, + "y": 4.420539874608858 + }, + "prevControl": { + "x": 6.238630527817403, + "y": 1.091455064194009 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.9, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/deploy/pathplanner/paths/New New Path.path b/deploy/pathplanner/paths/New New Path.path new file mode 100644 index 0000000..e1f95c8 --- /dev/null +++ b/deploy/pathplanner/paths/New New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.8826385542168675, + "y": 7.389807228915663 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.220939759036145, + "y": 7.0 + }, + "prevControl": { + "x": 6.499710843373493, + "y": 7.269602409638554 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/deploy/pathplanner/paths/New Path.path b/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..a4e69f1 --- /dev/null +++ b/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,118 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.7877318116975744, + "y": 7.521968616262482 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7267617689015686, + "y": 7.276134094151213 + }, + "prevControl": { + "x": 0.8184634218099796, + "y": 7.551239052876446 + }, + "nextControl": { + "x": 0.4938659058487873, + "y": 6.577446504992867 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3995720399429386, + "y": 4.766034236804566 + }, + "prevControl": { + "x": 0.3547146932952925, + "y": 5.149853780313838 + }, + "nextControl": { + "x": 2.444429386590585, + "y": 4.3822146932952935 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.745192582025677, + "y": 7.276134094151213 + }, + "prevControl": { + "x": 1.5642461661911549, + "y": 6.981518589514979 + }, + "nextControl": { + "x": 3.926138997860202, + "y": 7.5707495987874465 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.209029957203994, + "y": 7.0 + }, + "prevControl": { + "x": 6.031611982881596, + "y": 7.4702139800285305 + }, + "nextControl": { + "x": 8.119417969141262, + "y": 6.636427182360927 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.179429386590584, + "y": 5.4647218259629105 + }, + "prevControl": { + "x": 7.623067047075606, + "y": 6.499814550641941 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/deploy/pathplanner/settings.json b/deploy/pathplanner/settings.json index c64d29d..122c446 100644 --- a/deploy/pathplanner/settings.json +++ b/deploy/pathplanner/settings.json @@ -1,25 +1,25 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, - "holonomicMode": true, - "pathFolders": [ + "robotWidth": 0.82, + "robotLength": 0.88, + "holonomicMode": false, + "pathFolders": [], + "autoFolders": [ "New Folder" ], - "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxVel": 1.0, + "defaultMaxAccel": 1.0, + "defaultMaxAngVel": 360.0, + "defaultMaxAngAccel": 540.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, - "robotMOI": 6.883, - "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, - "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, - "wheelCOF": 1.2, + "robotMass": 15.0, + "robotMOI": 1.589, + "robotTrackwidth": 0.655, + "driveWheelRadius": 0.152, + "driveGearing": 10.7, + "maxDriveSpeed": 5.77, + "driveMotorType": "NEO", + "driveCurrentLimit": 40.0, + "wheelCOF": 1.0, "flModuleX": 0.273, "flModuleY": 0.273, "frModuleX": 0.273, @@ -30,5 +30,8 @@ "brModuleY": -0.273, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, - "robotFeatures": [] + "robotFeatures": [ + "{\"name\":\"Circle\",\"type\":\"circle\",\"data\":{\"center\":{\"x\":-0.15,\"y\":0.0},\"radius\":0.2,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.3,\"y\":0.0},\"size\":{\"width\":0.8,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" + ] } \ No newline at end of file diff --git a/drivetrain.py b/drivetrain.py index f218594..10d45e8 100644 --- a/drivetrain.py +++ b/drivetrain.py @@ -1,136 +1,253 @@ import constants - -from commands2 import Subsystem +from commands2 import Subsystem, Command from typing import Optional -from camera import AprilTagCamera -from phoenix5 import WPI_VictorSPX -from wpilib import MotorControllerGroup, DriverStation, Encoder +from camera import Camera +from wpilib import DriverStation, Field2d, SmartDashboard from navx import AHRS from wpilib.drive import DifferentialDrive -from wpimath.controller import PIDController +from wpimath.controller import PIDController, SimpleMotorFeedforwardMeters +from wpimath.kinematics import ( + DifferentialDriveOdometry, + DifferentialDriveWheelSpeeds, + ChassisSpeeds, +) from wpimath.geometry import Pose2d, Rotation2d -from pathplannerlib.config import RobotConfig +from rev import ( + SparkMax, + SparkMaxConfig, + ResetMode, + PersistMode, + SparkLowLevel, + ClosedLoopSlot, +) +from wpilib.simulation import DifferentialDrivetrainSim +from wpimath.system.plant import LinearSystemId, DCMotor from pathplannerlib.auto import AutoBuilder from pathplannerlib.controller import PPLTVController -from wpimath.kinematics import DifferentialDriveOdometry, ChassisSpeeds, DifferentialDriveKinematics -from wpimath.units import inchesToMeters +from pathplannerlib.config import RobotConfig class Drivetrain(Subsystem): - def __init__(self, camera: AprilTagCamera) -> None: - self.left_front_motor = WPI_VictorSPX(constants.kLeftFrontId) - self.left_back_motor = WPI_VictorSPX(constants.kLeftBackId) - self.right_front_motor = WPI_VictorSPX(constants.kRightFrontId) - self.right_back_motor = WPI_VictorSPX(constants.kRightBackId) + def __init__(self) -> None: + self.left_front_motor = SparkMax( + constants.kLeftFrontId, constants.kDrivetrainMotorType + ) + self.left_back_motor = SparkMax( + constants.kLeftBackId, constants.kDrivetrainMotorType + ) + self.right_front_motor = SparkMax( + constants.kRightFrontId, constants.kDrivetrainMotorType + ) + self.right_back_motor = SparkMax( + constants.kRightBackId, constants.kDrivetrainMotorType + ) + + self.drivetrain = DifferentialDrive( + self.left_front_motor, self.right_front_motor + ) + self.drivetrain.setSafetyEnabled(True) + self.field = Field2d() + + """ + self.drivetrain_system = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3) + self.drivetrain_simulator = DifferentialDrivetrainSim( + self.drivetrain_system, + DCMotor.NEO(4), + 8, + constants.kTrackWidth, + constants.kWheelDiameter / 2, + None + ) + """ + + config = SparkMaxConfig() + + config.smartCurrentLimit(constants.kDrivetrainSmartCurrentLimit) + config.setIdleMode(constants.kDrivetrainIdleMode) + config.closedLoop.pid(*constants.kDrivetrainPID) + config.closedLoop.velocityFF(constants.kvVoltSecondsPerMeter) + config.closedLoop.maxMotion.maxAcceleration( + constants.kMaxAccelerationMetersPerSecondSquared + ) + config.closedLoop.maxMotion.maxVelocity(constants.kMaxVelocityMetersPerSecond) + config.closedLoop.setFeedbackSensor(constants.kFeedbackSensor) + + config.encoder.positionConversionFactor(constants.kRotationsToMeters) + config.encoder.velocityConversionFactor( + constants.kRotationsPerMinuteToMetersPerSecond + ) + config.inverted(constants.kLeftMotorsInverted) + + self.left_front_motor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ) + + config.follow(constants.kLeftFrontId) + self.left_back_motor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ) + + config.disableFollowerMode() + config.inverted(constants.kRightMotorsInverted) + + self.right_front_motor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ) + config.follow(constants.kRightFrontId) + self.right_back_motor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ) + + config.disableFollowerMode() - self.left_motors = MotorControllerGroup(self.left_front_motor, self.left_back_motor) - self.right_motors = MotorControllerGroup(self.right_front_motor, self.right_back_motor) - self.right_motors.setInverted(True) - self.drivetrain = DifferentialDrive(self.left_motors, self.right_motors) + self.left_encoder = self.left_front_motor.getEncoder() + self.right_encoder = self.right_front_motor.getEncoder() - self.left_encoder = Encoder(*constants.kLeftEncoder) - self.right_encoder = Encoder(*constants.kRightEncoder) + self.left_closed_loop = self.left_front_motor.getClosedLoopController() + self.right_closed_loop = self.right_front_motor.getClosedLoopController() - self.left_encoder.setDistancePerPulse(constants.kDistancePerPulse) - self.right_encoder.setDistancePerPulse(constants.kDistancePerPulse) + self.left_encoder.setPosition(0) + self.right_encoder.setPosition(0) self.navx = AHRS.create_spi() self.navx.reset() - self.pid_angular = PIDController(*constants.kPIDAngularDrivetrain) - self.pid_forward = PIDController(*constants.kPIDForwardDrivetrain) + self.pid_angular = PIDController(*constants.kDrivetrainPID) + self.pid_forward = PIDController(*constants.kDrivetrainPID) rotation = Rotation2d.fromDegrees(self.navx.getAngle()) - self.pose = Pose2d(*constants.kInitialPose) - self.odometry = DifferentialDriveOdometry( - rotation, - self.left_encoder.getDistance(), - self.right_encoder.getDistance(), - self.pose + rotation, + self.left_encoder.getPosition(), + self.right_encoder.getPosition(), + Pose2d(*constants.kInitialPose), ) - self.kinematics = DifferentialDriveKinematics( - constants.kTrackWidthInMeters + + self.feedforward = SimpleMotorFeedforwardMeters( + constants.ksVolts, + constants.kvVoltSecondsPerMeter, + constants.kaVoltSecondsSquaredPerMeter, ) - config = RobotConfig.fromGUISettings() + try: + pathConfig = RobotConfig.fromGUISettings() + except: + raise Exception("ERROR: No Robot Config Loaded.") AutoBuilder.configure( - self.odometry.getPose, + self.getPose, self.resetPose, - self.getRobotRelativeSpeeds, - lambda speeds, feedforwards: self.driveRobotRelative(speeds), + self.getRelativeSpeeds, + lambda speeds, feedforwards: self.driveWithRelativeSpeeds(speeds), PPLTVController(0.02), - config, + pathConfig, self.shouldFlipPath, self ) + + def stop(self) -> None: + self.drivetrain.arcadeDrive(0, 0) - self.camera = camera + def resetEncoders(self) -> None: + self.left_encoder.setPosition(0) + self.right_encoder.setPosition(0) - def shouldFlipPath(): + def shouldFlipPath(self) -> None: return DriverStation.getAlliance() == DriverStation.Alliance.kRed def resetPose(self, pose: Pose2d) -> None: self.odometry.resetPosition( Rotation2d.fromDegrees(self.navx.getAngle()), - self.left_encoder.getDistance(), - self.right_encoder.getDistance(), - pose + self.left_encoder.getPosition(), + self.right_encoder.getPosition(), + pose, + ) + + def getPose(self) -> Pose2d: + return self.odometry.getPose() + + def driveWithWheelSpeeds(self, speeds: DifferentialDriveWheelSpeeds) -> None: + left_feedforward = self.feedforward.calculate(speeds.left) + right_feedforward = self.feedforward.calculate(speeds.right) + + self.left_closed_loop.setReference( + speeds.left, + SparkLowLevel.ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + left_feedforward, + ) + self.right_closed_loop.setReference( + speeds.right, + SparkLowLevel.ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + right_feedforward, + ) + + def driveWithRelativeSpeeds(self, chassisSpeeds: ChassisSpeeds) -> None: + wheelSpeeds = constants.kDrivetrainKinematics.toWheelSpeeds(chassisSpeeds) + self.left_closed_loop.setSetpoint( + wheelSpeeds.left, SparkLowLevel.ControlType.kVelocity + ) + self.right_closed_loop.setSetpoint( + wheelSpeeds.right, SparkLowLevel.ControlType.kVelocity + ) + + def getWheelSpeeds(self) -> DifferentialDriveWheelSpeeds: + return DifferentialDriveWheelSpeeds( + self.left_encoder.getVelocity(), self.right_encoder.getVelocity() ) - def getRobotRelativeSpeeds(self) -> ChassisSpeeds: - wheelSpeeds = DifferentialDriveWheelSpeeds( - self.left_encoder.getRate(), - self.right_encoder.getRate() + def getRelativeSpeeds(self) -> ChassisSpeeds: + return constants.kDrivetrainKinematics.toChassisSpeeds( + DifferentialDriveWheelSpeeds( + self.left_encoder.getVelocity(), self.right_encoder.getVelocity() + ) ) - return self.kinematics.toChassisSpeeds(wheelSpeeds) - def front(self) -> None: - self.drivetrain.tankDrive(1, 0) + def forward(self) -> Command: + self.run(lambda: self.drivetrain.arcadeDrive(1, 0)) + + def backward(self) -> None: + self.run(lambda: self.drivetrain.arcadeDrive(-1, 0)) - def back(self) -> None: - self.drivetrain.tankDrive(-1, 0) + def arcadeDrive(self, speed: float, rotate: float) -> Command: + self.run(lambda: self.drivetrain.arcadeDrive(speed, rotate)) - def arcadeDrive(self, speed: float, rotate: float) -> None: - self.drivetrain.arcadeDrive(speed, rotate) + def cheesyDrive(self, speed: float, rotate: float) -> Command: + self.run(lambda: self.drivetrain.curvatureDrive(speed, rotate)) - def tankDrive(self, left_speed: float, right_speed: float) -> None: - self.drivetrain.tankDrive(left_speed, right_speed) + def tankDrive(self, left_speed: float, right_speed: float) -> Command: + self.run(lambda: self.drivetrain.tankDrive(left_speed, right_speed)) - def updateOdometry(self): - """Updates the field-relative position.""" + def periodic(self) -> None: + self.field.setRobotPose(self.odometry.getPose()) + SmartDashboard.putData("Field", self.field) self.odometry.update( Rotation2d.fromDegrees(self.navx.getAngle()), - self.left_encoder.getDistance(), - self.right_encoder.getDistance(), + self.left_encoder.getPosition(), + self.right_encoder.getPosition(), ) - def arcadeDriveAlign(self, tag: int) -> None: - yaw = self.camera.getYaw(tag) + def arcadeDriveAlign(self, camera: Camera, tag: int) -> None: + yaw = camera.getYaw(tag) turn = self.pid_angular.calculate(yaw, 0) if yaw != -1 else 0 self.drivetrain.arcadeDrive(0, turn) - - def arcadeDriveAimAndRange(self, tag: int) -> None: - yaw, range = self.camera.getYawWithRange(tag) - range = self.pid_forward.calculate(range, constants.kGoalRangeMeters) if yaw != -1 else 0 - rotation = self.pid_angular.calculate(yaw, 0) - self.drivetrain.arcadeDrive(range, rotation) - - def turnToDegrees(self, setpoint: Optional[int]) -> None: - self.drivetrain.arcadeDrive(0, self.pid_angular.calculate(self.navx.getAngle(), self.pid_angular.getSetpoint())) - - def turnTo90DegreesPositive(self, setpoint: Optional[int]) -> None: - setpoint = 90 / 360 - self.drivetrain.arcadeDrive(0, self.pid_angular.calculate(self.navx.getAngle(), setpoint)) - - def turnTo90DegreesNegative(self, setpoint: Optional[int]) -> None: - setpoint = 90 / 360 - - self.drivetrain.arcadeDrive(0, self.pid_angular.calculate(self.navx.getAngle(), -setpoint)) - - def turnTo180Degrees(self, setpoint: Optional[int]) -> None: - setpoint = 180 / 360 + def arcadeDriveAimAndRange(self, camera: Camera, tag: int) -> None: + yaw, range = camera.getYawWithRange(tag) + range = ( + self.pid_forward.calculate(range, constants.kGoalRangeMeters) + if yaw != -1 + else 0 + ) + rotation = self.pid_angular.calculate(yaw, 0) if yaw != -1 else 0 + self.drivetrain.arcadeDrive(range, rotation) - self.drivetrain.arcadeDrive(0, self.pid_angular.calculate(self.navx.getAngle(), setpoint)) + def zRotationFromDegrees(self, setpoint: Optional[float]) -> None: + self.pid_angular.setSetpoint(setpoint) + self.drivetrain.arcadeDrive( + 0, + self.pid_angular.calculate( + self.navx.getAngle(), self.pid_angular.getSetpoint() + ), + ) diff --git a/genericjoystick.py b/genericjoystick.py index e404681..4e888ac 100644 --- a/genericjoystick.py +++ b/genericjoystick.py @@ -2,155 +2,207 @@ from buttons import g_xbox_360_map, g_ps4_controller import constants -class GenericJoystick(GenericHID): + +class GenericJoystick(GenericHID): def __init__(self, port: int) -> None: super().__init__(port) - self.name = self.getName() - def getA(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['cross']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['a']) - + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["cross"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["a"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["a"]) + return False + def getB(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['circle']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['b']) - + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["circle"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["b"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["b"]) + return False + def getX(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['square']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['x']) - + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["square"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["x"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["x"]) + return False + def getY(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['triangle']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['y']) - - def getLb(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['l1']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['lb']) - - def getRb(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['r1']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['rb']) - + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["triangle"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["y"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["y"]) + return False + + def getLeftBumper(self) -> bool: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["l1"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["lb"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["lb"]) + return False + + def getRightBumper(self) -> bool: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["r1"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["rb"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["rb"]) + return False + def getBack(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['share']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['back']) - + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["share"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["back"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["back"]) + return False + def getStart(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['options']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['start']) - - def getL_stick(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['l3']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['press_left_stick']) - - def getR_stick(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['r3']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['press_right_stick']) - - def getPov_up(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['pov-up']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['pov-up']) - - def getPov_down(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['pov-down']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['pov-down']) - - def getPov_left(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['pov-left']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['pov-left']) - - def getPov_right(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawButton(g_ps4_controller['pov-right']) - case constants.kXboxController: - return self.getRawButton(g_xbox_360_map['pov-right']) - - def getL_x_axis(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawAxis(g_ps4_controller['left-x-axis']) - case constants.kXboxController: - return self.getRawAxis(g_xbox_360_map['left-x-axis']) - - def getL_y_stick(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawAxis(g_ps4_controller['left-y-axis']) - case constants.kXboxController: - return self.getRawAxis(g_xbox_360_map['left-y-axis']) - - def getR_x_stick(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawAxis(g_ps4_controller['right-x-axis']) - case constants.kXboxController: - return self.getRawAxis(g_xbox_360_map['right-x-axis']) - - def getR_y_stick(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawAxis(g_ps4_controller['right-y-axis']) - case constants.kXboxController: - return self.getRawAxis(g_xbox_360_map['right-y-axis']) - - def getL_trigger_axis(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawAxis(g_ps4_controller['left-trigger-axis']) - case constants.kXboxController: - return self.getRawAxis(g_xbox_360_map['left-trigger-axis']) - - def getR_trigger_axis(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawAxis(g_ps4_controller['right-trigger-axis']) - case constants.kXboxController: - return self.getRawAxis(g_xbox_360_map['right-trigger-axis']) - - def getY(self) -> bool: - match self.name: - case constants.kGenericPS4Controller: - return self.getRawAxis(g_ps4_controller['triangle']) - case constants.kXboxController: - return self.getRawAxis(g_xbox_360_map['y']) + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["options"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["start"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["start"]) + return False + + def getLeftStick(self) -> bool: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["l3"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["press-left-stick"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["press-left-stick"]) + return False + + def getRightStick(self) -> bool: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawButton(g_ps4_controller["r3"]) + case constants.kRealXboxController: + return self.getRawButton(g_xbox_360_map["press-right_stick"]) + case constants.kSimXboxController: + return self.getRawButton(g_xbox_360_map["press-right-stick"]) + return False + + def getPOVUp(self) -> int: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getPOV(g_ps4_controller["pov-up"]) + case constants.kRealXboxController: + return self.getPOV(g_xbox_360_map["pov-up"]) + case constants.kSimXboxController: + return self.getPOV(g_xbox_360_map["pov-up"]) + return -1 + + def getPOVDown(self) -> int: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getPOV(g_ps4_controller["pov-down"]) + case constants.kRealXboxController: + return self.getPOV(g_xbox_360_map["pov-down"]) + case constants.kSimXboxController: + return self.getPOV(g_xbox_360_map["pov-down"]) + return -1 + + def getPOVLeft(self) -> bool: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getPOV(g_ps4_controller["pov-left"]) + case constants.kRealXboxController: + return self.getPOV(g_xbox_360_map["pov-left"]) + case constants.kSimXboxController: + return self.getPOV(g_xbox_360_map["pov-left"]) + return -1 + + def getPOVRight(self) -> bool: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getPOV(g_ps4_controller["pov-right"]) + case constants.kRealXboxController: + return self.getPOV(g_xbox_360_map["pov-right"]) + case constants.kSimXboxController: + return self.getPOV(g_xbox_360_map["pov-right"]) + return -1 + + def getLeftXAxis(self) -> float: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawAxis(g_ps4_controller["left-x-stick"]) + case constants.kRealXboxController: + return self.getRawAxis(g_xbox_360_map["left-x-stick"]) + case constants.kSimXboxController: + return self.getRawAxis(g_xbox_360_map["left-x-stick"]) + return 0 + + def getLeftYAxis(self) -> float: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawAxis(g_ps4_controller["left-y-stick"]) + case constants.kRealXboxController: + return self.getRawAxis(g_xbox_360_map["left-y-stick"]) + case constants.kSimXboxController: + return self.getRawAxis(g_xbox_360_map["left-y-stick"]) + return 0 + + def getRightXAxis(self) -> float: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawAxis(g_ps4_controller["right-x-stick"]) + case constants.kRealXboxController: + return self.getRawAxis(g_xbox_360_map["right-x-stick"]) + case constants.kSimXboxController: + return self.getRawAxis(g_xbox_360_map["right-x-stick"]) + return 0 + + def getRightYAxis(self) -> float: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawAxis(g_ps4_controller["right-y-stick"]) + case constants.kRealXboxController: + return self.getRawAxis(g_xbox_360_map["right-y-stick"]) + case constants.kSimXboxController: + return self.getRawAxis(g_xbox_360_map["right-y-stick"]) + return 0 + + def getLeftTriggerAxis(self) -> float: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawAxis(g_ps4_controller["left-trigger-axis"]) + case constants.kRealXboxController: + return self.getRawAxis(g_xbox_360_map["left-trigger-axis"]) + case constants.kSimXboxController: + return self.getRawAxis(g_xbox_360_map["left-trigger-axis"]) + return 0 + + def getRightTriggerAxis(self) -> float: + match self.getName(): + case constants.kGenericPS4Controller: + return self.getRawAxis(g_ps4_controller["right-trigger-axis"]) + case constants.kRealXboxController: + return self.getRawAxis(g_xbox_360_map["right-trigger-axis"]) + case constants.kSimXboxController: + return self.getRawAxis(g_xbox_360_map["right-trigger-axis"]) + return 0 diff --git a/intake.py b/intake.py index 18bf0cc..86d91f1 100644 --- a/intake.py +++ b/intake.py @@ -1,34 +1,58 @@ -import wpilib -import wpilib.drive -import rev -import wpimath.controller +from phoenix5 import WPI_VictorSPX, ControlMode +from commands2 import Subsystem +from wpilib import Timer, SmartDashboard +import constants -class Intake: +class Intake(Subsystem): + def __init__(self) -> None: + self.pivot = WPI_VictorSPX(constants.kIntakeAngleMotor) + self.roller = WPI_VictorSPX(constants.kIntakeTrackMotor) + self.pivotUp = False + self.lastBurstTime = 0 - def __init__(self): + kPivotTimeUp = SmartDashboard.putNumber("up", 0.5) + kPivotTimeDown = SmartDashboard.putNumber("down", 0.5) - self.arm_motor = rev.SparkMax(1, rev.SparkLowLevel.MotorType.kBrushless) - self.roller_motor = rev.SparkMax(2, rev.SparkLowLevel.MotorType.kBrushless) + def isPivotUp(self) -> bool: + return self.pivotUp - self.arm_pid = wpimath.controller.PIDController(0.1, 0.0, 0.0) + def periodic(self) -> None: + elapsed = Timer.getFPGATimestamp() - self.lastBurstTime + percent = 0 + kPivotTimeUp = SmartDashboard.getNumber("up", 0) + kPivotTimeDown = SmartDashboard.getNumber("down", 0) - def turnOnIntake(self): - setpoint = 15 - self.roller_motor.set(1) + if self.pivotUp: + percent = -1 if elapsed < kPivotTimeUp else 0 + else: + percent = 1 if elapsed < kPivotTimeDown else 0 + + self.pivot.set(ControlMode.PercentOutput, percent) + + def startPivotUp(self) -> None: + if self.pivotUp: + return - while not self.arm_pid.inSetPoint: - self.arm_motor.set(self.arm_pid.calculate(self.arm_motor.getEncoder().getPosition(), setpoint)) + self.pivotUp = True + self.lastBurstTime = Timer.getFPGATimestamp() - def turnOffIntake(self): - setpoint = 0 - self.roller_motor.set(0) + def stopPivot(self) -> None: + self.pivot.set(0) - while not self.arm_pid.inSetPoint: - self.arm_motor.set(self.arm_pid.calculate(self.arm_motor.getEncoder().getPosition(), setpoint)) - + def startPivotDown(self) -> None: + if not self.pivotUp: + return + + self.pivotUp = False + self.lastBurstTime = Timer.getFPGATimestamp() + def get(self) -> None: + self.roller.set(-1) + def stop(self) -> None: + self.roller.set(0) - + def release(self) -> None: + self.roller.set(1) \ No newline at end of file diff --git a/led.py b/led.py index 772e86d..a0d456a 100644 --- a/led.py +++ b/led.py @@ -1,23 +1,20 @@ from wpilib import SerialPort import constants + class LEDController: def __init__(self) -> None: - self.arduino = SerialPort( - constants.kBaudRate, - constants.kLEDUSBPort - ) + self.arduino = SerialPort(constants.kBaudRate, constants.kLEDUSBPort) def activateRedColor(self) -> None: - changeColor('r') + changeColor("r") def activateGreenColor(self) -> None: - changeColor('g') + changeColor("g") def activateBlueColor(self) -> None: - changeColor('b') + changeColor("b") def changeColor(char: str) -> None: - byte_obj = char.encode('ascii') + byte_obj = char.encode("ascii") self.arduino.write(byte_obj) - diff --git a/pyproject.toml b/pyproject.toml index ddb4f37..d209b1e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -13,7 +13,7 @@ robotpy_version = "2026.2.1" components = [ # "all", # "apriltag", - # "commands2", + "commands2", # "cscore", # "romi", # "sim", @@ -23,11 +23,11 @@ components = [ # Other pip packages to install, such as vendor packages (each element # is equivalent to a line in requirements.txt) requires = [ + "robotpy-pathplannerlib", "robotpy-navx", "robotpy-rev", - "photonlibpy", + "photonlibpy", "robotpy-ctre", "robotpy-pykit", - "robotpy-pathplannerlib", - "robotpy-commands-v2" + "limelightlib-python" ] diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..08226a0 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,2 @@ +robotpy==2026.2.1 +black==26.1.0 diff --git a/robot.py b/robot.py index 8dbbe76..7cbd14c 100644 --- a/robot.py +++ b/robot.py @@ -1,27 +1,54 @@ -from wpilib import TimedRobot +import constants from drivetrain import Drivetrain -from camera import AprilTagCamera -from turret import Turret from genericjoystick import GenericJoystick -import constants +from autonomous.basicauto import BasicAuto +from commands2 import TimedCommandRobot, CommandScheduler, Command +from typing import Optional +from commands2.cmd import run +from camera import Camera, PhotonVisionCamera, LimelightCamera +from turret import Turret +from commands2.button import JoystickButton +from intake import Intake +from pathplannerlib.auto import AutoBuilder +from wpilib import SmartDashboard, SendableChooser +from wpilib.interfaces import GenericHID + + +class Robot(TimedCommandRobot): + autonomous: Optional[Command] = None + drivetrain: Drivetrain = Drivetrain() + intake: Intake = Intake() + autoChooser: SendableChooser = AutoBuilder.buildAutoChooser() + driverJoystick: GenericHID = GenericJoystick(constants.kJoystickDriverPort) -class Robot(TimedRobot): def robotInit(self) -> None: - self.camera = AprilTagCamera(constants.kCameraName) - self.drivetrain = Drivetrain(self.camera) - self.turret = Turret(self.camera) - self.driver_joystick = GenericJoystick(constants.kJoystickDriverPort) - self.codriver_joystick = GenericJoystick(constants.kJoystickCoDriverPort) + self.autoChooser.addOption("Basic Auto", BasicAuto(self.drivetrain)) + SmartDashboard.putData("Auto Chooser", self.autoChooser) + + JoystickButton(self.driverJoystick, 1).onTrue( + run( + lambda: self.intake.startPivotUp(), + self.intake + ) + ) + + JoystickButton(self.driverJoystick, 2).onTrue( + run( + lambda: self.intake.startPivotDown(), + self.intake + ) + ) - def robotPeriodic(self) -> None: - self.drivetrain.updateOdometry() + self.drivetrain.setDefaultCommand( + self.drivetrain.arcadeDrive( + self.driverJoystick.getLeftYAxis(), + self.driverJoystick.getRightXAxis(), + ) + ) + + def autonomousInit(self) -> None: + self.autonomous = self.autoChooser.getSelected() + self.autonomous.schedule() - def teleopPeriodic(self) -> None: - if self.driver_joystick.getA(): - self.turret.yawLeft() - elif self.driver_joystick.getB(): - self.turret.yawRight() - elif self.driver_joystick.getX(): - self.turret.TurretAlign(1) - else: - self.turret.turnOffKraken() + def autonomousExit(self) -> None: + CommandScheduler.getInstance().cancelAll() \ No newline at end of file diff --git a/run_tests.sh b/run_tests.sh new file mode 100755 index 0000000..dda37de --- /dev/null +++ b/run_tests.sh @@ -0,0 +1,5 @@ +#!/bin/bash -e + +echo "Running RobotPy tests..." +python3 -m robotpy test +echo "Tests finished successfully!" \ No newline at end of file diff --git a/shooter.py b/shooter.py new file mode 100644 index 0000000..1ebcc90 --- /dev/null +++ b/shooter.py @@ -0,0 +1,58 @@ +from rev import ( + SparkMax, + SparkLowLevel, + ResetMode, + PersistMode, + SparkMaxConfig, + SparkBaseConfig, +) +from commands2 import Subsystem +from wpilib import SmartDashboard + + +class Shooter(Subsystem): + def __init__(self, kS: int, kV: int, kA: int, setpoint: int) -> None: + self.motor = SparkMax(11, SparkLowLevel.MotorType.kBrushless) + + config = SparkMaxConfig() + + config.smartCurrentLimit(40) + config.setIdleMode(SparkBaseConfig.IdleMode.kCoast) + + config.closedLoop.pid(0, 0, 0) + config.closedLoop.feedForward.kS(kS) + config.closedLoop.feedForward.kV(kV) + config.closedLoop.feedForward.kA(kA) + + self.motor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ) + + # self.controller = self.motor.getClosedLoopController() + # config.closedLoop.maxMotion.cruiseVelocity() + # config.closedLoop.maxAcceleration.cruiseVelocity() + # config.closedLoop.allowedProfileError.cruiseVelocity() + + # self.motor.setSetpoint(setpoint, SparkLowLevel.ControlType.kMAXMotionVelocityControl) + SmartDashboard.putNumber("kS", 0.1) + + def periodic(self) -> None: + self.setFeedforwardConstraints(SmartDashboard.getNumber("kS", 0), 0, 0) + + def setFeedforwardConstraints(self, kS: int, kV: int, kA: int) -> None: + config = SparkMaxConfig() + + config.smartCurrentLimit(40) + config.setIdleMode(SparkBaseConfig.IdleMode.kCoast) + + config.closedLoop.pid(0, 0, 0) + config.closedLoop.feedForward.kS(kS) + config.closedLoop.feedForward.kV(kV) + config.closedLoop.feedForward.kA(kA) + + self.motor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters + ) + + def activateShooter(self) -> None: + self.motor.set(0.7) diff --git a/turret.py b/turret.py index 3ed0c93..cd5d335 100644 --- a/turret.py +++ b/turret.py @@ -1,42 +1,24 @@ -import wpilib +from phoenix6.hardware import TalonFX +from camera import Camera +from wpimath.controller import PIDController import rev -import phoenix6 -import wpimath.controller -from camera import AprilTagCamera class Turret: - def __init__(self, camera: AprilTagCamera): - self.shooter1 = rev.SparkMax(1, rev.SparkLowLevel.MotorType.kBrushless) - self.shooter2 = rev.SparkMax(2, rev.SparkLowLevel.MotorType.kBrushless) - self.kraken = phoenix6.hardware.TalonFX(20) - self.pitch = rev.SparkMax(3, rev.SparkLowLevel.MotorType.kBrushless) - - self.shooter = wpilib.MotorControllerGroup(self.shooter1, self.shooter2) - self.shooter1.setInverted(True) - self.pid_angular = wpimath.controller.PIDController(0.1, 0, 0) - self.pid_forward = wpimath.controller.PIDController(0.1, 0, 0) - self.camera = camera - - def shooterSpeed(self, speed): - self.shooter.set(speed) + def __init__(self): + self.yaw = rev.SparkMax(51, rev.SparkMax.MotorType.kBrushless) + self.pid_angular = PIDController(0.005, 0, 0) def yawLeft(self): - self.kraken.set(1) + self.yaw.set(1) - def yawRight(self): - self.kraken.set(-1) - - def turnOffKraken(self): - self.kraken.set(0) + def yawRight(self): + self.yaw.set(-1) - def pitchUp(self): - self.pitch.set(1) + def stop(self): + self.yaw.set(0) - def pitchDown(self): - self.pitch.set(-1) - - def TurretAlign(self, tag: int) -> None: - yaw = self.camera.getYaw(tag) - turn = self.pid_angular.calculate(yaw, 0) if yaw != -1 else 0 - self.kraken.set(turn) + def turretAlign(self, tag: int, camera: Camera) -> None: + tag_yaw = camera.getYawFromTag(tag) + turn = self.pid_angular.calculate(tag_yaw, 0) if tag_yaw != -1 else 0 + self.yaw.set(turn) diff --git a/utils.py b/utils.py index ecc4176..3929442 100644 --- a/utils.py +++ b/utils.py @@ -1,6 +1,7 @@ from math import tan from wpilib import SerialPort + class Utils: @staticmethod def calculateDistanceToTargetMeters(