From 34ff9c131c2855db1fa8d4c70e303b04fbb9674e Mon Sep 17 00:00:00 2001 From: joelzoto <118407587+joelzoto@users.noreply.github.com> Date: Sat, 1 Feb 2025 14:09:42 -0500 Subject: [PATCH] Fixed Algae Shooter Pivot and wrote the set angle command. --- .../algaeshooterpivot/AlgaeShooterPivot.java | 120 ++++++++++++------ .../commands/SetAngleAlgaePivot.java | 84 ++++++++++++ ....2.1.json => PathplannerLib-2025.2.2.json} | 8 +- ...Lib-2025.0.1.json => REVLib-2025.0.2.json} | 12 +- 4 files changed, 178 insertions(+), 46 deletions(-) create mode 100644 src/main/java/frc/robot/algaeshooterpivot/commands/SetAngleAlgaePivot.java rename vendordeps/{PathplannerLib-2025.2.1.json => PathplannerLib-2025.2.2.json} (87%) rename vendordeps/{REVLib-2025.0.1.json => REVLib-2025.0.2.json} (90%) diff --git a/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java b/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java index bca0e15..0e86259 100644 --- a/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java +++ b/src/main/java/frc/robot/algaeshooterpivot/AlgaeShooterPivot.java @@ -6,44 +6,51 @@ 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.CANcoder; 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.wpilibj.smartdashboard.SmartDashboard; 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 int kAlgaePivotTalonID = 19; + public static int kAlgaePivotEncoder = 1; public static boolean isInverted = false; static final Slot0Configs kAlgaeShooterPivotConfigs = new Slot0Configs() + .withKG(0.0) .withKS(0.0) - .withKV(0.115) + .withKV(0.0) + .withKA(0.0) .withKP(0.0) .withKI(0.0) .withKD(0.0); + + public static final Rotation2d kMaxAngularVelocity = Rotation2d.fromDegrees(200); //120 + public static final Rotation2d kMaxAngularAcceleration = Rotation2d.fromDegrees(300); + public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(180); + public static final Rotation2d kMaxAnglePhysical = Rotation2d.fromDegrees(175); + + public static final int KMaxVoltage = 30; + + // kFFAngleOffset is the differnce between our zero point (handoff) and 'true' zero (parallel to the ground) + private static final Rotation2d kFFAngleOffset = Rotation2d.fromDegrees(30); } private static AlgaeShooterPivot mInstance; + private final CANcoder mEncoder; private TalonFX mKraken; + private Constraints mConstraints; + private final ProfiledPIDController mPPIDController; + private final ArmFeedforward mFFController; public AlgaeShooterPivot() { - mKraken = new TalonFX(Settings.kTalonID); + mKraken = new TalonFX(Settings.kAlgaePivotTalonID); var talonFXConfigurator = mKraken.getConfigurator(); var motorConfigs = new MotorOutputConfigs(); @@ -52,25 +59,32 @@ public AlgaeShooterPivot() { talonFXConfigurator.apply(motorConfigs); mKraken.getConfigurator().apply(Settings.kAlgaeShooterPivotConfigs); + + mEncoder = new CANcoder(Settings.kAlgaePivotEncoder, "Canivore"); + mConstraints = new Constraints( Settings.kMaxAngularVelocity.getRadians(), Settings.kMaxAngularAcceleration.getRadians()); + mPPIDController = new ProfiledPIDController(Settings.kAlgaeShooterPivotConfigs.kP, Settings.kAlgaeShooterPivotConfigs.kI, Settings.kAlgaeShooterPivotConfigs.kD, mConstraints); + + mFFController = new ArmFeedforward(Settings.kAlgaeShooterPivotConfigs.kS, Settings.kAlgaeShooterPivotConfigs.kG, Settings.kAlgaeShooterPivotConfigs.kV, Settings.kAlgaeShooterPivotConfigs.kA); + + //mEncoder = mKraken.getAbsoluteEncoder(); // 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); + // final TrapezoidProfile m_profile = new TrapezoidProfile( + // new TrapezoidProfile.Constraints(Settings.kMaxAngularVelocity, Settings.kMaxAngularAcceleration) + // ); + + // 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) { @@ -78,10 +92,44 @@ public static AlgaeShooterPivot getInstance() { } return mInstance; } - public void setPivotVelocity(Rotation2d velocity) { - mKraken.setControl(new VelocityVoltage(velocity.getRotations())); + + public void setTargetAngle(Rotation2d angle) { + mPPIDController.setGoal(angle.getRadians()); } - public Rotation2d getPivotVelocity() { - return Rotation2d.fromRotations(mKraken.getVelocity().getValueAsDouble()); + + public Rotation2d getAngularVelocity() { + return Rotation2d.fromRotations(mEncoder.getVelocity().getValueAsDouble()).div(140.0 * 60.0); + } + + + public Rotation2d getAngle() { + var pos = mEncoder.getPosition().getValueAsDouble(); + + // Accounts for encoder gear ratio + pos /= 2.0; + if (pos > Settings.kMaxAnglePhysical.getRotations()) { + pos = 0; + } + return Rotation2d.fromRotations(pos); + } + + public void resetAngle() { + mPPIDController.setGoal(getAngle().getRadians()); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Shooter Pivot Angle (radians)", getAngle().getRadians()); + SmartDashboard.putNumber("Shooter Pivot Angular Velocity (radians / sec)", getAngularVelocity().getRadians()); + SmartDashboard.putNumber("Shooter Pivot Angle (degrees)", getAngle().getDegrees()); + SmartDashboard.putNumber("Shooter Pivot Angular Velocity (degrees / sec)", getAngularVelocity().getDegrees()); + SmartDashboard.putNumber("Profilled PID Controller Vel", mPPIDController.getSetpoint().velocity); + + double speed = mPPIDController.calculate(getAngle().getRadians()); + speed += mFFController.calculate(getAngle().getRadians() - Settings.kFFAngleOffset.getRadians(), mPPIDController.getSetpoint().velocity); + + SmartDashboard.putNumber("mPPIDC + mFFC Output", speed); + + mKraken.setVoltage(speed); } } diff --git a/src/main/java/frc/robot/algaeshooterpivot/commands/SetAngleAlgaePivot.java b/src/main/java/frc/robot/algaeshooterpivot/commands/SetAngleAlgaePivot.java new file mode 100644 index 0000000..c94a80f --- /dev/null +++ b/src/main/java/frc/robot/algaeshooterpivot/commands/SetAngleAlgaePivot.java @@ -0,0 +1,84 @@ +// 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.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.algaeshooterpivot.AlgaeShooterPivot; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SetAngleAlgaePivot extends Command { + AlgaeShooterPivot mAlgaeShooterPivot; + Supplier targetSupplier; + boolean singleShot = true; + + public enum Preset { + kZero(Rotation2d.fromDegrees(2)), + kHandoff(Rotation2d.fromDegrees(0)), + kHandoffClear(Rotation2d.fromDegrees(5)), + kShooterNear(Rotation2d.fromDegrees(0)), + kShooterMid(Rotation2d.fromDegrees(22.25)), + kShooterFarAuton(Rotation2d.fromDegrees(26)), + kShooterFar(Rotation2d.fromDegrees(25.5)), + kTrap(Rotation2d.fromDegrees(153.15)), + kClimb(Rotation2d.fromDegrees(170)), + kPass(Rotation2d.fromDegrees(10)), + kAmp(Rotation2d.fromDegrees(85)); + + Rotation2d target; + + Preset(Rotation2d target) { + this.target = target; + } + + public double getDegrees() { + return target.getDegrees(); + } + + public Rotation2d getRotation2d() { + return target; + } + } + + public SetAngleAlgaePivot(Rotation2d angleIn) { + mAlgaeShooterPivot = AlgaeShooterPivot.getInstance(); + targetSupplier = ()-> angleIn; + } + public SetAngleAlgaePivot(Supplier angleIn) { + mAlgaeShooterPivot = AlgaeShooterPivot.getInstance(); + targetSupplier = angleIn; + } + + public SetAngleAlgaePivot(Supplier angleIn, boolean singleSet) { + mAlgaeShooterPivot = AlgaeShooterPivot.getInstance(); + targetSupplier = angleIn; + singleShot = singleSet; + } + + @Override + public void initialize() { + + + } + + @Override + public void execute() { + mAlgaeShooterPivot.setTargetAngle(targetSupplier.get()); + } + + + @Override + public boolean isFinished() { + return singleShot; + } + + @Override + public void end(boolean interrupted) { + + } +} + diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib-2025.2.2.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.1.json rename to vendordeps/PathplannerLib-2025.2.2.json index 71e25f3..a5bf9ee 100644 --- a/vendordeps/PathplannerLib-2025.2.1.json +++ b/vendordeps/PathplannerLib-2025.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.1.json", + "fileName": "PathplannerLib-2025.2.2.json", "name": "PathplannerLib", - "version": "2025.2.1", + "version": "2025.2.2", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.1" + "version": "2025.2.2" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", + "version": "2025.2.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib-2025.0.2.json similarity index 90% rename from vendordeps/REVLib-2025.0.1.json rename to vendordeps/REVLib-2025.0.2.json index c998054..c29aefa 100644 --- a/vendordeps/REVLib-2025.0.1.json +++ b/vendordeps/REVLib-2025.0.2.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.1.json", + "fileName": "REVLib-2025.0.2.json", "name": "REVLib", - "version": "2025.0.1", + "version": "2025.0.2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.1" + "version": "2025.0.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false,