Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-08 16:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add better feeder idle, shooter aim lead.

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-24 13:50:30 -07:00
parent 17052c892b
commit 6e8078e0be
5 changed files with 83 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+8 -8
View File
@@ -99,7 +99,7 @@ public class RobotContainer {
);
private Command RobotShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.setShooterReady()),
new InstantCommand(() -> m_robotShooter.spinUpShooting()),
new RunCommand(
() -> {
m_robotSwerveDrive.driveFacingPosition(
@@ -110,9 +110,9 @@ public class RobotContainer {
}, m_robotSwerveDrive),
new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)),
new WaitCommand(5),
new InstantCommand(()->m_robotShooter.setShooterShoot()),
new InstantCommand(()->m_robotShooter.allowShooting()),
new WaitCommand(10),
new InstantCommand(()->m_robotShooter.setShooterNOTShoot())
new InstantCommand(()->m_robotShooter.denyShooting())
);
// private Command RobotShoot = new SequentialCommandGroup(
@@ -256,18 +256,18 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5)
.onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterNotReady();
m_robotShooter.spinUpIdle();
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterReady();
m_robotShooter.spinUpShooting();
m_robotIntake.setMode(IntakeMode.Idle);
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterReadyFeeder();
m_robotShooter.spinUpFeeding();
}));
@@ -275,9 +275,9 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterShoot();
m_robotShooter.allowShooting();
})).onFalse(new InstantCommand(() -> {
m_robotShooter.setShooterNOTShoot();
m_robotShooter.denyShooting();
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+34 -13
View File
@@ -1,5 +1,6 @@
package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Rotation;
import static edu.wpi.first.units.Units.RotationsPerSecond;
@@ -7,8 +8,10 @@ import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.Constants;
import frc4388.robot.subsystems.LED;
@@ -59,24 +62,24 @@ public class Shooter extends SubsystemBase {
private ShooterMode mode = ShooterMode.Idle;
private boolean shooterButtonReady = false;
public void setShooterShooting() {
public void spinUpShooting() {
this.mode = ShooterMode.Shooting;
}
public void setShooterFeeding() {
public void spinUpFeeding() {
this.mode = ShooterMode.Feeding;
}
public void setShooterIdle() {
public void spinUpIdle() {
this.mode = ShooterMode.Idle;
}
public void setShooterShoot() {
public void allowShooting() {
shooterButtonReady = true;
}
public void setShooterNOTShoot() {
public void denyShooting() {
shooterButtonReady = false;
}
@@ -95,18 +98,30 @@ public class Shooter extends SubsystemBase {
io.updateInputs(state);
ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds;
double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2));
double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI));
// Get robot positon and speeds
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2));
double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI));
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
//
double distanceToHub = (robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm());
// Calculate aim lead
// Get the current speed of the robot
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond
);
// Calculate a point to aim ahead of the actual position.
Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation());
// Get the robot's aim distance to hub
double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
//Center of hub to cameras in inches
Logger.recordOutput("Hub Dist", distanceToHub);
boolean driverError =
// XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
// AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
@@ -199,7 +214,13 @@ public class Shooter extends SubsystemBase {
break;
case Idle:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
io.setShooterCurrentLimitSpeed(
state,
ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get(),
Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()),
RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
);
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
break;
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+6 -2
View File
@@ -18,14 +18,18 @@ public class ShooterConstants {
public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5;
public static final double INDEXER_GEAR_RATIO = 1.;
// public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40);
public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35);
// public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.3);
public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4);
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0);
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
// Shoot mode tolerances
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+6
View File
@@ -38,6 +38,12 @@ public interface ShooterIO {
// public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setShooterCurrentLimitSpeed(
ShooterState state,
double percentOutput,
Current currentLimit,
AngularVelocity targetVelocity
) {}
// public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setIndexerOutput(ShooterState state, double percentOutput) {}
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+29
View File
@@ -1,9 +1,13 @@
package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
public class ShooterReal implements ShooterIO {
@@ -54,6 +58,31 @@ public class ShooterReal implements ShooterIO {
m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps));
}
@Override
public void setShooterCurrentLimitSpeed(
ShooterState state,
double percentOutput,
Current currentLimit,
AngularVelocity targetVelocity
) {
state.motor1TargetVelocity = targetVelocity;
state.motor2TargetVelocity = targetVelocity;
double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps));
double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2;
if(
Math.abs(currentLimit.in(Amps)) > current &&
Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
) {
m_shooter1Motor.set(percentOutput);
m_shooter2Motor.set(percentOutput);
} else {
m_shooter1Motor.set(0);
m_shooter2Motor.set(0);
}
}
@Override
public void setIndexerOutput(ShooterState state, double percentOutput) {
state.indexerTargetOutput = percentOutput;