Team_2648/2026-Robot-Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

good flywheel and hood control

Browse Source
This commit is contained in:
Tylr-J42
2026-03-01 19:15:36 -05:00
parent db443cfe63
commit fb937d86dc
5 changed files with 28 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/main/java/frc/robot/RobotContainer.java
View File

@@ -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
}

15
src/main/java/frc/robot/constants/HoodConstants.java
View File

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

6
src/main/java/frc/robot/constants/ModuleConstants.java
View File

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

8
src/main/java/frc/robot/constants/ShooterConstants.java
View File

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

6
src/main/java/frc/robot/subsystems/Hood.java
View File

@@ -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) {