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

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
elastic-layout.json
+86 -55
View File
@@ -8,10 +8,10 @@
"layouts": [
{
"title": "Tag Processed",
"x": 384.0,
"x": 896.0,
"y": 0.0,
"width": 256.0,
"height": 256.0,
"height": 384.0,
"type": "List Layout",
"properties": {
"label_position": "TOP"
@@ -55,27 +55,11 @@
}
],
"containers": [
{
"title": "MatchTime",
"x": 0.0,
"y": 0.0,
"width": 384.0,
"height": 256.0,
"type": "Match Time",
"properties": {
"topic": "/AdvantageKit/DriverStation/MatchTime",
"period": 0.06,
"data_type": "double",
"time_display_mode": "Minutes and Seconds",
"red_start_time": 15,
"yellow_start_time": 30
}
},
{
"title": "RetractedLimit",
"x": 0.0,
"y": 384.0,
"width": 128.0,
"x": 512.0,
"y": 0.0,
"width": 256.0,
"height": 128.0,
"type": "Boolean Box",
"properties": {
@@ -90,9 +74,9 @@
},
{
"title": "Auto Chooser",
"x": 1024.0,
"y": 256.0,
"width": 384.0,
"x": 896.0,
"y": 384.0,
"width": 256.0,
"height": 128.0,
"type": "ComboBox Chooser",
"properties": {
@@ -103,9 +87,9 @@
},
{
"title": "Roller Percent Output",
"x": 0.0,
"x": 512.0,
"y": 256.0,
"width": 256.0,
"width": 384.0,
"height": 128.0,
"type": "Number Slider",
"properties": {
@@ -120,8 +104,8 @@
},
{
"title": "Shooter OVERRIDE Velocity",
"x": 128.0,
"y": 384.0,
"x": 768.0,
"y": 0.0,
"width": 128.0,
"height": 128.0,
"type": "Text Display",
@@ -134,8 +118,8 @@
},
{
"title": "TRIM SHOOTER SPEED",
"x": 256.0,
"y": 256.0,
"x": 512.0,
"y": 384.0,
"width": 384.0,
"height": 128.0,
"type": "Number Slider",
@@ -151,8 +135,8 @@
},
{
"title": "Mode",
"x": 256.0,
"y": 384.0,
"x": 512.0,
"y": 128.0,
"width": 384.0,
"height": 128.0,
"type": "Text Display",
@@ -164,30 +148,49 @@
}
},
{
"title": "Time to rev",
"x": 768.0,
"y": 128.0,
"width": 256.0,
"height": 128.0,
"type": "Large Text Display",
"title": "IsActive",
"x": 0.0,
"y": 0.0,
"width": 384.0,
"height": 256.0,
"type": "Boolean Box",
"properties": {
"topic": "/AdvantageKit/RealOutputs/Time to rev",
"topic": "/AdvantageKit/RealOutputs/HubShift/IsActive",
"period": 0.06,
"data_type": "double"
"data_type": "boolean",
"true_color": 4283215696,
"false_color": 4294198070,
"true_icon": "None",
"false_icon": "None"
}
},
{
"title": "Arm angle extended",
"x": 1152.0,
"y": 128.0,
"width": 256.0,
"title": "RemainingInShift",
"x": 0.0,
"y": 256.0,
"width": 384.0,
"height": 128.0,
"type": "Text Display",
"type": "Match Time",
"properties": {
"topic": "/SmartDashboard/Arm angle extended",
"topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
"time_display_mode": "Minutes and Seconds",
"red_start_time": 15,
"yellow_start_time": 30
}
},
{
"title": "Phase",
"x": 0.0,
"y": 384.0,
"width": 384.0,
"height": 128.0,
"type": "Large Text Display",
"properties": {
"topic": "/AdvantageKit/RealOutputs/HubShift/Phase",
"period": 0.06,
"data_type": "string"
}
}
]
@@ -527,8 +530,8 @@
},
{
"title": "Shooter Idle max current",
"x": 384.0,
"y": 512.0,
"x": 1024.0,
"y": 384.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
@@ -555,8 +558,8 @@
},
{
"title": "Shooter OVERRIDE Velocity",
"x": 640.0,
"y": 512.0,
"x": 1024.0,
"y": 256.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
@@ -575,12 +578,40 @@
"grid_layout": {
"layouts": [],
"containers": [
{
"title": "Stalled Motor: ",
"x": 512.0,
"y": 128.0,
"width": 384.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/AdvantageKit/RealOutputs/Stalled Motor: ",
"period": 0.06,
"data_type": "string",
"show_submit_button": false
}
},
{
"title": "Shooter idle % output",
"x": 128.0,
"y": 256.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/SmartDashboard/Shooter idle % output",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
}
},
{
"title": "Auto Chooser",
"x": 0.0,
"y": 0.0,
"width": 640.0,
"height": 768.0,
"x": 512.0,
"y": 256.0,
"width": 384.0,
"height": 128.0,
"type": "ComboBox Chooser",
"properties": {
"topic": "/SmartDashboard/Auto Chooser",
src/main/java/frc4388/robot/RobotContainer.java
+115 -6
View File
@@ -125,7 +125,7 @@ public class RobotContainer {
// );
private Command RobotRev = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter),
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
IntakeExtended,
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
);
@@ -149,7 +149,7 @@ public class RobotContainer {
public RobotContainer() {
configureButtonBindings();
configureSINGLEBindings();
// Called on first robot enable
DeferredBlock.addBlock(() -> {
@@ -313,7 +313,7 @@ public class RobotContainer {
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Idle);
m_robotIntake.rollerStop();
m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds());
m_robotShooter.spinUpShooting();
}))
.onFalse(new InstantCommand(() -> {
m_robotShooter.spinUpIdle();
@@ -350,16 +350,125 @@ public class RobotContainer {
// m_robotClimber.toggleDeployed();
// }));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
}
private void configureSINGLEBindings() {
//Driver controls
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// TEST-> the driver is holding the left trigger, drive slow and rotation up
// new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
// .onTrue(new InstantCommand(() -> {
// m_robotSwerveDrive.setToSlow();
// m_robotSwerveDrive.shiftUpRot();
// }))
// .onFalse(new InstantCommand(() -> {
// m_robotSwerveDrive.setToFast();
// m_robotSwerveDrive.shiftDownRot();
// }));
//TEST - > X positino on wheels
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
.whileTrue(new RunCommand(() -> {
m_robotSwerveDrive.defenseXPosition();
}, m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> {
m_robotSwerveDrive.stopDefenseXPosition();
}));
//TEST - > PID positinon
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.indexerStalled();
currentPose = m_robotSwerveDrive.getCurrentPose();
}))
.whileTrue(new RunCommand(() -> {
m_stayInPosition.goToTargetPose(currentPose);
}, m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> {
m_robotSwerveDrive.softStop();
}));
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
.onTrue(new InstantCommand(() -> {
m_robotSwerveDrive.setToSlow();
}))
.whileTrue(new RunCommand(() -> {
m_robotSwerveDrive.driveFacingPosition(
getDeadbandedDriverController().getLeft(),
FieldPositions.HUB_POSITION,
ShooterConstants.AIM_LEAD_TIME.get()
);
}, m_robotSwerveDrive)
);
// D-PAD fine alignment
new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
.whileTrue(new RunCommand(
() -> m_robotSwerveDrive.driveFine(
new Translation2d(
1,
Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
),
getDeadbandedDriverController().getRight(), 0.15
), m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
//allow shooting with right trigger
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
.onTrue(new InstantCommand(() -> {
m_robotShooter.allowShooting();
})).onFalse(new InstantCommand(() -> {
m_robotShooter.denyShooting();
}));
//set shooter ready (rev) with left trigger hold
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Idle);
m_robotIntake.rollerStop();
m_robotShooter.spinUpShooting();
}))
.onFalse(new InstantCommand(() -> {
m_robotShooter.spinUpIdle();
}));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Extending);
}))
.onFalse(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Idle);
}));
}
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Retracting);
}))
.onFalse(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Idle);
}));
}
//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));
src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java
+1 -3
View File
@@ -34,7 +34,7 @@ public class StayInPosition extends PID {
//Same here
translationY = 0.2;
}
if (Math.abs(translationX) < 0.01 && Math.abs(translationY) < 0.01) {
if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) {
driveTranslation = new Translation2d();
} else {
//TODO: Tweak for best corrector against another bot
@@ -42,8 +42,6 @@ public class StayInPosition extends PID {
}
drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation());
// If above doesn't work
//drive.driveFieldAngle(driveTranslation, targetPose.getRotation());
}
@Override
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 = 170;
public static final String GIT_SHA = "917aaa774650423095bb33033fe57b1eb1c12a1a";
public static final String GIT_DATE = "2026-03-19 15:46:28 MDT";
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 String GIT_BRANCH = "MiraOrg";
public static final String BUILD_DATE = "2026-03-19 18:16:37 MDT";
public static final long BUILD_UNIX_TIME = 1773965797063L;
public static final String BUILD_DATE = "2026-03-20 15:31:47 MDT";
public static final long BUILD_UNIX_TIME = 1774042307703L;
public static final int DIRTY = 1;
private BuildConstants(){}
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;