Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ public static class IntakeConstants
public static final Voltage INTAKE_VOLTS = Volts.of(4.0);
public static final Voltage REVERSE_VOLTS = Volts.of(-6.0);
public static final Current ROLLER_CURRENT_LIMIT_EXTENDED = Amps.of(80);
public static final Current ROLLER_CURRENT_LIMIT_ACTIVE = Amps.of(40);
public static final Current ROLLER_CURRENT_LIMIT_ACTIVE = Amps.of(60);
public static final Current EXTENSION_CURRENT_LIMIT = Amps.of(60);
public static final int CAMERA_DEVICE_INDEX = 0;
public static final String CAMERA_NAME = "IntakeCam";
Expand Down
62 changes: 44 additions & 18 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,21 +11,19 @@

import edu.wpi.first.epilogue.Logged;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Dimensionless;
import edu.wpi.first.units.measure.LinearVelocity;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.DriveConstants;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.TunerConstants;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.util.MeasureUtil;
import frc.robot.NetParams;

@Logged
public class RobotContainer
Expand All @@ -40,9 +38,9 @@ public class RobotContainer
private final Intake _intake = new Intake();
private final Shooter _shooter = new Shooter(_drive::getState, _intake::isExtended, _intake::isRetracted);
private final Autos _autos = new Autos(_drive, _shooter, _intake);
private boolean _driverPOVReleased = true;
// private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE;
private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM;
private NetParams _params;
private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM;

public RobotContainer()
{
Expand All @@ -51,24 +49,30 @@ public RobotContainer()

private LinearVelocity getDrive()
{
return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getY())).times((-_driver.getRawAxis(3) + 1) / 2.0), DriveConstants.TRANSLATE_DEADBAND);
return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getY())).times((-_driver.getRawAxis(3) + 1) * 0.375 + 0.25), DriveConstants.TRANSLATE_DEADBAND);
}

private LinearVelocity getStrafe()
{
return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getX())).times((-_driver.getRawAxis(3) + 1) / 2.0), DriveConstants.TRANSLATE_DEADBAND);
return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getX())).times((-_driver.getRawAxis(3) + 1) * 0.375 + 0.25), DriveConstants.TRANSLATE_DEADBAND);
}

private AngularVelocity getRotate()
{
return MeasureUtil.applyDeadband(DriveConstants.MAX_ANGULAR_RATE.times(Value.of(-_driver.getTwist())).times((-_driver.getRawAxis(3) + 1) / 2.0), DriveConstants.ROTATE_DEADBAND);
return MeasureUtil.applyDeadband(DriveConstants.MAX_ANGULAR_RATE.times(Value.of(-_driver.getTwist())).times((-_driver.getRawAxis(3) + 1) * 0.375 + 0.25), DriveConstants.ROTATE_DEADBAND);
}

private SwerveRequest.FieldCentric getFieldCentricRequest()
{
return _fieldCentric.withVelocityX(getDrive()).withVelocityY(getStrafe()).withRotationalRate(getRotate());
}

private void setManualFlywheelRPM(double rpm)
{
_manualFlywheelRPM = Math.max(0.0, rpm);
_shooter.setManualFlywheel(_manualFlywheelRPM);
}

