good flywheel and hood control

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

@@ -109,7 +109,7 @@ public class RobotContainer {
         // after the electronics rebuild. For testing normal operation nothing about the Drivetrain
         // class should need to change
         drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true));
-        /*drivetrain.setDefaultCommand(
+        drivetrain.setDefaultCommand(
             drivetrain.drive(
                 driver::getLeftY,
                 driver::getLeftX, 
@@ -117,7 +117,7 @@ public class RobotContainer {
                 () -> true
             )
         );
-
+/* 
         // This needs to be tested after a prolonged amount of driving around <i>aggressively</i>.
         // Do things like going over the bump repeatedly, spin around a bunch, etc. 
         // If this works well over time, then this is likely all we need
@@ -226,6 +226,10 @@ public class RobotContainer {
         secondary.x().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kDown));
         secondary.y().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kUp));
 
+        secondary.povUp().whileTrue(hood.trackToAngle(() -> Math.toRadians(40.0)));
+        secondary.povDown().whileTrue(hood.trackToAngle(() -> Math.toRadians(0.0)));
+        
+
         // TODO Some means of testing hood PIDF
         // TODO Some means of testing climber PIDF
     }

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

@@ -6,6 +6,7 @@ import java.io.FileReader;
 import java.io.IOException;
 import java.nio.file.Path;
 
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -17,16 +18,17 @@ public class HoodConstants {
     // TODO Real Values
     public static final int kMotorCANID = 12;
 
-    public static final double kConversionFactor = 3.0*147.0/8.0;
+    public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI;
 
-    public static final double kP = 0;
+    public static final double kP = 1.75;
     public static final double kI = 0;
     public static final double kD = 0;
-    public static final double kS = 0;
+    public static final double kS = 0.435;
     public static final double kV = 0;
     public static final double kA = 0;
-    public static final double kStartupAngle = 0;
+    public static final double kStartupAngle = 0.0;
-    public static final double kMaxManualSpeedMultiplier = 1;
+    public static final double kMaxManualSpeedMultiplier = 0.1;
+    public static final double kTolerance = Math.toRadians(0.5);
 
     public static final double kAmpsToTriggerPositionReset = 10;
 
@@ -36,7 +38,7 @@ public class HoodConstants {
 
     public static final int kCurrentLimit = 15;
 
-    public static final boolean kInverted = false;
+    public static final boolean kInverted = true;
     public static final boolean kUseInterpolatorForAngle = false;
 
     public static final IdleMode kIdleMode = IdleMode.kBrake;
@@ -60,6 +62,7 @@ public class HoodConstants {
             .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kP, kI, kD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(kTolerance, ClosedLoopSlot.kSlot0)
             .feedForward
                 .sva(kS, kV, kA);
 

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

@@ -7,6 +7,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -63,10 +64,12 @@ public class ModuleConstants {
     public static final double kTurningMotorReduction = 150.0/7.0;
     public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction;
     // TODO Adjust? Let over from 2025
-    public static final double kTurnP = 1;
+    public static final double kTurnP = 12;
     public static final double kTurnI = 0;
     public static final double kTurnD = 0;
 
+    public static final double kTurnTolerance = Math.toRadians(0.25);
+
     public static final boolean kIsEncoderInverted = false;
 
     // TODO How sensitive is too sensitive?
@@ -118,6 +121,7 @@ public class ModuleConstants {
             .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kTurnP, kTurnI, kTurnD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(kTurnTolerance, ClosedLoopSlot.kSlot0)
             .positionWrappingEnabled(true)
             .positionWrappingInputRange(0, 2 * Math.PI);
             

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

@@ -9,7 +9,7 @@ import edu.wpi.first.math.util.Units;
 public class ShooterConstants {
     public enum ShooterSpeeds {
         kHubSpeed(3000.0),
-        kFeedSpeed(3000.0);
+        kFeedSpeed(5000.0);
 
         private double speedMPS;
         private double speedRPM;
@@ -39,14 +39,14 @@ public class ShooterConstants {
     public static final boolean kLeftShooterMotorInverted = true;
     public static final boolean kRightShooterMotorInverted = false;
 
-    public static final double kLeftP = 0.0;//0.001;
+    public static final double kLeftP = 0.001;
     public static final double kLeftI = 0;
     public static final double kLeftD = 0;
     public static final double kLeftS = 0;
-    public static final double kLeftV = 0.00121;
+    public static final double kLeftV = 0.0013;
     public static final double kLeftA = 0;
 
-    public static final double kRightP = 0.001;//0.1;
+    public static final double kRightP = 0.001;
     public static final double kRightI = 0;
     public static final double kRightD = 0.000;
     public static final double kRightS = 0;

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

@@ -43,6 +43,7 @@ public class Hood extends SubsystemBase {
         );   
 
         encoder = motor.getEncoder();
+        encoder.setPosition(HoodConstants.kStartupAngle);
 
         controller = motor.getClosedLoopController();
 
@@ -68,9 +69,10 @@ public class Hood extends SubsystemBase {
     @Override
     public void periodic() {
         Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
-        Logger.recordOutput("Hood/CurrentTarget", currentTargetDegrees);
+        Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
-        Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
+        Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));
         Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
+        Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
     }
 
     public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {