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

summary

Browse Source
This commit is contained in:
mimigamin
2026-03-21 13:04:40 -06:00
parent 89a1f34a4a
commit 3c1b94a2d8
8 changed files with 46 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+3 -5
View File
@@ -133,8 +133,7 @@ public class Shooter extends SubsystemBase {
chassisSpeeds.vyMetersPerSecond
);
Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION);
Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle();
Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle().minus(m_SwerveDrive.getPose2d().getRotation());
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
if (TimesNegativeOne.isRed) {
@@ -154,7 +153,8 @@ public class Shooter extends SubsystemBase {
// XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
// AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() |
distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get() |
Math.abs(ang.getDegrees()) > ShooterConstants.AIM_ANGLE.get();
double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
@@ -186,9 +186,7 @@ public class Shooter extends SubsystemBase {
break;
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
+1 -1
View File
@@ -45,7 +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 AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 15);
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);
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+16 -16
View File
@@ -57,22 +57,22 @@ public class ShooterReal implements ShooterIO {
@Override
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.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) {