diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8c81ad0..e8cf350 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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 aggressively. // 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 } diff --git a/src/main/java/frc/robot/constants/HoodConstants.java b/src/main/java/frc/robot/constants/HoodConstants.java index f5c7ec6..30a4ab6 100644 --- a/src/main/java/frc/robot/constants/HoodConstants.java +++ b/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 kMaxManualSpeedMultiplier = 1; + public static final double kStartupAngle = 0.0; + 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); diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index a0c39c2..9122d2e 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/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); diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index cf691cb..941bd07 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index b47e507..27a025e 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/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/CurrentAngle", encoder.getPosition()); + Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees)); + 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) {