FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

PID updates for shooter, velocity compensation (commented out)

This commit is contained in:
Scott Jackson 2026-10-08 19:24:51 -04:00
parent 2e87c28fe4
commit 8868183c62
5 changed files with 75 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/frc/robot/Constants.java
View File

@ -144,6 +144,8 @@ public final class Constants {
public static final double HOMING_SPEED = .05;
public static final double TURRET_GEAR_RATIO = 6;
public static final double TURRET_RANGE_ROTATIONS = 0.5; // The turret can rotate 180°
public static final double SHOOTER_HEIGHT_INCHES = 22.0; // Shooter is 22" from the floor
public static final double HUB_OPENING_HEIGHT_INCHES = 72.0; // Hub opening (hexagon top) is 72" from the floor
public static final int HOOD_SERVO_CHANNEL = 0;
public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this

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

@ -90,7 +90,7 @@ public class RobotContainer {
// m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
// m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
//m_driverController.x().toggleOnTrue(getAutonomousCommand())((new UltraShooterCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
m_driverController.x().toggleOnTrue(new UltraShooterCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
m_driverController.x().toggleOnTrue(new UltraShooterCommand(hopperSubsystem, turretSubsystem, driveSubsystem));
m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID()));
m_driverController.y().onTrue(intakeSubsystem.deploy());
}

24
src/main/java/frc/robot/Utils.java
View File

