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/deploy/pathplanner/autos/DriveShoot-Hori.auto
+38
View File
@@ -0,0 +1,38 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Shoot driving across"
}
},
{
"type": "named",
"data": {
"name": "Robot Shoot Driving"
}
},
{
"type": "named",
"data": {
"name": "Robot Shoot"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/paths/Shoot driving across.path
+102
View File
@@ -0,0 +1,102 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.9326282051282067,
"y": 6.5815769230769225
},
"prevControl": null,
"nextControl": {
"x": 1.9932621113872893,
"y": 6.3390412980405895
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.9326282051282067,
"y": 1.6628461538461545
},
"prevControl": {
"x": 1.936134248900721,
"y": 1.9128215679513527
},
"nextControl": {
"x": 1.9291221613556924,
"y": 1.4128707397409563
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.9326282051282067,
"y": 6.5815769230769225
},
"prevControl": {
"x": 1.9198835421653975,
"y": 6.331901987278464
},
"nextControl": {
"x": 1.9453728680910158,
"y": 6.8312518588753814
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.9326282051282067,
"y": 1.6628461538461545
},
"prevControl": {
"x": 1.9337461053582308,
"y": 1.9128436544318115
},
"nextControl": {
"x": 1.9315103048981825,
"y": 1.4128486532604976
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.9326282051282067,
"y": 6.5815769230769225
},
"prevControl": {
"x": 1.8878500514131944,
"y": 6.335619778537727
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 10.0,
"maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/Shoot driving vertical.path
+118
View File
@@ -0,0 +1,118 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.956,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 2.707930530821583,
"y": 4.031008683647302
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.3,
"y": 4.0
},
"prevControl": {
"x": 1.5499999999999998,
"y": 4.0
},
"nextControl": {
"x": 1.05,
"y": 4.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.9559102564102573,
"y": 4.0
},
"prevControl": {
"x": 2.7118639603594925,
"y": 3.9457652751099213
},
"nextControl": {
"x": 3.199956552461022,
"y": 4.054234724890079
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.3,
"y": 4.0
},
"prevControl": {
"x": 1.5434594129947146,
"y": 4.0568112156556175
},
"nextControl": {
"x": 1.0565405870052855,
"y": 3.9431887843443825
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.0,
"y": 4.0
},
"prevControl": {
"x": 2.7532300973825357,
"y": 3.9599423520140395
},
"nextControl": {
"x": 3.2467699026174643,
"y": 4.040057647985961
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.3,
"y": 4.0
},
"prevControl": {
"x": 1.526511041060335,
"y": 4.105795785727803
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 10.0,
"maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}
src/main/java/frc4388/robot/RobotContainer.java
+16
View File
@@ -5,9 +5,14 @@
/* the project. */
/*----------------------------------------------------------------------------*/
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot;
import java.io.File;
import java.util.Optional;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
@@ -24,6 +29,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// Commands
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -129,6 +135,15 @@ public class RobotContainer {
IntakeExtended,
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
);
private Command RobotShootDriving = new SequentialCommandGroup(
new InstantCommand(() ->
m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION)
),
new WaitCommand(99) // stays on for the duration of the auto segment
).finallyDo((interrupted) ->
m_robotSwerveDrive.disableRotationOverride()
);
private Command IntakeRetracted = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
@@ -170,6 +185,7 @@ public class RobotContainer {
NamedCommands.registerCommand("Robot Shoot", RobotShoot);
// NamedCommands.registerCommand("Lidar Intake", LidarIntake);
NamedCommands.registerCommand("Intake Extended", IntakeExtended);
NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving);
DriverStation.silenceJoystickConnectionWarning(true);
src/main/java/frc4388/robot/constants/BuildConstants.java
+5 -5
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 171;
public static final String GIT_SHA = "4ffbe3f5954581f07ac26ee6174113c909cdbaa7";
public static final String GIT_DATE = "2026-03-19 18:32:02 MDT";
public static final int GIT_REVISION = 172;
public static final String GIT_SHA = "69677d5ae9e63125a30857f5f7afa0756dfa9416";
public static final String GIT_DATE = "2026-03-20 15:40:07 MDT";
public static final String GIT_BRANCH = "MiraOrg";
public static final String BUILD_DATE = "2026-03-20 15:31:47 MDT";
public static final long BUILD_UNIX_TIME = 1774042307703L;
public static final String BUILD_DATE = "2026-03-20 19:59:37 MDT";
public static final long BUILD_UNIX_TIME = 1774058377112L;
public static final int DIRTY = 1;
private BuildConstants(){}
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;
}
}
}