private void configureBindings()
{
_drive.setDefaultCommand(_drive.applyRequest(this::getFieldCentricRequest));
Expand All @@ -79,7 +83,7 @@ private void configureBindings()
// _driver.button(1).whileTrue(Commands.parallel(_shooter.shoot(),
// Commands.startEnd(() -> _drive.disableVisionPoseCorrection(true), () ->
// _drive.disableVisionPoseCorrection(false))));
_driver.button(1).whileTrue(_shooter.manualShootCmd(() -> _params.flywheelSpeed));
_driver.button(1).whileTrue(_shooter.manualShootCmd(() -> _manualFlywheelRPM));
// _driver.button(2).whileTrue(Commands.startEnd(() -> _driveMultiplier =
// DriveConstants.SLOW_MODE_SCALE, () -> _driveMultiplier =
// DriveConstants.FULL_SPEED_SCALE));
Expand All @@ -89,24 +93,46 @@ private void configureBindings()
_driver.button(5).whileTrue(_shooter.pass());
_driver.button(6).whileTrue(_shooter.trackOnly());
_driver.button(7).onTrue(_drive.runOnce(_drive::seedFieldCentric));
_driver.button(8).onTrue(_shooter.setManualTurretAngle(Degrees.zero()));
_driver.button(9).onTrue(_shooter.setManualTurretAngle(Degrees.of(45.0)));
_driver.button(10).onTrue(_shooter.setManualTurretAngle(Degrees.of(-45.0)));
_driver.button(11).onTrue(_shooter.setManualTurretAngle(Degrees.of(90.0)));
_driver.button(12).onTrue(_shooter.setManualTurretAngle(Degrees.of(-90.0)));
_driver.button(13).onTrue(_shooter.setManualTurretAngle(Degrees.of(135.0)));
_driver.button(14).onTrue(_shooter.setManualTurretAngle(Degrees.of(-135.0)));
// _driver.button(8).onTrue(_shooter.setManualTurretAngle(Degrees.zero()));
// _driver.button(9).onTrue(_shooter.setManualTurretAngle(Degrees.of(45.0)));
// _driver.button(10).onTrue(_shooter.setManualTurretAngle(Degrees.of(-45.0)));
// _driver.button(11).onTrue(_shooter.setManualTurretAngle(Degrees.of(90.0)));
// _driver.button(12).onTrue(_shooter.setManualTurretAngle(Degrees.of(-90.0)));
// _driver.button(13).onTrue(_shooter.setManualTurretAngle(Degrees.of(135.0)));
// _driver.button(14).onTrue(_shooter.setManualTurretAngle(Degrees.of(-135.0)));
_driver.button(16).whileTrue(_drive.applyRequest(() -> _robotCentric.withVelocityX(getDrive()).withVelocityY(getStrafe()).withRotationalRate(getRotate())));

// Debounce the transition out of -1 by 50ms
Trigger povJustPressed = _driver.pov(-1).negate().debounce(0.05).and(() -> _driverPOVReleased);

povJustPressed.onTrue(Commands.runOnce(() ->
{
int angle = _driver.getHID().getPOV();
_driverPOVReleased = false;

_shooter.setManualTurretAngleCommand(Degrees.of(switch (angle)
{
case 0 -> -90;
case 180 -> 90;
case 225 -> 45;
case 315 -> -45;
default -> 0;
}));

}));

_driver.pov(-1).onTrue(Commands.runOnce(() -> _driverPOVReleased = true));

_operator.leftTrigger().whileTrue(_intake.runRollersForward());
_operator.leftBumper().whileTrue(_intake.runRollersReverse());
_operator.rightTrigger().whileTrue(_intake.jiggle());
_operator.y().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(MANUAL_FLYWHEEL_START_RPM)));
_operator.x().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM - MANUAL_FLYWHEEL_STEP_RPM)));
_operator.b().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM + MANUAL_FLYWHEEL_STEP_RPM)));
_operator.a().onTrue(Commands.runOnce(_shooter::stopManualFlywheel));
_operator.rightBumper().whileTrue(_shooter.runManualFeeder());
_operator.povDown().onTrue(_intake.getRetractCmd());
_operator.povUp().onTrue(_intake.getExtendCmd());

SmartDashboard.putData(_shooter.setTurretAngleCmd(() -> _params.turretAngle));
}

public Command getAutonomousCommand()
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/shooter/Shooter.java
Original file line number Diff line number Diff line change
Expand Up @@ -342,7 +342,7 @@ private void beginManualControl(boolean clearTurretTarget)
}
}

private void setManualTurretAngleCommand(Angle angle)
public void setManualTurretAngleCommand(Angle angle)
{
beginManualControl(true);
_turret.setManualAngle(angle);
Expand Down
Loading