@ -216,6 +216,30 @@ public class Utils {
return launchSpeedFPS;
}
/**
* Estimates the fuel's time of flight until it his the target,
* where the target is the top of the hub when scoring and the floor when passing
* launch speed in feet/second.
* This performs Equations 3-7 and 3-8 in the AutoShooter document.
* @param scoring true if the bot is scoring into the hub (false means passing)
* @param launchAngleDeg the target launch angle in degrees
* @param launchSpeedFPS the target launch speed in ft/s
* @return the approxmate fuel flight time in seconds
*/
public static double findTimeOfFlightScoring(boolean scoring, int launchAngleDeg, double launchSpeedFPS){
double velocityVertFPS = launchSpeedFPS * Math.sin(Math.toRadians((double)launchAngleDeg)); // The vertical component of fuel's velocity
double kGravityInFeet = 32.2; // Gravity constant in ft/s^2
double deltaHFeet = 0; // Heigh difference between launcher and target
if (scoring) {
deltaHFeet = (Constants.ShooterConstants.SHOOTER_HEIGHT_INCHES - Constants.ShooterConstants.HUB_OPENING_HEIGHT_INCHES) / 12;
} else {
deltaHFeet = Constants.ShooterConstants.SHOOTER_HEIGHT_INCHES / 12;
}
return ((velocityVertFPS + Math.sqrt(Math.pow(velocityVertFPS, 2) - (2 * kGravityInFeet * deltaHFeet))) / kGravityInFeet);
}
/**
* Calculates the flywheel RPM that's required to achieve the provided
* launch speed in feet/second.

52
src/main/java/frc/robot/commands/UltraShooterCommand.java
View File

@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj.DriverStation;
@ -21,7 +22,6 @@ import frc.robot.Constants.ShootingRegion;
import frc.robot.Utils;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.TurretSubsystem;
@ -29,7 +29,6 @@ public class UltraShooterCommand extends Command{
private HopperSubsystem hopperSubsystem;
private TurretSubsystem turretSubsystem;
private IntakeSubsystem intakeSubsystem; // TODO: is this necessary?
private DriveSubsystem driveSubsystem;
private Command waitCommand = Commands.waitSeconds(1.5);
@ -37,13 +36,12 @@ public class UltraShooterCommand extends Command{
private Alliance alliance; // Shooting behavior is based on the robot's alliance
public UltraShooterCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
public UltraShooterCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem){
this.hopperSubsystem = hopperSubsystem;
this.turretSubsystem = turretSubsystem;
this.intakeSubsystem = intakeSubsystem;
this.driveSubsystem = driveSubsystem;
addRequirements(hopperSubsystem, intakeSubsystem); // TODO: isintakeSubsystem necessary?
addRequirements(hopperSubsystem);
}
@Override
@ -59,7 +57,7 @@ public class UltraShooterCommand extends Command{
public void execute() {
Boolean okToShoot = false; // Tracks whether conditions are OK for shooting.
Boolean scoring = false; // Are we shooting into the hub?
Pose3d botPose = driveSubsystem.getBotPose(); // Find the pose once since we'll use it a few times
Pose3d botPose = driveSubsystem.getBotPose(); // Get and store the bot's pose once since we'll use it a few times
// STEPS 1 & 2: Get the field location of the initial target
// First, find the robot's current field location.
@ -111,12 +109,45 @@ public class UltraShooterCommand extends Command{
}
// STEP 5: Find final target based on the intial target and the robot's velocity.
// THIS IS NOT IMPLEMENTED - SO ASSUME STATIONARY ROBOT. Make final target the same as initial.
// VELOCITY ADJUSTMENTS ARE COMMENTED OUT, SO ASSUME STATIONARY ROBOT. Make final target the same as initial.
Pose3d finalTargetPose = initialTargetPose;
double distanceToFinalTargetMeters = distanceToInitialTargetMeters;
Logger.recordOutput("UltraShooter-FinalTarget", finalTargetPose);
Logger.recordOutput("UltraShooter-DistanceToFinalTarget(Meters)", distanceToFinalTargetMeters);
/*
// VELOCITY ADJUSTEMENTS - READY TO TRY. Uncomment this block.
// Estimate the fuel's time of flight.
double timeOfFlightSeconds = Utils.findTimeOfFlightScoring(scoring, initialHoodAngleDegrees, intitialLaunchSpeedFPS);
// TBA: Get the bot's current velocity
// SwerveDriveSystem in RoboLib must make its ChassisSpeeds accessible.
// Get the robot-relative ChassisSpeeds and robot heading from the swerve system.
// Then create field-relative ChassisSpeeds using the robot heading.
ChassisSpeeds robotBotRelativeSpeeds = driveSubsystem.driveSystem.GetRobotRelativeSpeeds();
Rotation2d botHeading = driveSubsystem.driveSystem.GetRobotHeading(); // TODO: adjust based on alliance since this comes straight from the Gyro?
ChassisSpeeds robotFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(robotBotRelativeSpeeds, botHeading);
// Get X and Y velocity values.
double botXVelMPS = robotFieldRelativeSpeeds.vxMetersPerSecond();
double botYVelMPS = robotFieldRelativeSpeeds.vyMetersPerSecond();
// Get translaion of initialTarget.
Translation3d initialTargetTr = initialTargetPose.getTranslation();
// Find X and Y displacements during flight.
double targetXDisplacementMeters = botXVelMPS * timeOfFlightSeconds;
double targetYDisplacementMeters = botYVelMPS * timeOfFlightSeconds;
// Find final target translation.
Translation3d adjustedTarget = new Translation3d(initialTargetTr.getX() - targetXDisplacementMeters,
initialTargetTr.getY() - targetYDisplacementMeters,
initialTargetTr.getZ()); // We don’t need to adjust z.
// TBA: Generate a Pose3d for the finalTarget
finalTargetPose = new Pose3d(adjustedTarget, initialTargetPose.getRotation());
*/
// STEP 6: Find hood angle and launch speed for the final target.
// THIS IS NOT IMPLEMENTED - SO ASSUME STATIONARY ROBOT. Make final target the same as initial.
double finalLaunchSpeedFPS = intitialLaunchSpeedFPS;
@ -195,13 +226,16 @@ public class UltraShooterCommand extends Command{
Logger.recordOutput("UltraShooter-State", "INACTIVE");
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
// Removed the following since we're don't setting turret targets.
/*
DriverStation.getAlliance().ifPresent((alliance) -> {
if (alliance == Alliance.Blue) {
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
} else {
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
}
});
*/
// turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
// TODO: Deactivate rumble if we decided to use the rumble feature when the robot prevents shooting

5
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -112,12 +112,13 @@ public class TurretSubsystem extends TestableSubsystem {
turretSim = turretMotor.getSimState();
// Launcher configuration
// Feedforward and PID values were determined experimentally on 10/4/2026.
launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless);
SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
config.voltageCompensation(10);
config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001
config.closedLoop.feedForward.kS(0.2).kV(0.000).kA(0.000);
config.closedLoop.feedForward.kS(0.2).kV(0.00005).kA(0.001);
// config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
config.closedLoop.positionWrappingEnabled(false);
@ -195,7 +196,7 @@ public class TurretSubsystem extends TestableSubsystem {
pos = -1.0;
}
Logger.recordOutput("ShooterHood-Angle", pos);
Logger.recordOutput("ShooterHood-ServoPos", pos);
}