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

Lots

Browse Source 
- shooting while moving corrected (vertical movements)
- reverse indexer before shooting
- defensive positions fixed values
- stalling motors led fixed (except shooter)
This commit is contained in:
mimigamin
2026-03-20 15:40:07 -06:00
parent 4ffbe3f595
commit 69677d5ae9
7 changed files with 268 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+19 -32
View File
@@ -13,12 +13,14 @@ 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.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.Constants;
import frc4388.robot.subsystems.intake.Intake;
import frc4388.robot.subsystems.led.LED;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.utility.compute.FieldPositions;
import frc4388.utility.compute.TimesNegativeOne;
public class Shooter extends SubsystemBase {
public ShooterIO io;
@@ -34,6 +36,8 @@ public class Shooter extends SubsystemBase {
public double distanceToHub = 5;
public double chassisXSpeed = 0;
public Shooter(
ShooterIO io,
SwerveDrive swerveDrive,
@@ -59,15 +63,13 @@ public class Shooter extends SubsystemBase {
public enum ShooterMode {
Shooting,
ManualShoot,
Idle,
IndexerStall
Idle
}
private ShooterMode mode = ShooterMode.Idle;
private boolean shooterButtonReady = false;
public void spinUpShooting(double chassisXSpeed) {
this.chassisXSpeed = chassisXSpeed;
public void spinUpShooting() {
this.mode = ShooterMode.Shooting;
}
@@ -87,10 +89,6 @@ public class Shooter extends SubsystemBase {
shooterButtonReady = true;
}
public void indexerStalled() {
mode = ShooterMode.IndexerStall;
}
public void denyShooting() {
shooterButtonReady = false;
}
@@ -122,26 +120,20 @@ public class Shooter extends SubsystemBase {
// 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();
if (TimesNegativeOne.isRed) {
chassisXSpeed = chassisSpeeds.vxMetersPerSecond;
} else {
chassisXSpeed = -chassisSpeeds.vxMetersPerSecond;
}
// 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());
Translation2d fieldPos = robotPose2d.getTranslation();
// Get the robot's aim distance to hub
distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
distanceToHub = (fieldPos.minus(FieldPositions.HUB_POSITION).getNorm());
//Center of hub to cameras in inches
//Center of hub to cameras in meters
Logger.recordOutput("Hub Dist", distanceToHub);
boolean driverError =
@@ -158,14 +150,14 @@ public class Shooter extends SubsystemBase {
//revtime calculations
// double shooterAcceleration =
double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond);
double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed).in(RotationsPerSecond);
double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
// double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
Logger.recordOutput("Time to rev", revTime);
switch (mode) {
case Shooting:
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed));
int bitmask = (
(shooterButtonReady ? 1 : 0) +
@@ -175,18 +167,18 @@ public class Shooter extends SubsystemBase {
switch (bitmask) {
case 0b000: // No errors but button is not pressed
io.setIndexerOutput(state, 0);
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
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
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
break;
case 0b010: // Bad shooter velocity, button is not pressed
case 0b011: // Bad shooter velocty, button is pressed
io.setIndexerOutput(state, 0);
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
break;
@@ -253,11 +245,6 @@ public class Shooter extends SubsystemBase {
io.setIndexerOutput(state, 0);
m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
break;
case IndexerStall:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get()));
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.INDEXER_REVERSE);
break;
}
io.motorStalled(state, m_Intake, m_robotLED);
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+22 -14
View File
@@ -2,6 +2,8 @@ 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;
@@ -31,10 +33,14 @@ public class ShooterConstants {
// 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.4);
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.2);
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 AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1);
// Shoot mode tolerances
@@ -49,21 +55,23 @@ public class ShooterConstants {
public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3);
//
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
// Model derived from points
// double speed =
// 1.11576*hubDistMeters*hubDistMeters +
// 0.318464*hubDistMeters +
// 30.6293;
double speed =
0.0593402*hubDistMeters*hubDistMeters +
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
double speed = 0;
if (Math.abs(chassisXSpeed) < 0.1){
speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + MODEL_TRIM.get();
// double speed =
// 0.00610938*hubDistMeters*hubDistMeters
// 5.65235*hubDistMeters +
// 22.82825;
} else if (chassisXSpeed > 0){
speed = 0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + chassisXSpeed * POS_OFFSET + 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();
}
double max = SHOOTER_MAX_VELOCITY.get();
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+20 -17
View File
@@ -55,21 +55,24 @@ public class ShooterReal implements ShooterIO {
}
@Override
public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {
if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) {
if (!m_shooterStalling) {
m_shooterStalling = true;
m_stallTimerShooter.restart();
}
if (m_stallTimerShooter.hasElapsed(5.0)) {
m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
motorStall = "Shooter Stalled";
System.out.println(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)));
}
} else {
m_shooterStalling = false;
m_stallTimerShooter.reset();
}
public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {
motorStall = "";
// if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) {
// if (!m_shooterStalling) {
// m_shooterStalling = true;
// m_stallTimerShooter.restart();
// }
// if (m_stallTimerShooter.hasElapsed(5.0)) {
// m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
// motorStall = "Shooter Stalled";
// System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond))));
// System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)));
// System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond)));
// }
// } else {
// m_shooterStalling = false;
// m_stallTimerShooter.reset();
// }
if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) {
if (!m_indexerStalling) {
@@ -79,7 +82,7 @@ public class ShooterReal implements ShooterIO {
if (m_stallTimerIndexer.hasElapsed(5.0)) {
m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
motorStall = "Indexer Stalled";
System.out.println(Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput));
System.out.println("Indexer Stalled: " + (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput)));
}
} else {
m_indexerStalling = false;
@@ -94,7 +97,7 @@ public class ShooterReal implements ShooterIO {
if (m_stallTimerRoller.hasElapsed(5.0)) {
m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
motorStall = "Roller Stalled";
System.out.println(Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()));
System.out.println("Roller Stalled: " + (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed())));
}
} else {
m_rollerStalling = false;