1 create motion subsystem

src/main/java/frc/robot/DeviceIDs.java

@@ -43,6 +43,7 @@ public static class rotorIDs {
 
   // Motion Profile IDs: 30~49
   public static class motionIDs {
-
+    public static final int HOOD_PIVOT_CAN = 32;
+    public static final int INTAKE_PIVOT_CAN = 30;
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -25,6 +25,7 @@
 import frc.robot.subsystems.DriverStateMachine;
 import frc.robot.subsystems.DriverStateMachine.DriverState;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Motion;
 import frc.robot.subsystems.RobotPoses;
 import frc.robot.subsystems.Rotors;
 import frc.robot.subsystems.StateMachine;
@@ -41,7 +42,9 @@ public class RobotContainer {
   private final SN_XboxController conDriver = new SN_XboxController(controllerIDs.DRIVER_USB);
 
   public static final Rotors rotorsInstance = new Rotors();
+  public static final Motion motionInstance = new Motion();
   private final Rotors loggedRotorsInstance = rotorsInstance;
+  private final Motion loggedMotionInstance = motionInstance;
   public static final Drivetrain subDrivetrain = new Drivetrain();
   private final Drivetrain loggedSubDrivetrain = subDrivetrain;
   public static final DriverStateMachine subDriverStateMachine = new DriverStateMachine(subDrivetrain);

src/main/java/frc/robot/constants/ConstMotion.java

@@ -4,7 +4,11 @@
 
 package frc.robot.constants;
 
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+
 /** Add your docs here. */
 public class ConstMotion {
   public static final double STOP = 0;
+  public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration();
+  public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration();
 }

src/main/java/frc/robot/constants/ConstPoseDrive.java

@@ -68,4 +68,4 @@ public static class PoseDriveGroup {
     EXAMPLE_POSE_DRIVE_GROUP.distanceTolerance = Units.Inches.of(1);
     EXAMPLE_POSE_DRIVE_GROUP.rotationTolerance = Units.Degrees.of(1);
   }
-}
+}

src/main/java/frc/robot/subsystems/Motion.java

@@ -4,13 +4,90 @@
 
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.Units.Degrees;
+import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.DeviceIDs;
+import frc.robot.Robot;
+import frc.robot.constants.ConstMotion;
 
 @Logged
 public class Motion extends SubsystemBase {
   /** Creates a new Motion. */
+  final TalonFX intakePivot = new TalonFX(DeviceIDs.motionIDs.INTAKE_PIVOT_CAN);
+  final TalonFX hoodPivot = new TalonFX(DeviceIDs.motionIDs.HOOD_PIVOT_CAN);
+
+  MotionMagicExpoVoltage hoodPivotMotionRequest = new MotionMagicExpoVoltage(0);
+  MotionMagicExpoVoltage intakePivotMotionRequest = new MotionMagicExpoVoltage(0);
+
+  Angle lastDesiredHoodPivotAngle = Degrees.zero();
+  Angle lastDesiredIntakePivotAngle = Degrees.zero();
+
+  private boolean hoodAtPostion = false;
+  private boolean intakePivotAtPosition = false;
+
   public Motion() {
+    intakePivot.getConfigurator().apply(ConstMotion.INTAKE_PIVOT_CONFIGURATION);
+    hoodPivot.getConfigurator().apply(ConstMotion.HOOD_PIVOT_CONFIGURATION);
+  }
+
+  /**
+   * this codes takes an angle and setting a desired angle for the Intake to
+   * allign to.
+   */
+  public void setIntakePivotAngle(Angle setAngle) {
+    intakePivot.setControl(intakePivotMotionRequest.withPosition(setAngle));
+    lastDesiredIntakePivotAngle = setAngle;
+  }
+
+  /**
+   * this codes takes an angle and setting a desired angle for the Hood to allign
+   * to
+   */
+  public void setHoodPivotAngle(Angle setAngle) {
+    hoodPivot.setControl(hoodPivotMotionRequest.withPosition(setAngle));
+    lastDesiredHoodPivotAngle = setAngle;
+  }
+
+  public Angle getIntakePivotAngle() {
+    if (Robot.isSimulation()) {
+      return lastDesiredIntakePivotAngle;
+    }
+    return intakePivot.getPosition().getValue();
+  }
+
+  public Angle getHoodPivotAngle() {
+    if (Robot.isSimulation()) {
+      return lastDesiredHoodPivotAngle;
+    }
+    return hoodPivot.getPosition().getValue();
+  }
+
+  /** checks to see if hood is at correct angle */
+  public boolean isHoodPivotAtAngle(Angle tolerance) {
+    Angle lowerlim = lastDesiredHoodPivotAngle.minus(tolerance);
+    Angle upperlim = lastDesiredHoodPivotAngle.plus(tolerance);
+
+    Angle hoodPivotAngle = getHoodPivotAngle();
+
+    hoodAtPostion = hoodPivotAngle.gte(lowerlim)
+        && hoodPivotAngle.lte(upperlim);
+    return hoodAtPostion;
+  }
+
+  public boolean isIntakePivotAtAngle(Angle tolerance) {
+    Angle lowerlim = lastDesiredIntakePivotAngle.minus(tolerance);
+    Angle upperlim = lastDesiredIntakePivotAngle.plus(tolerance);
+
+    Angle intakePivotAngle = getIntakePivotAngle();
+
+    intakePivotAtPosition = intakePivotAngle.gte(lowerlim)
+        && intakePivotAngle.lte(upperlim);
+    return intakePivotAtPosition;
   }
 
   @Override