From e88e106bf850c363afc07189df5f791830ef87fe Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 13 Jun 2026 21:04:35 -0700 Subject: [PATCH] prepAnywhere --- .../java/frc/robot/commands/SubCommands.java | 25 +++++++++++++++++++ .../commands/states/preps/PrepAnywhere.java | 11 ++++++++ .../java/frc/robot/constants/ConstField.java | 2 ++ .../java/frc/robot/constants/ConstMotion.java | 12 +++++++++ .../java/frc/robot/constants/ConstRotors.java | 16 ++++++++++++ .../java/frc/robot/subsystems/Drivetrain.java | 5 ++++ .../java/frc/robot/subsystems/Motion.java | 7 ++++++ .../java/frc/robot/subsystems/RobotPoses.java | 17 +++++++++++++ .../java/frc/robot/subsystems/Rotors.java | 6 +++++ 9 files changed, 101 insertions(+) create mode 100644 src/main/java/frc/robot/commands/SubCommands.java diff --git a/src/main/java/frc/robot/commands/SubCommands.java b/src/main/java/frc/robot/commands/SubCommands.java new file mode 100644 index 0000000..f749a1e --- /dev/null +++ b/src/main/java/frc/robot/commands/SubCommands.java @@ -0,0 +1,25 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import frc.robot.RobotContainer; + +public class SubCommands{ +public static Angle aim(boolean aimDT){ + Pose2d hubPose = RobotContainer.robotPose.getHub(); + Distance distanceToHub = RobotContainer.robotPose.getDistanceToHub(); + Angle targetHoodAngle = RobotContainer.motionInstance.getMappedHoodAngle(distanceToHub); + AngularVelocity targetFlyWheelSpeed = RobotContainer.rotorsInstance.getMappedFlywheelSpeed(distanceToHub); + Angle targetDrivetrainAngle = RobotContainer.drivetrainInstance.snapToTarget(hubPose); + + RobotContainer.rotorsInstance.setFlyWheelSpeeds(targetFlyWheelSpeed); + RobotContainer.motionInstance.setHoodPivotAngle(targetHoodAngle); + if (aimDT){ + RobotContainer.drivetrainInstance.setDriveRotation(targetDrivetrainAngle); + } + return targetDrivetrainAngle; +} + +} diff --git a/src/main/java/frc/robot/commands/states/preps/PrepAnywhere.java b/src/main/java/frc/robot/commands/states/preps/PrepAnywhere.java index 5855253..a726494 100644 --- a/src/main/java/frc/robot/commands/states/preps/PrepAnywhere.java +++ b/src/main/java/frc/robot/commands/states/preps/PrepAnywhere.java @@ -5,27 +5,38 @@ package frc.robot.commands.states.preps; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.commands.SubCommands; +import frc.robot.constants.ConstRotors; +import frc.robot.subsystems.StateMachine.RobotState; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class PrepAnywhere extends Command { /** Creates a new PrepAnywhere. */ public PrepAnywhere() { + addRequirements(RobotContainer.stateMachineInstance); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override public void initialize() { + RobotContainer.visionInstance.setVisionEnabled(true); + RobotContainer.stateMachineInstance.setRobotState(RobotState.PREP_ANYWHERE); + RobotContainer.drivetrainInstance.setManualDrive(false); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + SubCommands.aim(true); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + RobotContainer.drivetrainInstance.setManualDrive(true); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/constants/ConstField.java b/src/main/java/frc/robot/constants/ConstField.java index e7d3c50..348df57 100644 --- a/src/main/java/frc/robot/constants/ConstField.java +++ b/src/main/java/frc/robot/constants/ConstField.java @@ -131,11 +131,13 @@ public static boolean isRedAlliance() { }; public static class FieldElements { + private static final Pose2d HUB_POSE = new Pose2d(4.629215, 4.031745, new Rotation2d()); private static final Pose2d RESET_POSE = new Pose2d(0, 0, new Rotation2d()); } public static class FieldElementGroups { + public static final Pose2dAllianceSet HUB_POSE_SET = new Pose2dAllianceSet(FieldElements.HUB_POSE); public static final Pose2dAllianceSet RESET_POSE_SET = new Pose2dAllianceSet( FieldElements.RESET_POSE); } diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 118cfc7..9a370e4 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -13,12 +13,16 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Time; /** Add your docs here. */ public class ConstMotion { + + public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap(); + public static final Angle INTAKE_PIVOT_TOLERANCE = Degrees.of(1.0); public static final Time INTAKE_PIVOT_AGITATION_TIME = Units.Seconds.of(1.0 / 3.0); public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); @@ -39,6 +43,14 @@ public class ConstMotion { static { + hoodAngleMap.put(Units.Inches.of(200).in(Units.Inches), Degrees.of(29).in(Degrees)); + hoodAngleMap.put(Units.Inches.of(180).in(Units.Inches), Degrees.of(24).in(Degrees)); + hoodAngleMap.put(Units.Inches.of(140).in(Units.Inches), Degrees.of(24).in(Degrees)); + hoodAngleMap.put(Units.Inches.of(120).in(Units.Inches), Degrees.of(22).in(Degrees)); + hoodAngleMap.put(Units.Inches.of(100).in(Units.Inches), Degrees.of(16).in(Degrees)); + hoodAngleMap.put(Units.Inches.of(80).in(Units.Inches), Degrees.of(13.25).in(Degrees)); + hoodAngleMap.put(Units.Inches.of(50).in(Units.Inches), Degrees.of(1).in(Degrees)); + INTAKE_PIVOT_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Brake; INTAKE_PIVOT_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index 64c3624..4b9c44f 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -4,16 +4,22 @@ package frc.robot.constants; +import static edu.wpi.first.units.Units.RPM; + import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; /** Add your docs here. */ public class ConstRotors { + public static final InterpolatingDoubleTreeMap flyWheelSpeedMap = new InterpolatingDoubleTreeMap(); + public static final TalonFXConfiguration SERIALIZER_ROLLERS_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration INTAKE_ROLLERS_EAST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration INTAKE_ROLLERS_WEST_CONFIGURATION = new TalonFXConfiguration(); @@ -44,6 +50,16 @@ public class ConstRotors { static { + flyWheelSpeedMap.put(Units.Inches.of(190).in(Units.Inches), RPM.of(4200).in(RPM)); + flyWheelSpeedMap.put(Units.Inches.of(180).in(Units.Inches), RPM.of(4125).in(RPM)); + flyWheelSpeedMap.put(Units.Inches.of(165.1).in(Units.Inches), RPM.of(4100).in(RPM)); + flyWheelSpeedMap.put(Units.Inches.of(165).in(Units.Inches), RPM.of(4000).in(RPM)); + flyWheelSpeedMap.put(Units.Inches.of(160).in(Units.Inches), RPM.of(3800).in(RPM)); + flyWheelSpeedMap.put(Units.Inches.of(150).in(Units.Inches), RPM.of(3700).in(RPM)); + flyWheelSpeedMap.put(Units.Inches.of(140).in(Units.Inches), RPM.of(3600).in(RPM)); + flyWheelSpeedMap.put(Units.Inches.of(120).in(Units.Inches), RPM.of(3400).in(RPM)); + flyWheelSpeedMap.put(Units.Inches.of(0).in(Units.Inches), RPM.of(3400).in(RPM)); + // SHOOTER_TRANSFER_EAST_CONFIGURATION.MotorOutput.NeutralMode = // NeutralModeValue.Coast;/ diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index e575d46..b76600d 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -236,4 +236,9 @@ public Angle pigeonYaw() { public Rotation2d getRawHeading() { return getState().RawHeading; } + + public void setManualDrive(boolean b) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setManualDrive'"); + } } diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index edf68b8..c299534 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -10,7 +10,9 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.DeviceIDs; import frc.robot.Robot; @@ -35,6 +37,11 @@ public Motion() { hoodPivot.getConfigurator().apply(ConstMotion.HOOD_PIVOT_CONFIGURATION); } + public static Angle getMappedHoodAngle(Distance distance) { + double angle = ConstMotion.hoodAngleMap.get(distance.in(Units.Inches)); + return Degrees.of(angle); + } + /** * this codes takes an angle and setting a desired angle for the Intake to * allign to. diff --git a/src/main/java/frc/robot/subsystems/RobotPoses.java b/src/main/java/frc/robot/subsystems/RobotPoses.java index 7aab65d..f6c7cbe 100644 --- a/src/main/java/frc/robot/subsystems/RobotPoses.java +++ b/src/main/java/frc/robot/subsystems/RobotPoses.java @@ -5,16 +5,22 @@ package frc.robot.subsystems; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; +import frc.robot.constants.ConstField; @Logged public class RobotPoses extends SubsystemBase { /** Creates a new RobotPoses. */ + Distance distanceToHub = Units.Meters.of(0); + Field2d field2d = new Field2d(); FieldObject2d robotObject = field2d.getObject("Robot"); @@ -32,5 +38,16 @@ public void periodic() { // Robot Positions modelDrivetrain = new Pose3d(RobotContainer.drivetrainInstance.getPose()); + Pose2d hubPose = getHub(); + distanceToHub = Units.Meters.of(RobotContainer.drivetrainInstance.getPose().getTranslation().getDistance(hubPose.getTranslation())); + } + + public Pose2d getHub() { + return ConstField.FieldElementGroups.HUB_POSE_SET.getAlliancePoses().get(0); + + } + public Distance getDistanceToHub(){ + return distanceToHub; } + } diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index 0ee0be0..0f46e79 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -12,6 +12,7 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.DeviceIDs.rotorIDs; import frc.robot.Robot; @@ -58,6 +59,11 @@ public Rotors() { // final MotionMagicVelocityVoltage TransferVelocityRequest = new // MotionMagicVelocityVoltage(0);/ + public static AngularVelocity getMappedFlywheelSpeed(Distance distance) { + double rpm = ConstRotors.flyWheelSpeedMap.get(distance.in(Units.Inches)); + return Units.RPM.of(rpm); + } + public AngularVelocity getFlyWheelSpeeds() { if (Robot.isSimulation()) { return lastDesiredFlyWheelSpeed;