From 2dac9652fb5df128ccb75187214c23b2a6cb3b8e Mon Sep 17 00:00:00 2001 From: Ricky Zheng Date: Tue, 28 Jan 2025 17:59:44 -0500 Subject: [PATCH 01/34] Co-authored-by: WillyZheng827 Co-authored-by: ZiJiaG Co-authored-by: tahmid.uddin@stuypulse.com Co-authored-by: Mustafa Abdullah Co-authored-by: TrisalB Co-authored-by: George Zeng Co-authored-by: Gordon Doan --- .../stuypulse/robot/constants/Settings.java | 26 +++++++- .../stuypulse/robot/subsystems/arm/Arm.java | 64 +++++++++++++++++++ 2 files changed, 89 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 570bbaa..e93146f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -14,4 +14,28 @@ * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ -public interface Settings {} +public interface Settings { + + public interface Arm { + + double MAX_VEL = 0; + double MAX_ACCEL = 0; + public interface PID { + double kP = 0; + double kI = 0; + double kD = 0; + + } + + public interface FF { + double kS = 0; + double kV = 0; + double kA = 0; + double kG = 0; + } + + double L2_ANGLE = 0; + double L3_ANGLE = 0; + double L4_Angle = 0; + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java new file mode 100644 index 0000000..13f0508 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -0,0 +1,64 @@ +package com.stuypulse.robot.subsystems.arm; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.AbsoluteEncoder; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + + /* + * GOODJOB GUYS!! I BELIEVE IN YOU!! + * FIELDS THAT WE NEED + * - Kraken Motor + * - Target angle + * - Absolute Encoder + * - PID controller + * - FF controller + * - instance + */ + + private static Arm instance; + + private SmartNumber targetAngle; + + private AbsoluteEncoder armEncoder; + + private final ProfiledPIDController pidController; + private final ArmFeedforward ffController; + + static { + instance = new Arm(); + } + + public Arm() { + // super(); + targetAngle = new SmartNumber("Arm/Target Angle", 0); + armMotor = new TalonFX(0); + + + + + pidController = new ProfiledPIDController( + Settings.Arm.PID.kP, + Settings.Arm.PID.kI, + Settings.Arm.PID.kD, + new Constraints( + Settings.Arm.MAX_VEL, + Settings.Arm.MAX_ACCEL + ) + + + + ); + + + // pidController = new ProfiledPIDController(//kp, ki, kd settings); +} + +_} From ae2e51c3cdf8fce68c1765a2d8490405e51e7f4b Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Tue, 28 Jan 2025 18:25:16 -0500 Subject: [PATCH 02/34] Began implementing Coral Shooter and Coral Funnels Co-authored-by: Calvin Ye Co-authored-by: Jerry Jiang Co-authored-by: LinAndy10 Co-authored-by: Weifen Chen Co-authored-by: Rahul Deb --- .../com/stuypulse/robot/constants/Ports.java | 12 +++ .../stuypulse/robot/constants/Settings.java | 22 +++++- .../robot/subsystems/funnel/CoralFunnel.java | 17 +++++ .../subsystems/funnel/CoralFunnelImpl.java | 26 +++++++ .../subsystems/shooter/CoralShooter.java | 35 +++++++++ .../subsystems/shooter/CoralShooterImpl.java | 75 +++++++++++++++++++ 6 files changed, 186 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 1a2106d..f6df319 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -12,4 +12,16 @@ public interface Gamepad { int OPERATOR = 1; int DEBUGGER = 2; } + + // Set values later + public interface Shooter { + int MOTOR = 0; + int BEAM = 1; + } + + // Set values later + public interface Funnel { + int MOTOR = 0; + int BEAM = 1; + } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 570bbaa..89d9ce5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -14,4 +14,24 @@ * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ -public interface Settings {} +public interface Settings { + public interface Shooter { + double MAX_SHOOTER_RPM = 6380; // Max RPM of Talon FX (rpm) + double BB_DEBOUNCE = 0.0; + + public interface PID { + // ADJUST LATER + SmartNumber kP = new SmartNumber("kP", 0); + SmartNumber kI = new SmartNumber("kI", 0); + SmartNumber kD = new SmartNumber("kD", 0); + } + + public interface FeedForward { + // ADJUST LATER + SmartNumber kS = new SmartNumber("kS", 0); + SmartNumber kV = new SmartNumber("kV", 0); + SmartNumber kA = new SmartNumber("kA", 0); + } + + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java new file mode 100644 index 0000000..0f680ed --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java @@ -0,0 +1,17 @@ +package com.stuypulse.robot.subsystems.funnel; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class CoralFunnel extends SubsystemBase{ + + private static final CoralFunnel instance; + + static { + instance = new CoralFunnel(); + } + + public CoralFunnel getInstance() { + return instance; + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java new file mode 100644 index 0000000..9beb450 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java @@ -0,0 +1,26 @@ +package com.stuypulse.robot.subsystems.funnel; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.wpilibj.DigitalInput; + +public class CoralFunnelImpl extends CoralFunnel { + + private final TalonFX driveMotor; + private final DigitalInput motorBeam; + private boolean funnelState; + + public CoralFunnelImpl(){ + driveMotor = new TalonFX(Ports.Funnel.MOTOR); + motorBeam = new DigitalInput(Ports.Funnel.BEAM); + } + + @Override + public void periodic() { + super.periodic(); + + + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java new file mode 100644 index 0000000..320c7bc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java @@ -0,0 +1,35 @@ +package com.stuypulse.robot.subsystems.shooter; + +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class CoralShooter extends SubsystemBase { + + private static final CoralShooter instance; + + static { + instance = new CoralShooterImpl(); + } + + public static CoralShooter getInstance() { + return instance; + } + + private final SmartNumber targetRPM; + + public CoralShooter() { + targetRPM = new SmartNumber("CoralShooter/Target RPM", 0); + } + + public double getTargetRPM() { + return targetRPM.get(); + } + + public abstract boolean hasCoral(); + + public abstract boolean hasAlgae(); + + +} + diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java new file mode 100644 index 0000000..6ec6bc8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java @@ -0,0 +1,75 @@ +package com.stuypulse.robot.subsystems.shooter; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.wpilibj.DigitalInput; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class CoralShooterImpl extends CoralShooter { + + private final TalonFX driveMotor; + private final DigitalInput motorBeam; + private final BStream hasCoral; + + + public CoralShooterImpl(){ + + driveMotor = new TalonFX(Ports.Shooter.MOTOR); + motorBeam = new DigitalInput(Ports.Shooter.BEAM); + hasCoral = BStream.create(motorBeam).not() + .filtered(new BDebounce.Both(Settings.Shooter.BB_DEBOUNCE)); + + TalonFXConfiguration driveConfig = new TalonFXConfiguration(); + + Slot0Configs slot0 = new Slot0Configs(); + + slot0.kS = Settings.Shooter.FeedForward.kS.getAsDouble(); + slot0.kV = Settings.Shooter.FeedForward.kV.getAsDouble(); + slot0.kA = Settings.Shooter.FeedForward.kA.getAsDouble(); + + slot0.kP = Settings.Shooter.PID.kP.getAsDouble(); + slot0.kI = Settings.Shooter.PID.kI.getAsDouble(); + slot0.kD = Settings.Shooter.PID.kD.getAsDouble(); + + driveConfig.Slot0 = slot0; + + driveMotor.getConfigurator().apply(driveConfig); + } + + private double getShooterRPM() { + return Math.abs(driveMotor.get() * Settings.Shooter.MAX_SHOOTER_RPM); + } + + public void setShooterRPM(double targetRPM) { + driveMotor.set(targetRPM / Settings.Shooter.MAX_SHOOTER_RPM); + } + + @Override + public boolean hasCoral() { + if + return hasCoral.get(); + + } + + // @Override + // public boolean hasAlgae() { + // return false; + // } + + @Override + public void periodic() { + super.periodic(); + + SmartDashboard.putBoolean("Shooter/Has Coral", hasCoral()); + + SmartDashboard.putNumber("Shooter/RPM", getShooterRPM()); + + SmartDashboard.putNumber("Shooter/Voltage", driveMotor.get() * driveMotor.getDutyCycle().getValue()); // can someone check whether this actually gets the voltage + } +} From 3503acc53157d7787bb017825482749e2df096cc Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Tue, 28 Jan 2025 18:28:40 -0500 Subject: [PATCH 03/34] implemented elevator and commands Co-authored-by: simon Co-authored-by: Haiiden Co-authored-by: qingque6 Co-authored-by: BrianZ60 Co-authored-by: Ardian Agoes --- .../commands/elevator/ElevatorToBottom.java | 9 + .../commands/elevator/ElevatorToHandoff.java | 9 + .../commands/elevator/ElevatorToHeight.java | 21 ++ .../commands/elevator/ElevatorToLvl2.java | 9 + .../commands/elevator/ElevatorToLvl3.java | 9 + .../commands/elevator/ElevatorToLvl4.java | 8 + .../commands/elevator/ElevatorToTop.java | 10 + .../stuypulse/robot/constants/Constants.java | 22 +++ .../com/stuypulse/robot/constants/Ports.java | 6 + .../stuypulse/robot/constants/Settings.java | 37 +++- .../robot/subsystems/elevator/Elevator.java | 39 ++++ .../subsystems/elevator/ElevatorImpl.java | 101 ++++++++++ .../subsystems/elevator/ElevatorSimu.java | 91 +++++++++ .../elevator/ElevatorVisualizer.java | 187 ++++++++++++++++++ 14 files changed, 557 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java create mode 100644 src/main/java/com/stuypulse/robot/constants/Constants.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java new file mode 100644 index 0000000..bc6629b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Constants.Elevator; + +public class ElevatorToBottom extends ElevatorToHeight{ + public ElevatorToBottom(){ + super(Elevator.MIN_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java new file mode 100644 index 0000000..c8e6f39 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToHandoff extends ElevatorToHeight{ + public ElevatorToHandoff() { + super(Elevator.HANDOFF_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java new file mode 100644 index 0000000..f8a6da0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.subsystems.elevator.Elevator; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ElevatorToHeight extends InstantCommand { + private final Elevator elevator; + private final double targetHeight; + + public ElevatorToHeight(double targetHeight){ + elevator = Elevator.getInstance(); + this.targetHeight = targetHeight; + + addRequirements(elevator); + } + + public void initialize(){ + elevator.setTargetHeight(targetHeight); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java new file mode 100644 index 0000000..1d861e6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl2 extends ElevatorToHeight{ + public ElevatorToLvl2(){ + super(Elevator.L2_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java new file mode 100644 index 0000000..ff4a056 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl3 extends ElevatorToHeight{ + public ElevatorToLvl3(){ + super(Elevator.L3_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java new file mode 100644 index 0000000..d674636 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java @@ -0,0 +1,8 @@ +package com.stuypulse.robot.commands.elevator; +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl4 extends ElevatorToHeight{ + public ElevatorToLvl4() { + super(Elevator.L4_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java new file mode 100644 index 0000000..bba0cb4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Constants; + +public class ElevatorToTop extends ElevatorToHeight{ + public ElevatorToTop() { + super(Constants.Elevator.MAX_HEIGHT_METERS); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Constants.java b/src/main/java/com/stuypulse/robot/constants/Constants.java new file mode 100644 index 0000000..e9b30a1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/Constants.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.constants; + +import edu.wpi.first.math.util.Units; + +public class Constants { + public interface Elevator { + double MIN_HEIGHT_METERS = Units.inchesToMeters(9.09375); // FROM THE BOTTOM OF FIXED STAGE TO TOP OF CARRIAGE + double MAX_HEIGHT_METERS = Units.inchesToMeters(77); // FROM THE BOTTOM OF FIXED STAGE TO TOP ELEVATOR + + double MASS_KG = 10.0; + double DRUM_RADIUS_METERS = (MAX_HEIGHT_METERS / Encoders.NUM_ROTATIONS_TO_REACH_TOP * Encoders.GEARING) / 2 / Math.PI; + + public interface Encoders { + double GEARING = 4.0; + + double NUM_ROTATIONS_TO_REACH_TOP = (6 + 9.0 / 24) * GEARING; // Number of rotations that the motor has to spin, NOT the gear + + double POSITION_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP; + double VELOCITY_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP / 60; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 1a2106d..9ef9a4e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -12,4 +12,10 @@ public interface Gamepad { int OPERATOR = 1; int DEBUGGER = 2; } + + public interface Elevator { + int MOTOR = 0; + int BOTTOM_SWITCH = 1; + int TOP_SWITCH = 2; + } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 570bbaa..e6bf627 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -14,4 +14,39 @@ * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ -public interface Settings {} +public interface Settings { + + double DT = 0.020; + + public interface Elevator { + SmartNumber MAX_VELOCITY_METERS_PER_SECOND = new SmartNumber("Elevator/Max Velocity (m per s)", 1.0); + SmartNumber MAX_ACCEL_METERS_PER_SECOND_PER_SECOND = new SmartNumber("Elevator/Max Accel (m per s^2)", 2.0); + + // CHANGE + double HANDOFF_HEIGHT_METERS = 0.1; + double L2_HEIGHT_METERS = 0.25; + double L3_HEIGHT_METERS = 0.5; + double L4_HEIGHT_METERS = 0.75; + + double FEED_HEIGHT_METERS = 0.4; + + SmartNumber HEIGHT_TOLERANCE_METERS = new SmartNumber("Elevator/Height Tolerance (m)", 0.02); + + public interface PID { + SmartNumber kP = new SmartNumber("Elevator/Controller/kP",10); + SmartNumber kI = new SmartNumber("Elevator/Controller/kI",0.0); + SmartNumber kD = new SmartNumber("Elevator/Controller/kD",0.2); + } + + public interface FF { + SmartNumber kS = new SmartNumber("Elevator/Controller/kS",0.20506); + SmartNumber kV = new SmartNumber("Elevator/Controller/kV",3.7672); + SmartNumber kA = new SmartNumber("Elevator/Controller/kA", 0.27); + SmartNumber kG = new SmartNumber("Elevator/Controller/kG", 1.37); + } + + public interface Simulation { + double SCALE_FACTOR = 0.5 + 2.5/77; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..a0e1195 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,39 @@ +package com.stuypulse.robot.subsystems.elevator; + +import com.stuypulse.robot.Robot; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Elevator extends SubsystemBase { + + private static final Elevator instance; + + static { + if (Robot.isReal()) { + instance = new ElevatorImpl(); + } else { + instance = new ElevatorSimu(); + } + } + + private final ElevatorVisualizer visualizer; + + public static Elevator getInstance() { + return instance; + } + + public Elevator() { + visualizer = ElevatorVisualizer.getInstance(); + } + + public abstract void setTargetHeight(double height); + public abstract double getTargetHeight(); + public abstract double getCurrentHeight(); + public abstract boolean atTargetHeight(); + + public void periodic() { + visualizer.update(); + } + +} + \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java new file mode 100644 index 0000000..3e5f4ba --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -0,0 +1,101 @@ +package com.stuypulse.robot.subsystems.elevator; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.ElevatorFeedforward; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ElevatorImpl extends Elevator { + private final TalonFX motor; + + private final DigitalInput bottomBumpSwitch; + private final DigitalInput topBumpSwitch; + + private final SmartNumber targetHeight; + + private final Controller controller; + + private boolean hasBeenReset; + + public ElevatorImpl() { + motor = new TalonFX(Ports.Elevator.MOTOR); + + bottomBumpSwitch = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); + topBumpSwitch = new DigitalInput(Ports.Elevator.TOP_SWITCH); + + targetHeight = new SmartNumber("Elevator/Target Height", Constants.Elevator.MIN_HEIGHT_METERS); + + MotionProfile motionProfile = new MotionProfile(Settings.Elevator.MAX_VELOCITY_METERS_PER_SECOND, Settings.Elevator.MAX_ACCEL_METERS_PER_SECOND_PER_SECOND); + + controller = new MotorFeedforward(Settings.Elevator.FF.kS, Settings.Elevator.FF.kV, Settings.Elevator.FF.kA).position() + .add(new ElevatorFeedforward(Settings.Elevator.FF.kG)) + .add(new PIDController(Settings.Elevator.PID.kP, Settings.Elevator.PID.kI, Settings.Elevator.PID.kD)) + .setSetpointFilter(motionProfile); + + hasBeenReset = false; + } + + @Override + public void setTargetHeight(double height) { + targetHeight.set( + SLMath.clamp( + height, + Constants.Elevator.MIN_HEIGHT_METERS, + Constants.Elevator.MAX_HEIGHT_METERS + ) + ); + } + + @Override + public double getTargetHeight() { + return targetHeight.getAsDouble(); + } + + @Override + public double getCurrentHeight() { + return motor.getPosition().getValueAsDouble() * Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR; + } + + @Override + public boolean atTargetHeight() { + return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); + } + + private void setVoltage(double voltage) { + motor.setVoltage(voltage); + } + + @Override + public void periodic() { + super.periodic(); + + if (bottomBumpSwitch.get()) { + hasBeenReset = true; + motor.setPosition(0); + } + + if (topBumpSwitch.get()) { + hasBeenReset = true; + motor.setPosition(Constants.Elevator.MAX_HEIGHT_METERS / Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR); + } + + if (!hasBeenReset) { + setVoltage(-1); + } else { + controller.update(getTargetHeight(), getCurrentHeight()); + setVoltage(controller.getOutput()); + } + + SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java new file mode 100644 index 0000000..1b54558 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java @@ -0,0 +1,91 @@ +package com.stuypulse.robot.subsystems.elevator; + +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.ElevatorFeedforward; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ElevatorSimu extends Elevator { + + private final ElevatorSim sim; + private final double minHeight, maxHeight; + + private final SmartNumber targetHeight; + + private final Controller controller; + + ElevatorSimu() { + + sim = new ElevatorSim( + DCMotor.getNEO(2), + Constants.Elevator.Encoders.GEARING, + Constants.Elevator.MASS_KG, + Constants.Elevator.DRUM_RADIUS_METERS, + Constants.Elevator.MIN_HEIGHT_METERS, + Constants.Elevator.MAX_HEIGHT_METERS, + true, + Constants.Elevator.MIN_HEIGHT_METERS + ); + + minHeight = Constants.Elevator.MIN_HEIGHT_METERS; + maxHeight = Constants.Elevator.MAX_HEIGHT_METERS; + + targetHeight = new SmartNumber("Elevator/Target Height (m)", Constants.Elevator.MIN_HEIGHT_METERS); + + MotionProfile motionProfile = new MotionProfile(Settings.Elevator.MAX_VELOCITY_METERS_PER_SECOND, Settings.Elevator.MAX_ACCEL_METERS_PER_SECOND_PER_SECOND); + + controller = new MotorFeedforward(Settings.Elevator.FF.kS, Settings.Elevator.FF.kV, Settings.Elevator.FF.kA).position() + .add(new ElevatorFeedforward(Settings.Elevator.FF.kG)) + .add(new PIDController(Settings.Elevator.PID.kP, Settings.Elevator.PID.kI, Settings.Elevator.PID.kD)) + .setSetpointFilter(motionProfile); + } + + public ElevatorSim getSim() { + return sim; + } + + @Override + public void setTargetHeight(double height) { + targetHeight.set(SLMath.clamp(height, minHeight, maxHeight)); + } + + @Override + public double getTargetHeight() { + return targetHeight.get(); + } + + @Override + public double getCurrentHeight() { + return sim.getPositionMeters(); + } + + @Override + public boolean atTargetHeight() { + return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); + } + + @Override + public void periodic() { + super.periodic(); + + controller.update(getTargetHeight(), getCurrentHeight()); + sim.setInputVoltage(controller.getOutput()); + sim.update(Settings.DT); + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + + ElevatorVisualizer.getInstance().update(); + + SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java new file mode 100644 index 0000000..623ee97 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java @@ -0,0 +1,187 @@ +package com.stuypulse.robot.subsystems.elevator; + +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ElevatorVisualizer { + + private static final ElevatorVisualizer instance; + + static { + instance = new ElevatorVisualizer(); + } + + private final Mechanism2d elevator2d; + + private final MechanismRoot2d elevatorBL; + private final MechanismRoot2d elevatorTR; + + private final MechanismRoot2d outerBL; + private final MechanismRoot2d outerTR; + + private final MechanismRoot2d innerBL; + private final MechanismRoot2d innerTR; + + public static ElevatorVisualizer getInstance(){ + return instance; + } + + public ElevatorVisualizer() { + + // Mechanism2d + elevator2d = new Mechanism2d(Units.inchesToMeters(17), Units.inchesToMeters(150)); + + // Stage One + // Bottom Left Node + elevatorBL = elevator2d.getRoot("Elevator BL", Units.inchesToMeters(2), Constants.Elevator.MIN_HEIGHT_METERS); + + elevatorBL.append(new MechanismLigament2d( + "Left Tower", + Units.inchesToMeters(47), + 90, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + elevatorBL.append(new MechanismLigament2d( + "Bottom Tower", + Units.inchesToMeters(11), + 0, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + // Top Right Node + elevatorTR = elevator2d.getRoot("Elevator TR", Units.inchesToMeters(13), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + + elevatorTR.append(new MechanismLigament2d( + "Right Tower", + Units.inchesToMeters(47), + -90, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + elevatorTR.append(new MechanismLigament2d( + "Top Side", + Units.inchesToMeters(11), + 180, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + // Stage Two + // Bottom Left Node + outerBL = elevator2d.getRoot("Outer BL", Units.inchesToMeters(3), Constants.Elevator.MIN_HEIGHT_METERS); + + outerBL.append(new MechanismLigament2d( + "Left Side", + Units.inchesToMeters(47), + 90, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + outerBL.append(new MechanismLigament2d( + "Bottom Side", + Units.inchesToMeters(9), + 0, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + // Top Right Node + outerTR = elevator2d.getRoot("Outer TR", Units.inchesToMeters(12), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + + outerTR.append(new MechanismLigament2d( + "Top Side", + Units.inchesToMeters(9), + 180, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + outerTR.append(new MechanismLigament2d( + "Right Side", + Units.inchesToMeters(47), + -90, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + // Carriage + // Bottom Left Node + innerBL = elevator2d.getRoot("Inner BL", Units.inchesToMeters(4), Units.inchesToMeters(1) + Constants.Elevator.MIN_HEIGHT_METERS); + + innerBL.append(new MechanismLigament2d( + "Left Side", + Units.inchesToMeters(7), + 90, + 10, + new Color8Bit(Color.kPink) + ) + ); + + innerBL.append(new MechanismLigament2d( + "Bottom Side", + Units.inchesToMeters(7), + 0, + 10, + new Color8Bit(Color.kPink) + ) + ); + + // Top Right Node + innerTR = elevator2d.getRoot("Inner TR", Units.inchesToMeters(11), Units.inchesToMeters(8) + Constants.Elevator.MIN_HEIGHT_METERS); + + innerTR.append(new MechanismLigament2d( + "Top Side", + Units.inchesToMeters(7), + 180, + 10, + new Color8Bit(Color.kPink) + ) + ); + + innerTR.append(new MechanismLigament2d( + "Right Side", + Units.inchesToMeters(7), + -90, + 10, + new Color8Bit(Color.kPink) + ) + ); + + SmartDashboard.putData("Visualizers/Elevator", elevator2d); + } + + public void update() { + // Top of Carriage is Target Height + Elevator elevator = Elevator.getInstance(); + + outerBL.setPosition(Units.inchesToMeters(3), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) * Settings.Elevator.Simulation.SCALE_FACTOR + Constants.Elevator.MIN_HEIGHT_METERS); + outerTR.setPosition(Units.inchesToMeters(12), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) * Settings.Elevator.Simulation.SCALE_FACTOR + Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + + innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); + innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); + + } + +} \ No newline at end of file From 5107a8830baac41918169b0b6b1290d543ba36e9 Mon Sep 17 00:00:00 2001 From: Mustafa A Date: Tue, 28 Jan 2025 18:30:47 -0500 Subject: [PATCH 04/34] started arm impl --- .../stuypulse/robot/subsystems/arm/Arm.java | 52 +++++++++++++++---- 1 file changed, 43 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 13f0508..70d404d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { @@ -23,11 +24,12 @@ public class Arm extends SubsystemBase { * - instance */ + private static Arm instance; private SmartNumber targetAngle; - private AbsoluteEncoder armEncoder; + private TalonFX armMotor; private final ProfiledPIDController pidController; private final ArmFeedforward ffController; @@ -38,7 +40,7 @@ public class Arm extends SubsystemBase { public Arm() { // super(); - targetAngle = new SmartNumber("Arm/Target Angle", 0); + targetAngle = new SmartNumber("Arm/Target Angle", 0.0); armMotor = new TalonFX(0); @@ -51,14 +53,46 @@ public Arm() { new Constraints( Settings.Arm.MAX_VEL, Settings.Arm.MAX_ACCEL - ) - + ) + ); + + pidController.enableContinuousInput(-180,180); - ); + + ffController = new ArmFeedforward( + Settings.Arm.FF.kS, + Settings.Arm.FF.kG, + Settings.Arm.FF.kV, + Settings.Arm.FF.kA); + + } + + public void setTargetAngle(double targetAngle) { + this.targetAngle.set(targetAngle); + } + + public double getTargetAngle() { + return targetAngle.getAsDouble(); + } - - // pidController = new ProfiledPIDController(//kp, ki, kd settings); -} -_} + public double getArmAngle() { + return armMotor.getPosition().getValueAsDouble() * 360; + } + + @Override + public void periodic() { + armMotor.setVoltage( + pidController.calculate( + getArmAngle(), + getTargetAngle() + ) + + + ffController.calculate( + getArmAngle(), + armMotor.getVelocity().getValueAsDouble()) + ); + SmartDashboard.putNumber("Arm/Arm Angle", getArmAngle()); + } +} From 3fcee3e2db0343f4584614c4034e7dec078acfd7 Mon Sep 17 00:00:00 2001 From: Rahuldeb5 Date: Wed, 29 Jan 2025 20:42:47 -0500 Subject: [PATCH 05/34] Added stall detection to shooter and implemented motor for funnel --- .../stuypulse/robot/constants/Settings.java | 28 ++++++++- .../subsystems/funnel/CoralFunnelImpl.java | 59 ++++++++++++++++++- .../subsystems/shooter/CoralShooter.java | 4 +- .../subsystems/shooter/CoralShooterImpl.java | 42 +++++++++---- 4 files changed, 117 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 89d9ce5..d10212f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -17,6 +17,7 @@ public interface Settings { public interface Shooter { double MAX_SHOOTER_RPM = 6380; // Max RPM of Talon FX (rpm) + double TARGET_SHOOTER_RPM = 6000; // Target RPM of Talon FX (rpm) double BB_DEBOUNCE = 0.0; public interface PID { @@ -32,6 +33,31 @@ public interface FeedForward { SmartNumber kV = new SmartNumber("kV", 0); SmartNumber kA = new SmartNumber("kA", 0); } - + + double GEAR_RATIO = 0.0; + double DRIVE_CURRENT_THRESHOLD = 80; + double DRIVE_CURRENT_LIMIT = 120; + } + + public interface Funnel { + double MAX_FUNNEL_RPM = 6380; // Max RPM of Talon FX (rpm) + + public interface PID { + // ADJUST LATER + SmartNumber kP = new SmartNumber("kP", 0); + SmartNumber kI = new SmartNumber("kI", 0); + SmartNumber kD = new SmartNumber("kD", 0); + } + + public interface FeedForward { + // ADJUST LATER + SmartNumber kS = new SmartNumber("kS", 0); + SmartNumber kV = new SmartNumber("kV", 0); + SmartNumber kA = new SmartNumber("kA", 0); + } + + double GEAR_RATIO = 0.0; + double DRIVE_CURRENT_THRESHOLD = 80; + double DRIVE_CURRENT_LIMIT = 120; } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java index 9beb450..315a4b7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java @@ -1,26 +1,83 @@ package com.stuypulse.robot.subsystems.funnel; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.HighPassFilter; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class CoralFunnelImpl extends CoralFunnel { private final TalonFX driveMotor; + private final TalonFXConfiguration driveConfig; + private final DigitalInput motorBeam; private boolean funnelState; + private final IStream driveCurrent; + public CoralFunnelImpl(){ driveMotor = new TalonFX(Ports.Funnel.MOTOR); + motorBeam = new DigitalInput(Ports.Funnel.BEAM); + + driveConfig = new TalonFXConfiguration(); + + Slot0Configs slot0 = new Slot0Configs(); + + slot0.kS = Settings.Funnel.FeedForward.kS.getAsDouble(); + slot0.kV = Settings.Funnel.FeedForward.kV.getAsDouble(); + slot0.kA = Settings.Funnel.FeedForward.kA.getAsDouble(); + + slot0.kP = Settings.Funnel.PID.kP.getAsDouble(); + slot0.kI = Settings.Funnel.PID.kI.getAsDouble(); + slot0.kD = Settings.Funnel.PID.kD.getAsDouble(); + + driveConfig.Slot0 = slot0; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Feedback.SensorToMechanismRatio = Settings.Funnel.GEAR_RATIO; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = Settings.Funnel.DRIVE_CURRENT_LIMIT; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -Settings.Funnel.DRIVE_CURRENT_LIMIT; + driveConfig.CurrentLimits.StatorCurrentLimit = Settings.Funnel.DRIVE_CURRENT_LIMIT; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.02; + driveConfig.TorqueCurrent.TorqueNeutralDeadband = 0.05; + + driveCurrent = IStream.create(() -> driveMotor.getStatorCurrent().getValueAsDouble()) + .filtered(new HighPassFilter(Settings.Funnel.DRIVE_CURRENT_THRESHOLD)); + + driveMotor.getConfigurator().apply(driveConfig); + driveMotor.setPosition(0); + } + + private double getMotorRPM() { + return driveMotor.get() * Settings.Funnel.MAX_FUNNEL_RPM; + } + + public void setMotorRPM(double targetRPM) { + driveMotor.set(targetRPM / Settings.Funnel.MAX_FUNNEL_RPM); + } + + public boolean coralStuck() { + return driveCurrent.get() > Settings.Funnel.DRIVE_CURRENT_THRESHOLD; } @Override public void periodic() { super.periodic(); - + SmartDashboard.putNumber("Funnel/RPM", getMotorRPM()); + + SmartDashboard.putNumber("Funnel/Voltage", driveMotor.getMotorVoltage().getValueAsDouble()); + + SmartDashboard.putNumber("Funnel/Current", driveCurrent.get()); + + SmartDashboard.putBoolean("Funnel/Coral Stuck", coralStuck()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java index 320c7bc..f6896be 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java @@ -19,7 +19,7 @@ public static CoralShooter getInstance() { private final SmartNumber targetRPM; public CoralShooter() { - targetRPM = new SmartNumber("CoralShooter/Target RPM", 0); + targetRPM = new SmartNumber("Shooter/Target RPM", 0); } public double getTargetRPM() { @@ -29,7 +29,5 @@ public double getTargetRPM() { public abstract boolean hasCoral(); public abstract boolean hasAlgae(); - - } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java index 6ec6bc8..6b7ccc6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java @@ -3,29 +3,36 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import edu.wpi.first.wpilibj.DigitalInput; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.HighPassFilter; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class CoralShooterImpl extends CoralShooter { private final TalonFX driveMotor; + private final TalonFXConfiguration driveConfig; + private final DigitalInput motorBeam; private final BStream hasCoral; - + + private final IStream driveCurrent; public CoralShooterImpl(){ - driveMotor = new TalonFX(Ports.Shooter.MOTOR); + motorBeam = new DigitalInput(Ports.Shooter.BEAM); hasCoral = BStream.create(motorBeam).not() .filtered(new BDebounce.Both(Settings.Shooter.BB_DEBOUNCE)); - TalonFXConfiguration driveConfig = new TalonFXConfiguration(); + driveConfig = new TalonFXConfiguration(); Slot0Configs slot0 = new Slot0Configs(); @@ -38,12 +45,24 @@ public CoralShooterImpl(){ slot0.kD = Settings.Shooter.PID.kD.getAsDouble(); driveConfig.Slot0 = slot0; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Feedback.SensorToMechanismRatio = Settings.Shooter.GEAR_RATIO; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = Settings.Shooter.DRIVE_CURRENT_LIMIT; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -Settings.Shooter.DRIVE_CURRENT_LIMIT; + driveConfig.CurrentLimits.StatorCurrentLimit = Settings.Shooter.DRIVE_CURRENT_LIMIT; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.02; + driveConfig.TorqueCurrent.TorqueNeutralDeadband = 0.05; + + driveCurrent = IStream.create(() -> driveMotor.getStatorCurrent().getValueAsDouble()) + .filtered(new HighPassFilter(Settings.Shooter.DRIVE_CURRENT_THRESHOLD)); driveMotor.getConfigurator().apply(driveConfig); + driveMotor.setPosition(0); } private double getShooterRPM() { - return Math.abs(driveMotor.get() * Settings.Shooter.MAX_SHOOTER_RPM); + return driveMotor.get() * Settings.Shooter.MAX_SHOOTER_RPM; } public void setShooterRPM(double targetRPM) { @@ -52,15 +71,14 @@ public void setShooterRPM(double targetRPM) { @Override public boolean hasCoral() { - if return hasCoral.get(); - } - // @Override - // public boolean hasAlgae() { - // return false; - // } + // Uses stall detection to determine if the shooter has algae by checking if the current is above a certain threshold + @Override + public boolean hasAlgae() { + return driveCurrent.get() > Settings.Shooter.DRIVE_CURRENT_THRESHOLD; + } @Override public void periodic() { @@ -70,6 +88,8 @@ public void periodic() { SmartDashboard.putNumber("Shooter/RPM", getShooterRPM()); - SmartDashboard.putNumber("Shooter/Voltage", driveMotor.get() * driveMotor.getDutyCycle().getValue()); // can someone check whether this actually gets the voltage + SmartDashboard.putNumber("Shooter/Voltage", driveMotor.getMotorVoltage().getValueAsDouble()); + + SmartDashboard.putNumber("Shooter/Current", driveCurrent.get()); } } From 7236a707673e913c4e38d181fad586fa213a8631 Mon Sep 17 00:00:00 2001 From: Rahuldeb5 Date: Thu, 30 Jan 2025 11:57:48 -0500 Subject: [PATCH 06/34] Added default funnel command --- .../com/stuypulse/robot/RobotContainer.java | 11 +++- .../commands/funnel/FunnelDefaultCommand.java | 52 +++++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 8 +-- .../robot/subsystems/funnel/CoralFunnel.java | 31 +++++++++-- .../subsystems/funnel/CoralFunnelImpl.java | 12 ++++- .../subsystems/shooter/CoralShooter.java | 9 +++- .../subsystems/shooter/CoralShooterImpl.java | 6 ++- 7 files changed, 117 insertions(+), 12 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5b55349..5e2e521 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -6,7 +6,10 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.funnel.FunnelDefaultCommand; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.subsystems.funnel.CoralFunnel; +import com.stuypulse.robot.subsystems.shooter.CoralShooter; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -21,7 +24,9 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem - + private final CoralFunnel funnel = CoralFunnel.getInstance(); + private final CoralShooter shooter = CoralShooter.getInstance(); + // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -37,7 +42,9 @@ public RobotContainer() { /*** DEFAULTS ***/ /****************/ - private void configureDefaultCommands() {} + private void configureDefaultCommands() { + funnel.setDefaultCommand(new FunnelDefaultCommand()); + } /***************/ /*** BUTTONS ***/ diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java new file mode 100644 index 0000000..3e511c8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java @@ -0,0 +1,52 @@ +package com.stuypulse.robot.commands.funnel; + +import com.stuypulse.robot.subsystems.funnel.CoralFunnel; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FunnelDefaultCommand extends Command { + private final CoralFunnel funnel; + private boolean reversing = false; + private boolean coralStopped = false; + + public FunnelDefaultCommand() { + funnel = CoralFunnel.getInstance(); + addRequirements(funnel); + } + + @Override + public void execute() { + boolean coralDetected = funnel.getFunnelState(); + + if(coralDetected) { + funnel.setMotorRPM(0); + coralStopped = true; + return; + } + + if(coralStopped && !coralDetected) { + coralStopped = false; + funnel.setMotorRPM(funnel.getTargetRPM()); + } + + if(funnel.coralStuck() && !reversing) { + reversing = true; + funnel.setMotorRPM(-funnel.getTargetRPM()); + + new WaitCommand(1).andThen(() -> { + funnel.setMotorRPM(funnel.getTargetRPM()); + reversing = true; + }).schedule(); + } + + if(!reversing && !coralStopped) { + funnel.setMotorRPM(funnel.getTargetRPM()); + } + } + + @Override + public void end(boolean interrupted) { + funnel.setMotorRPM(0); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d10212f..2622098 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -16,8 +16,8 @@ */ public interface Settings { public interface Shooter { - double MAX_SHOOTER_RPM = 6380; // Max RPM of Talon FX (rpm) - double TARGET_SHOOTER_RPM = 6000; // Target RPM of Talon FX (rpm) + double MAX_SHOOTER_RPM = 6000; // Max RPM of KrakenX60 (rpm) + double TARGET_SHOOTER_RPM = 6000; // Target RPM of KrakenX60 (rpm) double BB_DEBOUNCE = 0.0; public interface PID { @@ -40,7 +40,9 @@ public interface FeedForward { } public interface Funnel { - double MAX_FUNNEL_RPM = 6380; // Max RPM of Talon FX (rpm) + double MAX_FUNNEL_RPM = 6000; // Max RPM of KrakenX60 (rpm) + double TARGET_FUNNEL_RPM = 6000; // Target RPM of KrakenX60 (rpm) + double BB_DEBOUNCE = 0.0; public interface PID { // ADJUST LATER diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java index 0f680ed..8c4a051 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java @@ -1,17 +1,42 @@ package com.stuypulse.robot.subsystems.funnel; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class CoralFunnel extends SubsystemBase{ +public abstract class CoralFunnel extends SubsystemBase{ private static final CoralFunnel instance; static { - instance = new CoralFunnel(); + instance = new CoralFunnelImpl(); } - public CoralFunnel getInstance() { + private final SmartNumber targetRPM; + + public static CoralFunnel getInstance() { return instance; } + public CoralFunnel() { + targetRPM = new SmartNumber("Funnel/Target RPM", Settings.Funnel.TARGET_FUNNEL_RPM); + } + + public double getTargetRPM() { + return targetRPM.get(); + } + + public abstract void setMotorRPM(double targetRPM); + + public abstract boolean coralStuck(); + + public abstract boolean getFunnelState(); + + @Override + public void periodic() { + SmartDashboard.putNumber("Funnel/Target RPM", getTargetRPM()); + } + } diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java index 315a4b7..fe9c04d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java @@ -6,6 +6,8 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.HighPassFilter; @@ -18,7 +20,7 @@ public class CoralFunnelImpl extends CoralFunnel { private final TalonFXConfiguration driveConfig; private final DigitalInput motorBeam; - private boolean funnelState; + private BStream funnelState; private final IStream driveCurrent; @@ -26,6 +28,8 @@ public CoralFunnelImpl(){ driveMotor = new TalonFX(Ports.Funnel.MOTOR); motorBeam = new DigitalInput(Ports.Funnel.BEAM); + funnelState = BStream.create(motorBeam).not() + .filtered(new BDebounce.Both(Settings.Funnel.BB_DEBOUNCE)); driveConfig = new TalonFXConfiguration(); @@ -68,6 +72,11 @@ public boolean coralStuck() { return driveCurrent.get() > Settings.Funnel.DRIVE_CURRENT_THRESHOLD; } + @Override + public boolean getFunnelState() { + return funnelState.get(); + } + @Override public void periodic() { super.periodic(); @@ -78,6 +87,7 @@ public void periodic() { SmartDashboard.putNumber("Funnel/Current", driveCurrent.get()); + SmartDashboard.putBoolean("Funnel/Has Coral", getFunnelState()); SmartDashboard.putBoolean("Funnel/Coral Stuck", coralStuck()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java index f6896be..3827689 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java @@ -1,7 +1,9 @@ package com.stuypulse.robot.subsystems.shooter; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class CoralShooter extends SubsystemBase { @@ -19,7 +21,7 @@ public static CoralShooter getInstance() { private final SmartNumber targetRPM; public CoralShooter() { - targetRPM = new SmartNumber("Shooter/Target RPM", 0); + targetRPM = new SmartNumber("Shooter/Target RPM", Settings.Shooter.TARGET_SHOOTER_RPM); } public double getTargetRPM() { @@ -29,5 +31,10 @@ public double getTargetRPM() { public abstract boolean hasCoral(); public abstract boolean hasAlgae(); + + @Override + public void periodic() { + SmartDashboard.putNumber("Shooter/Target RPM", getTargetRPM()); + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java index 6b7ccc6..6556039 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java @@ -79,17 +79,19 @@ public boolean hasCoral() { public boolean hasAlgae() { return driveCurrent.get() > Settings.Shooter.DRIVE_CURRENT_THRESHOLD; } + @Override public void periodic() { super.periodic(); - SmartDashboard.putBoolean("Shooter/Has Coral", hasCoral()); - SmartDashboard.putNumber("Shooter/RPM", getShooterRPM()); SmartDashboard.putNumber("Shooter/Voltage", driveMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Shooter/Current", driveCurrent.get()); + + SmartDashboard.putBoolean("Shooter/Has Coral", hasCoral()); + SmartDashboard.putBoolean("Shooter/Has Algae", hasAlgae()); } } From 39685286563fb39ccf51861ec7e269d3e1479efe Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Thu, 30 Jan 2025 17:50:18 -0500 Subject: [PATCH 07/34] reworked funnel default command only calls set rpm at the end --- .../commands/funnel/FunnelDefaultCommand.java | 29 +++++++++---------- .../stuypulse/robot/constants/Settings.java | 1 - 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java index 3e511c8..1cbd6c0 100644 --- a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java @@ -7,8 +7,8 @@ public class FunnelDefaultCommand extends Command { private final CoralFunnel funnel; - private boolean reversing = false; - private boolean coralStopped = false; + private boolean reversed = false; + private boolean funnelStopped = false; public FunnelDefaultCommand() { funnel = CoralFunnel.getInstance(); @@ -19,28 +19,27 @@ public FunnelDefaultCommand() { public void execute() { boolean coralDetected = funnel.getFunnelState(); - if(coralDetected) { - funnel.setMotorRPM(0); - coralStopped = true; - return; + if(coralDetected && !funnelStopped) { + funnelStopped = true; } - if(coralStopped && !coralDetected) { - coralStopped = false; - funnel.setMotorRPM(funnel.getTargetRPM()); + if(funnelStopped && !coralDetected) { + funnelStopped = false; } - if(funnel.coralStuck() && !reversing) { - reversing = true; - funnel.setMotorRPM(-funnel.getTargetRPM()); + if(funnel.coralStuck() && !reversed) { + reversed = true; new WaitCommand(1).andThen(() -> { - funnel.setMotorRPM(funnel.getTargetRPM()); - reversing = true; + reversed = false; }).schedule(); } - if(!reversing && !coralStopped) { + if (funnelStopped){ + funnel.setMotorRPM(0); + } else if (reversed){ + funnel.setMotorRPM(-funnel.getTargetRPM()); + } else { funnel.setMotorRPM(funnel.getTargetRPM()); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2622098..f959739 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,7 +5,6 @@ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; /*- From eeb39e5ee157e21179c293cec1ea8ef82264cea4 Mon Sep 17 00:00:00 2001 From: WennieC Date: Fri, 31 Jan 2025 01:18:40 -0500 Subject: [PATCH 08/34] Added funnel ramp motor voltage --- src/main/java/com/stuypulse/robot/constants/Settings.java | 1 + .../stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index f959739..27d2ce1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -42,6 +42,7 @@ public interface Funnel { double MAX_FUNNEL_RPM = 6000; // Max RPM of KrakenX60 (rpm) double TARGET_FUNNEL_RPM = 6000; // Target RPM of KrakenX60 (rpm) double BB_DEBOUNCE = 0.0; + double RAMP_RATE = 0.0; public interface PID { // ADJUST LATER diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java index fe9c04d..2d5f023 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java @@ -53,11 +53,16 @@ public CoralFunnelImpl(){ driveConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.02; driveConfig.TorqueCurrent.TorqueNeutralDeadband = 0.05; + //Stall Detection driveCurrent = IStream.create(() -> driveMotor.getStatorCurrent().getValueAsDouble()) .filtered(new HighPassFilter(Settings.Funnel.DRIVE_CURRENT_THRESHOLD)); driveMotor.getConfigurator().apply(driveConfig); driveMotor.setPosition(0); + + //Ramp Motor Voltage + driveConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Funnel.MAX_FUNNEL_RPM; + driveConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Funnel.MAX_FUNNEL_RPM; } private double getMotorRPM() { From afda06a2189758719416de31281b1dc297cb06f5 Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Fri, 31 Jan 2025 16:39:43 -0500 Subject: [PATCH 09/34] Added the shoot coral command Moved the shooter rpm commands to the abstract class --- .../robot/commands/shooter/ShootCoral.java | 25 +++++++++++++++++++ .../subsystems/shooter/CoralShooter.java | 4 +++ .../subsystems/shooter/CoralShooterImpl.java | 2 +- 3 files changed, 30 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShootCoral.java diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShootCoral.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShootCoral.java new file mode 100644 index 0000000..e9120e0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShootCoral.java @@ -0,0 +1,25 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.shooter.CoralShooter; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ShootCoral extends Command { + private final CoralShooter shooter; + + public ShootCoral(){ + shooter = CoralShooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void execute(){ + shooter.setShooterRPM(Settings.Shooter.MAX_SHOOTER_RPM); + } + + @Override + public void end(boolean interrupted) { + shooter.setShooterRPM(0); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java index 3827689..c212307 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java @@ -28,6 +28,10 @@ public double getTargetRPM() { return targetRPM.get(); } + public abstract double getShooterRPM(); + + public abstract void setShooterRPM(double targetRPM); + public abstract boolean hasCoral(); public abstract boolean hasAlgae(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java index 6556039..e432c24 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java @@ -61,7 +61,7 @@ public CoralShooterImpl(){ driveMotor.setPosition(0); } - private double getShooterRPM() { + public double getShooterRPM() { return driveMotor.get() * Settings.Shooter.MAX_SHOOTER_RPM; } From c13736183d4c06401c4eb1a83de0571b09a40526 Mon Sep 17 00:00:00 2001 From: WennieC Date: Fri, 31 Jan 2025 17:46:22 -0500 Subject: [PATCH 10/34] Finished Shooter commands, reworked algae detection, added setter for targetRPM Co-authored-by: Calvin Ye Co-authored-by: sixuanl Co-authored-by: ryanyu32 Co-authored-by: metereel Co-authored-by: LinAndy10 --- ...ootCoral.java => ShooterAcquireAlgae.java} | 16 +++++-------- .../shooter/ShooterDeacquireAlgae.java | 21 ++++++++++++++++ .../robot/commands/shooter/ShooterShoot.java | 24 +++++++++++++++++++ .../robot/commands/shooter/ShooterStop.java | 20 ++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 5 ++++ .../subsystems/shooter/CoralShooter.java | 4 ++++ .../subsystems/shooter/CoralShooterImpl.java | 14 +++++++++-- 7 files changed, 92 insertions(+), 12 deletions(-) rename src/main/java/com/stuypulse/robot/commands/shooter/{ShootCoral.java => ShooterAcquireAlgae.java} (50%) create mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterDeacquireAlgae.java create mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterShoot.java create mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShootCoral.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java similarity index 50% rename from src/main/java/com/stuypulse/robot/commands/shooter/ShootCoral.java rename to src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java index e9120e0..f412562 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShootCoral.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java @@ -3,23 +3,19 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.shooter.CoralShooter; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ShootCoral extends Command { +public class ShooterAcquireAlgae extends InstantCommand { + private final CoralShooter shooter; - public ShootCoral(){ + public ShooterAcquireAlgae(){ shooter = CoralShooter.getInstance(); addRequirements(shooter); } @Override - public void execute(){ - shooter.setShooterRPM(Settings.Shooter.MAX_SHOOTER_RPM); - } - - @Override - public void end(boolean interrupted) { - shooter.setShooterRPM(0); + public void initialize() { + shooter.setShooterRPM(Settings.Shooter.ALGAE_ACQUIRE_RPM.getAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterDeacquireAlgae.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterDeacquireAlgae.java new file mode 100644 index 0000000..dcc73bf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterDeacquireAlgae.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.shooter.CoralShooter; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterDeacquireAlgae extends InstantCommand { + + private final CoralShooter shooter; + + public ShooterDeacquireAlgae(){ + shooter = CoralShooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize() { + shooter.setShooterRPM(-Settings.Shooter.ALGAE_ACQUIRE_RPM.getAsDouble()); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterShoot.java new file mode 100644 index 0000000..20668c5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterShoot.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.shooter.CoralShooter; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterShoot extends InstantCommand { + private final CoralShooter shooter; + + public ShooterShoot(){ + shooter = CoralShooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize(){ + if (shooter.hasAlgae()){ + shooter.setShooterRPM(Settings.Shooter.ALGAE_TARGET_RPM.getAsDouble()); + } else if (shooter.hasCoral()){ + shooter.setShooterRPM(Settings.Shooter.CORAL_TARGET_RPM.getAsDouble()); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java new file mode 100644 index 0000000..92cb389 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.subsystems.shooter.CoralShooter; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterStop extends InstantCommand { + + private final CoralShooter shooter; + + public ShooterStop() { + shooter = CoralShooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize() { + shooter.setShooterRPM(0); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 27d2ce1..c6e8460 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -17,6 +17,11 @@ public interface Settings { public interface Shooter { double MAX_SHOOTER_RPM = 6000; // Max RPM of KrakenX60 (rpm) double TARGET_SHOOTER_RPM = 6000; // Target RPM of KrakenX60 (rpm) + + SmartNumber ALGAE_TARGET_RPM = new SmartNumber("Algae Target RPM", 6000); + SmartNumber CORAL_TARGET_RPM = new SmartNumber("Coral Target RPM", 5000); + SmartNumber ALGAE_ACQUIRE_RPM = new SmartNumber("Algae Acquire RPM", 3000); + double BB_DEBOUNCE = 0.0; public interface PID { diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java index c212307..f1ea7a6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java @@ -28,6 +28,10 @@ public double getTargetRPM() { return targetRPM.get(); } + public void setTargetRPM(double targetRPM){ + this.targetRPM.set(targetRPM); + } + public abstract double getShooterRPM(); public abstract void setShooterRPM(double targetRPM); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java index e432c24..7bd365f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java @@ -22,6 +22,7 @@ public class CoralShooterImpl extends CoralShooter { private final DigitalInput motorBeam; private final BStream hasCoral; + private boolean hasAlgae; private final IStream driveCurrent; @@ -66,6 +67,7 @@ public double getShooterRPM() { } public void setShooterRPM(double targetRPM) { + this.setTargetRPM(targetRPM); driveMotor.set(targetRPM / Settings.Shooter.MAX_SHOOTER_RPM); } @@ -74,9 +76,13 @@ public boolean hasCoral() { return hasCoral.get(); } - // Uses stall detection to determine if the shooter has algae by checking if the current is above a certain threshold @Override - public boolean hasAlgae() { + public boolean hasAlgae(){ + return this.hasAlgae; + } + + // Uses stall detection to determine if the shooter has algae by checking if the current is above a certain threshold + public boolean detectCurrentSpike() { return driveCurrent.get() > Settings.Shooter.DRIVE_CURRENT_THRESHOLD; } @@ -85,6 +91,10 @@ public boolean hasAlgae() { public void periodic() { super.periodic(); + if (this.getTargetRPM() != 0){ + this.hasAlgae = this.detectCurrentSpike(); + } + SmartDashboard.putNumber("Shooter/RPM", getShooterRPM()); SmartDashboard.putNumber("Shooter/Voltage", driveMotor.getMotorVoltage().getValueAsDouble()); From 8753a05da8fe2533a4bdf1398c8cffb83f60fbc0 Mon Sep 17 00:00:00 2001 From: Tmanxyz Date: Fri, 31 Jan 2025 18:04:13 -0500 Subject: [PATCH 11/34] made Armimpl and started Arm abstract class, added motionmagic and talonfx configs Co-authored-by: WillyZheng827 Co-authored-by: ZiJiaG Co-authored-by: Gordon Doan Co-authored-by: Maddonm Co-authored-by: Mustafa Abdullah Co-authored-by: Alvin Cheng --- ...rLib.json => PathplannerLib-2025.2.2.json} | 8 +- .../{Phoenix5.json => Phoenix5-5.35.1.json} | 24 ++-- .../{Phoenix6.json => Phoenix6-25.2.1.json} | 84 ++++++++----- .../{REVLib.json => REVLib-2025.0.2.json} | 15 +-- vendordeps/photonlib.json | 114 +++++++++--------- 5 files changed, 136 insertions(+), 109 deletions(-) rename vendordeps/{PathplannerLib.json => PathplannerLib-2025.2.2.json} (87%) rename vendordeps/{Phoenix5.json => Phoenix5-5.35.1.json} (92%) rename vendordeps/{Phoenix6.json => Phoenix6-25.2.1.json} (85%) rename vendordeps/{REVLib.json => REVLib-2025.0.2.json} (86%) diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-2025.2.2.json similarity index 87% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-2025.2.2.json index 396f92d..a5bf9ee 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-2025.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-2025.2.2.json", "name": "PathplannerLib", - "version": "2025.1.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.1.1" + "version": "2025.2.2" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5-5.35.1.json similarity index 92% rename from vendordeps/Phoenix5.json rename to vendordeps/Phoenix5-5.35.1.json index 0ab6d7c..69df8b5 100644 --- a/vendordeps/Phoenix5.json +++ b/vendordeps/Phoenix5-5.35.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix5-frc2025-latest.json", + "fileName": "Phoenix5-5.35.1.json", "name": "CTRE-Phoenix (v5)", - "version": "5.35.0", + "version": "5.35.1", "frcYear": "2025", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -32,19 +32,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.35.0" + "version": "5.35.1" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.35.0" + "version": "5.35.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.35.0", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.35.0", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,7 +74,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -106,7 +106,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -122,7 +122,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -154,7 +154,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.35.0", + "version": "5.35.1", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6-25.2.1.json similarity index 85% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-25.2.1.json index 7f4bd2e..1397da1 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-25.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-frc2025-latest.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.json b/vendordeps/REVLib-2025.0.2.json similarity index 86% rename from vendordeps/REVLib.json rename to vendordeps/REVLib-2025.0.2.json index 2c39628..c29aefa 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib-2025.0.2.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib.json", + "fileName": "REVLib-2025.0.2.json", "name": "REVLib", - "version": "2025.0.0", + "version": "2025.0.2", "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.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.2", "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.2", "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.2", "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 039d0a5..6af3d3e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,71 +1,71 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.0.0-beta-8", + "version": "v2025.1.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" ], "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", "jniDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-8", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } ], "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-8", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-8", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.1.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } ], "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-8" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-8" - } + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.1.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.1.1" + } ] - } \ No newline at end of file +} \ No newline at end of file From 53a6a7c5120e63df571cc651110bd84100d85012 Mon Sep 17 00:00:00 2001 From: WennieC Date: Fri, 31 Jan 2025 18:04:13 -0500 Subject: [PATCH 12/34] changed ramp rate constants thingy uwu --- .../stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java index 2d5f023..2ee6537 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java @@ -61,8 +61,8 @@ public CoralFunnelImpl(){ driveMotor.setPosition(0); //Ramp Motor Voltage - driveConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Funnel.MAX_FUNNEL_RPM; - driveConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Funnel.MAX_FUNNEL_RPM; + driveConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Funnel.RAMP_RATE; + driveConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Funnel.RAMP_RATE; } private double getMotorRPM() { From 13eabd234319711e8487aeac2a109cf1d85e5f36 Mon Sep 17 00:00:00 2001 From: Tmanxyz Date: Fri, 31 Jan 2025 18:05:09 -0500 Subject: [PATCH 13/34] Made ArmImpl and started Arm abstract class, added talonfx slot0 --- .../robot/commands/arm/ArmMoveToAngle.java | 22 +++ .../robot/commands/arm/ArmMoveToL2.java | 11 ++ .../robot/commands/arm/ArmMoveToL3.java | 11 ++ .../robot/commands/arm/ArmMoveToL4.java | 11 ++ .../com/stuypulse/robot/constants/Ports.java | 4 + .../stuypulse/robot/constants/Settings.java | 7 +- .../stuypulse/robot/subsystems/arm/Arm.java | 94 +---------- .../robot/subsystems/arm/ArmImpl.java | 149 ++++++++++++++++++ 8 files changed, 218 insertions(+), 91 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java new file mode 100644 index 0000000..ea2ca0e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.subsystems.arm.Arm; +import edu.wpi.first.wpilibj2.command.InstantCommand; + + +public class ArmMoveToAngle extends InstantCommand{ + private final Arm arm; + private final double angle; + + public ArmMoveToAngle(double angle){ + arm = Arm.getInstance(); + this.angle = angle; + } + + @Override + public void initialize(){ + arm.setTargetAngle(angle); + + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java new file mode 100644 index 0000000..cbc5ff8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java @@ -0,0 +1,11 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.constants.Settings; + + +public class ArmMoveToL2 extends ArmMoveToAngle{ + public ArmMoveToL2(){ + super(Settings.Arm.L2_ANGLE); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java new file mode 100644 index 0000000..e9b028d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java @@ -0,0 +1,11 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.constants.Settings; + + +public class ArmMoveToL3 extends ArmMoveToAngle{ + public ArmMoveToL3(){ + super(Settings.Arm.L3_ANGLE); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java new file mode 100644 index 0000000..bf38853 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java @@ -0,0 +1,11 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.constants.Settings; + + +public class ArmMoveToL4 extends ArmMoveToAngle{ + public ArmMoveToL4(){ + super(Settings.Arm.L4_ANGLE); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 1a2106d..0f63212 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -12,4 +12,8 @@ public interface Gamepad { int OPERATOR = 1; int DEBUGGER = 2; } + + public interface Arm { + int ARM_MOTOR = 0; + } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e93146f..e1dea64 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -36,6 +36,11 @@ public interface FF { double L2_ANGLE = 0; double L3_ANGLE = 0; - double L4_Angle = 0; + double L4_ANGLE = 0; + + public interface MotionMagic{ + double MAX_VEL = 0; + double MAX_ACCEL = 0; + } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 70d404d..18425f0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -1,98 +1,12 @@ package com.stuypulse.robot.subsystems.arm; -import com.ctre.phoenix6.hardware.TalonFX; -import com.revrobotics.AbsoluteEncoder; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.stuylib.network.SmartNumber; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Arm extends SubsystemBase { - - /* - * GOODJOB GUYS!! I BELIEVE IN YOU!! - * FIELDS THAT WE NEED - * - Kraken Motor - * - Target angle - * - Absolute Encoder - * - PID controller - * - FF controller - * - instance - */ - - - private static Arm instance; - - private SmartNumber targetAngle; - - private TalonFX armMotor; - private final ProfiledPIDController pidController; - private final ArmFeedforward ffController; +public abstract class Arm extends SubsystemBase { - static { - instance = new Arm(); + public static Arm getInstance() { + return instance; } - public Arm() { - // super(); - targetAngle = new SmartNumber("Arm/Target Angle", 0.0); - armMotor = new TalonFX(0); - - - - - pidController = new ProfiledPIDController( - Settings.Arm.PID.kP, - Settings.Arm.PID.kI, - Settings.Arm.PID.kD, - new Constraints( - Settings.Arm.MAX_VEL, - Settings.Arm.MAX_ACCEL - ) - ); - - pidController.enableContinuousInput(-180,180); - - - - ffController = new ArmFeedforward( - Settings.Arm.FF.kS, - Settings.Arm.FF.kG, - Settings.Arm.FF.kV, - Settings.Arm.FF.kA); - - } - - public void setTargetAngle(double targetAngle) { - this.targetAngle.set(targetAngle); - } - - public double getTargetAngle() { - return targetAngle.getAsDouble(); - } - - - public double getArmAngle() { - return armMotor.getPosition().getValueAsDouble() * 360; - } - - @Override - public void periodic() { - armMotor.setVoltage( - pidController.calculate( - getArmAngle(), - getTargetAngle() - ) - + - ffController.calculate( - getArmAngle(), - armMotor.getVelocity().getValueAsDouble()) - ); - SmartDashboard.putNumber("Arm/Arm Angle", getArmAngle()); - } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java new file mode 100644 index 0000000..a427bdf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -0,0 +1,149 @@ +package com.stuypulse.robot.subsystems.arm; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.revrobotics.AbsoluteEncoder; +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ArmImpl extends Arm { + + /* + * GOODJOB GUYS!! I BELIEVE IN YOU!! + * FIELDS THAT WE NEED + * - Kraken Motor done + * - Target angle done + * - Absolute Encoder no + * - PID controller + * - FF controller + * - instance done + */ + + + private static ArmImpl instance; + + private SmartNumber targetAngle; + private SmartNumber targetVoltage; + + + private TalonFX armMotor; + //ks, kv, ka, kg + + // private final ProfiledPIDController pidController; + //private final ArmFeedforward ffController; + + static { + instance = new ArmImpl(); + } + + public ArmImpl() { + + // super(); + private final TalonFXConfiguration config = new TalonFXConfiguration(); + targetAngle = new SmartNumber("Arm/Target Angle", 0.0); + armMotor = new TalonFX(Ports.Arm.ARM_MOTOR); + + Slot0Configs slot0 = new Slot0Configs(); + + config.Slot0 = slot0; + + slot0.kP = Settings.Arm.PID.kP; + slot0.kI = Settings.Arm.PID.kI; + slot0.kD = Settings.Arm.PID.kD; + + slot0.kS = Settings.Arm.FF.kS; + slot0.kV = Settings.Arm.FF.kV; + slot0.kA = Settings.Arm.FF.kA; + slot0.kG = Settings.Arm.FF.kG; + + + MotionMagicConfig motionMagicConfigs = talonFXConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = Settings.Arm.MotionMagic.MAX_VEL; // Target cruise velocity of 80 rps + motionMagicConfigs.MotionMagicAcceleration = Settings.Arm.MotionMagic.MAX_ACCEL; // Target acceleration of 160 rps/s (0.5 seconds) + + // actual gooner code + + + + // pidController = new ProfiledPIDController( + // Settings.Arm.PID.kP, + // Settings.Arm.PID.kI, + // Settings.Arm.PID.kD, + // new Constraints( + // Settings.Arm.MAX_VEL, + // Settings.Arm.MAX_ACCEL + // ) + // ); + + // pidController.enableContinuousInput(-180,180); + + + + // ffController = new ArmFeedforward( + // Settings.Arm.FF.kS, + // Settings.Arm.FF.kG, + // Settings.Arm.FF.kV, + // Settings.Arm.FF.kA); + + } + + + public static ArmImpl getInstance() { + return instance; + } + + public void setTargetAngle(double targetAngle) { + this.targetAngle.set(targetAngle); + } + + public double getTargetAngle() { + return targetAngle.getAsDouble(); + } + + + public double getArmAngle() { + return armMotor.getPosition().getValueAsDouble() * 360; + } + + // public void setPID(double kP, double kI, double kD) { + // config.Slot0.kP = kP; + // config.Slot0.kI = kI; + // config.Slot0.kD = kD; + + // } + + // public void setFF(double kS, double kV, double kA, double kG ){ + // //delete please + // //this is not what you do + // config.Slot0.kS = kS; + // config.Slot0.kV = kV; + // config.Slot0.kA = kA; + // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + + + + + // } + + public void setVoltage(double voltage) { + + } + + + + @Override + public void periodic() { + final MotionMagicVelocityVoltage m_request = new MotionMagicVelocityVoltage(Settings.Arm.); + + } +} + From a12406ff93969d79b2cac6639e5a5d366610a3a6 Mon Sep 17 00:00:00 2001 From: Baiulus Date: Fri, 31 Jan 2025 18:51:33 -0500 Subject: [PATCH 14/34] implemented motion magic, configured motors, used bstream, added logging, and changed visualizer Co-authored-by: simon Co-authored-by: EvanK-h Co-authored-by: Ardian Agoes Co-authored-by: qingque6 Co-authored-by: BrianZ60 Co-authored-by: Matthew Cheng Co-authored-by: Zixi Qiao Co-authored-by: ryanyu32 Co-authored-by: shiva4503 --- networktables.json | 1 + simgui-ds.json | 92 ++++++++++++++++++ simgui.json | 29 ++++++ .../com/stuypulse/robot/RobotContainer.java | 2 + .../commands/elevator/ElevatorToHeight.java | 12 ++- .../stuypulse/robot/constants/Settings.java | 11 +++ .../robot/subsystems/elevator/Elevator.java | 2 + .../subsystems/elevator/ElevatorImpl.java | 94 +++++++++++-------- .../subsystems/elevator/ElevatorSimu.java | 13 ++- .../elevator/ElevatorVisualizer.java | 15 ++- 10 files changed, 223 insertions(+), 48 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..d705b1b --- /dev/null +++ b/simgui.json @@ -0,0 +1,29 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Autonomous": "String Chooser", + "/SmartDashboard/Visualizers/Elevator": "Mechanism2d" + }, + "windows": { + "/SmartDashboard/Visualizers/Elevator": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Elevator": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5b55349..49fe7f1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.robot.subsystems.elevator.Elevator; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -21,6 +22,7 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem + public final Elevator elevator = Elevator.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java index f8a6da0..28baf31 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java @@ -1,21 +1,31 @@ package com.stuypulse.robot.commands.elevator; +import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.subsystems.elevator.Elevator; +import com.stuypulse.stuylib.streams.booleans.BStream; import edu.wpi.first.wpilibj2.command.InstantCommand; public class ElevatorToHeight extends InstantCommand { private final Elevator elevator; private final double targetHeight; + private final BStream belowTop, aboveBottom; public ElevatorToHeight(double targetHeight){ elevator = Elevator.getInstance(); this.targetHeight = targetHeight; + belowTop = BStream.create(() -> !elevator.atTop()) + .and(() -> targetHeight < Constants.Elevator.MAX_HEIGHT_METERS); + aboveBottom = BStream.create(() -> !elevator.atBottom()) + .and(() -> targetHeight > Constants.Elevator.MIN_HEIGHT_METERS); + addRequirements(elevator); } public void initialize(){ - elevator.setTargetHeight(targetHeight); + if (belowTop.get() && aboveBottom.get()) { + elevator.setTargetHeight(targetHeight); + } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e6bf627..d36719b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -30,6 +30,17 @@ public interface Elevator { double FEED_HEIGHT_METERS = 0.4; + double RAMP_RATE = 0.1; + + // FIND OUT REAL GEAR RATIO + double GEAR_RATIO = 1.0/5.0; + double CURRENT_LIMIT = 5.0; + + // Magic motion, change RPS + double TARGET_CRUISE_VELOCITY = 0.0; //Rotations Per Second + double TARGET_ACCELERATION = 0.0; //Rotations Per Second^2 + double TARGET_JERK = 0.0; //Rotations Per Second^3 + SmartNumber HEIGHT_TOLERANCE_METERS = new SmartNumber("Elevator/Height Tolerance (m)", 0.02); public interface PID { diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java index a0e1195..52864cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -30,6 +30,8 @@ public Elevator() { public abstract double getTargetHeight(); public abstract double getCurrentHeight(); public abstract boolean atTargetHeight(); + public abstract boolean atTop(); + public abstract boolean atBottom(); public void periodic() { visualizer.update(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java index 3e5f4ba..787838e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -1,48 +1,65 @@ package com.stuypulse.robot.subsystems.elevator; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.feedback.PIDController; -import com.stuypulse.stuylib.control.feedforward.ElevatorFeedforward; -import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; -import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ElevatorImpl extends Elevator { private final TalonFX motor; - - private final DigitalInput bottomBumpSwitch; - private final DigitalInput topBumpSwitch; - private final SmartNumber targetHeight; - - private final Controller controller; - - private boolean hasBeenReset; + private final DigitalInput bumpSwitchTop, bumpSwitchBottom; public ElevatorImpl() { motor = new TalonFX(Ports.Elevator.MOTOR); - - bottomBumpSwitch = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); - topBumpSwitch = new DigitalInput(Ports.Elevator.TOP_SWITCH); - targetHeight = new SmartNumber("Elevator/Target Height", Constants.Elevator.MIN_HEIGHT_METERS); - MotionProfile motionProfile = new MotionProfile(Settings.Elevator.MAX_VELOCITY_METERS_PER_SECOND, Settings.Elevator.MAX_ACCEL_METERS_PER_SECOND_PER_SECOND); + bumpSwitchBottom = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); + bumpSwitchTop = new DigitalInput(Ports.Elevator.TOP_SWITCH); - controller = new MotorFeedforward(Settings.Elevator.FF.kS, Settings.Elevator.FF.kV, Settings.Elevator.FF.kA).position() - .add(new ElevatorFeedforward(Settings.Elevator.FF.kG)) - .add(new PIDController(Settings.Elevator.PID.kP, Settings.Elevator.PID.kI, Settings.Elevator.PID.kD)) - .setSetpointFilter(motionProfile); + TalonFXConfiguration elevatorMotorConfig = new TalonFXConfiguration(); + + Slot0Configs slot0 = new Slot0Configs(); + + slot0.kS = Settings.Elevator.FF.kS.getAsDouble(); + slot0.kV = Settings.Elevator.FF.kV.getAsDouble(); + slot0.kA = Settings.Elevator.FF.kA.getAsDouble(); + slot0.kG = Settings.Elevator.FF.kG.getAsDouble(); + slot0.kP = Settings.Elevator.PID.kP.getAsDouble(); + slot0.kI = Settings.Elevator.PID.kI.getAsDouble(); + slot0.kD = Settings.Elevator.PID.kD.getAsDouble(); + + elevatorMotorConfig.Slot0 = slot0; - hasBeenReset = false; + // Base motor configs + elevatorMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + elevatorMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + elevatorMotorConfig.Feedback.SensorToMechanismRatio = Settings.Elevator.GEAR_RATIO; + + // Current limiting + elevatorMotorConfig.CurrentLimits.StatorCurrentLimit = Settings.Elevator.CURRENT_LIMIT; + elevatorMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + // Ramp motor voltage + elevatorMotorConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE; + elevatorMotorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE; + + var elevatorMotionMagicConfigs = elevatorMotorConfig.MotionMagic; + elevatorMotionMagicConfigs.MotionMagicCruiseVelocity = Settings.Elevator.TARGET_CRUISE_VELOCITY; + elevatorMotionMagicConfigs.MotionMagicAcceleration = Settings.Elevator.TARGET_ACCELERATION; + elevatorMotionMagicConfigs.MotionMagicJerk = Settings.Elevator.TARGET_JERK; + + motor.getConfigurator().apply(elevatorMotorConfig); } @Override @@ -71,31 +88,26 @@ public boolean atTargetHeight() { return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); } - private void setVoltage(double voltage) { - motor.setVoltage(voltage); + @Override + public boolean atBottom() { + return !bumpSwitchBottom.get(); + } + + @Override + public boolean atTop() { + return !bumpSwitchTop.get(); } @Override public void periodic() { super.periodic(); - if (bottomBumpSwitch.get()) { - hasBeenReset = true; - motor.setPosition(0); - } - - if (topBumpSwitch.get()) { - hasBeenReset = true; - motor.setPosition(Constants.Elevator.MAX_HEIGHT_METERS / Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR); - } - - if (!hasBeenReset) { - setVoltage(-1); - } else { - controller.update(getTargetHeight(), getCurrentHeight()); - setVoltage(controller.getOutput()); - } + final MotionMagicVoltage controlRequest = new MotionMagicVoltage(getTargetHeight()/Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR); + motor.setControl(controlRequest); SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); + SmartDashboard.putNumber("Climb/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Climb/Motor Current", motor.getStatorCurrent().getValueAsDouble()); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java index 1b54558..f0e396e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java @@ -74,7 +74,18 @@ public double getCurrentHeight() { public boolean atTargetHeight() { return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); } + + // atTop() and atBottom() are unused + @Override + public boolean atTop() { + return false; + } + @Override + public boolean atBottom() { + return false; + } + @Override public void periodic() { super.periodic(); @@ -84,7 +95,7 @@ public void periodic() { sim.update(Settings.DT); RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); - ElevatorVisualizer.getInstance().update(); + ElevatorVisualizer.getInstance().update(); // delete this line later SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java index 623ee97..bf7eb98 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java @@ -176,11 +176,16 @@ public void update() { // Top of Carriage is Target Height Elevator elevator = Elevator.getInstance(); - outerBL.setPosition(Units.inchesToMeters(3), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) * Settings.Elevator.Simulation.SCALE_FACTOR + Constants.Elevator.MIN_HEIGHT_METERS); - outerTR.setPosition(Units.inchesToMeters(12), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) * Settings.Elevator.Simulation.SCALE_FACTOR + Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); - - innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); - innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); + if (elevator.getTargetHeight() <= Units.inchesToMeters(47)) { + innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); + innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); + } else { + outerBL.setPosition(Units.inchesToMeters(3), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS)); + outerTR.setPosition(Units.inchesToMeters(12), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) + Units.inchesToMeters(47)); + + innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); + innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); + } } From 2534b33be18e812bb024d2625f216aefb08ce7da Mon Sep 17 00:00:00 2001 From: Mustafa A Date: Fri, 31 Jan 2025 19:09:06 -0500 Subject: [PATCH 15/34] arm impl done, will clean up later --- .../robot/commands/arm/ArmMoveToFunnel.java | 0 .../robot/commands/arm/ArmMoveToL2.java | 5 +- .../robot/commands/arm/ArmMoveToL3.java | 5 +- .../robot/commands/arm/ArmMoveToL4.java | 5 +- .../com/stuypulse/robot/constants/Ports.java | 1 + .../stuypulse/robot/constants/Settings.java | 5 +- .../stuypulse/robot/subsystems/arm/Arm.java | 12 +++ .../robot/subsystems/arm/ArmImpl.java | 78 +++++++++---------- 8 files changed, 63 insertions(+), 48 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java index cbc5ff8..4cd45ef 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java @@ -7,5 +7,8 @@ public class ArmMoveToL2 extends ArmMoveToAngle{ public ArmMoveToL2(){ super(Settings.Arm.L2_ANGLE); } - + @Override + public void initialize(){ + super.initialize(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java index e9b028d..703f203 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java @@ -7,5 +7,8 @@ public class ArmMoveToL3 extends ArmMoveToAngle{ public ArmMoveToL3(){ super(Settings.Arm.L3_ANGLE); } - + @Override + public void initialize(){ + super.initialize(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java index bf38853..8b61d9a 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java @@ -7,5 +7,8 @@ public class ArmMoveToL4 extends ArmMoveToAngle{ public ArmMoveToL4(){ super(Settings.Arm.L4_ANGLE); } - + @Override + public void initialize(){ + super.initialize(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 0f63212..54c4488 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -15,5 +15,6 @@ public interface Gamepad { public interface Arm { int ARM_MOTOR = 0; + int ARM_ENCODER = 0; } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e1dea64..9706f85 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -18,8 +18,7 @@ public interface Settings { public interface Arm { - double MAX_VEL = 0; - double MAX_ACCEL = 0; + public interface PID { double kP = 0; double kI = 0; @@ -37,6 +36,8 @@ public interface FF { double L2_ANGLE = 0; double L3_ANGLE = 0; double L4_ANGLE = 0; + double FUNNEL_ANGLE = 0; + double GEAR_RATIO = 0; public interface MotionMagic{ double MAX_VEL = 0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 18425f0..3ad2f36 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -5,8 +5,20 @@ public abstract class Arm extends SubsystemBase { + public static final Arm instance; + + static { + instance = new ArmImpl(); + } public static Arm getInstance() { return instance; } + public abstract void setTargetAngle(double TargetAngle); + + public abstract double getTargetAngle(); + + public abstract double getArmAngle(); + + // public abstract void setVoltage(double voltage); } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index a427bdf..9ec3449 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -1,16 +1,20 @@ package com.stuypulse.robot.subsystems.arm; + +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.GravityTypeValue; import com.revrobotics.AbsoluteEncoder; import com.stuypulse.stuylib.network.SmartNumber; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -18,7 +22,6 @@ public class ArmImpl extends Arm { /* - * GOODJOB GUYS!! I BELIEVE IN YOU!! * FIELDS THAT WE NEED * - Kraken Motor done * - Target angle done @@ -32,29 +35,25 @@ public class ArmImpl extends Arm { private static ArmImpl instance; private SmartNumber targetAngle; - private SmartNumber targetVoltage; - private TalonFX armMotor; - //ks, kv, ka, kg - - // private final ProfiledPIDController pidController; - //private final ArmFeedforward ffController; + private CANcoder armEncoder; static { instance = new ArmImpl(); } public ArmImpl() { + // super(); - private final TalonFXConfiguration config = new TalonFXConfiguration(); targetAngle = new SmartNumber("Arm/Target Angle", 0.0); armMotor = new TalonFX(Ports.Arm.ARM_MOTOR); - - Slot0Configs slot0 = new Slot0Configs(); + armEncoder = new CANcoder(Ports.Arm.ARM_ENCODER); - config.Slot0 = slot0; + TalonFXConfiguration config = new TalonFXConfiguration(); + + Slot0Configs slot0 = new Slot0Configs(); slot0.kP = Settings.Arm.PID.kP; slot0.kI = Settings.Arm.PID.kI; @@ -65,34 +64,26 @@ public ArmImpl() { slot0.kA = Settings.Arm.FF.kA; slot0.kG = Settings.Arm.FF.kG; - - MotionMagicConfig motionMagicConfigs = talonFXConfigs.MotionMagic; + config.Slot0 = slot0; + config.Feedback.SensorToMechanismRatio = Settings.Arm.GEAR_RATIO; + config.Feedback.FeedbackRemoteSensorID = armEncoder.getDeviceID(); + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + + + MotionMagicConfigs motionMagicConfigs = config.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = Settings.Arm.MotionMagic.MAX_VEL; // Target cruise velocity of 80 rps motionMagicConfigs.MotionMagicAcceleration = Settings.Arm.MotionMagic.MAX_ACCEL; // Target acceleration of 160 rps/s (0.5 seconds) - // actual gooner code - - - - // pidController = new ProfiledPIDController( - // Settings.Arm.PID.kP, - // Settings.Arm.PID.kI, - // Settings.Arm.PID.kD, - // new Constraints( - // Settings.Arm.MAX_VEL, - // Settings.Arm.MAX_ACCEL - // ) - // ); - - // pidController.enableContinuousInput(-180,180); - - + config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.0; //set as constant later + config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.0; //set as constant later + + + armMotor.getConfigurator().apply(config); + armMotor.getConfigurator().apply(motionMagicConfigs); - // ffController = new ArmFeedforward( - // Settings.Arm.FF.kS, - // Settings.Arm.FF.kG, - // Settings.Arm.FF.kV, - // Settings.Arm.FF.kA); + + // actual gooner code } @@ -122,8 +113,6 @@ public double getArmAngle() { // } // public void setFF(double kS, double kV, double kA, double kG ){ - // //delete please - // //this is not what you do // config.Slot0.kS = kS; // config.Slot0.kV = kV; // config.Slot0.kA = kA; @@ -134,15 +123,18 @@ public double getArmAngle() { // } - public void setVoltage(double voltage) { - - } + + @Override public void periodic() { - final MotionMagicVelocityVoltage m_request = new MotionMagicVelocityVoltage(Settings.Arm.); + + final MotionMagicVelocityVoltage m_request = new MotionMagicVelocityVoltage(Settings.Arm.MotionMagic.MAX_VEL); + armMotor.setControl(m_request); + + } } From 9506dfedd0588d76704a36fc469a811025d9454f Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Sat, 1 Feb 2025 11:21:13 -0500 Subject: [PATCH 16/34] fixed logging names --- .../com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java index 787838e..f87ffd3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -106,8 +106,8 @@ public void periodic() { motor.setControl(controlRequest); SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); - SmartDashboard.putNumber("Climb/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Climb/Motor Current", motor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Elevator/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Elevator/Motor Current", motor.getStatorCurrent().getValueAsDouble()); } } \ No newline at end of file From ae6b873de8c67c0acd79203b03283b5305bc4c37 Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Sat, 1 Feb 2025 11:27:33 -0500 Subject: [PATCH 17/34] fixed commands --- .../stuypulse/robot/commands/elevator/ElevatorToBottom.java | 4 ++++ .../stuypulse/robot/commands/elevator/ElevatorToHandoff.java | 4 ++++ .../com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java | 4 ++++ .../com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java | 4 ++++ .../com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java | 4 ++++ .../com/stuypulse/robot/commands/elevator/ElevatorToTop.java | 4 ++++ 6 files changed, 24 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java index bc6629b..0176469 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java @@ -6,4 +6,8 @@ public class ElevatorToBottom extends ElevatorToHeight{ public ElevatorToBottom(){ super(Elevator.MIN_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java index c8e6f39..47e289b 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java @@ -6,4 +6,8 @@ public class ElevatorToHandoff extends ElevatorToHeight{ public ElevatorToHandoff() { super(Elevator.HANDOFF_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java index 1d861e6..20028fa 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java @@ -6,4 +6,8 @@ public class ElevatorToLvl2 extends ElevatorToHeight{ public ElevatorToLvl2(){ super(Elevator.L2_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java index ff4a056..d0ea7ad 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java @@ -6,4 +6,8 @@ public class ElevatorToLvl3 extends ElevatorToHeight{ public ElevatorToLvl3(){ super(Elevator.L3_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java index d674636..db9b49b 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java @@ -5,4 +5,8 @@ public class ElevatorToLvl4 extends ElevatorToHeight{ public ElevatorToLvl4() { super(Elevator.L4_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java index bba0cb4..2260061 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java @@ -6,5 +6,9 @@ public class ElevatorToTop extends ElevatorToHeight{ public ElevatorToTop() { super(Constants.Elevator.MAX_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } From 58a11328b336f837a1c90427f8402aae39c525e9 Mon Sep 17 00:00:00 2001 From: Ricky Zheng Date: Sat, 1 Feb 2025 12:07:47 -0500 Subject: [PATCH 18/34] added new commands(L2-L4 back and barge), new constants(L2-L4 back, barge), added logging in periodic --- .../robot/commands/arm/ArmMoveToFunnel.java | 14 ++++++++++++++ .../robot/commands/arm/ArmMoveToL2Back.java | 13 +++++++++++++ .../{ArmMoveToL2.java => ArmMoveToL2Front.java} | 6 +++--- .../robot/commands/arm/ArmMoveToL3Back.java | 13 +++++++++++++ .../{ArmMoveToL3.java => ArmMoveToL3Front.java} | 6 +++--- .../robot/commands/arm/ArmMoveToL4Back.java | 13 +++++++++++++ .../{ArmMoveToL4.java => ArmMoveToL4Front.java} | 6 +++--- .../com/stuypulse/robot/constants/Settings.java | 12 +++++++++--- .../stuypulse/robot/subsystems/arm/ArmImpl.java | 8 ++++---- 9 files changed, 75 insertions(+), 16 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Back.java rename src/main/java/com/stuypulse/robot/commands/arm/{ArmMoveToL2.java => ArmMoveToL2Front.java} (57%) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Back.java rename src/main/java/com/stuypulse/robot/commands/arm/{ArmMoveToL3.java => ArmMoveToL3Front.java} (57%) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Back.java rename src/main/java/com/stuypulse/robot/commands/arm/{ArmMoveToL4.java => ArmMoveToL4Front.java} (57%) diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java index e69de29..6a8fcb7 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java @@ -0,0 +1,14 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.constants.Settings; + + +public class ArmMoveToFunnel extends ArmMoveToAngle{ + public ArmMoveToFunnel(){ + super(Settings.Arm.FUNNEL_ANGLE); + } + @Override + public void initialize(){ + super.initialize(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Back.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Back.java new file mode 100644 index 0000000..7085b34 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Back.java @@ -0,0 +1,13 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.constants.Settings; + +public class ArmMoveToL2Back extends ArmMoveToAngle{ + public ArmMoveToL2Back(){ + super(Settings.Arm.L2_ANGLE_BACK); + } + @Override + public void initialize(){ + super.initialize(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Front.java similarity index 57% rename from src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java rename to src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Front.java index 4cd45ef..afe2b89 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Front.java @@ -3,9 +3,9 @@ import com.stuypulse.robot.constants.Settings; -public class ArmMoveToL2 extends ArmMoveToAngle{ - public ArmMoveToL2(){ - super(Settings.Arm.L2_ANGLE); +public class ArmMoveToL2Front extends ArmMoveToAngle{ + public ArmMoveToL2Front(){ + super(Settings.Arm.L2_ANGLE_FRONT); } @Override public void initialize(){ diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Back.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Back.java new file mode 100644 index 0000000..c7bd058 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Back.java @@ -0,0 +1,13 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.constants.Settings; + +public class ArmMoveToL3Back extends ArmMoveToAngle { + public ArmMoveToL3Back(){ + super(Settings.Arm.L3_ANGLE_BACK); + } + @Override + public void initialize(){ + super.initialize(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Front.java similarity index 57% rename from src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java rename to src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Front.java index 703f203..0ee2625 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Front.java @@ -3,9 +3,9 @@ import com.stuypulse.robot.constants.Settings; -public class ArmMoveToL3 extends ArmMoveToAngle{ - public ArmMoveToL3(){ - super(Settings.Arm.L3_ANGLE); +public class ArmMoveToL3Front extends ArmMoveToAngle{ + public ArmMoveToL3Front(){ + super(Settings.Arm.L3_ANGLE_FRONT); } @Override public void initialize(){ diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Back.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Back.java new file mode 100644 index 0000000..1ebcdde --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Back.java @@ -0,0 +1,13 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.constants.Settings; + +public class ArmMoveToL4Back extends ArmMoveToAngle { + public ArmMoveToL4Back(){ + super(Settings.Arm.L4_ANGLE_BACK); + } + @Override + public void initialize(){ + super.initialize(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Front.java similarity index 57% rename from src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java rename to src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Front.java index 8b61d9a..7fed96f 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Front.java @@ -3,9 +3,9 @@ import com.stuypulse.robot.constants.Settings; -public class ArmMoveToL4 extends ArmMoveToAngle{ - public ArmMoveToL4(){ - super(Settings.Arm.L4_ANGLE); +public class ArmMoveToL4Front extends ArmMoveToAngle{ + public ArmMoveToL4Front(){ + super(Settings.Arm.L4_ANGLE_FRONT); } @Override public void initialize(){ diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9706f85..71b3205 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -33,11 +33,17 @@ public interface FF { double kG = 0; } - double L2_ANGLE = 0; - double L3_ANGLE = 0; - double L4_ANGLE = 0; + double L2_ANGLE_FRONT = 0; + double L3_ANGLE_FRONT = 0; + double L4_ANGLE_FRONT = 0; + + double L2_ANGLE_BACK = 0; + double L3_ANGLE_BACK = 0; + double L4_ANGLE_BACK = 0; + double FUNNEL_ANGLE = 0; double GEAR_RATIO = 0; + double BARGE_ANGLE = 0; public interface MotionMagic{ double MAX_VEL = 0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 9ec3449..61104c9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -39,9 +39,7 @@ public class ArmImpl extends Arm { private TalonFX armMotor; private CANcoder armEncoder; - static { - instance = new ArmImpl(); - } + public ArmImpl() { @@ -67,7 +65,7 @@ public ArmImpl() { config.Slot0 = slot0; config.Feedback.SensorToMechanismRatio = Settings.Arm.GEAR_RATIO; config.Feedback.FeedbackRemoteSensorID = armEncoder.getDeviceID(); - config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; MotionMagicConfigs motionMagicConfigs = config.MotionMagic; @@ -134,6 +132,8 @@ public void periodic() { final MotionMagicVelocityVoltage m_request = new MotionMagicVelocityVoltage(Settings.Arm.MotionMagic.MAX_VEL); armMotor.setControl(m_request); + SmartDashboard.putNumber("Arm/targetAngle", getTargetAngle()); + SmartDashboard.putNumber("Arm/currentAngle",getArmAngle()); } From e500491e3f4a929cef9890e38cd282329af059af Mon Sep 17 00:00:00 2001 From: Mustafa Abdullah Date: Sat, 1 Feb 2025 14:02:05 -0500 Subject: [PATCH 19/34] changed angles to Rotation2ds, fixed periodic, added GravityType, changed commands to use Rotation2d --- .../robot/commands/arm/ArmMoveToAngle.java | 6 +- .../stuypulse/robot/constants/Settings.java | 23 ++++--- .../stuypulse/robot/subsystems/arm/Arm.java | 8 ++- .../robot/subsystems/arm/ArmImpl.java | 68 ++++++------------- 4 files changed, 41 insertions(+), 64 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java index ea2ca0e..d8f1a06 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java @@ -1,14 +1,16 @@ package com.stuypulse.robot.commands.arm; import com.stuypulse.robot.subsystems.arm.Arm; + +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; public class ArmMoveToAngle extends InstantCommand{ private final Arm arm; - private final double angle; + private final Rotation2d angle; - public ArmMoveToAngle(double angle){ + public ArmMoveToAngle(Rotation2d angle){ arm = Arm.getInstance(); this.angle = angle; } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 71b3205..8f22dcb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,8 +5,8 @@ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartBoolean; -import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.math.geometry.Rotation2d; /*- * File containing tunable settings for every subsystem on the robot. @@ -33,17 +33,20 @@ public interface FF { double kG = 0; } - double L2_ANGLE_FRONT = 0; - double L3_ANGLE_FRONT = 0; - double L4_ANGLE_FRONT = 0; + Rotation2d L2_ANGLE_FRONT = Rotation2d.fromDegrees(0); + Rotation2d L3_ANGLE_FRONT = Rotation2d.fromDegrees(0); + Rotation2d L4_ANGLE_FRONT = Rotation2d.fromDegrees(0); - double L2_ANGLE_BACK = 0; - double L3_ANGLE_BACK = 0; - double L4_ANGLE_BACK = 0; + Rotation2d L2_ANGLE_BACK = Rotation2d.fromDegrees(0); + Rotation2d L3_ANGLE_BACK = Rotation2d.fromDegrees(0); + Rotation2d L4_ANGLE_BACK = Rotation2d.fromDegrees(0); - double FUNNEL_ANGLE = 0; + Rotation2d FUNNEL_ANGLE = Rotation2d.fromDegrees(0); double GEAR_RATIO = 0; - double BARGE_ANGLE = 0; + Rotation2d BARGE_ANGLE = Rotation2d.fromDegrees(0); + double OFFSET = 0; + double PID_RAMPING = 0; + double FF_RAMPING = 0; public interface MotionMagic{ double MAX_VEL = 0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 3ad2f36..c133e93 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -1,5 +1,6 @@ package com.stuypulse.robot.subsystems.arm; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -10,15 +11,16 @@ public abstract class Arm extends SubsystemBase { static { instance = new ArmImpl(); } + public static Arm getInstance() { return instance; } - public abstract void setTargetAngle(double TargetAngle); + public abstract void setTargetAngle(Rotation2d TargetAngle); - public abstract double getTargetAngle(); + public abstract Rotation2d getTargetAngle(); - public abstract double getArmAngle(); + public abstract Rotation2d getArmAngle(); // public abstract void setVoltage(double voltage); } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 61104c9..85f16b4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -6,35 +6,18 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.GravityTypeValue; -import com.revrobotics.AbsoluteEncoder; -import com.stuypulse.stuylib.network.SmartNumber; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ArmImpl extends Arm { - /* - * FIELDS THAT WE NEED - * - Kraken Motor done - * - Target angle done - * - Absolute Encoder no - * - PID controller - * - FF controller - * - instance done - */ - - - private static ArmImpl instance; - - private SmartNumber targetAngle; + private Rotation2d targetAngle; private TalonFX armMotor; @@ -43,9 +26,7 @@ public class ArmImpl extends Arm { public ArmImpl() { - - // super(); - targetAngle = new SmartNumber("Arm/Target Angle", 0.0); + targetAngle = Rotation2d.fromDegrees(0.0); armMotor = new TalonFX(Ports.Arm.ARM_MOTOR); armEncoder = new CANcoder(Ports.Arm.ARM_ENCODER); @@ -61,20 +42,23 @@ public ArmImpl() { slot0.kV = Settings.Arm.FF.kV; slot0.kA = Settings.Arm.FF.kA; slot0.kG = Settings.Arm.FF.kG; - + slot0.GravityType = GravityTypeValue.Arm_Cosine; + config.Slot0 = slot0; config.Feedback.SensorToMechanismRatio = Settings.Arm.GEAR_RATIO; + // config.Feedback.RotorToSensorRatio = 0.0; config.Feedback.FeedbackRemoteSensorID = armEncoder.getDeviceID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + MotionMagicConfigs motionMagicConfigs = config.MotionMagic; motionMagicConfigs.MotionMagicCruiseVelocity = Settings.Arm.MotionMagic.MAX_VEL; // Target cruise velocity of 80 rps motionMagicConfigs.MotionMagicAcceleration = Settings.Arm.MotionMagic.MAX_ACCEL; // Target acceleration of 160 rps/s (0.5 seconds) - config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.0; //set as constant later - config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.0; //set as constant later + config.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Arm.PID_RAMPING; + config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Arm.FF_RAMPING; armMotor.getConfigurator().apply(config); @@ -85,22 +69,17 @@ public ArmImpl() { } - - public static ArmImpl getInstance() { - return instance; - } - - public void setTargetAngle(double targetAngle) { - this.targetAngle.set(targetAngle); + public void setTargetAngle(Rotation2d targetAngle) { + this.targetAngle = targetAngle; } - public double getTargetAngle() { - return targetAngle.getAsDouble(); + public Rotation2d getTargetAngle() { + return targetAngle; } - public double getArmAngle() { - return armMotor.getPosition().getValueAsDouble() * 360; + public Rotation2d getArmAngle() { + return Rotation2d.fromRotations(armMotor.getPosition().getValueAsDouble() - Settings.Arm.OFFSET); } // public void setPID(double kP, double kI, double kD) { @@ -116,26 +95,17 @@ public double getArmAngle() { // config.Slot0.kA = kA; // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - - - // } - - - - - @Override public void periodic() { - final MotionMagicVelocityVoltage m_request = new MotionMagicVelocityVoltage(Settings.Arm.MotionMagic.MAX_VEL); - armMotor.setControl(m_request); + armMotor.setControl(new PositionVoltage(getTargetAngle().getRotations())); - SmartDashboard.putNumber("Arm/targetAngle", getTargetAngle()); - SmartDashboard.putNumber("Arm/currentAngle",getArmAngle()); - + SmartDashboard.putNumber("Arm/targetAngle", getTargetAngle().getDegrees()); + SmartDashboard.putNumber("Arm/currentAngle",getArmAngle().getDegrees()); + } } From c02548d656b4e2887efec3ceb46dcaccc5dfed5a Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Sat, 1 Feb 2025 15:06:46 -0500 Subject: [PATCH 20/34] Fixed code review Co-authored-by: Weifen Chen Co-authored-by: Rahul Deb --- .../com/stuypulse/robot/RobotContainer.java | 4 +- .../commands/funnel/FunnelDefaultCommand.java | 30 ++--- .../robot/commands/funnel/FunnelStop.java | 18 +++ .../ShooterAcquireAlgae.java | 10 +- .../lokishooter/ShooterAcquireCoral.java | 21 ++++ .../ShooterDeacquireAlgae.java | 10 +- .../lokishooter/ShooterShootBack.java | 24 ++++ .../lokishooter/ShooterShootFront.java | 24 ++++ .../{shooter => lokishooter}/ShooterStop.java | 10 +- .../robot/commands/shooter/ShooterShoot.java | 24 ---- .../stuypulse/robot/constants/Settings.java | 56 +++------ .../robot/subsystems/funnel/CoralFunnel.java | 22 +--- .../subsystems/funnel/CoralFunnelImpl.java | 93 +++++++-------- .../subsystems/lokishooter/LokiShooter.java | 24 ++++ .../lokishooter/LokiShooterImpl.java | 89 +++++++++++++++ .../subsystems/shooter/CoralShooter.java | 48 -------- .../subsystems/shooter/CoralShooterImpl.java | 107 ------------------ 17 files changed, 289 insertions(+), 325 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java rename src/main/java/com/stuypulse/robot/commands/{shooter => lokishooter}/ShooterAcquireAlgae.java (52%) create mode 100644 src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java rename src/main/java/com/stuypulse/robot/commands/{shooter => lokishooter}/ShooterDeacquireAlgae.java (52%) create mode 100644 src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java create mode 100644 src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java rename src/main/java/com/stuypulse/robot/commands/{shooter => lokishooter}/ShooterStop.java (50%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterShoot.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooter.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5e2e521..7b3dea0 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -9,7 +9,7 @@ import com.stuypulse.robot.commands.funnel.FunnelDefaultCommand; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.funnel.CoralFunnel; -import com.stuypulse.robot.subsystems.shooter.CoralShooter; +import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -25,7 +25,7 @@ public class RobotContainer { // Subsystem private final CoralFunnel funnel = CoralFunnel.getInstance(); - private final CoralShooter shooter = CoralShooter.getInstance(); + private final LokiShooter shooter = LokiShooter.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java index 1cbd6c0..106fd07 100644 --- a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java @@ -8,7 +8,6 @@ public class FunnelDefaultCommand extends Command { private final CoralFunnel funnel; private boolean reversed = false; - private boolean funnelStopped = false; public FunnelDefaultCommand() { funnel = CoralFunnel.getInstance(); @@ -17,35 +16,26 @@ public FunnelDefaultCommand() { @Override public void execute() { - boolean coralDetected = funnel.getFunnelState(); + setState(); - if(coralDetected && !funnelStopped) { - funnelStopped = true; - } - - if(funnelStopped && !coralDetected) { - funnelStopped = false; - } + changeState(); + } - if(funnel.coralStuck() && !reversed) { + private void setState(){ + if(funnel.isStalling() && !reversed) { reversed = true; new WaitCommand(1).andThen(() -> { reversed = false; }).schedule(); } + } - if (funnelStopped){ - funnel.setMotorRPM(0); - } else if (reversed){ - funnel.setMotorRPM(-funnel.getTargetRPM()); + private void changeState(){ + if (reversed){ + funnel.reverse(); } else { - funnel.setMotorRPM(funnel.getTargetRPM()); + funnel.forward(); } } - - @Override - public void end(boolean interrupted) { - funnel.setMotorRPM(0); - } } diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java new file mode 100644 index 0000000..68ea499 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java @@ -0,0 +1,18 @@ +package com.stuypulse.robot.commands.funnel; + +import com.stuypulse.robot.subsystems.funnel.CoralFunnel; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class FunnelStop extends InstantCommand { + private final CoralFunnel funnel; + public FunnelStop() { + funnel = CoralFunnel.getInstance(); + addRequirements(funnel); + } + + @Override + public void initialize(){ + funnel.stop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java similarity index 52% rename from src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java rename to src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java index f412562..bca6ff0 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java @@ -1,21 +1,21 @@ -package com.stuypulse.robot.commands.shooter; +package com.stuypulse.robot.commands.lokishooter; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.shooter.CoralShooter; +import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; import edu.wpi.first.wpilibj2.command.InstantCommand; public class ShooterAcquireAlgae extends InstantCommand { - private final CoralShooter shooter; + private final LokiShooter shooter; public ShooterAcquireAlgae(){ - shooter = CoralShooter.getInstance(); + shooter = LokiShooter.getInstance(); addRequirements(shooter); } @Override public void initialize() { - shooter.setShooterRPM(Settings.Shooter.ALGAE_ACQUIRE_RPM.getAsDouble()); + shooter.setSpeed(Settings.Shooter.ALGAE_ACQUIRE_SPEED.getAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java new file mode 100644 index 0000000..a5372ad --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.lokishooter; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; + +public class ShooterAcquireCoral extends InstantCommand { + + private final LokiShooter shooter; + + public ShooterAcquireCoral(){ + shooter = LokiShooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize() { + shooter.setSpeed(-Settings.Shooter.CORAL_ACQUIRE_SPEED.getAsDouble()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterDeacquireAlgae.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java similarity index 52% rename from src/main/java/com/stuypulse/robot/commands/shooter/ShooterDeacquireAlgae.java rename to src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java index dcc73bf..8aa6936 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterDeacquireAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java @@ -1,21 +1,21 @@ -package com.stuypulse.robot.commands.shooter; +package com.stuypulse.robot.commands.lokishooter; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.shooter.CoralShooter; +import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; import edu.wpi.first.wpilibj2.command.InstantCommand; public class ShooterDeacquireAlgae extends InstantCommand { - private final CoralShooter shooter; + private final LokiShooter shooter; public ShooterDeacquireAlgae(){ - shooter = CoralShooter.getInstance(); + shooter = LokiShooter.getInstance(); addRequirements(shooter); } @Override public void initialize() { - shooter.setShooterRPM(-Settings.Shooter.ALGAE_ACQUIRE_RPM.getAsDouble()); + shooter.setSpeed(-Settings.Shooter.ALGAE_DEACQUIRE_SPEED.getAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java new file mode 100644 index 0000000..c762fff --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.commands.lokishooter; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterShootBack extends InstantCommand { + private final LokiShooter shooter; + + public ShooterShootBack(){ + shooter = LokiShooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize(){ + if (shooter.hasAlgae()){ + shooter.setSpeed(-Settings.Shooter.ALGAE_BACK_SPEED.getAsDouble()); + } else if (shooter.hasCoral()){ + shooter.setSpeed(-Settings.Shooter.CORAL_BACK_SPEED.getAsDouble()); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java new file mode 100644 index 0000000..1fcffb6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.commands.lokishooter; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterShootFront extends InstantCommand { + private final LokiShooter shooter; + + public ShooterShootFront(){ + shooter = LokiShooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize(){ + if (shooter.hasAlgae()){ + shooter.setSpeed(Settings.Shooter.ALGAE_FRONT_SPEED.getAsDouble()); + } else if (shooter.hasCoral()){ + shooter.setSpeed(Settings.Shooter.CORAL_FRONT_SPEED.getAsDouble()); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterStop.java similarity index 50% rename from src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java rename to src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterStop.java index 92cb389..ef8a64e 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterStop.java @@ -1,20 +1,20 @@ -package com.stuypulse.robot.commands.shooter; +package com.stuypulse.robot.commands.lokishooter; -import com.stuypulse.robot.subsystems.shooter.CoralShooter; +import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; import edu.wpi.first.wpilibj2.command.InstantCommand; public class ShooterStop extends InstantCommand { - private final CoralShooter shooter; + private final LokiShooter shooter; public ShooterStop() { - shooter = CoralShooter.getInstance(); + shooter = LokiShooter.getInstance(); addRequirements(shooter); } @Override public void initialize() { - shooter.setShooterRPM(0); + shooter.setSpeed(0); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterShoot.java deleted file mode 100644 index 20668c5..0000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterShoot.java +++ /dev/null @@ -1,24 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.shooter.CoralShooter; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ShooterShoot extends InstantCommand { - private final CoralShooter shooter; - - public ShooterShoot(){ - shooter = CoralShooter.getInstance(); - addRequirements(shooter); - } - - @Override - public void initialize(){ - if (shooter.hasAlgae()){ - shooter.setShooterRPM(Settings.Shooter.ALGAE_TARGET_RPM.getAsDouble()); - } else if (shooter.hasCoral()){ - shooter.setShooterRPM(Settings.Shooter.CORAL_TARGET_RPM.getAsDouble()); - } - } -} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index c6e8460..acf5fcf 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -15,56 +15,30 @@ */ public interface Settings { public interface Shooter { - double MAX_SHOOTER_RPM = 6000; // Max RPM of KrakenX60 (rpm) - double TARGET_SHOOTER_RPM = 6000; // Target RPM of KrakenX60 (rpm) - - SmartNumber ALGAE_TARGET_RPM = new SmartNumber("Algae Target RPM", 6000); - SmartNumber CORAL_TARGET_RPM = new SmartNumber("Coral Target RPM", 5000); - SmartNumber ALGAE_ACQUIRE_RPM = new SmartNumber("Algae Acquire RPM", 3000); + SmartNumber ALGAE_FRONT_SPEED = new SmartNumber("Algae Target Speed", 1); + SmartNumber CORAL_FRONT_SPEED = new SmartNumber("Coral Target Speed",0.75); + SmartNumber ALGAE_BACK_SPEED = new SmartNumber("Algae Target Speed", 1); + SmartNumber CORAL_BACK_SPEED = new SmartNumber("Coral Target Speed",0.75); + SmartNumber ALGAE_ACQUIRE_SPEED = new SmartNumber("Algae Acquire Speed", 0.45); + SmartNumber ALGAE_DEACQUIRE_SPEED = new SmartNumber("Algae Deacquire Speed", 0.45); + SmartNumber CORAL_ACQUIRE_SPEED = new SmartNumber("Coral Acquire Speed", 0.3); double BB_DEBOUNCE = 0.0; - - public interface PID { - // ADJUST LATER - SmartNumber kP = new SmartNumber("kP", 0); - SmartNumber kI = new SmartNumber("kI", 0); - SmartNumber kD = new SmartNumber("kD", 0); - } - - public interface FeedForward { - // ADJUST LATER - SmartNumber kS = new SmartNumber("kS", 0); - SmartNumber kV = new SmartNumber("kV", 0); - SmartNumber kA = new SmartNumber("kA", 0); - } + double CORAL_STALLING_DEBOUNCE = 0.0; + double ALGAE_DEBOUNCE = 0.0; - double GEAR_RATIO = 0.0; - double DRIVE_CURRENT_THRESHOLD = 80; - double DRIVE_CURRENT_LIMIT = 120; + double DRIVE_CURRENT_THRESHOLD = 30; + double DRIVE_CURRENT_LIMIT = 40; } public interface Funnel { - double MAX_FUNNEL_RPM = 6000; // Max RPM of KrakenX60 (rpm) - double TARGET_FUNNEL_RPM = 6000; // Target RPM of KrakenX60 (rpm) + SmartNumber MOTOR_SPEED = new SmartNumber("Funnel Speed", 0.0); double BB_DEBOUNCE = 0.0; + double FUNNEL_STALLING = 0.0; double RAMP_RATE = 0.0; - - public interface PID { - // ADJUST LATER - SmartNumber kP = new SmartNumber("kP", 0); - SmartNumber kI = new SmartNumber("kI", 0); - SmartNumber kD = new SmartNumber("kD", 0); - } - - public interface FeedForward { - // ADJUST LATER - SmartNumber kS = new SmartNumber("kS", 0); - SmartNumber kV = new SmartNumber("kV", 0); - SmartNumber kA = new SmartNumber("kA", 0); - } double GEAR_RATIO = 0.0; - double DRIVE_CURRENT_THRESHOLD = 80; - double DRIVE_CURRENT_LIMIT = 120; + double DRIVE_CURRENT_THRESHOLD = 30; + double DRIVE_CURRENT_LIMIT = 40; } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java index 8c4a051..334b1fd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java @@ -1,9 +1,5 @@ package com.stuypulse.robot.subsystems.funnel; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.stuylib.network.SmartNumber; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class CoralFunnel extends SubsystemBase{ @@ -14,29 +10,21 @@ public abstract class CoralFunnel extends SubsystemBase{ instance = new CoralFunnelImpl(); } - private final SmartNumber targetRPM; - public static CoralFunnel getInstance() { return instance; } public CoralFunnel() { - targetRPM = new SmartNumber("Funnel/Target RPM", Settings.Funnel.TARGET_FUNNEL_RPM); } - public double getTargetRPM() { - return targetRPM.get(); - } + public abstract void forward(); - public abstract void setMotorRPM(double targetRPM); + public abstract void reverse(); - public abstract boolean coralStuck(); + public abstract boolean isStalling(); - public abstract boolean getFunnelState(); + public abstract boolean hasCoral(); - @Override - public void periodic() { - SmartDashboard.putNumber("Funnel/Target RPM", getTargetRPM()); - } + public abstract void stop(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java index 2ee6537..65da2f9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java @@ -1,6 +1,5 @@ package com.stuypulse.robot.subsystems.funnel; -import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -9,90 +8,82 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.IStream; -import com.stuypulse.stuylib.streams.numbers.filters.HighPassFilter; - import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class CoralFunnelImpl extends CoralFunnel { - private final TalonFX driveMotor; - private final TalonFXConfiguration driveConfig; + private final TalonFX funnelMotor; + private final TalonFXConfiguration funnelConfig; private final DigitalInput motorBeam; - private BStream funnelState; + private BStream hasCoral; + private BStream isStalling; - private final IStream driveCurrent; + private final IStream funnelCurrent; public CoralFunnelImpl(){ - driveMotor = new TalonFX(Ports.Funnel.MOTOR); + funnelMotor = new TalonFX(Ports.Funnel.MOTOR); motorBeam = new DigitalInput(Ports.Funnel.BEAM); - funnelState = BStream.create(motorBeam).not() - .filtered(new BDebounce.Both(Settings.Funnel.BB_DEBOUNCE)); - driveConfig = new TalonFXConfiguration(); + //Stall Detection + funnelCurrent = IStream.create(() -> Math.abs(funnelMotor.getStatorCurrent().getValueAsDouble())); - Slot0Configs slot0 = new Slot0Configs(); + hasCoral = BStream.create(motorBeam).not() + .filtered(new BDebounce.Both(Settings.Funnel.BB_DEBOUNCE)); + isStalling = BStream.create(() -> funnelCurrent.get() > Settings.Funnel.DRIVE_CURRENT_THRESHOLD) + .filtered(new BDebounce.Both(Settings.Funnel.FUNNEL_STALLING)); - slot0.kS = Settings.Funnel.FeedForward.kS.getAsDouble(); - slot0.kV = Settings.Funnel.FeedForward.kV.getAsDouble(); - slot0.kA = Settings.Funnel.FeedForward.kA.getAsDouble(); - - slot0.kP = Settings.Funnel.PID.kP.getAsDouble(); - slot0.kI = Settings.Funnel.PID.kI.getAsDouble(); - slot0.kD = Settings.Funnel.PID.kD.getAsDouble(); - - driveConfig.Slot0 = slot0; - driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - driveConfig.Feedback.SensorToMechanismRatio = Settings.Funnel.GEAR_RATIO; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = Settings.Funnel.DRIVE_CURRENT_LIMIT; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -Settings.Funnel.DRIVE_CURRENT_LIMIT; - driveConfig.CurrentLimits.StatorCurrentLimit = Settings.Funnel.DRIVE_CURRENT_LIMIT; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; - driveConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.02; - driveConfig.TorqueCurrent.TorqueNeutralDeadband = 0.05; + funnelConfig = new TalonFXConfiguration(); - //Stall Detection - driveCurrent = IStream.create(() -> driveMotor.getStatorCurrent().getValueAsDouble()) - .filtered(new HighPassFilter(Settings.Funnel.DRIVE_CURRENT_THRESHOLD)); - - driveMotor.getConfigurator().apply(driveConfig); - driveMotor.setPosition(0); + funnelConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + funnelConfig.Feedback.SensorToMechanismRatio = Settings.Funnel.GEAR_RATIO; + funnelConfig.TorqueCurrent.PeakForwardTorqueCurrent = Settings.Funnel.DRIVE_CURRENT_LIMIT; + funnelConfig.TorqueCurrent.PeakReverseTorqueCurrent = -Settings.Funnel.DRIVE_CURRENT_LIMIT; + funnelConfig.CurrentLimits.StatorCurrentLimit = Settings.Funnel.DRIVE_CURRENT_LIMIT; + funnelConfig.CurrentLimits.StatorCurrentLimitEnable = true; //Ramp Motor Voltage - driveConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Funnel.RAMP_RATE; - driveConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Funnel.RAMP_RATE; + funnelConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Funnel.RAMP_RATE; + funnelConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Funnel.RAMP_RATE; + + funnelMotor.getConfigurator().apply(funnelConfig); } - private double getMotorRPM() { - return driveMotor.get() * Settings.Funnel.MAX_FUNNEL_RPM; + @Override + public void forward() { + this.funnelMotor.set(Settings.Funnel.MOTOR_SPEED.getAsDouble()); } - public void setMotorRPM(double targetRPM) { - driveMotor.set(targetRPM / Settings.Funnel.MAX_FUNNEL_RPM); + @Override + public void reverse() { + this.funnelMotor.set(-Settings.Funnel.MOTOR_SPEED.getAsDouble()); } - public boolean coralStuck() { - return driveCurrent.get() > Settings.Funnel.DRIVE_CURRENT_THRESHOLD; + @Override + public void stop() { + this.funnelMotor.set(0); + } + + public boolean isStalling() { + return isStalling.get(); } @Override - public boolean getFunnelState() { - return funnelState.get(); + public boolean hasCoral() { + return hasCoral.get(); } @Override public void periodic() { super.periodic(); - SmartDashboard.putNumber("Funnel/RPM", getMotorRPM()); - - SmartDashboard.putNumber("Funnel/Voltage", driveMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Funnel/Voltage", funnelMotor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Funnel/Current", driveCurrent.get()); + SmartDashboard.putNumber("Funnel/Current", funnelCurrent.get()); - SmartDashboard.putBoolean("Funnel/Has Coral", getFunnelState()); - SmartDashboard.putBoolean("Funnel/Coral Stuck", coralStuck()); + SmartDashboard.putBoolean("Funnel/Has Coral", hasCoral()); + SmartDashboard.putBoolean("Funnel/Coral IsStuck", isStalling()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooter.java b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooter.java new file mode 100644 index 0000000..0e68237 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooter.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.subsystems.lokishooter; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class LokiShooter extends SubsystemBase { + + private static final LokiShooter instance; + + static { + instance = new LokiShooterImpl(); + } + public static LokiShooter getInstance() { + return instance; + } + + public abstract void setSpeed(double speed); + + public abstract double getSpeed(); + + public abstract boolean hasCoral(); + + public abstract boolean hasAlgae(); +} + diff --git a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java new file mode 100644 index 0000000..f95adcd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java @@ -0,0 +1,89 @@ +package com.stuypulse.robot.subsystems.lokishooter; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.wpilibj.DigitalInput; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import com.stuypulse.stuylib.streams.numbers.IStream; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +public class LokiShooterImpl extends LokiShooter { + + private final TalonFX shooterMotor; + private final TalonFXConfiguration shooterConfig; + + private final DigitalInput motorBeam; + private final BStream hasCoral; + private final BStream hasAlgae; + + private final IStream shooterCurrent; + + public LokiShooterImpl(){ + shooterMotor = new TalonFX(Ports.Shooter.MOTOR); + + motorBeam = new DigitalInput(Ports.Shooter.BEAM); + + shooterCurrent = IStream.create(() -> shooterMotor.getStatorCurrent().getValueAsDouble()); + + hasCoral = BStream.create(motorBeam).not() + .filtered(new BDebounce.Both(Settings.Shooter.BB_DEBOUNCE)); + hasAlgae = BStream.create(this::detectCurrentSpike) + .filtered(new BDebounce.Both(Settings.Shooter.ALGAE_DEBOUNCE)); + + shooterConfig = new TalonFXConfiguration(); + + shooterConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + shooterConfig.TorqueCurrent.PeakForwardTorqueCurrent = Settings.Shooter.DRIVE_CURRENT_LIMIT; + shooterConfig.TorqueCurrent.PeakReverseTorqueCurrent = -Settings.Shooter.DRIVE_CURRENT_LIMIT; + shooterConfig.CurrentLimits.StatorCurrentLimit = Settings.Shooter.DRIVE_CURRENT_LIMIT; + shooterConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + shooterMotor.getConfigurator().apply(shooterConfig); + } + + @Override + public void setSpeed(double speed){ + this.shooterMotor.set(speed); + } + + @Override + public double getSpeed(){ + return this.shooterMotor.get(); + } + + @Override + public boolean hasCoral() { + return this.hasCoral.get(); + } + + @Override + public boolean hasAlgae(){ + return this.hasAlgae.get(); + } + + // Uses stall detection to determine if the shooter has algae by checking if the current is above a certain threshold + public boolean detectCurrentSpike() { + return shooterCurrent.get() > Settings.Shooter.DRIVE_CURRENT_THRESHOLD; + } + + + @Override + public void periodic() { + super.periodic(); + + SmartDashboard.putNumber("Shooter/Voltage", shooterMotor.getMotorVoltage().getValueAsDouble()); + + SmartDashboard.putNumber("Shooter/Current", shooterCurrent.get()); + + SmartDashboard.putNumber("Shooter/Speed", getSpeed()); + + SmartDashboard.putBoolean("Shooter/Has Coral", hasCoral()); + SmartDashboard.putBoolean("Shooter/Has Algae", hasAlgae()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java deleted file mode 100644 index f1ea7a6..0000000 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooter.java +++ /dev/null @@ -1,48 +0,0 @@ -package com.stuypulse.robot.subsystems.shooter; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.stuylib.network.SmartNumber; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public abstract class CoralShooter extends SubsystemBase { - - private static final CoralShooter instance; - - static { - instance = new CoralShooterImpl(); - } - - public static CoralShooter getInstance() { - return instance; - } - - private final SmartNumber targetRPM; - - public CoralShooter() { - targetRPM = new SmartNumber("Shooter/Target RPM", Settings.Shooter.TARGET_SHOOTER_RPM); - } - - public double getTargetRPM() { - return targetRPM.get(); - } - - public void setTargetRPM(double targetRPM){ - this.targetRPM.set(targetRPM); - } - - public abstract double getShooterRPM(); - - public abstract void setShooterRPM(double targetRPM); - - public abstract boolean hasCoral(); - - public abstract boolean hasAlgae(); - - @Override - public void periodic() { - SmartDashboard.putNumber("Shooter/Target RPM", getTargetRPM()); - } -} - diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java deleted file mode 100644 index 7bd365f..0000000 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/CoralShooterImpl.java +++ /dev/null @@ -1,107 +0,0 @@ -package com.stuypulse.robot.subsystems.shooter; - -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; - -import edu.wpi.first.wpilibj.DigitalInput; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; -import com.stuypulse.stuylib.streams.numbers.IStream; -import com.stuypulse.stuylib.streams.numbers.filters.HighPassFilter; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class CoralShooterImpl extends CoralShooter { - - private final TalonFX driveMotor; - private final TalonFXConfiguration driveConfig; - - private final DigitalInput motorBeam; - private final BStream hasCoral; - private boolean hasAlgae; - - private final IStream driveCurrent; - - public CoralShooterImpl(){ - driveMotor = new TalonFX(Ports.Shooter.MOTOR); - - motorBeam = new DigitalInput(Ports.Shooter.BEAM); - hasCoral = BStream.create(motorBeam).not() - .filtered(new BDebounce.Both(Settings.Shooter.BB_DEBOUNCE)); - - driveConfig = new TalonFXConfiguration(); - - Slot0Configs slot0 = new Slot0Configs(); - - slot0.kS = Settings.Shooter.FeedForward.kS.getAsDouble(); - slot0.kV = Settings.Shooter.FeedForward.kV.getAsDouble(); - slot0.kA = Settings.Shooter.FeedForward.kA.getAsDouble(); - - slot0.kP = Settings.Shooter.PID.kP.getAsDouble(); - slot0.kI = Settings.Shooter.PID.kI.getAsDouble(); - slot0.kD = Settings.Shooter.PID.kD.getAsDouble(); - - driveConfig.Slot0 = slot0; - driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - driveConfig.Feedback.SensorToMechanismRatio = Settings.Shooter.GEAR_RATIO; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = Settings.Shooter.DRIVE_CURRENT_LIMIT; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -Settings.Shooter.DRIVE_CURRENT_LIMIT; - driveConfig.CurrentLimits.StatorCurrentLimit = Settings.Shooter.DRIVE_CURRENT_LIMIT; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; - driveConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.02; - driveConfig.TorqueCurrent.TorqueNeutralDeadband = 0.05; - - driveCurrent = IStream.create(() -> driveMotor.getStatorCurrent().getValueAsDouble()) - .filtered(new HighPassFilter(Settings.Shooter.DRIVE_CURRENT_THRESHOLD)); - - driveMotor.getConfigurator().apply(driveConfig); - driveMotor.setPosition(0); - } - - public double getShooterRPM() { - return driveMotor.get() * Settings.Shooter.MAX_SHOOTER_RPM; - } - - public void setShooterRPM(double targetRPM) { - this.setTargetRPM(targetRPM); - driveMotor.set(targetRPM / Settings.Shooter.MAX_SHOOTER_RPM); - } - - @Override - public boolean hasCoral() { - return hasCoral.get(); - } - - @Override - public boolean hasAlgae(){ - return this.hasAlgae; - } - - // Uses stall detection to determine if the shooter has algae by checking if the current is above a certain threshold - public boolean detectCurrentSpike() { - return driveCurrent.get() > Settings.Shooter.DRIVE_CURRENT_THRESHOLD; - } - - - @Override - public void periodic() { - super.periodic(); - - if (this.getTargetRPM() != 0){ - this.hasAlgae = this.detectCurrentSpike(); - } - - SmartDashboard.putNumber("Shooter/RPM", getShooterRPM()); - - SmartDashboard.putNumber("Shooter/Voltage", driveMotor.getMotorVoltage().getValueAsDouble()); - - SmartDashboard.putNumber("Shooter/Current", driveCurrent.get()); - - SmartDashboard.putBoolean("Shooter/Has Coral", hasCoral()); - SmartDashboard.putBoolean("Shooter/Has Algae", hasAlgae()); - } -} From 4f897188ff7a272244807125fcfe1d5b96e1d77d Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Sat, 1 Feb 2025 15:40:16 -0500 Subject: [PATCH 21/34] Everything from code review: Use WPI Distance for targetHeight, config gravity type for elevator, Set target jerk to 0, reset encoder at bottom, added ElevatorToBottom() in robot init as a comment, added 3 more commands for funnel side, reworked ElevatorToHeight() Co-authored-by: BrianZ60 Co-authored-by: Baiulus Co-authored-by: Haiiden --- src/main/java/com/stuypulse/robot/Robot.java | 7 ++++- .../commands/elevator/ElevatorToBottom.java | 4 --- .../commands/elevator/ElevatorToHandoff.java | 3 -- .../commands/elevator/ElevatorToHeight.java | 22 ++++++------- .../commands/elevator/ElevatorToLvl2.java | 6 +--- .../commands/elevator/ElevatorToLvl2Alt.java | 9 ++++++ .../elevator/ElevatorToLvl2Funnel.java | 9 ++++++ .../commands/elevator/ElevatorToLvl3.java | 13 -------- .../commands/elevator/ElevatorToLvl3Alt.java | 9 ++++++ .../elevator/ElevatorToLvl3Funnel.java | 10 ++++++ .../commands/elevator/ElevatorToLvl4.java | 12 ------- .../commands/elevator/ElevatorToLvl4Alt.java | 9 ++++++ .../elevator/ElevatorToLvl4Funnel.java | 9 ++++++ .../commands/elevator/ElevatorToTop.java | 5 --- .../stuypulse/robot/constants/Constants.java | 2 ++ .../stuypulse/robot/constants/Settings.java | 20 ++++++------ .../robot/subsystems/elevator/Elevator.java | 2 +- .../subsystems/elevator/ElevatorImpl.java | 31 +++++++++++-------- .../subsystems/elevator/ElevatorSimu.java | 6 ---- 19 files changed, 104 insertions(+), 84 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e0f788d..76307d0 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,6 +5,8 @@ package com.stuypulse.robot; +import com.stuypulse.robot.commands.elevator.ElevatorToBottom; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -15,12 +17,15 @@ public class Robot extends TimedRobot { private Command auto; /*************************/ - /*** ROBOT SCHEDULEING ***/ + /*** ROBOT SCHEDULING ***/ /*************************/ @Override public void robotInit() { robot = new RobotContainer(); + + // Check with Philip to automatically reset elevator encoder + // new ElevatorToBottom().schedule(); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java index 0176469..bc6629b 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java @@ -6,8 +6,4 @@ public class ElevatorToBottom extends ElevatorToHeight{ public ElevatorToBottom(){ super(Elevator.MIN_HEIGHT_METERS); } - - public void initialize() { - super.initialize(); - } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java index 47e289b..a15139d 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java @@ -7,7 +7,4 @@ public ElevatorToHandoff() { super(Elevator.HANDOFF_HEIGHT_METERS); } - public void initialize() { - super.initialize(); - } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java index 28baf31..95d40c8 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java @@ -1,31 +1,29 @@ package com.stuypulse.robot.commands.elevator; -import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.subsystems.elevator.Elevator; -import com.stuypulse.stuylib.streams.booleans.BStream; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.Command; public class ElevatorToHeight extends InstantCommand { + + public static Command untilDone(double height) { + return new ElevatorToHeight(height) + .andThen(new WaitUntilCommand(() -> Elevator.getInstance().atTargetHeight())); + } + private final Elevator elevator; private final double targetHeight; - private final BStream belowTop, aboveBottom; public ElevatorToHeight(double targetHeight){ elevator = Elevator.getInstance(); this.targetHeight = targetHeight; - - belowTop = BStream.create(() -> !elevator.atTop()) - .and(() -> targetHeight < Constants.Elevator.MAX_HEIGHT_METERS); - aboveBottom = BStream.create(() -> !elevator.atBottom()) - .and(() -> targetHeight > Constants.Elevator.MIN_HEIGHT_METERS); addRequirements(elevator); } public void initialize(){ - if (belowTop.get() && aboveBottom.get()) { - elevator.setTargetHeight(targetHeight); - } + elevator.setTargetHeight(targetHeight); } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java index 20028fa..b9f1f6f 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java @@ -3,11 +3,7 @@ import com.stuypulse.robot.constants.Settings.Elevator; public class ElevatorToLvl2 extends ElevatorToHeight{ - public ElevatorToLvl2(){ + public ElevatorToLvl2Alt(){ super(Elevator.L2_HEIGHT_METERS); } - - public void initialize() { - super.initialize(); - } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java new file mode 100644 index 0000000..7510446 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl2Alt extends ElevatorToHeight{ + public ElevatorToLvl2Alt(){ + super(Elevator.ALT_L2_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java new file mode 100644 index 0000000..6abad77 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl2Funnel extends ElevatorToHeight{ + public ElevatorToLvl2Funnel(){ + super(Elevator.FUNNEL_L2_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java deleted file mode 100644 index d0ea7ad..0000000 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java +++ /dev/null @@ -1,13 +0,0 @@ -package com.stuypulse.robot.commands.elevator; - -import com.stuypulse.robot.constants.Settings.Elevator; - -public class ElevatorToLvl3 extends ElevatorToHeight{ - public ElevatorToLvl3(){ - super(Elevator.L3_HEIGHT_METERS); - } - - public void initialize() { - super.initialize(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java new file mode 100644 index 0000000..59801d2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl3Alt extends ElevatorToHeight{ + public ElevatorToLvl3Alt () { + super(Elevator.ALT_L3_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java new file mode 100644 index 0000000..7b72017 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl3Funnel extends ElevatorToHeight{ + public ElevatorToLvl3Funnel() { + super(Elevator.FUNNEL_L3_HEIGHT_METERS); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java deleted file mode 100644 index db9b49b..0000000 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java +++ /dev/null @@ -1,12 +0,0 @@ -package com.stuypulse.robot.commands.elevator; -import com.stuypulse.robot.constants.Settings.Elevator; - -public class ElevatorToLvl4 extends ElevatorToHeight{ - public ElevatorToLvl4() { - super(Elevator.L4_HEIGHT_METERS); - } - - public void initialize() { - super.initialize(); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java new file mode 100644 index 0000000..a8a7f38 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl4Alt extends ElevatorToHeight{ + public ElevatorToLvl4Alt(){ + super(Elevator.ALT_L4_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java new file mode 100644 index 0000000..8e206f0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl4Funnel extends ElevatorToHeight{ + public ElevatorToLvl4Funnel() { + super(Elevator.FUNNEL_L4_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java index 2260061..c893688 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java @@ -6,9 +6,4 @@ public class ElevatorToTop extends ElevatorToHeight{ public ElevatorToTop() { super(Constants.Elevator.MAX_HEIGHT_METERS); } - - public void initialize() { - super.initialize(); - } - } diff --git a/src/main/java/com/stuypulse/robot/constants/Constants.java b/src/main/java/com/stuypulse/robot/constants/Constants.java index e9b30a1..bcee24f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Constants.java +++ b/src/main/java/com/stuypulse/robot/constants/Constants.java @@ -17,6 +17,8 @@ public interface Encoders { double POSITION_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP; double VELOCITY_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP / 60; + + double DISTANCE_PER_ROTATION = 0.0; } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d36719b..82db2c1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,7 +5,6 @@ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; /*- @@ -16,7 +15,7 @@ */ public interface Settings { - double DT = 0.020; + double DT = 0.020; // 20ms Differential Time public interface Elevator { SmartNumber MAX_VELOCITY_METERS_PER_SECOND = new SmartNumber("Elevator/Max Velocity (m per s)", 1.0); @@ -24,11 +23,13 @@ public interface Elevator { // CHANGE double HANDOFF_HEIGHT_METERS = 0.1; - double L2_HEIGHT_METERS = 0.25; - double L3_HEIGHT_METERS = 0.5; - double L4_HEIGHT_METERS = 0.75; - - double FEED_HEIGHT_METERS = 0.4; + // front and funnel + double ALT_L2_HEIGHT_METERS = 0.25; + double ALT_L3_HEIGHT_METERS = 0.5; + double ALT_L4_HEIGHT_METERS = 0.75; + double FUNNEL_L2_HEIGHT_METERS = 0.3; // funnel side; should be higher than L2 + double FUNNEL_L3_HEIGHT_METERS = 0.55; // funnel side; should be higher than L3 + double FUNNEL_L4_HEIGHT_METERS = 0.8; double RAMP_RATE = 0.1; @@ -44,9 +45,10 @@ public interface Elevator { SmartNumber HEIGHT_TOLERANCE_METERS = new SmartNumber("Elevator/Height Tolerance (m)", 0.02); public interface PID { - SmartNumber kP = new SmartNumber("Elevator/Controller/kP",10); + //tune + SmartNumber kP = new SmartNumber("Elevator/Controller/kP",0.0); SmartNumber kI = new SmartNumber("Elevator/Controller/kI",0.0); - SmartNumber kD = new SmartNumber("Elevator/Controller/kD",0.2); + SmartNumber kD = new SmartNumber("Elevator/Controller/kD",0.0); } public interface FF { diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java index 52864cb..a92c2df 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -2,6 +2,7 @@ import com.stuypulse.robot.Robot; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Elevator extends SubsystemBase { @@ -30,7 +31,6 @@ public Elevator() { public abstract double getTargetHeight(); public abstract double getCurrentHeight(); public abstract boolean atTargetHeight(); - public abstract boolean atTop(); public abstract boolean atBottom(); public void periodic() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java index f87ffd3..783cd40 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.constants.Constants; @@ -11,30 +12,34 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; - +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ElevatorImpl extends Elevator { private final TalonFX motor; private final SmartNumber targetHeight; - private final DigitalInput bumpSwitchTop, bumpSwitchBottom; + private final DigitalInput bumpSwitchBottom; + private final Distance targetHeightMeters; public ElevatorImpl() { motor = new TalonFX(Ports.Elevator.MOTOR); targetHeight = new SmartNumber("Elevator/Target Height", Constants.Elevator.MIN_HEIGHT_METERS); - + targetHeightMeters = Units.Meters.of(targetHeight.getAsDouble()); bumpSwitchBottom = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); - bumpSwitchTop = new DigitalInput(Ports.Elevator.TOP_SWITCH); TalonFXConfiguration elevatorMotorConfig = new TalonFXConfiguration(); Slot0Configs slot0 = new Slot0Configs(); + slot0.GravityType = GravityTypeValue.Elevator_Static; + slot0.kS = Settings.Elevator.FF.kS.getAsDouble(); slot0.kV = Settings.Elevator.FF.kV.getAsDouble(); slot0.kA = Settings.Elevator.FF.kA.getAsDouble(); slot0.kG = Settings.Elevator.FF.kG.getAsDouble(); + slot0.kP = Settings.Elevator.PID.kP.getAsDouble(); slot0.kI = Settings.Elevator.PID.kI.getAsDouble(); slot0.kD = Settings.Elevator.PID.kD.getAsDouble(); @@ -57,7 +62,8 @@ public ElevatorImpl() { var elevatorMotionMagicConfigs = elevatorMotorConfig.MotionMagic; elevatorMotionMagicConfigs.MotionMagicCruiseVelocity = Settings.Elevator.TARGET_CRUISE_VELOCITY; elevatorMotionMagicConfigs.MotionMagicAcceleration = Settings.Elevator.TARGET_ACCELERATION; - elevatorMotionMagicConfigs.MotionMagicJerk = Settings.Elevator.TARGET_JERK; + elevatorMotionMagicConfigs.MotionMagicJerk = Settings.Elevator.TARGET_JERK; // Set to 0, not being used + motor.getConfigurator().apply(elevatorMotorConfig); } @@ -66,7 +72,7 @@ public ElevatorImpl() { public void setTargetHeight(double height) { targetHeight.set( SLMath.clamp( - height, + height, Constants.Elevator.MIN_HEIGHT_METERS, Constants.Elevator.MAX_HEIGHT_METERS ) @@ -75,12 +81,12 @@ public void setTargetHeight(double height) { @Override public double getTargetHeight() { - return targetHeight.getAsDouble(); + return targetHeightMeters.magnitude(); } @Override public double getCurrentHeight() { - return motor.getPosition().getValueAsDouble() * Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR; + return motor.getPosition().getValueAsDouble() * Constants.Elevator.Encoders.DISTANCE_PER_ROTATION; } @Override @@ -93,11 +99,6 @@ public boolean atBottom() { return !bumpSwitchBottom.get(); } - @Override - public boolean atTop() { - return !bumpSwitchTop.get(); - } - @Override public void periodic() { super.periodic(); @@ -105,6 +106,10 @@ public void periodic() { final MotionMagicVoltage controlRequest = new MotionMagicVoltage(getTargetHeight()/Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR); motor.setControl(controlRequest); + if (atBottom()) { + motor.setPosition(0); + } + SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); SmartDashboard.putNumber("Elevator/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Elevator/Motor Current", motor.getStatorCurrent().getValueAsDouble()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java index f0e396e..58eb6a4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java @@ -74,12 +74,6 @@ public double getCurrentHeight() { public boolean atTargetHeight() { return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); } - - // atTop() and atBottom() are unused - @Override - public boolean atTop() { - return false; - } @Override public boolean atBottom() { From 3c6bf2b37102c88d75928ff4162498d7c550095d Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Sat, 1 Feb 2025 15:40:53 -0500 Subject: [PATCH 22/34] deleted extra command --- .../robot/commands/elevator/ElevatorToLvl2.java | 9 --------- 1 file changed, 9 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java deleted file mode 100644 index b9f1f6f..0000000 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.elevator; - -import com.stuypulse.robot.constants.Settings.Elevator; - -public class ElevatorToLvl2 extends ElevatorToHeight{ - public ElevatorToLvl2Alt(){ - super(Elevator.L2_HEIGHT_METERS); - } -} \ No newline at end of file From f76367bd11ba6ddff81b3a510aa88eedfd07d5f1 Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Sat, 1 Feb 2025 20:19:32 -0500 Subject: [PATCH 23/34] Added isFinished and ened methods to shooter commands --- .../commands/lokishooter/ShooterAcquireAlgae.java | 10 ++++++++++ .../commands/lokishooter/ShooterAcquireCoral.java | 10 ++++++++++ .../commands/lokishooter/ShooterDeacquireAlgae.java | 10 ++++++++++ .../robot/commands/lokishooter/ShooterShootBack.java | 10 ++++++++++ .../robot/commands/lokishooter/ShooterShootFront.java | 10 ++++++++++ 5 files changed, 50 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java index bca6ff0..54532c5 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java @@ -18,4 +18,14 @@ public ShooterAcquireAlgae(){ public void initialize() { shooter.setSpeed(Settings.Shooter.ALGAE_ACQUIRE_SPEED.getAsDouble()); } + + @Override + public boolean isFinished(){ + return shooter.hasAlgae(); + } + + @Override + public void end(boolean interrupted){ + shooter.setSpeed(0); + } } diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java index a5372ad..7313070 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java @@ -18,4 +18,14 @@ public ShooterAcquireCoral(){ public void initialize() { shooter.setSpeed(-Settings.Shooter.CORAL_ACQUIRE_SPEED.getAsDouble()); } + + @Override + public boolean isFinished(){ + return shooter.hasCoral(); + } + + @Override + public void end(boolean interrupted){ + shooter.setSpeed(0); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java index 8aa6936..b299f6c 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java @@ -18,4 +18,14 @@ public ShooterDeacquireAlgae(){ public void initialize() { shooter.setSpeed(-Settings.Shooter.ALGAE_DEACQUIRE_SPEED.getAsDouble()); } + + @Override + public boolean isFinished(){ + return !shooter.hasAlgae(); + } + + @Override + public void end(boolean interrupted){ + shooter.setSpeed(0); + } } diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java index c762fff..c8c6780 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java @@ -21,4 +21,14 @@ public void initialize(){ shooter.setSpeed(-Settings.Shooter.CORAL_BACK_SPEED.getAsDouble()); } } + + @Override + public boolean isFinished(){ + return !shooter.hasAlgae() && !shooter.hasCoral(); + } + + @Override + public void end(boolean interrupted){ + shooter.setSpeed(0); + } } diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java index 1fcffb6..77ad4e5 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java @@ -21,4 +21,14 @@ public void initialize(){ shooter.setSpeed(Settings.Shooter.CORAL_FRONT_SPEED.getAsDouble()); } } + + @Override + public boolean isFinished(){ + return !shooter.hasAlgae() && !shooter.hasCoral(); + } + + @Override + public void end(boolean interrupted){ + shooter.setSpeed(0); + } } From b2a2778704fdc21a6b723047a12b25c694122b0f Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Sun, 2 Feb 2025 09:17:18 -0500 Subject: [PATCH 24/34] Made all instant command normal commands Readded stopping functionality back to funnel default command renamed changeState to runState in default command for clarity removed this in all methods changed detectCurrentSpike to private --- .../commands/funnel/FunnelDefaultCommand.java | 11 ++++++++--- .../robot/commands/funnel/FunnelStop.java | 18 ------------------ .../lokishooter/ShooterAcquireAlgae.java | 4 ++-- .../lokishooter/ShooterAcquireCoral.java | 4 ++-- .../lokishooter/ShooterDeacquireAlgae.java | 4 ++-- .../commands/lokishooter/ShooterShootBack.java | 4 ++-- .../lokishooter/ShooterShootFront.java | 4 ++-- .../robot/subsystems/funnel/CoralFunnel.java | 3 --- .../subsystems/funnel/CoralFunnelImpl.java | 6 +++--- .../subsystems/lokishooter/LokiShooter.java | 1 + .../lokishooter/LokiShooterImpl.java | 10 +++++----- 11 files changed, 27 insertions(+), 42 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java index 106fd07..a7ca593 100644 --- a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java @@ -1,12 +1,14 @@ package com.stuypulse.robot.commands.funnel; import com.stuypulse.robot.subsystems.funnel.CoralFunnel; +import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.WaitCommand; public class FunnelDefaultCommand extends Command { private final CoralFunnel funnel; + private boolean stopped = false; private boolean reversed = false; public FunnelDefaultCommand() { @@ -18,10 +20,11 @@ public FunnelDefaultCommand() { public void execute() { setState(); - changeState(); + runState(); } private void setState(){ + stopped = LokiShooter.getInstance().hasCoral(); if(funnel.isStalling() && !reversed) { reversed = true; @@ -31,8 +34,10 @@ private void setState(){ } } - private void changeState(){ - if (reversed){ + private void runState(){ + if (stopped){ + funnel.stop(); + } else if (reversed){ funnel.reverse(); } else { funnel.forward(); diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java deleted file mode 100644 index 68ea499..0000000 --- a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java +++ /dev/null @@ -1,18 +0,0 @@ -package com.stuypulse.robot.commands.funnel; - -import com.stuypulse.robot.subsystems.funnel.CoralFunnel; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class FunnelStop extends InstantCommand { - private final CoralFunnel funnel; - public FunnelStop() { - funnel = CoralFunnel.getInstance(); - addRequirements(funnel); - } - - @Override - public void initialize(){ - funnel.stop(); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java index 54532c5..4a1a451 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireAlgae.java @@ -3,9 +3,9 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class ShooterAcquireAlgae extends InstantCommand { +public class ShooterAcquireAlgae extends Command { private final LokiShooter shooter; diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java index 7313070..6b7eeb7 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java @@ -1,11 +1,11 @@ package com.stuypulse.robot.commands.lokishooter; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; -public class ShooterAcquireCoral extends InstantCommand { +public class ShooterAcquireCoral extends Command { private final LokiShooter shooter; diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java index b299f6c..d81b5ec 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java @@ -3,9 +3,9 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class ShooterDeacquireAlgae extends InstantCommand { +public class ShooterDeacquireAlgae extends Command { private final LokiShooter shooter; diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java index c8c6780..d8211fe 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java @@ -3,9 +3,9 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class ShooterShootBack extends InstantCommand { +public class ShooterShootBack extends Command { private final LokiShooter shooter; public ShooterShootBack(){ diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java index 77ad4e5..f4b1517 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java @@ -3,9 +3,9 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.lokishooter.LokiShooter; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class ShooterShootFront extends InstantCommand { +public class ShooterShootFront extends Command { private final LokiShooter shooter; public ShooterShootFront(){ diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java index 334b1fd..19aae95 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnel.java @@ -14,9 +14,6 @@ public static CoralFunnel getInstance() { return instance; } - public CoralFunnel() { - } - public abstract void forward(); public abstract void reverse(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java index 65da2f9..1806733 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java @@ -53,17 +53,17 @@ public CoralFunnelImpl(){ @Override public void forward() { - this.funnelMotor.set(Settings.Funnel.MOTOR_SPEED.getAsDouble()); + funnelMotor.set(Settings.Funnel.MOTOR_SPEED.getAsDouble()); } @Override public void reverse() { - this.funnelMotor.set(-Settings.Funnel.MOTOR_SPEED.getAsDouble()); + funnelMotor.set(-Settings.Funnel.MOTOR_SPEED.getAsDouble()); } @Override public void stop() { - this.funnelMotor.set(0); + funnelMotor.set(0); } public boolean isStalling() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooter.java b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooter.java index 0e68237..eaf3f3b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooter.java @@ -9,6 +9,7 @@ public abstract class LokiShooter extends SubsystemBase { static { instance = new LokiShooterImpl(); } + public static LokiShooter getInstance() { return instance; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java index f95adcd..3584a02 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java @@ -49,26 +49,26 @@ public LokiShooterImpl(){ @Override public void setSpeed(double speed){ - this.shooterMotor.set(speed); + shooterMotor.set(speed); } @Override public double getSpeed(){ - return this.shooterMotor.get(); + return shooterMotor.get(); } @Override public boolean hasCoral() { - return this.hasCoral.get(); + return hasCoral.get(); } @Override public boolean hasAlgae(){ - return this.hasAlgae.get(); + return hasAlgae.get(); } // Uses stall detection to determine if the shooter has algae by checking if the current is above a certain threshold - public boolean detectCurrentSpike() { + private boolean detectCurrentSpike() { return shooterCurrent.get() > Settings.Shooter.DRIVE_CURRENT_THRESHOLD; } From dfd0264cb0262a4b329f4149cb5ab67a4fddeb8f Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Sun, 2 Feb 2025 17:51:26 -0500 Subject: [PATCH 25/34] Changed the lokishooter in funnel default command to an instance variable --- .../robot/commands/funnel/FunnelDefaultCommand.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java index a7ca593..a1ee1cb 100644 --- a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java @@ -8,11 +8,15 @@ public class FunnelDefaultCommand extends Command { private final CoralFunnel funnel; + private final LokiShooter shooter; + private boolean stopped = false; private boolean reversed = false; public FunnelDefaultCommand() { funnel = CoralFunnel.getInstance(); + shooter = LokiShooter.getInstance(); + addRequirements(funnel); } @@ -24,7 +28,8 @@ public void execute() { } private void setState(){ - stopped = LokiShooter.getInstance().hasCoral(); + stopped = shooter.hasCoral(); + if(funnel.isStalling() && !reversed) { reversed = true; From b6fb9030c3aface353224ba6e5abdf2c4bcaf868 Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Mon, 3 Feb 2025 16:24:02 -0500 Subject: [PATCH 26/34] Added ramp rate to shooter --- src/main/java/com/stuypulse/robot/constants/Settings.java | 3 +++ .../robot/subsystems/lokishooter/LokiShooterImpl.java | 3 +++ 2 files changed, 6 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index acf5fcf..85c4a31 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -26,6 +26,8 @@ public interface Shooter { double BB_DEBOUNCE = 0.0; double CORAL_STALLING_DEBOUNCE = 0.0; double ALGAE_DEBOUNCE = 0.0; + + double RAMP_RATE = 0.0; double DRIVE_CURRENT_THRESHOLD = 30; double DRIVE_CURRENT_LIMIT = 40; @@ -35,6 +37,7 @@ public interface Funnel { SmartNumber MOTOR_SPEED = new SmartNumber("Funnel Speed", 0.0); double BB_DEBOUNCE = 0.0; double FUNNEL_STALLING = 0.0; + double RAMP_RATE = 0.0; double GEAR_RATIO = 0.0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java index 3584a02..d185105 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java @@ -43,6 +43,9 @@ public LokiShooterImpl(){ shooterConfig.TorqueCurrent.PeakReverseTorqueCurrent = -Settings.Shooter.DRIVE_CURRENT_LIMIT; shooterConfig.CurrentLimits.StatorCurrentLimit = Settings.Shooter.DRIVE_CURRENT_LIMIT; shooterConfig.CurrentLimits.StatorCurrentLimitEnable = true; + //Ramp Motor Voltage + shooterConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Shooter.RAMP_RATE; + shooterConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Shooter.RAMP_RATE; shooterMotor.getConfigurator().apply(shooterConfig); } From bcceb78c594bf3a93ed0fe55931b16d4b8f90ff1 Mon Sep 17 00:00:00 2001 From: Baiulus Date: Mon, 3 Feb 2025 16:41:38 -0500 Subject: [PATCH 27/34] Fixed visualizer scaling, added ramp current, deleted targetHeightMeters Co-authored-by: BrianZ60 Co-authored-by: Zixi Qiao Co-authored-by: Ardian Agoes Co-authored-by: Haiiden Co-authored-by: Alvin Cheng --- .../stuypulse/robot/constants/Settings.java | 5 +- .../robot/subsystems/elevator/Elevator.java | 1 - .../subsystems/elevator/ElevatorImpl.java | 13 ++- .../elevator/ElevatorVisualizer.java | 102 ++++-------------- 4 files changed, 32 insertions(+), 89 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 82db2c1..1cc2a4a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -30,8 +30,9 @@ public interface Elevator { double FUNNEL_L2_HEIGHT_METERS = 0.3; // funnel side; should be higher than L2 double FUNNEL_L3_HEIGHT_METERS = 0.55; // funnel side; should be higher than L3 double FUNNEL_L4_HEIGHT_METERS = 0.8; - - double RAMP_RATE = 0.1; + + double RAMP_RATE_CURRENT = 1.0; + double RAMP_RATE_VOLTAGE = 0.1; // FIND OUT REAL GEAR RATIO double GEAR_RATIO = 1.0/5.0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java index a92c2df..d702aff 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -2,7 +2,6 @@ import com.stuypulse.robot.Robot; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Elevator extends SubsystemBase { diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java index 783cd40..703c5ba 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -12,8 +12,6 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -21,12 +19,10 @@ public class ElevatorImpl extends Elevator { private final TalonFX motor; private final SmartNumber targetHeight; private final DigitalInput bumpSwitchBottom; - private final Distance targetHeightMeters; public ElevatorImpl() { motor = new TalonFX(Ports.Elevator.MOTOR); targetHeight = new SmartNumber("Elevator/Target Height", Constants.Elevator.MIN_HEIGHT_METERS); - targetHeightMeters = Units.Meters.of(targetHeight.getAsDouble()); bumpSwitchBottom = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); TalonFXConfiguration elevatorMotorConfig = new TalonFXConfiguration(); @@ -56,8 +52,11 @@ public ElevatorImpl() { elevatorMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Ramp motor voltage - elevatorMotorConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE; - elevatorMotorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE; + elevatorMotorConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE_VOLTAGE; + elevatorMotorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE_VOLTAGE; + + elevatorMotorConfig.OpenLoopRamps.TorqueOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE_CURRENT; + elevatorMotorConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE_CURRENT; var elevatorMotionMagicConfigs = elevatorMotorConfig.MotionMagic; elevatorMotionMagicConfigs.MotionMagicCruiseVelocity = Settings.Elevator.TARGET_CRUISE_VELOCITY; @@ -81,7 +80,7 @@ public void setTargetHeight(double height) { @Override public double getTargetHeight() { - return targetHeightMeters.magnitude(); + return targetHeight.getAsDouble(); } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java index bf7eb98..692f750 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java @@ -25,11 +25,8 @@ public class ElevatorVisualizer { private final MechanismRoot2d elevatorBL; private final MechanismRoot2d elevatorTR; - private final MechanismRoot2d outerBL; - private final MechanismRoot2d outerTR; - - private final MechanismRoot2d innerBL; - private final MechanismRoot2d innerTR; + private final MechanismRoot2d stageTwoBL; + private final MechanismRoot2d stageTwoTR; public static ElevatorVisualizer getInstance(){ return instance; @@ -38,7 +35,7 @@ public static ElevatorVisualizer getInstance(){ public ElevatorVisualizer() { // Mechanism2d - elevator2d = new Mechanism2d(Units.inchesToMeters(17), Units.inchesToMeters(150)); + elevator2d = new Mechanism2d(Units.inchesToMeters(14), Units.inchesToMeters(150)); // Stage One // Bottom Left Node @@ -46,7 +43,7 @@ public ElevatorVisualizer() { elevatorBL.append(new MechanismLigament2d( "Left Tower", - Units.inchesToMeters(47), + Units.inchesToMeters(39), 90, 10, new Color8Bit(Color.kOrange) @@ -55,7 +52,7 @@ public ElevatorVisualizer() { elevatorBL.append(new MechanismLigament2d( "Bottom Tower", - Units.inchesToMeters(11), + Units.inchesToMeters(10), //Change 0, 10, new Color8Bit(Color.kOrange) @@ -63,11 +60,11 @@ public ElevatorVisualizer() { ); // Top Right Node - elevatorTR = elevator2d.getRoot("Elevator TR", Units.inchesToMeters(13), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + elevatorTR = elevator2d.getRoot("Elevator TR", Units.inchesToMeters(12), Units.inchesToMeters(39) + Constants.Elevator.MIN_HEIGHT_METERS); elevatorTR.append(new MechanismLigament2d( "Right Tower", - Units.inchesToMeters(47), + Units.inchesToMeters(39), -90, 10, new Color8Bit(Color.kOrange) @@ -76,7 +73,7 @@ public ElevatorVisualizer() { elevatorTR.append(new MechanismLigament2d( "Top Side", - Units.inchesToMeters(11), + Units.inchesToMeters(10), //Change 180, 10, new Color8Bit(Color.kOrange) @@ -85,20 +82,20 @@ public ElevatorVisualizer() { // Stage Two // Bottom Left Node - outerBL = elevator2d.getRoot("Outer BL", Units.inchesToMeters(3), Constants.Elevator.MIN_HEIGHT_METERS); + stageTwoBL = elevator2d.getRoot("Outer BL", Units.inchesToMeters(3), Constants.Elevator.MIN_HEIGHT_METERS); - outerBL.append(new MechanismLigament2d( + stageTwoBL.append(new MechanismLigament2d( "Left Side", - Units.inchesToMeters(47), + Units.inchesToMeters(35), 90, 10, new Color8Bit(Color.kYellow) ) ); - outerBL.append(new MechanismLigament2d( + stageTwoBL.append(new MechanismLigament2d( "Bottom Side", - Units.inchesToMeters(9), + Units.inchesToMeters(6), 0, 10, new Color8Bit(Color.kYellow) @@ -106,87 +103,34 @@ public ElevatorVisualizer() { ); // Top Right Node - outerTR = elevator2d.getRoot("Outer TR", Units.inchesToMeters(12), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + stageTwoTR = elevator2d.getRoot("Outer TR", Units.inchesToMeters(12), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); - outerTR.append(new MechanismLigament2d( - "Top Side", - Units.inchesToMeters(9), - 180, - 10, - new Color8Bit(Color.kYellow) - ) - ); - - outerTR.append(new MechanismLigament2d( + stageTwoTR.append(new MechanismLigament2d( "Right Side", - Units.inchesToMeters(47), + Units.inchesToMeters(35), -90, 10, new Color8Bit(Color.kYellow) ) ); - - // Carriage - // Bottom Left Node - innerBL = elevator2d.getRoot("Inner BL", Units.inchesToMeters(4), Units.inchesToMeters(1) + Constants.Elevator.MIN_HEIGHT_METERS); - - innerBL.append(new MechanismLigament2d( - "Left Side", - Units.inchesToMeters(7), - 90, - 10, - new Color8Bit(Color.kPink) - ) - ); - - innerBL.append(new MechanismLigament2d( - "Bottom Side", - Units.inchesToMeters(7), - 0, - 10, - new Color8Bit(Color.kPink) - ) - ); - // Top Right Node - innerTR = elevator2d.getRoot("Inner TR", Units.inchesToMeters(11), Units.inchesToMeters(8) + Constants.Elevator.MIN_HEIGHT_METERS); - - innerTR.append(new MechanismLigament2d( + stageTwoTR.append(new MechanismLigament2d( "Top Side", - Units.inchesToMeters(7), + Units.inchesToMeters(6), 180, 10, - new Color8Bit(Color.kPink) - ) - ); - - innerTR.append(new MechanismLigament2d( - "Right Side", - Units.inchesToMeters(7), - -90, - 10, - new Color8Bit(Color.kPink) + new Color8Bit(Color.kYellow) ) ); - + SmartDashboard.putData("Visualizers/Elevator", elevator2d); } public void update() { - // Top of Carriage is Target Height + // Top of Stage Two is target height Elevator elevator = Elevator.getInstance(); - - if (elevator.getTargetHeight() <= Units.inchesToMeters(47)) { - innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); - innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); - } else { - outerBL.setPosition(Units.inchesToMeters(3), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS)); - outerTR.setPosition(Units.inchesToMeters(12), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) + Units.inchesToMeters(47)); - - innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); - innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); - } - + stageTwoBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(4)); + stageTwoTR.setPosition(Units.inchesToMeters(10), elevator.getCurrentHeight() + Units.inchesToMeters(39)); } } \ No newline at end of file From dad92b87e77dea2ad1efe3a9a72830b21214f0a7 Mon Sep 17 00:00:00 2001 From: Mustafa A Date: Mon, 3 Feb 2025 16:45:02 -0500 Subject: [PATCH 28/34] added offset to encoder config --- .../stuypulse/robot/constants/Settings.java | 3 ++- .../stuypulse/robot/subsystems/arm/Arm.java | 1 - .../robot/subsystems/arm/ArmImpl.java | 19 ++++++++++++++++--- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8f22dcb..530a739 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -44,7 +44,8 @@ public interface FF { Rotation2d FUNNEL_ANGLE = Rotation2d.fromDegrees(0); double GEAR_RATIO = 0; Rotation2d BARGE_ANGLE = Rotation2d.fromDegrees(0); - double OFFSET = 0; + double ARM_OFFSET = 0; + double ENCODER_OFFSET = 0; double PID_RAMPING = 0; double FF_RAMPING = 0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index c133e93..542111d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -22,5 +22,4 @@ public static Arm getInstance() { public abstract Rotation2d getArmAngle(); - // public abstract void setVoltage(double voltage); } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 85f16b4..923ec4b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -1,11 +1,13 @@ package com.stuypulse.robot.subsystems.arm; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -57,12 +59,17 @@ public ArmImpl() { motionMagicConfigs.MotionMagicCruiseVelocity = Settings.Arm.MotionMagic.MAX_VEL; // Target cruise velocity of 80 rps motionMagicConfigs.MotionMagicAcceleration = Settings.Arm.MotionMagic.MAX_ACCEL; // Target acceleration of 160 rps/s (0.5 seconds) + MagnetSensorConfigs magnet_config = new MagnetSensorConfigs(); + magnet_config.MagnetOffset = Settings.Arm.ENCODER_OFFSET; + + config.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Arm.PID_RAMPING; config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Arm.FF_RAMPING; armMotor.getConfigurator().apply(config); armMotor.getConfigurator().apply(motionMagicConfigs); + armEncoder.getConfigurator().apply(magnet_config); // actual gooner code @@ -70,6 +77,11 @@ public ArmImpl() { } public void setTargetAngle(Rotation2d targetAngle) { + // if(getArmAngle().getDegrees() > 360) { + // this.targetAngle = Rotation2d.fromDegrees(360); + // } else { + // this.targetAngle = targetAngle; + // } this.targetAngle = targetAngle; } @@ -79,7 +91,7 @@ public Rotation2d getTargetAngle() { public Rotation2d getArmAngle() { - return Rotation2d.fromRotations(armMotor.getPosition().getValueAsDouble() - Settings.Arm.OFFSET); + return Rotation2d.fromRotations(armMotor.getPosition().getValueAsDouble() - Settings.Arm.ARM_OFFSET); } // public void setPID(double kP, double kI, double kD) { @@ -100,12 +112,13 @@ public Rotation2d getArmAngle() { @Override public void periodic() { - armMotor.setControl(new PositionVoltage(getTargetAngle().getRotations())); + MotionMagicVoltage armOutput = new MotionMagicVoltage(getTargetAngle().getDegrees() * 360.0); + // armMotor.setControl(new PositionVoltage(getTargetAngle().getRotations())); + armMotor.setControl(armOutput); SmartDashboard.putNumber("Arm/targetAngle", getTargetAngle().getDegrees()); SmartDashboard.putNumber("Arm/currentAngle",getArmAngle().getDegrees()); - } } From d071406f0eaf8710a304fa903391df3a6ed5f278 Mon Sep 17 00:00:00 2001 From: Mustafa A Date: Mon, 3 Feb 2025 16:51:14 -0500 Subject: [PATCH 29/34] added max jerk, fixed extraneous comments --- .../stuypulse/robot/constants/Settings.java | 1 + .../robot/subsystems/arm/ArmImpl.java | 22 ++++--------------- 2 files changed, 5 insertions(+), 18 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 530a739..1ea7bb1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -52,6 +52,7 @@ public interface FF { public interface MotionMagic{ double MAX_VEL = 0; double MAX_ACCEL = 0; + double JERK = 0; } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 923ec4b..1b4fc6f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -57,7 +57,8 @@ public ArmImpl() { MotionMagicConfigs motionMagicConfigs = config.MotionMagic; motionMagicConfigs.MotionMagicCruiseVelocity = Settings.Arm.MotionMagic.MAX_VEL; // Target cruise velocity of 80 rps - motionMagicConfigs.MotionMagicAcceleration = Settings.Arm.MotionMagic.MAX_ACCEL; // Target acceleration of 160 rps/s (0.5 seconds) + motionMagicConfigs.MotionMagicAcceleration = Settings.Arm.MotionMagic.MAX_ACCEL; // Target acceleration of 160 rps/s (0.5 seconds) + motionMagicConfigs.MotionMagicJerk = Settings.Arm.MotionMagic.JERK; MagnetSensorConfigs magnet_config = new MagnetSensorConfigs(); magnet_config.MagnetOffset = Settings.Arm.ENCODER_OFFSET; @@ -91,28 +92,13 @@ public Rotation2d getTargetAngle() { public Rotation2d getArmAngle() { - return Rotation2d.fromRotations(armMotor.getPosition().getValueAsDouble() - Settings.Arm.ARM_OFFSET); + return Rotation2d.fromRotations(armMotor.getPosition().getValueAsDouble()); } - // public void setPID(double kP, double kI, double kD) { - // config.Slot0.kP = kP; - // config.Slot0.kI = kI; - // config.Slot0.kD = kD; - - // } - - // public void setFF(double kS, double kV, double kA, double kG ){ - // config.Slot0.kS = kS; - // config.Slot0.kV = kV; - // config.Slot0.kA = kA; - // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - - // } - @Override public void periodic() { - MotionMagicVoltage armOutput = new MotionMagicVoltage(getTargetAngle().getDegrees() * 360.0); + MotionMagicVoltage armOutput = new MotionMagicVoltage(getTargetAngle().getRotations()); // armMotor.setControl(new PositionVoltage(getTargetAngle().getRotations())); armMotor.setControl(armOutput); From 768059c3dfc6b8be610a639301280f81a998c0cf Mon Sep 17 00:00:00 2001 From: Mustafa A Date: Mon, 3 Feb 2025 17:00:43 -0500 Subject: [PATCH 30/34] added current ramping --- .../com/stuypulse/robot/constants/Settings.java | 1 + .../stuypulse/robot/subsystems/arm/ArmImpl.java | 15 +++------------ 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 1ea7bb1..f6ba6b4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -48,6 +48,7 @@ public interface FF { double ENCODER_OFFSET = 0; double PID_RAMPING = 0; double FF_RAMPING = 0; + double CURRENT_RAMP = 0; public interface MotionMagic{ double MAX_VEL = 0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 1b4fc6f..4047e8b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -52,8 +52,6 @@ public ArmImpl() { config.Feedback.FeedbackRemoteSensorID = armEncoder.getDeviceID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - - MotionMagicConfigs motionMagicConfigs = config.MotionMagic; motionMagicConfigs.MotionMagicCruiseVelocity = Settings.Arm.MotionMagic.MAX_VEL; // Target cruise velocity of 80 rps @@ -63,26 +61,19 @@ public ArmImpl() { MagnetSensorConfigs magnet_config = new MagnetSensorConfigs(); magnet_config.MagnetOffset = Settings.Arm.ENCODER_OFFSET; - config.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Arm.PID_RAMPING; config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Arm.FF_RAMPING; - + config.OpenLoopRamps.TorqueOpenLoopRampPeriod = Settings.Arm.PID_RAMPING; + config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = Settings.Arm.FF_RAMPING; + armMotor.getConfigurator().apply(config); armMotor.getConfigurator().apply(motionMagicConfigs); armEncoder.getConfigurator().apply(magnet_config); - - // actual gooner code - } public void setTargetAngle(Rotation2d targetAngle) { - // if(getArmAngle().getDegrees() > 360) { - // this.targetAngle = Rotation2d.fromDegrees(360); - // } else { - // this.targetAngle = targetAngle; - // } this.targetAngle = targetAngle; } From f658ec7901acb4a30d1f5ce4355695b6f8cc11e8 Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Mon, 3 Feb 2025 17:32:02 -0500 Subject: [PATCH 31/34] Made Bstreams final Changed motorBeam on shooter to receiver Changed motorBeam on funnel to ir Removed front and back shooting for algae, changed to just shooting Fixed bug where coral acquired in the wrong direction --- .../commands/lokishooter/ShooterAcquireCoral.java | 2 +- ...terDeacquireAlgae.java => ShooterShootAlgae.java} | 2 +- .../robot/commands/lokishooter/ShooterShootBack.java | 6 ++---- .../commands/lokishooter/ShooterShootFront.java | 6 ++---- .../java/com/stuypulse/robot/constants/Ports.java | 4 ++-- .../java/com/stuypulse/robot/constants/Settings.java | 8 +++----- .../robot/subsystems/funnel/CoralFunnelImpl.java | 12 ++++++------ .../subsystems/lokishooter/LokiShooterImpl.java | 6 +++--- 8 files changed, 20 insertions(+), 26 deletions(-) rename src/main/java/com/stuypulse/robot/commands/lokishooter/{ShooterDeacquireAlgae.java => ShooterShootAlgae.java} (89%) diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java index 6b7eeb7..d7e8d88 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterAcquireCoral.java @@ -16,7 +16,7 @@ public ShooterAcquireCoral(){ @Override public void initialize() { - shooter.setSpeed(-Settings.Shooter.CORAL_ACQUIRE_SPEED.getAsDouble()); + shooter.setSpeed(Settings.Shooter.CORAL_ACQUIRE_SPEED.getAsDouble()); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootAlgae.java similarity index 89% rename from src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java rename to src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootAlgae.java index d81b5ec..b948005 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterDeacquireAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootAlgae.java @@ -16,7 +16,7 @@ public ShooterDeacquireAlgae(){ @Override public void initialize() { - shooter.setSpeed(-Settings.Shooter.ALGAE_DEACQUIRE_SPEED.getAsDouble()); + shooter.setSpeed(Settings.Shooter.ALGAE_SHOOT_SPEED.getAsDouble()); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java index d8211fe..7c3de11 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootBack.java @@ -15,16 +15,14 @@ public ShooterShootBack(){ @Override public void initialize(){ - if (shooter.hasAlgae()){ - shooter.setSpeed(-Settings.Shooter.ALGAE_BACK_SPEED.getAsDouble()); - } else if (shooter.hasCoral()){ + if (shooter.hasCoral()){ shooter.setSpeed(-Settings.Shooter.CORAL_BACK_SPEED.getAsDouble()); } } @Override public boolean isFinished(){ - return !shooter.hasAlgae() && !shooter.hasCoral(); + return !shooter.hasCoral(); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java index f4b1517..5675a6a 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootFront.java @@ -15,16 +15,14 @@ public ShooterShootFront(){ @Override public void initialize(){ - if (shooter.hasAlgae()){ - shooter.setSpeed(Settings.Shooter.ALGAE_FRONT_SPEED.getAsDouble()); - } else if (shooter.hasCoral()){ + if (shooter.hasCoral()){ shooter.setSpeed(Settings.Shooter.CORAL_FRONT_SPEED.getAsDouble()); } } @Override public boolean isFinished(){ - return !shooter.hasAlgae() && !shooter.hasCoral(); + return !shooter.hasCoral(); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index f6df319..94aad0f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -16,12 +16,12 @@ public interface Gamepad { // Set values later public interface Shooter { int MOTOR = 0; - int BEAM = 1; + int RECEIVER = 1; } // Set values later public interface Funnel { int MOTOR = 0; - int BEAM = 1; + int IR = 1; } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 85c4a31..92a1bb9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -15,13 +15,11 @@ */ public interface Settings { public interface Shooter { - SmartNumber ALGAE_FRONT_SPEED = new SmartNumber("Algae Target Speed", 1); SmartNumber CORAL_FRONT_SPEED = new SmartNumber("Coral Target Speed",0.75); - SmartNumber ALGAE_BACK_SPEED = new SmartNumber("Algae Target Speed", 1); SmartNumber CORAL_BACK_SPEED = new SmartNumber("Coral Target Speed",0.75); - SmartNumber ALGAE_ACQUIRE_SPEED = new SmartNumber("Algae Acquire Speed", 0.45); - SmartNumber ALGAE_DEACQUIRE_SPEED = new SmartNumber("Algae Deacquire Speed", 0.45); SmartNumber CORAL_ACQUIRE_SPEED = new SmartNumber("Coral Acquire Speed", 0.3); + SmartNumber ALGAE_ACQUIRE_SPEED = new SmartNumber("Algae Acquire Speed", 0.45); + SmartNumber ALGAE_SHOOT_SPEED = new SmartNumber("Algae Shoot Speed", 0.45); double BB_DEBOUNCE = 0.0; double CORAL_STALLING_DEBOUNCE = 0.0; @@ -35,7 +33,7 @@ public interface Shooter { public interface Funnel { SmartNumber MOTOR_SPEED = new SmartNumber("Funnel Speed", 0.0); - double BB_DEBOUNCE = 0.0; + double IR_DEBOUNCE = 0.0; double FUNNEL_STALLING = 0.0; double RAMP_RATE = 0.0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java index 1806733..01dd009 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/CoralFunnelImpl.java @@ -16,22 +16,22 @@ public class CoralFunnelImpl extends CoralFunnel { private final TalonFX funnelMotor; private final TalonFXConfiguration funnelConfig; - private final DigitalInput motorBeam; - private BStream hasCoral; - private BStream isStalling; + private final DigitalInput irSensor; + private final BStream hasCoral; + private final BStream isStalling; private final IStream funnelCurrent; public CoralFunnelImpl(){ funnelMotor = new TalonFX(Ports.Funnel.MOTOR); - motorBeam = new DigitalInput(Ports.Funnel.BEAM); + irSensor = new DigitalInput(Ports.Funnel.IR); //Stall Detection funnelCurrent = IStream.create(() -> Math.abs(funnelMotor.getStatorCurrent().getValueAsDouble())); - hasCoral = BStream.create(motorBeam).not() - .filtered(new BDebounce.Both(Settings.Funnel.BB_DEBOUNCE)); + hasCoral = BStream.create(irSensor).not() + .filtered(new BDebounce.Both(Settings.Funnel.IR_DEBOUNCE)); isStalling = BStream.create(() -> funnelCurrent.get() > Settings.Funnel.DRIVE_CURRENT_THRESHOLD) .filtered(new BDebounce.Both(Settings.Funnel.FUNNEL_STALLING)); diff --git a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java index d185105..28abef6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/lokishooter/LokiShooterImpl.java @@ -18,7 +18,7 @@ public class LokiShooterImpl extends LokiShooter { private final TalonFX shooterMotor; private final TalonFXConfiguration shooterConfig; - private final DigitalInput motorBeam; + private final DigitalInput beamReceiver; private final BStream hasCoral; private final BStream hasAlgae; @@ -27,11 +27,11 @@ public class LokiShooterImpl extends LokiShooter { public LokiShooterImpl(){ shooterMotor = new TalonFX(Ports.Shooter.MOTOR); - motorBeam = new DigitalInput(Ports.Shooter.BEAM); + beamReceiver = new DigitalInput(Ports.Shooter.RECEIVER); shooterCurrent = IStream.create(() -> shooterMotor.getStatorCurrent().getValueAsDouble()); - hasCoral = BStream.create(motorBeam).not() + hasCoral = BStream.create(beamReceiver).not() .filtered(new BDebounce.Both(Settings.Shooter.BB_DEBOUNCE)); hasAlgae = BStream.create(this::detectCurrentSpike) .filtered(new BDebounce.Both(Settings.Shooter.ALGAE_DEBOUNCE)); From 19b8b1c479aa7bcfa7c68d774cd3c1eda615ef05 Mon Sep 17 00:00:00 2001 From: Kalimul Date: Mon, 3 Feb 2025 17:37:54 -0500 Subject: [PATCH 32/34] add curly bracket --- src/main/java/com/stuypulse/robot/constants/Ports.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index b82aebd..4003d8b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -23,6 +23,7 @@ public interface Shooter { public interface Funnel { int MOTOR = 0; int IR = 1; + } public interface Elevator { int MOTOR = 0; From 975a5a4d731e9ca9dfcbb24032aa9df54ccbcb50 Mon Sep 17 00:00:00 2001 From: Mustafa A Date: Mon, 3 Feb 2025 17:39:58 -0500 Subject: [PATCH 33/34] changed doubles to SmartNumbers --- .../stuypulse/robot/constants/Settings.java | 25 +++++++++++-------- .../robot/subsystems/arm/ArmImpl.java | 24 +++++++++--------- 2 files changed, 26 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index f6ba6b4..a813e37 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -6,6 +6,8 @@ package com.stuypulse.robot.constants; +import com.stuypulse.stuylib.network.SmartNumber; + import edu.wpi.first.math.geometry.Rotation2d; /*- @@ -20,17 +22,17 @@ public interface Arm { public interface PID { - double kP = 0; - double kI = 0; - double kD = 0; + SmartNumber kP = new SmartNumber("Arm/PID/kP", 0.0); + SmartNumber kI = new SmartNumber("Arm/PID/kI", 0); + SmartNumber kD = new SmartNumber("Arm/PID/kD", 0); } public interface FF { - double kS = 0; - double kV = 0; - double kA = 0; - double kG = 0; + SmartNumber kS = new SmartNumber("Arm/PID/kP", 0.0); + SmartNumber kV = new SmartNumber("Arm/PID/kI", 0); + SmartNumber kA = new SmartNumber("Arm/PID/kD", 0); + SmartNumber kG = new SmartNumber("Arm/PID/kG", 0); } Rotation2d L2_ANGLE_FRONT = Rotation2d.fromDegrees(0); @@ -42,13 +44,14 @@ public interface FF { Rotation2d L4_ANGLE_BACK = Rotation2d.fromDegrees(0); Rotation2d FUNNEL_ANGLE = Rotation2d.fromDegrees(0); - double GEAR_RATIO = 0; Rotation2d BARGE_ANGLE = Rotation2d.fromDegrees(0); + double GEAR_RATIO = 0; double ARM_OFFSET = 0; double ENCODER_OFFSET = 0; - double PID_RAMPING = 0; - double FF_RAMPING = 0; - double CURRENT_RAMP = 0; + SmartNumber PID_RAMPING = new SmartNumber("Arm/PID_RAMP",0); + SmartNumber FF_RAMPING = new SmartNumber("Arm/FF_RAMP", 0); + SmartNumber CURRENT_RAMPING = new SmartNumber("Arm/CURRENT_RAMP",0); + public interface MotionMagic{ double MAX_VEL = 0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 4047e8b..9894a97 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -36,14 +36,14 @@ public ArmImpl() { Slot0Configs slot0 = new Slot0Configs(); - slot0.kP = Settings.Arm.PID.kP; - slot0.kI = Settings.Arm.PID.kI; - slot0.kD = Settings.Arm.PID.kD; - - slot0.kS = Settings.Arm.FF.kS; - slot0.kV = Settings.Arm.FF.kV; - slot0.kA = Settings.Arm.FF.kA; - slot0.kG = Settings.Arm.FF.kG; + slot0.kP = Settings.Arm.PID.kP.getAsDouble(); + slot0.kI = Settings.Arm.PID.kI.getAsDouble(); + slot0.kD = Settings.Arm.PID.kD.getAsDouble(); + + slot0.kS = Settings.Arm.FF.kS.getAsDouble(); + slot0.kV = Settings.Arm.FF.kV.getAsDouble(); + slot0.kA = Settings.Arm.FF.kA.getAsDouble(); + slot0.kG = Settings.Arm.FF.kG.getAsDouble(); slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0 = slot0; @@ -61,10 +61,10 @@ public ArmImpl() { MagnetSensorConfigs magnet_config = new MagnetSensorConfigs(); magnet_config.MagnetOffset = Settings.Arm.ENCODER_OFFSET; - config.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Arm.PID_RAMPING; - config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Arm.FF_RAMPING; - config.OpenLoopRamps.TorqueOpenLoopRampPeriod = Settings.Arm.PID_RAMPING; - config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = Settings.Arm.FF_RAMPING; + config.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Arm.PID_RAMPING.getAsDouble(); + config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Arm.FF_RAMPING.getAsDouble(); + config.OpenLoopRamps.TorqueOpenLoopRampPeriod = Settings.Arm.PID_RAMPING.getAsDouble(); + config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = Settings.Arm.FF_RAMPING.getAsDouble(); armMotor.getConfigurator().apply(config); From e4fec275f93d1bd3caf692c60e8c829902f415f0 Mon Sep 17 00:00:00 2001 From: Kalimul Date: Mon, 3 Feb 2025 17:40:48 -0500 Subject: [PATCH 34/34] Rename ShooterShootAlgae.java --- .../robot/commands/lokishooter/ShooterShootAlgae.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootAlgae.java b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootAlgae.java index b948005..88d4b50 100644 --- a/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/lokishooter/ShooterShootAlgae.java @@ -5,11 +5,11 @@ import edu.wpi.first.wpilibj2.command.Command; -public class ShooterDeacquireAlgae extends Command { +public class ShooterShootAlgae extends Command { private final LokiShooter shooter; - public ShooterDeacquireAlgae(){ + public ShooterShootAlgae(){ shooter = LokiShooter.getInstance(); addRequirements(shooter); }