From dc64eed506e4348f3504faa7eb9b1b27010d451d Mon Sep 17 00:00:00 2001 From: joelzoto <118407587+joelzoto@users.noreply.github.com> Date: Mon, 27 Jan 2025 20:19:20 -0500 Subject: [PATCH] Algae Shooter Pivot and Flywheel Code --- .../robot/algaeflywheel/AlgaeFlyWheel.java | 87 +++++++++++++++++++ .../algaeshooterpivot/AlgaeShooterPivot.java | 87 +++++++++++++++++++ vendordeps/AdvantageKit.json | 35 ++++++++ ....1.1.json => PathplannerLib-2025.2.1.json} | 8 +- ...enix6-25.1.0.json => Phoenix6-25.2.1.json} | 84 ++++++++++++------ ...Lib-2025.0.0.json => REVLib-2025.0.1.json} | 15 ++-- vendordeps/photonlib.json | 12 +-- 7 files changed, 282 insertions(+), 46 deletions(-) create mode 100644 src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java create mode 100644 src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java create mode 100644 vendordeps/AdvantageKit.json rename vendordeps/{PathplannerLib-2025.1.1.json => PathplannerLib-2025.2.1.json} (87%) rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-25.2.1.json} (85%) rename vendordeps/{REVLib-2025.0.0.json => REVLib-2025.0.1.json} (86%) diff --git a/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java b/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java new file mode 100644 index 0000000..096845c --- /dev/null +++ b/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java @@ -0,0 +1,87 @@ +// 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. + +package frc.robot.algaeflywheel; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** Add your docs here. */ +public class AlgaeFlyWheel extends SubsystemBase{ + public static class Settings { + static final int kLeftId = 27; + static final int kRightId = 28; + + static final Slot0Configs kFlywheelConfigs = new Slot0Configs() + .withKS(0.0) + .withKV(0.115) + .withKP(0.0); + + // 5800 RPM at motor; 11600 RPM at wheels + public static final Rotation2d kMaxAngularVelocity = Rotation2d.fromRotations(6000.0 / 60.0); + + public static final double kCurrentLimit = 60.0; + } + + private static AlgaeFlyWheel mInstance; + private final TalonFX mKrakenLeft, mKrakenRight; + + private AlgaeFlyWheel() { + mKrakenLeft = new TalonFX(Settings.kLeftId); + var leftTalonFXConfigurator = mKrakenLeft.getConfigurator(); + var leftMotorConfigs = new MotorOutputConfigs(); + + leftMotorConfigs.Inverted = InvertedValue.Clockwise_Positive; + leftTalonFXConfigurator.apply(leftMotorConfigs); + + + mKrakenRight = new TalonFX(Settings.kRightId); + var rightTalonFXConfigurator = mKrakenRight.getConfigurator(); + var righttMotorConfigs = new MotorOutputConfigs(); + + righttMotorConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + rightTalonFXConfigurator.apply(righttMotorConfigs); + } + + // 57 degreedf + + public static AlgaeFlyWheel getInstance() { + if (mInstance == null) { + mInstance = new AlgaeFlyWheel(); + } + return mInstance; + } + + public void setLeftFlywheelVelocity(Rotation2d velocity) { + mKrakenLeft.setControl(new VelocityVoltage(velocity.getRotations())); + } + + public void setRightFlywheelVelocity(Rotation2d velocity) { + mKrakenRight.setControl(new VelocityVoltage(velocity.getRotations())); + } + + public Rotation2d getLeftFlywheelVelocity() { + return Rotation2d.fromRotations(mKrakenLeft.getVelocity().getValueAsDouble()); + } + + public Rotation2d getRightFlywheelVelocity() { + return Rotation2d.fromRotations(mKrakenRight.getVelocity().getValueAsDouble()); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Left Flywheel Velocity (RPM)", mKrakenLeft.getVelocity().getValueAsDouble() * 60); + SmartDashboard.putNumber("Right Flywheel Velocity (RPM)", mKrakenRight.getVelocity().getValueAsDouble() * 60); + } +} + + + diff --git a/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java b/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java new file mode 100644 index 0000000..bca0e15 --- /dev/null +++ b/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java @@ -0,0 +1,87 @@ +// 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. + +package frc.robot.algaeshooterpivot; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** Add your docs here. */ +public class AlgaeShooterPivot extends SubsystemBase{ + public class Settings{ + public static int kTalonID = 19; + public static boolean isInverted = false; + + static final Slot0Configs kAlgaeShooterPivotConfigs = new Slot0Configs() + .withKS(0.0) + .withKV(0.115) + .withKP(0.0) + .withKI(0.0) + .withKD(0.0); + } + private static AlgaeShooterPivot mInstance; + private TalonFX mKraken; + + public AlgaeShooterPivot() { + mKraken = new TalonFX(Settings.kTalonID); + + var talonFXConfigurator = mKraken.getConfigurator(); + var motorConfigs = new MotorOutputConfigs(); + + motorConfigs.Inverted = InvertedValue.Clockwise_Positive; + talonFXConfigurator.apply(motorConfigs); + + mKraken.getConfigurator().apply(Settings.kAlgaeShooterPivotConfigs); + + // Trapezoid profile with max velocity 80 rps, max accel 160 rps/s + final TrapezoidProfile m_profile = new TrapezoidProfile( + new TrapezoidProfile.Constraints(80, 160) + ); + // Final target of 200 rot, 0 rps + TrapezoidProfile.State m_goal = new TrapezoidProfile.State(200, 0); + TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); + + // create a position closed-loop request, voltage output, slot 0 configs + final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); + + // calculate the next profile setpoint + m_setpoint = m_profile.calculate(0.020, m_setpoint, m_goal); + + // send the request to the device + m_request.Position = m_setpoint.position; + m_request.Velocity = m_setpoint.velocity; + mKraken.setControl(m_request); + } + public static AlgaeShooterPivot getInstance() { + if (mInstance == null) { + mInstance = new AlgaeShooterPivot(); + } + return mInstance; + } + public void setPivotVelocity(Rotation2d velocity) { + mKraken.setControl(new VelocityVoltage(velocity.getRotations())); + } + public Rotation2d getPivotVelocity() { + return Rotation2d.fromRotations(mKraken.getVelocity().getValueAsDouble()); + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..03df051 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.2.1.json similarity index 87% rename from vendordeps/PathplannerLib-2025.1.1.json rename to vendordeps/PathplannerLib-2025.2.1.json index 6322388..71e25f3 100644 --- a/vendordeps/PathplannerLib-2025.1.1.json +++ b/vendordeps/PathplannerLib-2025.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.1.1.json", + "fileName": "PathplannerLib-2025.2.1.json", "name": "PathplannerLib", - "version": "2025.1.1", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.1.1" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.2.1.json similarity index 85% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-25.2.1.json index 473f6a8..1397da1 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-25.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-25.2.1.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.2.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.2.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +351,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib-2025.0.1.json similarity index 86% rename from vendordeps/REVLib-2025.0.0.json rename to vendordeps/REVLib-2025.0.1.json index cde6011..c998054 100644 --- a/vendordeps/REVLib-2025.0.0.json +++ b/vendordeps/REVLib-2025.0.1.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.0.json", + "fileName": "REVLib-2025.0.1.json", "name": "REVLib", - "version": "2025.0.0", + "version": "2025.0.1", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,19 +12,18 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index db43d6d..6af3d3e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.0.0-beta-8", + "version": "v2025.1.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-8", + "version": "v2025.1.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-8", + "version": "v2025.1.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-8", + "version": "v2025.1.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-8" + "version": "v2025.1.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-8" + "version": "v2025.1.1" } ] } \ No newline at end of file