diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index dc61c45..3734440 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,6 +4,7 @@
package frc.robot;
+import com.ctre.phoenix.led.CANdle;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.hardware.TalonFX;
@@ -25,6 +26,7 @@
import frc.robot.intake.HeldGamePiece;
import frc.robot.intake.IntakeState;
import frc.robot.intake.IntakeSubsystem;
+import frc.robot.lights.LightsSubsystem;
import frc.robot.localization.LocalizationSubsystem;
import frc.robot.managers.autobalance.Autobalance;
import frc.robot.managers.autorotate.AutoRotate;
@@ -32,6 +34,7 @@
import frc.robot.managers.superstructure.States;
import frc.robot.managers.superstructure.SuperstructureManager;
import frc.robot.managers.superstructure.SuperstructureMotionManager;
+import frc.robot.managers.superstructure.SuperstructureState;
import frc.robot.shoulder.ShoulderSubsystem;
import frc.robot.swerve.SwerveModule;
import frc.robot.swerve.SwerveSubsystem;
@@ -105,6 +108,8 @@ public class Robot extends LoggedRobot {
private final LocalizationSubsystem localization = new LocalizationSubsystem(swerve, imu);
+ private final LightsSubsystem lights = new LightsSubsystem(new CANdle(Config.CANDLE_ID), intake, superstructure);
+
private final Autos autos = new Autos(localization, swerve, intake, wrist);
private Command autoCommand;
@@ -191,9 +196,10 @@ private void configureButtonBindings() {
.onFalse(swerve.disableXSwerveCommand());
// Operator controls
- operatorController.y().onTrue(superstructure.getScoreAlignCommand(NodeHeight.HIGH));
- operatorController.b().onTrue(superstructure.getScoreAlignCommand(NodeHeight.MID));
- operatorController.a().onTrue(superstructure.setStateCommand(States.STOWED));
+ operatorController.y().onTrue(superstructure.getScoreAlignCommand(() -> NodeHeight.HIGH));
+ operatorController.b().onTrue(superstructure.getScoreAlignCommand(() -> NodeHeight.MID));
+ // A or X for stow
+ operatorController.a().or(operatorController.x()).onTrue(superstructure.setStateCommand(States.STOWED));
// Manual intake override
operatorController
diff --git a/src/main/java/frc/robot/controller/DriveController.java b/src/main/java/frc/robot/controller/DriveController.java
index 2303576..8f53682 100644
--- a/src/main/java/frc/robot/controller/DriveController.java
+++ b/src/main/java/frc/robot/controller/DriveController.java
@@ -46,7 +46,7 @@ public double getForwardPercentage() {
/** The rotation about the robot's z-axis as a percentage (-1 <= x <= 1) */
public double getThetaPercentage() {
- return joystickScale(-1 * getRightX());
+ return joystickScale(getRightX());
}
public NodeKind getAutoScoreNodeKind() {
diff --git a/src/main/java/frc/robot/intake/IntakeSubsystem.java b/src/main/java/frc/robot/intake/IntakeSubsystem.java
index 215abc0..9fdddc9 100644
--- a/src/main/java/frc/robot/intake/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/intake/IntakeSubsystem.java
@@ -53,12 +53,16 @@ public void robotPeriodic() {
@Override
public void enabledPeriodic() {
- if (goalState == IntakeState.OUTTAKE_CONE) {
+ if (goalState == IntakeState.MANUAL_INTAKE) {
+ motor.set(-0.6);
+ } else if (goalState == IntakeState.MANUAL_OUTTAKE) {
+ motor.set(0.6);
+ } else if (goalState == IntakeState.OUTTAKE_CONE) {
motor.set(0.4);
} else if (goalState == IntakeState.OUTTAKE_CUBE) {
motor.set(0.3);
} else if (gamePiece == HeldGamePiece.CUBE) {
- motor.set(-0.6);
+ motor.set(-0.75);
} else if (gamePiece == HeldGamePiece.CONE) {
motor.set(-0.6);
} else if (goalState == IntakeState.INTAKE_CUBE) {
diff --git a/src/main/java/frc/robot/lights/LightsSubsystem.java b/src/main/java/frc/robot/lights/LightsSubsystem.java
index 252f80f..69483e3 100644
--- a/src/main/java/frc/robot/lights/LightsSubsystem.java
+++ b/src/main/java/frc/robot/lights/LightsSubsystem.java
@@ -17,6 +17,7 @@
import frc.robot.managers.superstructure.SuperstructureManager;
import frc.robot.util.scheduling.LifecycleSubsystem;
import frc.robot.util.scheduling.SubsystemPriority;
+import org.littletonrobotics.junction.Logger;
public class LightsSubsystem extends LifecycleSubsystem {
@@ -29,7 +30,6 @@ public class LightsSubsystem extends LifecycleSubsystem {
private final Timer blinkTimer = new Timer();
private Color color = Color.kWhite;
- // TODO: Copy paste blink pattern enum from other repo
private BlinkPattern blinkPattern = BlinkPattern.SOLID;
public LightsSubsystem(
@@ -112,4 +112,10 @@ public void enabledPeriodic() {
}
}
}
+
+ @Override
+ public void robotPeriodic() {
+ Logger.getInstance().recordOutput("Lights/BlinkPattern", blinkPattern.toString());
+ Logger.getInstance().recordOutput("Lights/Color", color.toString());
+ }
}
diff --git a/src/main/java/frc/robot/managers/superstructure/Positions.java b/src/main/java/frc/robot/managers/superstructure/Positions.java
index 29f0e30..6724fa2 100644
--- a/src/main/java/frc/robot/managers/superstructure/Positions.java
+++ b/src/main/java/frc/robot/managers/superstructure/Positions.java
@@ -11,7 +11,7 @@ public class Positions {
new SuperstructurePosition(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0));
public static final SuperstructurePosition INTAKING_CUBE_FLOOR =
- new SuperstructurePosition(Rotation2d.fromDegrees(-25), Rotation2d.fromDegrees(-120));
+ new SuperstructurePosition(Rotation2d.fromDegrees(-31), Rotation2d.fromDegrees(-140));
public static final SuperstructurePosition INTAKING_CUBE_SHELF =
new SuperstructurePosition(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(180));
@@ -20,12 +20,12 @@ public class Positions {
public static final SuperstructurePosition CUBE_NODE_LOW_BACK =
new SuperstructurePosition(Rotation2d.fromDegrees(4.7), Rotation2d.fromDegrees(93.9));
public static final SuperstructurePosition CUBE_NODE_MID =
- new SuperstructurePosition(Rotation2d.fromDegrees(75), Rotation2d.fromDegrees(190));
+ new SuperstructurePosition(Rotation2d.fromDegrees(76), Rotation2d.fromDegrees(190));
public static final SuperstructurePosition CUBE_NODE_HIGH =
new SuperstructurePosition(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(190));
public static final SuperstructurePosition INTAKING_CONE_FLOOR =
- new SuperstructurePosition(Rotation2d.fromDegrees(-33), Rotation2d.fromDegrees(-152));
+ new SuperstructurePosition(Rotation2d.fromDegrees(-32), Rotation2d.fromDegrees(-152));
public static final SuperstructurePosition INTAKING_CONE_SHELF =
new SuperstructurePosition(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(180));
public static final SuperstructurePosition INTAKING_CONE_SINGLE_SUBSTATION =
@@ -36,7 +36,7 @@ public class Positions {
public static final SuperstructurePosition CONE_NODE_LOW_BACK =
new SuperstructurePosition(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(89));
public static final SuperstructurePosition CONE_NODE_MID =
- new SuperstructurePosition(Rotation2d.fromDegrees(75), Rotation2d.fromDegrees(190));
+ new SuperstructurePosition(Rotation2d.fromDegrees(120), Rotation2d.fromDegrees(-70));
public static final SuperstructurePosition CONE_NODE_HIGH =
new SuperstructurePosition(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(190));
diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java
index 7d45600..bf322d7 100644
--- a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java
+++ b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java
@@ -76,6 +76,17 @@ public void robotPeriodic() {
Logger.getInstance()
.recordOutput("Superstructure/GoalState/GoalIntakeNow", goalState.intakeNow);
Logger.getInstance().recordOutput("Superstructure/Mode", mode.toString());
+ Logger.getInstance().recordOutput("Superstructure/ScoringProgress", scoringProgress.toString());
+ if (scoringHeight == null) {
+ Logger.getInstance().recordOutput("Superstructure/ScoringHeight", "(null)");
+ } else {
+ Logger.getInstance().recordOutput("Superstructure/ScoringHeight", scoringHeight.toString());
+ }
+ if (manualIntakeState == null) {
+ Logger.getInstance().recordOutput("Superstructure/ManualIntakeState", "(null)");
+ } else {
+ Logger.getInstance().recordOutput("Superstructure/ManualIntakeState", manualIntakeState.toString());
+ }
}
public void setIntakeOverride(IntakeState intakeState) {
@@ -193,36 +204,41 @@ private SuperstructureScoringState getScoringState(NodeHeight height) {
}
}
- public Command getScoreAlignCommand(NodeHeight height) {
+ public Command getScoreAlignCommand(Supplier height) {
return Commands.runOnce(
() -> {
- scoringHeight = height;
+ scoringHeight = height.get();
scoringProgress = ScoringProgress.ALIGNING;
+ System.out.println("scoring height in score align command: " + scoringHeight.toString());
+ System.out.println("height.get() in score align command: " + height.get().toString());
})
- .andThen(setStateCommand(getScoringState(height).aligning))
+ .andThen(setStateCommand(() -> getScoringState(height.get()).aligning))
.withName("ScoreAlignCommand");
}
public Command getScoreFinishCommand() {
- return getScoreFinishCommand(scoringHeight == null ? NodeHeight.LOW : scoringHeight)
+ return getScoreFinishCommand(() -> scoringHeight == null ? NodeHeight.LOW : scoringHeight)
.withName("ScoreFinishCommand");
}
- public Command getScoreFinishCommand(NodeHeight height) {
+ public Command getScoreFinishCommand(Supplier height) {
return Commands.runOnce(
() -> {
- scoringHeight = height;
+ scoringHeight = height.get();
scoringProgress = ScoringProgress.PLACING;
+ System.out.println("scoring height in score finish command: " + scoringHeight.toString());
+ System.out.println("height.get() in score finish command: " + height.get().toString());
})
- .andThen(setStateCommand(getScoringState(height).aligning))
- .andThen(setStateCommand(getScoringState(height).scoring))
+ .andThen(setStateCommand(getScoringState(height.get()).aligning))
+ .andThen(setStateCommand(getScoringState(height.get()).scoring))
.andThen(
Commands.runOnce(
() -> {
- scoringHeight = null;
+ System.out.println("scoring height in score finish command: " + scoringHeight.toString());
+ System.out.println("height.get() in score finish command: " + height.get().toString()); scoringHeight = null;
scoringProgress = ScoringProgress.DONE_SCORING;
}))
- .andThen(stowFast().unless(() -> height != NodeHeight.LOW))
+ .andThen(stowFast().unless(() -> height.get() != NodeHeight.LOW))
.withName("ScoreFinishCommand");
}
}