diff --git a/comp/src/main/java/org/team100/frc2026/ShooterHood.java b/comp/src/main/java/org/team100/frc2026/ShooterHood.java index aa823b3..62da640 100644 --- a/comp/src/main/java/org/team100/frc2026/ShooterHood.java +++ b/comp/src/main/java/org/team100/frc2026/ShooterHood.java @@ -1,5 +1,135 @@ package org.team100.frc2026; -public class ShooterHood { - +import java.util.Optional; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import org.team100.lib.config.Identity; +import org.team100.lib.config.PIDConstants; +import org.team100.lib.controller.r1.PIDFeedback; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.mechanism.RotaryMechanism; +import org.team100.lib.motor.MotorPhase; +import org.team100.lib.motor.NeutralMode100; +import org.team100.lib.motor.ctre.KrakenX44Motor; +import org.team100.lib.motor.ctre.KrakenX60Motor; +import org.team100.lib.motor.sim.SimulatedBareMotor; +import org.team100.lib.profile.r1.IncrementalProfile; +import org.team100.lib.profile.r1.TrapezoidIncrementalProfile; +import org.team100.lib.reference.r1.IncrementalProfileReferenceR1; +import org.team100.lib.reference.r1.ProfileReferenceR1; +import org.team100.lib.sensor.position.absolute.sim.SimulatedRotaryPositionSensor; +import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder; +import org.team100.lib.sensor.position.incremental.ctre.Talon6Encoder; +import org.team100.lib.servo.AngularPositionServo; +import org.team100.lib.servo.OnboardAngularPositionServo; +import org.team100.lib.servo.OutboardAngularPositionServo; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.turret.Turret.Solution; +import org.team100.lib.util.CanId; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class ShooterHood extends SubsystemBase { + private final AngularPositionServo m_servo; + + public ShooterHood(LoggerFactory parent, CanId canID) { + LoggerFactory log = parent.type(this); + + switch (Identity.instance) { + + case TEST_BOARD_B0 -> { + float gearRatio = 10; + PIDConstants PID = PIDConstants.makePositionPID(log, 1); + double supplyLimit = 50; + double statorLimit = 20; + KrakenX44Motor m_motor = new KrakenX44Motor( + log, // LoggerFactor y parent, + canID, // CanId canId, + NeutralMode100.COAST, // NeutralMode neutral, + MotorPhase.REVERSE, // MotorPhase motorPhase, + supplyLimit, // og 50 //double supplyLimit, + statorLimit, // og 2 //double statorLimit, + KrakenX60Motor.highFrictionFF(log), // Feedforward100 ff + KrakenX60Motor.highFriction(log), + PID // PIDConstants pid, + ); + Talon6Encoder encoder = m_motor.encoder(); + + TrapezoidIncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); + ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, () -> profile, 0.05, 0.05); + double initialPosition = 0; + RotaryMechanism climberMech = new RotaryMechanism( + log, m_motor, encoder, initialPosition, gearRatio, + Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); + m_servo = new OutboardAngularPositionServo(log, climberMech, ref); + + } + + default -> { + SimulatedBareMotor m_motor = new SimulatedBareMotor(log, 600); + + IncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05); + ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, () -> profile, 0.05, 0.05); + PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1); + + IncrementalBareEncoder encoder = m_motor.encoder(); + SimulatedRotaryPositionSensor sensor = new SimulatedRotaryPositionSensor(log, encoder, 1); + + RotaryMechanism climberMech = new RotaryMechanism( + log, m_motor, sensor, 1, + Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); + + m_servo = new OnboardAngularPositionServo(log, climberMech, ref, feedback); + } + } + } + + + private Optional getAbsoluteBearingInstantaneous() { + ModelSE2 state = m_state.get(); + Translation2d target = m_target.get(); + return Optional.of( + new Solution( + target.minus(state.translation()).getAngle(), + Rotation2d.kZero)); + } + + @Override + public void periodic() { + m_servo.periodic(); + } + + public Command gotoAngle(DoubleSupplier trigger) { + return new FunctionalCommand( + () -> reset(), // onInit + () -> setAngle(trigger.getAsDouble()), // onExecute + interrupted -> { // onEnd + }, + () -> m_servo.atGoal(), // isFinished + this).withName("Shooter Hood Angler"); + } + + public Command stop() { + return run(this::stopServo).withName("Shooter Hood Stop"); + } + + public void stopServo() { + m_servo.stop(); + } + + private void reset() { + m_servo.reset(); + } + + /** Use a profile to set the position. */ + private void setAngle(double value) { + m_servo.setPositionProfiled(value, 0); + } + } diff --git a/comp/src/main/java/org/team100/frc2026/ShooterTable.java b/comp/src/main/java/org/team100/frc2026/ShooterTable.java new file mode 100644 index 0000000..3a85f50 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/ShooterTable.java @@ -0,0 +1,32 @@ +package org.team100.frc2026; + +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + +/** Interpolates gun elevation in radians, given range in meters. */ +public class ShooterTable { + + public static final ShooterTable instance = new ShooterTable(); + + private final InterpolatingDoubleTreeMap m_table; + + public ShooterTable() { + m_table = new InterpolatingDoubleTreeMap(); + loadTable(); + } + + public double getAngleRad(double rangeM) { + return m_table.get(rangeM); + } + + public void loadTable() { + m_table.put(1.49, 0.9); + m_table.put(2.07, 0.78); + m_table.put(2.5, 0.66); + m_table.put(3.02, 0.59); + m_table.put(3.59, 0.53); + m_table.put(4.1, 0.475); + m_table.put(4.5, 0.44); + + } + +} \ No newline at end of file diff --git a/comp/src/main/java/org/team100/frc2026/robot/Binder.java b/comp/src/main/java/org/team100/frc2026/robot/Binder.java index 444dcad..94c1a4f 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Binder.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Binder.java @@ -120,6 +120,8 @@ public void bind() { m_machinery.m_intake.stop()); m_machinery.m_extender.setDefaultCommand( m_machinery.m_extender.stop()); + m_machinery.m_shooterHood.setDefaultCommand( + m_machinery.m_shooterHood.stop()); /////////////////////////// // @@ -138,6 +140,7 @@ public void bind() { /// /// SUBSYSTEMS /// + whileTrue(driver::b, m_machinery.m_shooter.shoot()); whileTrue(driver::x, m_machinery.m_intake.intake()); diff --git a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java index 081029c..b5cda44 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java @@ -10,6 +10,7 @@ import org.team100.frc2026.IntakeExtend; import org.team100.frc2026.Serializer; import org.team100.frc2026.Shooter; +import org.team100.frc2026.ShooterHood; import org.team100.lib.coherence.Takt; import org.team100.lib.indicator.Beeper; import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; @@ -68,7 +69,7 @@ public class Machinery { public final TrajectoryVisualization m_trajectoryViz; public final SwerveKinodynamics m_swerveKinodynamics; final AprilTagRobotLocalizer m_localizer; - public final SwerveDriveSubsystem m_drive; + public final static SwerveDriveSubsystem m_drive; final Beeper m_beeper; final Shooter m_shooter; final Intake m_intake; @@ -76,6 +77,7 @@ public class Machinery { final Serializer m_serializer; final ClimberExtension m_ClimberExtension; final Climber m_Climber; + final ShooterHood m_shooterHood; public Machinery() { @@ -94,6 +96,7 @@ public Machinery() { m_serializer = new Serializer(driveLog); m_ClimberExtension = new ClimberExtension(driveLog); m_Climber = new Climber(driveLog); + m_shooterHood = new ShooterHood(driveLog, null); //////////////////////////////////////////////////////////// //