Skip to content

Commit

Permalink
Implement ICAT (#232)
Browse files Browse the repository at this point in the history
  • Loading branch information
@dunep
dunep authored Mar 17, 2024
1 parent 0a68da7 commit 02cead4
Show file tree
Hide file tree
Showing 21 changed files with 882 additions and 323 deletions.
26 changes: 19 additions & 7 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,17 +7,20 @@
import com.ctre.phoenix.led.ColorFlowAnimation;
import com.ctre.phoenix.led.RainbowAnimation;
import com.ctre.phoenix.led.ColorFlowAnimation.Direction;
import com.ctre.phoenix6.signals.GravityTypeValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.frcteam3255.components.swerve.SN_SwerveConstants;
import com.frcteam3255.preferences.SN_BooleanPreference;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.util.Units;
import frc.robot.Constants.constDrivetrain.pracBot;

/*
* @formatter:off
Expand All @@ -37,9 +40,14 @@
* are exempt from this
*/
public final class Constants {

public static class constClimber {
public static final double GEAR_RATIO = 327.6;
public static final double GEAR_RATIO = 18.56;
public static final NeutralModeValue NEUTRAL_MODE = NeutralModeValue.Brake;
public static final GravityTypeValue GRAVITY_TYPE = GravityTypeValue.Elevator_Static;

public static final SN_BooleanPreference climberInverted = new SN_BooleanPreference("climberInverted", true);

public static final InvertedValue MOTOR_INVERTED = InvertedValue.Clockwise_Positive;
}

public static class constControllers {
Expand Down Expand Up @@ -83,10 +91,10 @@ public static class pracBot {

// In Rotations: Obtain by aligning all of the wheels in the correct direction
// and copy-pasting the Raw Absolute Encoder value
public static final double FRONT_LEFT_ABS_ENCODER_OFFSET = -0.155518;
public static final double FRONT_RIGHT_ABS_ENCODER_OFFSET = 0.039062;
public static final double BACK_LEFT_ABS_ENCODER_OFFSET = 0.399658;
public static final double BACK_RIGHT_ABS_ENCODER_OFFSET = -0.418945;
public static final double FRONT_LEFT_ABS_ENCODER_OFFSET = -0.152832;
public static final double FRONT_RIGHT_ABS_ENCODER_OFFSET = 0.032471;
public static final double BACK_LEFT_ABS_ENCODER_OFFSET = -0.107666;
public static final double BACK_RIGHT_ABS_ENCODER_OFFSET = 0.095215;

/**
* <p>
Expand Down Expand Up @@ -132,6 +140,10 @@ public static class pracBot {
}

public static class constIntake {
public static final double ABS_ENCODER_OFFSET = 0.102385;
public static final boolean ABS_ENCODER_INVERT = false;
public static final double GEAR_RATIO = 28.13;
public static final NeutralModeValue PIVOT_NEUTRAL_MODE = NeutralModeValue.Brake;
}

public static class constLEDs {
Expand Down Expand Up @@ -331,6 +343,6 @@ public static class constTransfer {
* Locations that the robot can attempt to lock onto.
*/
public enum LockedLocation {
NONE, SPEAKER
NONE, SPEAKER, AMP
}
}
12 changes: 10 additions & 2 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@ public class Robot extends TimedRobot {

private RobotContainer m_robotContainer;

private boolean hasAutonomousRun = false;

@Override
public void robotInit() {
SN_Preferences.useDefaults();
Expand Down Expand Up @@ -69,11 +71,13 @@ public void disabledExit() {
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.clearSubsystemMovements().schedule();

RobotContainer.zeroClimber().schedule();
// RobotContainer.stowIntakePivot().schedule();
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
hasAutonomousRun = true;
}

@Override
Expand All @@ -89,7 +93,11 @@ public void teleopInit() {
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
m_robotContainer.clearSubsystemMovements().schedule();

if (!hasAutonomousRun) {
RobotContainer.zeroClimber().schedule();
// RobotContainer.stowIntakePivot().schedule();
}
}

@Override
Expand Down
122 changes: 52 additions & 70 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
Expand All @@ -21,11 +20,12 @@
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.InstantCommand;
import frc.robot.Constants.constControllers;
import frc.robot.Constants.LockedLocation;
import frc.robot.Constants.constLEDs;
import frc.robot.RobotMap.mapControllers;
import frc.robot.RobotPreferences.prefClimber;
import frc.robot.RobotPreferences.prefIntake;
import frc.robot.RobotPreferences.prefPitch;
import frc.robot.RobotPreferences.prefVision;
import frc.robot.RobotPreferences.prefShooter;
Expand All @@ -41,8 +41,10 @@
import frc.robot.commands.ManualHoodMovement;
import frc.robot.commands.ManualTurretMovement;
import frc.robot.commands.Panic;
import frc.robot.commands.PrepAmp;
import frc.robot.commands.SetLEDS;
import frc.robot.commands.TransferGamePiece;
import frc.robot.commands.ZeroClimber;
import frc.robot.commands.UnaliveShooter;
import frc.robot.commands.ZeroPitch;
import frc.robot.commands.ZeroTurret;
Expand Down Expand Up @@ -115,16 +117,16 @@ public RobotContainer() {
conDriver.btn_X,
isPracticeBot()));

subTurret.setDefaultCommand(new LockTurret(subTurret, subDrivetrain, subClimber));
subPitch.setDefaultCommand(new LockPitch(subPitch, subDrivetrain, subClimber));
subTurret.setDefaultCommand(new LockTurret(subTurret, subDrivetrain));
subPitch.setDefaultCommand(new LockPitch(subPitch, subDrivetrain));
subShooter.setDefaultCommand(new Shoot(subShooter, subLEDs));
subLEDs
.setDefaultCommand(new SetLEDS(subLEDs, subShooter, subTurret, subPitch, subTransfer,
conDriver.btn_RightBumper));

// Register Autonomous Named Commands
NamedCommands.registerCommand("IntakeGamePiece",
new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subClimber, subPitch, subShooter));
new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber));

// View controls at:
// src\main\assets\controllerMap2024.png
Expand All @@ -134,6 +136,7 @@ public RobotContainer() {

subDrivetrain.resetModulesToAbsolute();
subTurret.resetTurretToAbsolutePosition();
subIntake.resetPivotToAbsolute();
subLEDs.clearAnimation();
}

Expand All @@ -143,50 +146,46 @@ private void configureDriverBindings(SN_XboxController controller) {
.onTrue(
Commands.runOnce(() -> subDrivetrain.resetPoseToPose(FieldConstants.GET_FIELD_POSITIONS()[6].toPose2d())));

// // Climb Up

// controller.btn_LeftTrigger.whileTrue(
// Commands.run(() ->
// subClimber.setClimberMotorSpeed(prefClimber.climberMotorUpSpeed.getValue()),
// subClimber)
// .alongWith(Commands.runOnce(() -> subTurret.setTurretAngle(0, false))));
// controller.btn_LeftTrigger.onFalse(
// Commands.run(() -> subClimber.setClimberMotorSpeed(0), subClimber));

// // Climb Down
// controller.btn_RightTrigger.whileTrue(
// Commands.run(() ->
// subClimber.setClimberMotorSpeed(prefClimber.climberMotorDownSpeed.getValue()),
// subClimber)
// .alongWith(Commands.runOnce(() -> subTurret.setTurretAngle(0, false))));
// controller.btn_RightTrigger.onFalse(
// Commands.run(() -> subClimber.setClimberMotorSpeed(0), subClimber));
controller.btn_LeftTrigger
.whileTrue(
Commands.either(
new PrepAmp(subIntake, subPitch, subTransfer, subTurret, subShooter),
Commands.run(() -> subClimber.setPercentOutput(prefClimber.climberUpSpeed.getValue())),
() -> getLockedLocation() != LockedLocation.AMP).repeatedly())
.onFalse(Commands.runOnce(() -> subClimber.setPercentOutput(0)));

controller.btn_RightTrigger
.onTrue(Commands.runOnce(() -> subClimber.setCurrentLimiting(false)))
.whileTrue(Commands.run(() -> subClimber.setPercentOutput(prefClimber.climberDownSpeed.getValue())))
.onFalse(Commands.run(() -> subClimber.setPercentOutput(0))
.alongWith(Commands.runOnce(() -> subClimber.setCurrentLimiting(false))));

controller.btn_RightBumper.whileTrue(Commands.run(() -> subDrivetrain.setDefenseMode(), subDrivetrain))
.whileFalse(Commands.runOnce(() -> subLEDs.clearAnimation()));
}

private void configureOperatorBindings(SN_XboxController controller) {
controller.btn_LeftTrigger
.whileTrue(new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subClimber, subPitch, subShooter));
.whileTrue(new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber));
controller.btn_LeftBumper
.onTrue(Commands.runOnce(() -> setLockedLocation(LockedLocation.NONE)))
.whileTrue(new ManualTurretMovement(subTurret, controller.axis_RightX));

