Make Turret ╰(*°▽°*)╯

src/main/java/frc/robot/RobotMap.java

@@ -31,4 +31,13 @@ public static class mapDrivetrain {
         public static final int BACK_RIGHT_STEER_CAN = 7;
         public static final int BACK_RIGHT_ABSOLUTE_ENCODER_CAN = 3;
     }
+
+    public static class mapRotors {
+        public static final int BALL_SHOOTER_CAN = 10;
+    }
+
+    public static class mapMotion {
+        public static final int TURRET_PIVOT_CAN = 20;
+        public static final int HOOD_CAN = 21;
+    }
 }

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

@@ -4,6 +4,78 @@
 
 package frc.robot.constants;
 
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
+
 /** Add your docs here. */
 public class ConstMotion {
+  public static TalonFXConfiguration TURRET_CONFIG = new TalonFXConfiguration();
+
+  static {
+    // turret motor config
+    // TODO: tune pid values
+    TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    TURRET_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+    TURRET_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Degrees.of(60).in(Units.Rotations);
+    TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+    TURRET_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Degrees.of(0).in(Units.Rotations);
+    TURRET_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static;
+    TURRET_CONFIG.Slot0.kP = 0;
+    TURRET_CONFIG.Slot0.kI = 0;
+    TURRET_CONFIG.Slot0.kD = 0;
+    TURRET_CONFIG.Slot0.kS = 0;
+    TURRET_CONFIG.Slot0.kG = 0;
+
+    TURRET_CONFIG.Feedback.SensorToMechanismRatio = 0; // TODO: replace with actual ratio
+    TURRET_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0;
+    TURRET_CONFIG.MotionMagic.MotionMagicAcceleration = 0;
+    TURRET_CONFIG.MotionMagic.MotionMagicExpo_kV = 0;
+    TURRET_CONFIG.MotionMagic.MotionMagicExpo_kA = 0;
+    TURRET_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true;
+    TURRET_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits
+    TURRET_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits
+    TURRET_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1;
+  }
+
+  public static TalonFXConfiguration HOOD_CONFIG = new TalonFXConfiguration();
+
+  static {
+    // Hood motor config
+    // TODO: tune pid values
+    HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    HOOD_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+    HOOD_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Degrees.of(60).in(Units.Rotations);
+    HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+    HOOD_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Degrees.of(0).in(Units.Rotations);
+    HOOD_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static;
+    HOOD_CONFIG.Slot0.kP = 0;
+    HOOD_CONFIG.Slot0.kI = 0;
+    HOOD_CONFIG.Slot0.kD = 0;
+    HOOD_CONFIG.Slot0.kS = 0;
+    HOOD_CONFIG.Slot0.kG = 0;
+
+    HOOD_CONFIG.Feedback.SensorToMechanismRatio = 0; // TODO: replace with actual ratio
+    HOOD_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0;
+    HOOD_CONFIG.MotionMagic.MotionMagicAcceleration = 0;
+    HOOD_CONFIG.MotionMagic.MotionMagicExpo_kV = 0;
+    HOOD_CONFIG.MotionMagic.MotionMagicExpo_kA = 0;
+    HOOD_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true;
+    HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30; // TODO: tune current limits
+    HOOD_CONFIG.CurrentLimits.SupplyCurrentLimit = 60; // TODO: tune current limits
+    HOOD_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1;
+  }
+
+  public static final double POSITION_TOLERANCE = Units.Degrees.of(1).in(Units.Rotations);
+
+  public static class MechanismPositionGroup {
+    public Angle turretPivotMotorAngle;
+    public Angle hoodMotorAngle;
+  }
 }

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

@@ -4,6 +4,20 @@
 
 package frc.robot.constants;
 
-/** Add your docs here. */
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
 public class ConstRotors {
+  public static TalonFXConfiguration BALL_SHOOTER_CONFIG = new TalonFXConfiguration();
+
+  static {
+    BALL_SHOOTER_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    BALL_SHOOTER_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true;
+    BALL_SHOOTER_CONFIG.CurrentLimits.SupplyCurrentLimit = 85; // TODO: tune current limits
+    BALL_SHOOTER_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 60; // TODO: tune current limits
+    BALL_SHOOTER_CONFIG.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+  }
+
+  public static final double BALL_SHOOTER_SPEED = 0.2; // TODO: Replace with actual speed
 }

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

@@ -4,14 +4,105 @@
 
 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 com.ctre.phoenix6.signals.NeutralModeValue;
