Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Drive shoot auto and shooter tolerence

Browse Source
This commit is contained in:
mimigamin
2026-03-20 20:01:06 -06:00
parent 69677d5ae9
commit 95c8a167a5
8 changed files with 351 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+11 -1
View File
@@ -9,6 +9,7 @@ import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.units.measure.AngularVelocity;
@@ -114,12 +115,19 @@ public class Shooter extends SubsystemBase {
public void periodic() {
// FaultReporter.register(this); // TODO Implement fault reporter
Logger.processInputs("Shooter", state);
io.updateInputs(state);
// Get robot positon and speeds
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond
);
Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION);
Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle();
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
@@ -171,8 +179,10 @@ public class Shooter extends SubsystemBase {
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
break;
case 0b001: // No errors and shoot button is pressed
case 0b001: // No errors and shoot button is pressed
if (ang.getDegrees() < ShooterConstants.AIM_ANGLE.get()){ // robot is aimed at hub
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
}
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
break;
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+5 -6
View File
@@ -2,8 +2,6 @@ package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
@@ -37,8 +35,8 @@ public class ShooterConstants {
public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0);
public static final double NEG_OFFSET = 8.;
public static final double POS_OFFSET = 8.;
public static final ConfigurableDouble NEG_OFFSET = new ConfigurableDouble("Negative offset", 8.);
public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.);
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1);
@@ -47,6 +45,7 @@ public class ShooterConstants {
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 5);
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
@@ -65,12 +64,12 @@ public class ShooterConstants {
} else if (chassisXSpeed > 0){
speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + chassisXSpeed * POS_OFFSET + MODEL_TRIM.get();
30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get();
} else { // Negative is closer to hub
speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + chassisXSpeed * NEG_OFFSET + MODEL_TRIM.get();
30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get();
}
double max = SHOOTER_MAX_VELOCITY.get();
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+56 -34
View File
@@ -7,6 +7,7 @@ package frc4388.robot.subsystems.swerve;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import java.util.Optional;
import java.util.function.Supplier;
import org.littletonrobotics.junction.AutoLogOutput;
@@ -20,6 +21,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
import com.pathplanner.lib.util.PathPlannerLogging;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
@@ -65,6 +67,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
private final PIDController m_rotationOverridePID = new PIDController(
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
SwerveDriveConstants.PIDConstants.AIM_kD.get()
);
private boolean m_useRotationOverride = false;
private Translation2d m_rotationOverrideTarget = new Translation2d();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
@@ -83,6 +93,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
// Handle exception as needed
config = null;
}
PPHolonomicDriveController driveController = new PPHolonomicDriveController(
new PIDConstants(5.0, 0.0, 0.0), // Translation PID
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID (used when override is OFF)
);
driveController.setRotationTargetOverride(() -> {
if (!m_useRotationOverride) return Optional.empty();
Rotation2d targetAngle = getPose2d()
.getTranslation()
.minus(m_rotationOverrideTarget)
.getAngle();
return Optional.of(targetAngle);
});
// DoubleSupplier a = () -> 1.d;
AutoBuilder.configure(
() -> {
@@ -94,11 +118,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
(speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
// Also optionally outputs individual module feedforwards
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
// holonomic drive trains
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
),
driveController, // <-- use the variable, not inline new PPHolonomicDriveController(...)
config, // The robot configuration
() -> {
// Boolean supplier that controls when the path will be mirrored for the red
@@ -235,6 +255,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
}
public void aimAtPosition(Translation2d fieldPos, double aimLeadTime) {
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond
);
Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(chassisSpeeds.vxMetersPerSecond)
.withVelocityY(chassisSpeeds.vyMetersPerSecond)
.withTargetDirection(ang);
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
SwerveDriveConstants.PIDConstants.AIM_kD.get()
);
io.setControl(ctrl);
}
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
// reason to have a robot
@@ -408,33 +449,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
driveFieldAngle(leftStick, ang);
}
public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, double ballVelocity, double distanceToHub) {
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond
);
if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){
double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity));
fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
Logger.recordOutput("Offset Value", aimOffset);
}
Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
Logger.recordOutput("Aim at Offset", fieldPos);
driveFieldAngle(leftStick, ang);
}
public Pose2d getCurrentPose(){
return state.currentPose;
}
@@ -526,6 +540,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
softStop();
}
public void enableRotationOverride(Translation2d fieldTarget) {
m_rotationOverrideTarget = fieldTarget;
m_useRotationOverride = true;
}
public void disableRotationOverride() {
m_useRotationOverride = false;
}
@Override
public void periodic() {
// This method will be called once per scheduler run\
@@ -663,5 +686,4 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
return status;
}
}
}