controller.btn_LeftStick.whileTrue(new Panic(subLEDs));
controller.btn_North
.whileTrue(new IntakeFromSource(subShooter, subTransfer, subPitch, subTurret, subClimber));
controller.btn_South.whileTrue(new SpitGamePiece(subIntake, subTransfer, subPitch, subClimber));
.whileTrue(new IntakeFromSource(subShooter, subTransfer, subPitch, subTurret));
controller.btn_South.whileTrue(new SpitGamePiece(subIntake, subTransfer, subPitch));
controller.btn_West.onTrue(Commands.runOnce(() -> subTransfer.setGamePieceCollected(true)));
controller.btn_East.onTrue(Commands.runOnce(() -> setLockedLocation(LockedLocation.NONE)))
.whileTrue(new ManualHoodMovement(subPitch, controller.axis_RightX));

controller.btn_RightTrigger.whileTrue(new TransferGamePiece(subShooter, subTurret, subTransfer, subPitch))
controller.btn_RightTrigger
.whileTrue(new TransferGamePiece(subShooter, subTurret, subTransfer, subPitch, subIntake))
.onFalse(Commands.runOnce(() -> subTransfer.setFeederNeutralOutput())
.alongWith(Commands.runOnce(() -> subTransfer.setTransferNeutralOutput()))
.alongWith(new UnaliveShooter(subShooter, subTurret, subPitch, subClimber, subLEDs)));
.alongWith(new UnaliveShooter(subShooter, subTurret, subPitch, subLEDs)));

