Fix errors

This commit is contained in:
Shikhar
2026-02-16 15:57:24 -07:00
parent f88619a1da
commit 8551843831
5 changed files with 42 additions and 33 deletions
@@ -13,7 +13,6 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotBase;
@@ -35,7 +34,6 @@ import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake;
import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.intake.Intake.IntakeMode;
import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.Shooter;
import frc4388.robot.subsystems.shooter.Shooter.ShooterMode;
import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision; import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.DeferredBlock; import frc4388.utility.DeferredBlock;
@@ -252,12 +250,18 @@ public class RobotContainer {
//Operator Controls //Operator Controls
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5)
.onTrue(new InstantCommand(() -> { .onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Extended); m_robotIntake.setMode(IntakeMode.Extending);
}))
.onFalse(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Idle);
})); }));
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Retracting);
}))
.onFalse(new InstantCommand(() -> { .onFalse(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Retracted); m_robotIntake.setMode(IntakeMode.Idle);
})); }));
new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5) new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5)
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = ""; public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified"; public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 59; public static final int GIT_REVISION = 60;
public static final String GIT_SHA = "7f251857dc4fc6f925d5900e9822f20c8841fa1d"; public static final String GIT_SHA = "f88619a1da6b3adf64df3c4277deeca5bed6ee21";
public static final String GIT_DATE = "2026-02-14 15:05:25 MST"; public static final String GIT_DATE = "2026-02-14 15:57:33 MST";
public static final String GIT_BRANCH = "operator-controls"; public static final String GIT_BRANCH = "operator-controls";
public static final String BUILD_DATE = "2026-02-14 15:56:10 MST"; public static final String BUILD_DATE = "2026-02-16 15:54:00 MST";
public static final long BUILD_UNIX_TIME = 1771109770332L; public static final long BUILD_UNIX_TIME = 1771282440915L;
public static final int DIRTY = 1; public static final int DIRTY = 1;
private BuildConstants(){} private BuildConstants(){}
@@ -7,9 +7,7 @@ import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
public class Intake extends SubsystemBase { public class Intake extends SubsystemBase {
public IntakeIO io; public IntakeIO io;
@@ -29,7 +27,8 @@ public class Intake extends SubsystemBase {
Extended, Extended,
Retracted, Retracted,
Extending, Extending,
Retracting Retracting,
Idle
} }
private IntakeMode mode = IntakeMode.Extended; private IntakeMode mode = IntakeMode.Extended;
@@ -73,22 +72,19 @@ public class Intake extends SubsystemBase {
io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
break; break;
case Retracted: case Retracted:
if (!state.retractedLimit){ io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
io.stopArm();
} else {
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
}
io.setRollerVelocity(state, RotationsPerSecond.of(0)); io.setRollerVelocity(state, RotationsPerSecond.of(0));
break; break;
case Extending: case Extending:
io.armExtend(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); io.armOutput(0.1);
io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
break; break;
case Retracting: case Retracting:
if (!state.retractedLimit){ io.armOutput(-0.1);
io.stopArm(); io.setRollerVelocity(state, RotationsPerSecond.of(0));
} else { break;
io.armRetract(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); case Idle:
} io.stopArm();
break; break;
} }
@@ -36,8 +36,7 @@ public interface IntakeIO {
public default void setArmAngle(IntakeState state, Angle angle) {} public default void setArmAngle(IntakeState state, Angle angle) {}
public default void stopArm() {} public default void stopArm() {}
public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {} public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {}
public default void armExtend(double percentOutput) {} public default void armOutput(double percentOutput) {}
public default void armRetract(double percentOutput) {}
public default void updateInputs(IntakeState state) {} public default void updateInputs(IntakeState state) {}
public default void updateGains() {} public default void updateGains() {}
} }
@@ -1,5 +1,9 @@
package frc4388.robot.subsystems.intake; package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.Rotation;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
@@ -69,6 +73,7 @@ public class IntakeReal implements IntakeIO {
@Override @Override
public void setArmAngle(IntakeState state, Angle angle) { public void setArmAngle(IntakeState state, Angle angle) {
state.armTargetAngle = angle; state.armTargetAngle = angle;
// Assume that the angle is always accurate, because I think we will use a shaft encoder // Assume that the angle is always accurate, because I think we will use a shaft encoder
// Assume that 0 degrees = forwards. Might need an offset here // Assume that 0 degrees = forwards. Might need an offset here
@@ -79,20 +84,25 @@ public class IntakeReal implements IntakeIO {
Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
m_armMotor.setControl(armPosition.withPosition(motorAngle)); m_armMotor.setControl(
armPosition
.withPosition(motorAngle)
.withLimitReverseMotion(!m_armLimitSwitch.get())
);
} }
@Override @Override
public void stopArm(){ public void stopArm(){
m_armMotor.set(0); m_armMotor.set(0);
} }
@Override @Override
public void armExtend(double percentOutput){ public void armOutput(double percentOutput){
m_armMotor.set(percentOutput); var d = new DutyCycleOut(0);
} m_armMotor.setControl(
d.withOutput(percentOutput)
@Override .withLimitReverseMotion(!m_armLimitSwitch.get())
public void armRetract(double percentOutput){ );
m_armMotor.set(percentOutput);
} }
@Override @Override