+
+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.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.*;
+import frc.robot.constants.ConstMotion.MechanismPositionGroup;
+import frc.robot.Robot;
+import frc.robot.RobotMap.*;
 
+@Logged
 public class Motion extends SubsystemBase {
   /** Creates a new Motion. */
-  public Motion() {}
+  final TalonFX turretMotor = new TalonFX(mapMotion.TURRET_PIVOT_CAN);
+  TalonFX hoodMotor = new TalonFX(mapMotion.HOOD_CAN);
+
+  private Angle turretLastDesiredAngle = Degrees.zero();
+  private Angle hoodLastDesiredAngle = Degrees.zero();
+  MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0);
+
+  public Motion() {
+    turretMotor.getConfigurator().apply(ConstMotion.TURRET_CONFIG);
+    hoodMotor.getConfigurator().apply(ConstMotion.HOOD_CONFIG);
+  }
+
+  public final void setHoodAngle(Angle angle, int slot) {
+    hoodMotor.setControl(positionRequest.withPosition(angle).withSlot(slot));
+    hoodLastDesiredAngle = angle;
+  }
+
+  public final void setTurretAngle(Angle angle, int slot) {
+    turretMotor.setControl(positionRequest.withPosition(angle).withSlot(slot));
+    turretLastDesiredAngle = angle;
+  }
+
+  public void setHoodCoastMode(boolean coastMode) {
+    if (coastMode) {
+      ConstMotion.HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+      hoodMotor.getConfigurator().apply(ConstMotion.HOOD_CONFIG);
+    } else {
+      ConstMotion.HOOD_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+      hoodMotor.getConfigurator().apply(ConstMotion.HOOD_CONFIG);
+    }
+  }
+
+  public void setTurretCoastMode(boolean coastMode) {
+    if (coastMode) {
+      ConstMotion.TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+      turretMotor.getConfigurator().apply(ConstMotion.TURRET_CONFIG);
+    } else {
+      ConstMotion.TURRET_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+      turretMotor.getConfigurator().apply(ConstMotion.TURRET_CONFIG);
+    }
+  }
+
+  public Angle getHoodAngle() {
+    if (Robot.isSimulation()) {
+      return hoodLastDesiredAngle;
+    }
+    return hoodMotor.getPosition().getValue();
+  }
+
+  public Angle getTurretPivotAngle() {
+    if (Robot.isSimulation()) {
+      return turretLastDesiredAngle;
+    }
+    return turretMotor.getPosition().getValue();
+  }
+
+  public AngularVelocity getTurretPivotVelocity() {
+    return turretMotor.getRotorVelocity().getValue();
+  }
+
+  public AngularVelocity getHoodVelocity() {
+    return hoodMotor.getRotorVelocity().getValue();
+  }
+
+  public boolean isTurretPivotVelocityZero() {
+    return getTurretPivotVelocity().isNear(Units.RotationsPerSecond.zero(), 0.01);
+  }
+
+  public boolean isHoodVelocityZero() {
+    return getHoodVelocity().isNear(Units.RotationsPerSecond.zero(), 0.01);
+  }
+
+  public boolean arePositionsAtSetPoint(MechanismPositionGroup positionGroup) {
+    return (getTurretPivotAngle().isNear(positionGroup.turretPivotMotorAngle, ConstMotion.POSITION_TOLERANCE)
+        && getHoodAngle().isNear(positionGroup.hoodMotorAngle, ConstMotion.POSITION_TOLERANCE));
+  }
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    //
+
   }
 }

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

@@ -4,11 +4,37 @@
 
 package frc.robot.subsystems;
 
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotMap.mapRotors;
+import frc.robot.constants.*;
 
+@Logged
 public class Rotors extends SubsystemBase {
   /** Creates a new Rotors. */
-  public Rotors() {}
+
+  TalonFX ballShooterMotor;
+
+  public Rotors() {
+    ballShooterMotor = new TalonFX(mapRotors.BALL_SHOOTER_CAN);
+
+    ballShooterMotor.getConfigurator().apply(ConstRotors.BALL_SHOOTER_CONFIG);
+  }
+
+  public void setBallShooterMotorSpeed(double speed) {
+    ballShooterMotor.set(speed);
+  }
+
+  public double getBallShooterMotorVelocity() {
+    return ballShooterMotor.getRotorVelocity().getValue().in(Units.RPM);
+  }
+
+  public void stopBallShooterMotor() {
+    ballShooterMotor.set(0);
+  }
 
   @Override
   public void periodic() {