controller.btn_RightBumper.onTrue(new UnaliveShooter(subShooter, subTurret, subPitch, subClimber, subLEDs)
controller.btn_RightBumper.onTrue(new UnaliveShooter(subShooter, subTurret, subPitch, subLEDs)
.alongWith(Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false))));

controller.btn_Back.onTrue(new ZeroTurret(subTurret));
Expand All @@ -199,16 +198,7 @@ private void configureOperatorBindings(SN_XboxController controller) {
.alongWith(Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false))));

// Amp Preset
controller.btn_B.onTrue(Commands.runOnce(() -> setLockedLocation(LockedLocation.NONE))
.alongWith(
Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterAmpVelocity.getValue(),
prefShooter.rightShooterAmpVelocity.getValue()))
.alongWith(Commands.runOnce(() -> subTurret.setTurretAngle(prefTurret.turretAmpPresetPos.getValue(),
subClimber.collidesWithTurret())))
.alongWith(Commands.runOnce(
() -> subPitch.setPitchAngle(prefPitch.pitchAmpAngle.getValue(),
subClimber.collidesWithPitch()))))
.alongWith(Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false))));
controller.btn_B.whileTrue(new PrepAmp(subIntake, subPitch, subTransfer, subTurret, subShooter));

// Subwoofer Preset
controller.btn_X.onTrue(Commands.runOnce(() -> setLockedLocation(LockedLocation.NONE))
Expand All @@ -217,36 +207,18 @@ private void configureOperatorBindings(SN_XboxController controller) {
prefShooter.rightShooterSubVelocity.getValue())))
.alongWith(
Commands.runOnce(
() -> subTurret.setTurretAngle(prefTurret.turretSubPresetPos.getValue(),
subClimber.collidesWithTurret())))
() -> subTurret.setTurretAngle(prefTurret.turretSubPresetPos.getValue())))
.alongWith(Commands.runOnce(
() -> subPitch.setPitchAngle(prefPitch.pitchSubAngle.getValue(),
subClimber.collidesWithPitch())))
() -> subPitch.setPitchAngle(prefPitch.pitchSubAngle.getValue())))
.alongWith(Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(true))));

