diff --git a/src/main/java/com/team900/frc2026/factories/HoodFactory.java b/src/main/java/com/team900/frc2026/factories/HoodFactory.java index 4cb45cdb..c25cd26b 100644 --- a/src/main/java/com/team900/frc2026/factories/HoodFactory.java +++ b/src/main/java/com/team900/frc2026/factories/HoodFactory.java @@ -1,31 +1,74 @@ package com.team900.frc2026.factories; import com.team900.frc2026.RobotContainer; +import com.team900.frc2026.RobotState; import com.team900.frc2026.subsystems.hood.HoodConstants; import com.team900.frc2026.subsystems.hood.HoodSubsystem; +import com.team900.lib.util.ShooterSetpoint; + import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; + import java.util.function.Supplier; public class HoodFactory { // Sets the hood to a fixed position in radians - public static Command setPosition(RobotContainer container, double radians) { + public static Command setPositionMotionMagicCommand(double radians, RobotContainer container) { + HoodSubsystem hood = container.getHoodSubsystem(); + return hood.motionMagicSetpointCommand(() -> radians).withName("Hood Set Position"); + } + + public static Command aimHoodToPose( + Supplier setPointSupplier, RobotContainer container) { HoodSubsystem hood = container.getHoodSubsystem(); - return hood.motionMagicSetpointCommand(() -> radians) - .withName("Hood Set Position"); + return Commands.run( + () -> + hood.setPositionRadians( + setPointSupplier.get().getHoodRadians(), + setPointSupplier.get().getHoodFF()), + hood) + .withName("Aim Hood to Pose (rad)"); } // Sets the hood to a fixed position and finishes when it arrives within the tolerance public static Command setPositionBlocking( - RobotContainer container, double radians, double tolerance) { - HoodSubsystem hood = container.getHoodSubsystem(); - return hood.motionMagicSetpointCommandBlocking(() -> radians, tolerance) + double radians, double tolerance, RobotContainer container) { + return container + .getHoodSubsystem() + .motionMagicSetpointCommandBlocking(() -> radians, tolerance) .withName("Hood Set Position Blocking"); } // Stows the hood public static Command stow(RobotContainer container) { - return setPosition(container, HoodConstants.kHoodStowTrenchPositionRadians) + return setPositionMotionMagicCommand( + HoodConstants.kHoodStowTrenchPositionRadians, container) .withName("Hood Stow"); } + + public static Command zero(RobotContainer container) { + return new InstantCommand(container.getHoodSubsystem()::disableSoftLimits) + .andThen( + container + .getHoodSubsystem() + .dutyCycleCommand(() -> -0.05) + .until(RobotState.getInstance()::getHoodHasZeroed) + .andThen(container.getHoodSubsystem()::enableSoftLimits)); + } + + public static Command pass(RobotContainer container, double tolerance) { + return container + .getHoodSubsystem() + .motionMagicSetpointCommandBlocking(() -> 0.6, tolerance) + .withName("Hood pass Position Blocking"); + } + + public static Command shoot(RobotContainer container, double tolerance) { + return container + .getHoodSubsystem() + .motionMagicSetpointCommandBlocking(() -> 0.3, tolerance) + .withName("Hood Shoot Position Blocking"); + } } diff --git a/src/main/java/com/team900/frc2026/factories/ShooterFactory.java b/src/main/java/com/team900/frc2026/factories/ShooterFactory.java index 984b95bf..88208716 100644 --- a/src/main/java/com/team900/frc2026/factories/ShooterFactory.java +++ b/src/main/java/com/team900/frc2026/factories/ShooterFactory.java @@ -6,9 +6,8 @@ import com.team900.frc2026.RobotContainer; import com.team900.frc2026.subsystems.shooter.ShooterConstants; -import com.team900.lib.util.ShooterSetpoint; + import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.Supplier; public class ShooterFactory { @@ -17,11 +16,7 @@ public class ShooterFactory { /* Commands for shooting */ public static Command idle() { - return container.getShooterSubsystem().setTorqueCurrentFOC(() -> ShooterConstants.kIdleRPM); - } - public static Command setShooterRPS(Supplier setpointSupplier) { - var shooter = container.getShooterSubsystem(); - return shooter.velocitySetpointCommand(setpointSupplier.get()::getShooterRPS); + return container.getShooterSubsystem().setTorqueCurrentFOC(ShooterConstants.IDLE_RPS); } } diff --git a/src/main/java/com/team900/frc2026/factories/ShootingFactory.java b/src/main/java/com/team900/frc2026/factories/ShootingFactory.java index 0c95e882..afd69c24 100644 --- a/src/main/java/com/team900/frc2026/factories/ShootingFactory.java +++ b/src/main/java/com/team900/frc2026/factories/ShootingFactory.java @@ -4,14 +4,48 @@ package com.team900.frc2026.factories; +import com.team900.frc2026.RobotContainer; +import com.team900.frc2026.RobotState; +import com.team900.lib.util.ShooterSetpoint; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import java.util.function.Supplier; public class ShootingFactory { /* Commands for shooting */ - public static Command SpinBoth() { - - return null; + public static Command shoot( + Supplier setPointSupplier, RobotContainer container) { + return (new ParallelCommandGroup( + ShooterFactory.setShooterRPS(setPointSupplier, container), + SuperstructureFactory.aim(setPointSupplier, container)) + .until( + () -> + MathUtil.isNear( + setPointSupplier.get().getShooterRPS(), + container + .getShooterSubsystem() + .getCurrentVelocity(), + 1) + && MathUtil.isNear( + setPointSupplier.get().getHoodRadians(), + container + .getHoodSubsystem() + .getCurrentPosition(), + 1) + && MathUtil.isNear( + 0, + RobotState.getInstance() + .getLatestRotationRobotToHub() + .getDegrees(), + 3))) + .andThen( + new ParallelCommandGroup( + IntakeFactory.runIntake(container), + HandoffFactory.runHandoff(container), + SpindexerFactory.runSpindexer(container)) + .onlyWhile(container.getDriveCommand()::isNearTarget)); } -} +} \ No newline at end of file diff --git a/src/main/java/com/team900/frc2026/factories/SuperstructureFactory.java b/src/main/java/com/team900/frc2026/factories/SuperstructureFactory.java index b43b23a6..fd8c3747 100644 --- a/src/main/java/com/team900/frc2026/factories/SuperstructureFactory.java +++ b/src/main/java/com/team900/frc2026/factories/SuperstructureFactory.java @@ -1,18 +1,45 @@ package com.team900.frc2026.factories; +import java.util.function.Supplier; + import com.team900.frc2026.RobotContainer; +import com.team900.lib.util.ShooterSetpoint; + import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; + +import java.util.function.Supplier; +import java.util.function.DoubleSupplier; public class SuperstructureFactory { - public static Command aimAndShootAll(RobotContainer container) { - return null; - /*Command to shoot all balls in robot (should auto aim) + public static Command aim( + Supplier setPointSupplier, + RobotContainer container, + DoubleSupplier turretRadiansFromCenter, + DoubleSupplier turretFF) { + // return null; + return new ParallelCommandGroup( + new InstantCommand(() -> container.getDriveCommand().setKAiming(true)), + HoodFactory.aimHoodToPose(setPointSupplier, container), + TurretFactory.aimTurretToPoseDegrees(container, turretRadiansFromCenter,turretFF)); + /*Command to aim at target * Aim turret - * Aim hood - * Maintain shooter and handoff speed - * Move spindexer to load to turret as needed - */ + * Aim hood */ + } + + public static Command stow(RobotContainer container) { + return new ParallelCommandGroup(HoodFactory.stow(container)); + } + + public static Command trench(RobotContainer container) { + return new ParallelCommandGroup( + HoodFactory.stow(container), IntakeFactory.deploySlapdown(container)); + } + + public static Command intakeDeploy(RobotContainer container) { + return IntakeFactory.deploySlapdown(container); } } diff --git a/src/main/java/com/team900/frc2026/subsystems/hood/HoodConstants.java b/src/main/java/com/team900/frc2026/subsystems/hood/HoodConstants.java index ecf69512..b3f5ed35 100644 --- a/src/main/java/com/team900/frc2026/subsystems/hood/HoodConstants.java +++ b/src/main/java/com/team900/frc2026/subsystems/hood/HoodConstants.java @@ -25,7 +25,6 @@ public class HoodConstants { public static final double kHoodMinPositionRadians = Math.PI / 12; public static final double kHoodMaxPositionRadians = Math.PI / 4; public static final double kHoodZeroedAngleDegrees = 15; - public static final double kHoodEpsilon = Units.degreesToRadians(1.0); public static final double kHoodShootingEpsilon = Units.degreesToRadians(5.0); //TODO: find this experimetnatlly @@ -36,7 +35,7 @@ public class HoodConstants { public static CanCoderConfig kHoodCanCoderConfig = new CanCoderConfig(); static { - // subsystem configs + // subsystem configs, TODO: add the freaking talon debicve id kHoodConfig.name = "Hood"; kHoodConfig.cancoderToUnitsRatio = 170. / 10.; @@ -44,15 +43,15 @@ public class HoodConstants { kHoodConfig.kMaxPositionUnits = Math.PI / 4; kHoodConfig.kMinPositionUnits = Math.PI / 12; kHoodConfig.momentOfInertia = 0.0255356814; - kHoodConfig.talonCANID = new CANDeviceId(30, Constants.kCanBusCanivoreMech); + kHoodConfig.talonCANID = new CANDeviceId(0, Constants.kCanBusCanivoreMech); kHoodConfig.unitToRotorRatio = 15.625 * 170. / 10.; // configs for sim kHoodConfig.ratioForSim = kHoodGearRatio; kHoodConfig.cancoderUnitsForSim = 1; - // cancoder config TODO: add the freaking cancoder debicve id - kHoodCanCoderConfig.CANID = new CANDeviceId(0, Constants.kCanBusCanivoreMech); + // cancoder config + kHoodCanCoderConfig.CANID = new CANDeviceId(30, Constants.kCanBusCanivoreMech); kHoodCanCoderConfig.config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; kHoodCanCoderConfig.config.MagnetSensor.MagnetOffset = 0; kHoodCanCoderConfig.config.MagnetSensor.SensorDirection = diff --git a/src/main/java/com/team900/frc2026/subsystems/intake/IntakeRollerConstants.java b/src/main/java/com/team900/frc2026/subsystems/intake/IntakeRollerConstants.java index 8b223e6d..adff84ee 100644 --- a/src/main/java/com/team900/frc2026/subsystems/intake/IntakeRollerConstants.java +++ b/src/main/java/com/team900/frc2026/subsystems/intake/IntakeRollerConstants.java @@ -1,6 +1,9 @@ package com.team900.frc2026.subsystems.intake; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.team900.frc2026.Constants; import com.team900.lib.drivers.CANDeviceId; import com.team900.lib.subsystems.ServoMotorSubsystemConfig; @@ -10,15 +13,28 @@ public class IntakeRollerConstants { public static ServoMotorSubsystemConfig kIntakeRollerConfig = new ServoMotorSubsystemConfig(); static { - kIntakeRollerConfig.fxConfig = new TalonFXConfiguration(); kIntakeRollerConfig.name = "Intake"; - kIntakeRollerConfig.talonCANID = new CANDeviceId(60, Constants.kCanBusCanivoreMech); - // Top Roller - 16t:24t 24t:15t 30t:18t overall: 0.5625:1 - // Middle Roller - 16t:24t 24t:15t 30t:18t 18t:18t overall: 0.5625:1 - // Bottom Roller - 16t:24t 24t:15t 30t:18t 36t:36t 15t:15t overall: 0.5625:1 - kIntakeRollerConfig.unitToRotorRatio = 16.0 / 24.0 * 24.0 / 15.0 * 30.0 / 18.0; - } + kIntakeRollerConfig.talonCANID = new CANDeviceId(60, new CANBus("mech")); + // Top Roller - 16t:24t 24t:15t 30t:15t overall: 0.46875:1 + // Middle Roller - 16t:24t 24t:15t 30t:15t 24t:24t overall: 0.46875:1 + // Bottom Roller - 16t:24t 24t:15t 30t:15t 36t:36t 15t:15t overall: 0.46875:1 + kIntakeRollerConfig.unitToRotorRatio = 16.0 / 24.0 * 24.0 / 15.0 * 30.0 / 15.0; + + kIntakeRollerConfig.fxConfig = new TalonFXConfiguration(); + kIntakeRollerConfig.fxConfig.OpenLoopRamps = Constants.makeDefaultOpenLoopRampConfig(); + kIntakeRollerConfig.fxConfig.CurrentLimits.StatorCurrentLimit = 80; + kIntakeRollerConfig.fxConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kIntakeRollerConfig.fxConfig.CurrentLimits.SupplyCurrentLimit = 70; + kIntakeRollerConfig.fxConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + kIntakeRollerConfig.fxConfig.CurrentLimits.SupplyCurrentLowerLimit = 40; + kIntakeRollerConfig.fxConfig.CurrentLimits.SupplyCurrentLowerTime = 1; + kIntakeRollerConfig.fxConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + kIntakeRollerConfig.fxConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + + } + public static final double kIntakeGearRatio = 0.46875; public static final double kIntakeDutyCycle = 0.5; public static final double kIntakeDutyCycleExhaust = -0.5; } diff --git a/src/main/java/com/team900/frc2026/subsystems/shooter/ShooterConstants.java b/src/main/java/com/team900/frc2026/subsystems/shooter/ShooterConstants.java index 7e19c1f8..c8466a22 100644 --- a/src/main/java/com/team900/frc2026/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/com/team900/frc2026/subsystems/shooter/ShooterConstants.java @@ -1,6 +1,11 @@ package com.team900.frc2026.subsystems.shooter; +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.team900.frc2026.Constants; import com.team900.lib.drivers.CANDeviceId; import com.team900.lib.subsystems.ServoMotorSubsystemWithFollowersConfig; @@ -8,20 +13,63 @@ public class ShooterConstants { - public static ServoMotorSubsystemWithFollowersConfig kShooterConfig = - new ServoMotorSubsystemWithFollowersConfig(); + + public static ServoMotorSubsystemWithFollowersConfig kShooterConfig = new ServoMotorSubsystemWithFollowersConfig(); + public static ServoMotorSubsystemWithFollowersConfig.FollowerConfig kShooterLeftConfig = new FollowerConfig(); static { - kShooterConfig.name = "Shooter"; - kShooterConfig.talonCANID = new CANDeviceId(55, Constants.kCanBusCanivoreMech); - kShooterConfig.unitToRotorRatio = 1.0; + kShooterConfig.name = "Shooter Right"; + kShooterConfig.talonCANID = new CANDeviceId(55, new CANBus("mech")); + kShooterConfig.unitToRotorRatio = 1; + + kShooterConfig.fxConfig = new TalonFXConfiguration(); kShooterConfig.fxConfig.OpenLoopRamps = Constants.makeDefaultOpenLoopRampConfig(); - kShooterConfig.followers = new FollowerConfig[] {new FollowerConfig()}; - kShooterConfig.followers[0].config.talonCANID = - new CANDeviceId(56, Constants.kCanBusCanivoreMech); - kShooterConfig.followers[0].config.fxConfig = new TalonFXConfiguration(); - } + kShooterConfig.fxConfig.CurrentLimits.StatorCurrentLimit = 150; + kShooterConfig.fxConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kShooterConfig.fxConfig.CurrentLimits.SupplyCurrentLimit = 80; + kShooterConfig.fxConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + kShooterConfig.fxConfig.CurrentLimits.SupplyCurrentLowerLimit = 40; + kShooterConfig.fxConfig.CurrentLimits.SupplyCurrentLowerTime = 1; + kShooterConfig.fxConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + kShooterConfig.fxConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + kShooterConfig.fxConfig.TorqueCurrent.PeakForwardTorqueCurrent = 150; + kShooterConfig.fxConfig.TorqueCurrent.PeakReverseTorqueCurrent = 150; + + + - public static final double kIdleRPM = 2000.0; -} + + kShooterLeftConfig.config.name = "Shooter Left"; + kShooterLeftConfig.inverted = false; + kShooterLeftConfig.config.momentOfInertia = 1; + kShooterLeftConfig.config.talonCANID = new CANDeviceId(56, new CANBus("mech")); + kShooterLeftConfig.config.unitToRotorRatio = 1; + +kShooterLeftConfig.config.fxConfig = new TalonFXConfiguration(); + kShooterLeftConfig.config.fxConfig.OpenLoopRamps = Constants.makeDefaultOpenLoopRampConfig(); + kShooterLeftConfig.config.fxConfig.CurrentLimits.StatorCurrentLimit = 150; + kShooterLeftConfig.config.fxConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kShooterLeftConfig.config.fxConfig.CurrentLimits.SupplyCurrentLimit = 80; + kShooterLeftConfig.config.fxConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + kShooterLeftConfig.config.fxConfig.CurrentLimits.SupplyCurrentLowerLimit = 40; + kShooterLeftConfig.config.fxConfig.CurrentLimits.SupplyCurrentLowerTime = 1; + kShooterLeftConfig.config.fxConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + kShooterLeftConfig.config.fxConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + kShooterLeftConfig.config.fxConfig.TorqueCurrent.PeakForwardTorqueCurrent = 150; + kShooterLeftConfig.config.fxConfig.TorqueCurrent.PeakReverseTorqueCurrent = 150; + + + + kShooterConfig.followers[0] = kShooterLeftConfig; + + + + } + public static final double kShooterGearRatio = 1; + public static final double kShooterDutyCycle = 0.5; + public static final double kShooterDutyCycleExhaust = -0.5; + public static final DoubleSupplier IDLE_RPS = null; // TODO: find this experimentally +} \ No newline at end of file diff --git a/src/main/java/com/team900/frc2026/subsystems/turret/TurretConstants.java b/src/main/java/com/team900/frc2026/subsystems/turret/TurretConstants.java index 123b154e..4e4a0473 100644 --- a/src/main/java/com/team900/frc2026/subsystems/turret/TurretConstants.java +++ b/src/main/java/com/team900/frc2026/subsystems/turret/TurretConstants.java @@ -44,7 +44,7 @@ public class TurretConstants { public static final double toleranceRad = 0.1; /* * - * config.Slot0.kS = 0.18; + * config.Slot0.kS = 0.18; config.Slot0.kP = 6.0; config.Slot0.kD = 0.1; config.Slot0.kV = 0.120;