prepAnywhere

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;
+}
+
+}

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.

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);
   }

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(5.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();
@@ -40,6 +44,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;

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;/
 

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'");
+  }
 }

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;
@@ -36,6 +38,11 @@ public Motion() {
     hoodPivot.setPosition(ConstMotion.HOOD_NONE_ANGLE);
   }
 
+  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.

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;
   }
+
 }

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;
@@ -70,6 +71,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;