// Trap Preset
// controller.btn_Y.onTrue(Commands.runOnce(() ->
// setLockedLocation(LockedLocation.NONE)).alongWith(
// Commands.runOnce(() ->
// subShooter.setDesiredVelocities(prefShooter.leftShooterTrapVelocity.getValue(),
// prefShooter.rightShooterTrapVelocity.getValue())))
// .alongWith(
// Commands.runOnce(
// () -> subTurret.setTurretAngle(prefTurret.turretTrapPresetPos.getValue(),
// subClimber.collidesWithTurret())))
// .alongWith(Commands.runOnce(
// () -> subPitch.setPitchAngle(prefPitch.pitchTrapAngle.getValue(),
// subClimber.collidesWithPitch()))));

controller.btn_Y.whileTrue((Commands.runOnce(() -> subClimber.setClimberVoltage(-2))));
controller.btn_Y.onFalse((Commands.runOnce(() -> subClimber.setClimberVoltage(0))));
controller.btn_Y.onFalse((Commands.runOnce(() -> subClimber.configure(true))));

// STOW
controller.btn_Y.onTrue(Commands.runOnce(() -> subIntake.setPivotAngle(prefIntake.pivotStowAngle.getValue())));
}

private void configureAutoSelector() {
autoChooser.setDefaultOption("Default Auto",
new DefaultAuto(subDrivetrain, subIntake, subLEDs, subPitch, subShooter, subTransfer, subTurret, subClimber));
new DefaultAuto(subDrivetrain, subIntake, subLEDs, subPitch, subShooter, subTransfer, subTurret));
}

public Command getAutonomousCommand() {
Expand Down Expand Up @@ -298,14 +270,6 @@ public static void logPDHValues() {
}
}

/**
* Sets all applicable subsystem's last desired location to their current
* location
*/
public Command clearSubsystemMovements() {
return new InstantCommand((() -> subTurret.setTurretNeutralOutput()));
}

// --- Locking Logic ---

public static void setLockedLocation(LockedLocation location) {
Expand All @@ -331,6 +295,24 @@ public static Command zeroPitch() {
return new ZeroPitch(subPitch).withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming).withTimeout(3);
}

/**
* Returns the command to zero the climber. This will make the climber move
* itself
* downwards until it sees a current spike and cancel any incoming commands that
* require the pitch motor. If the zeroing does not end within 3 seconds, it
* will interrupt itself.
*
* @return The command to zero the pitch
*/
public static Command zeroClimber() {
return new ZeroClimber(subClimber).withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)
.withTimeout(3);
}

public static Command stowIntakePivot() {
return Commands.runOnce(() -> subIntake.setPivotAngle(prefIntake.pivotStowAngle.getValue()));
}

public void setAutoPlacementLEDs(Optional<Alliance> alliance) {
startingPosition = autoChooser.getSelected().getInitialPose().get();

Expand Down
18 changes: 10 additions & 8 deletions src/main/java/frc/robot/RobotMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,13 +14,6 @@ public static class mapControllers {
// MOTORS: 30 -> 39
public static class mapClimber {
public static final int CLIMBER_MOTOR_CAN = 30;
public static final int CLIMBER_ABSOLUTE_ENCODER_DIO = 2;
}

// MOTORS: 50 -> 59
public static class mapTransfer {
public static final int FEEDER_MOTOR_CAN = 50;
public static final int TRANSFER_MOTOR_CAN = 51;
}

// MOTORS: 00 -> 09
Expand Down Expand Up @@ -51,7 +44,10 @@ public static class mapDrivetrain {

// MOTORS: 20 -> 29
public static class mapIntake {
public static final int INTAKE_ROLLER_MOTOR_CAN = 20;
public static final int ROLLER_CAN = 20;
public static final int PIVOT_CAN = 21;
public static final int ABSOLUTE_ENCODER_DIO = 2;

}

public static class mapLEDs {
Expand All @@ -75,6 +71,12 @@ public static class mapTurret {
public static final int TURRET_ABSOLUTE_ENCODER_DIO = 1;
}

// MOTORS: 50 -> 59
public static class mapTransfer {
public static final int FEEDER_MOTOR_CAN = 50;
public static final int TRANSFER_MOTOR_CAN = 51;
}

public static class mapVision {
}
}
Loading

0 comments on commit 02cead4

Please sign in to comment.