Merge branch 'main' of https://github.com/frcteam3044/2025swervebase

FRCTeam3044 · Jan 24, 2025 · 55d9977 · 55d9977
2 parents f9c7372 + 756171a
commit 55d9977
Show file tree

Hide file tree

Showing 22 changed files with 295 additions and 179 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -26,6 +26,8 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.statemachine.StateMachine;
+import frc.robot.subsystems.EndEffector.EndEffector;
+import frc.robot.subsystems.EndEffector.EndEffectorIOSpark;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveCommands;
 import frc.robot.subsystems.drive.DriveConstants;
@@ -35,6 +37,10 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOSpark;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.elevator.ElevatorIOSpark;
+import frc.robot.subsystems.shoulder.Shoulder;
+import frc.robot.subsystems.shoulder.ShoulderIOSpark;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
@@ -58,6 +64,9 @@ public class RobotContainer {
         // Subsystems
         private final Drive drive;
         private final Vision vision;
+        private final Elevator elevator;
+        private final Shoulder shoulder;
+        private final EndEffector endEffector;
         private SwerveDriveSimulation driveSimulation = null;
 
         // Controller
@@ -86,6 +95,9 @@ public RobotContainer() {
                                                 new ModuleIOSpark(3));
 
                                 this.vision = new Vision(drive);
+                                elevator = new Elevator(new ElevatorIOSpark());
+                                shoulder = new Shoulder(new ShoulderIOSpark());
+                                endEffector = new EndEffector(new EndEffectorIOSpark());
                                 break;
 
                         case SIM:
@@ -110,6 +122,9 @@ public RobotContainer() {
                                                                 camera1Name, robotToCamera1,
                                                                 driveSimulation::getSimulatedDriveTrainPose));
                                 vision.setPoseSupplier(driveSimulation::getSimulatedDriveTrainPose);
+                                elevator = new Elevator(new ElevatorIOSpark());
+                                shoulder = new Shoulder(new ShoulderIOSpark());
+                                endEffector = new EndEffector(new EndEffectorIOSpark());
                                 break;
 
                         default:
@@ -128,12 +143,15 @@ public RobotContainer() {
                                 vision = new Vision(drive, new VisionIO() {
                                 }, new VisionIO() {
                                 });
+                                elevator = new Elevator(new ElevatorIOSpark());
+                                shoulder = new Shoulder(new ShoulderIOSpark());
+                                endEffector = new EndEffector(new EndEffectorIOSpark());
                                 break;
                 }
 
                 AllianceUtil.setRobot(drive::getPose);
 
-                stateMachine = new StateMachine(driverController, operatorController, drive);
+                stateMachine = new StateMachine(driverController, operatorController, drive, elevator, shoulder, endEffector);
 
                 // Set up auto routines
                 autoChooser = new LoggedDashboardChooser<>("Auto Choices");

diff --git a/src/main/java/frc/robot/statemachine/GoToStationIntkae.java b/src/main/java/frc/robot/statemachine/GoToStationIntkae.java
diff --git a/src/main/java/frc/robot/statemachine/StateMachine.java b/src/main/java/frc/robot/statemachine/StateMachine.java
@@ -24,18 +24,21 @@
 import frc.robot.statemachine.states.tele.ScoreAlgaeProcessor;
 import frc.robot.statemachine.states.tele.ScoreCoral;
 import frc.robot.statemachine.states.tele.ScoreGamePiece;
+import frc.robot.subsystems.EndEffector.EndEffector;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.shoulder.Shoulder;
 
 public class StateMachine extends StateMachineBase {
         public StateMachine(CommandXboxController driverController, CommandXboxController operatorController,
-                        Drive drive) {
+                        Drive drive, Elevator elevator, Shoulder shoulder, EndEffector endEffector) {
                 super();
                 State disabled = new DisabledState(this);
                 currentState = disabled;
 
                 State teleop = new TeleState(this);
                 State auto = new AutoState(this);
-                State test = new TestState(this, driverController);
+                State test = new TestState(this, driverController, elevator, shoulder, endEffector);
 
                 this.registerToRootState(test, auto, teleop, disabled);
 
@@ -65,25 +68,25 @@ public StateMachine(CommandXboxController driverController, CommandXboxControlle
                                 .withChild(goToIntake)
                                 .withChild(intakeGamePiece);
 
-                // Children inside children
-                goToScoringPosition.withChild(goToScoreCoral)
-                                .withChild(goToScoreAlgae);
 
-                scoreGamePiece.withChild(scoreCoral)
-                                .withChild(scoreAlgae);
+                goToScoringPosition.withChild(goToScoreCoral, () -> false, 0, "Has coral")
+                                .withChild(goToScoreAlgae, () -> false, 1, "Has algae");
 
-                goToIntake.withChild(goToReefIntake)
-                                .withChild(goToStationIntake);
+                scoreGamePiece.withChild(scoreCoral, () -> false, 0, "Has coral")
+                                .withChild(scoreAlgae, () -> false, 1, "Has algae");
 
-                intakeGamePiece.withChild(intakeCoral)
-                                .withChild(intakeAlgae);
+                goToIntake.withChild(goToReefIntake, () -> false, 0, "Reef selected")
+                                .withChild(goToStationIntake, () -> false, 1, "Station selected");
+
+                intakeGamePiece.withChild(intakeCoral, () -> false, 0, "Reef selected")
+                                .withChild(intakeAlgae, () -> false, 1, "Station selected");
 
                 // Specific Algae intake and score
-                goToScoreAlgae.withChild(goToScoreNet)
-                                .withChild(goToScoreProcessor);
+                goToScoreAlgae.withChild(goToScoreNet, () -> false, 0, "Net selected")
+                                .withChild(goToScoreProcessor, () -> false, 1, "Processor selected");
 
-                scoreAlgae.withChild(scoreAlgaeNet)
-                                .withChild(scoreAlgaeProcessor);
+                scoreAlgae.withChild(scoreAlgaeNet, () -> false, 0, "Net selected")
+                                .withChild(scoreAlgaeProcessor, () -> false, 1, "Processor selected");
 
                 // Example, will be button board later
                 manual.withTransition(goToScoringPosition, () -> false, "Driver presses score")

diff --git a/src/main/java/frc/robot/statemachine/reusable/State.java b/src/main/java/frc/robot/statemachine/reusable/State.java
@@ -147,16 +147,16 @@ public State withModeTransitions(State disabled, State teleop, State auto, State
      * 
      * @return This state
      */
-    public State withChild(State child, BooleanSupplier condition, int priority) {
-        addChild(child, condition, priority, false);
+    public State withChild(State child, BooleanSupplier condition, int priority, String entranceConditionName) {
+        addChild(child, condition, priority, false, entranceConditionName);
         return this;
     }
 
     /**
      * Add a child state to this state (will never be entered by default)
      */
     public State withChild(State child) {
-        addChild(child, () -> false, Integer.MAX_VALUE, false);
+        addChild(child, () -> false, Integer.MAX_VALUE, false, "impossible");
         return this;
     }
 
@@ -167,7 +167,7 @@ public State withChild(State child) {
      * @return This state
      */
     public State withDefaultChild(State child) {
-        addChild(child, () -> true, Integer.MAX_VALUE, true);
+        addChild(child, () -> true, Integer.MAX_VALUE, true, "default");
         return this;
     }
 
@@ -326,14 +326,14 @@ void setParentState(State parentState) {
         this.parentState = parentState;
     }
 
-    private void addChild(State child, BooleanSupplier condition, int priority, boolean isDefault) {
+    private void addChild(State child, BooleanSupplier condition, int priority, boolean isDefault, String entranceConditionName) {
         if (isDefault) {
             if (hasDefaultChild)
                 throw new RuntimeException("A state can only have one default child");
             hasDefaultChild = true;
         }
         child.setParentState(this);
         children.add(child);
-        entranceConditions.add(new TransitionInfo(child, condition, priority, null));
+        entranceConditions.add(new TransitionInfo(child, condition, priority, entranceConditionName));
     }
 }
diff --git a/src/main/java/frc/robot/statemachine/states/TestState.java b/src/main/java/frc/robot/statemachine/states/TestState.java
@@ -4,12 +4,20 @@
 import frc.robot.statemachine.reusable.SmartXboxController;
 import frc.robot.statemachine.reusable.State;
 import frc.robot.statemachine.reusable.StateMachineBase;
+import frc.robot.subsystems.EndEffector.EndEffector;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.shoulder.Shoulder;
 
 public class TestState extends State {
 
-        public TestState(StateMachineBase stateMachine, CommandXboxController controller) {
+        public TestState(StateMachineBase stateMachine, CommandXboxController controller, Elevator elevator, Shoulder shoulder, EndEffector endEffector) {
                 super(stateMachine);
                 @SuppressWarnings("unused")
                 SmartXboxController testController = new SmartXboxController(controller, loop);
+
+                startWhenActive(elevator.elevatorMove(controller::getRightY));
+                startWhenActive(shoulder.manualPivot(controller::getLeftY));
+                controller.a().onTrue(endEffector.runIntake(() -> 1.0));
+                controller.b().onTrue(endEffector.runIntake(() -> -1.0));
         }
 }
diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java
@@ -12,24 +12,6 @@ public class EndEffector extends SubsystemBase {
     private final EndEffectorIO io;
     private final EndEffectorIOInputsAutoLogged inputs = new EndEffectorIOInputsAutoLogged();
 
-    enum LevelAngle {
-        L1(0, 0, 0, 0),
-        L23(0, 0, 0, 0),
-        L4(0, 0, 0, 0);
-
-        private double closeDist;
-        private double closeAngle;
-        private double farDist;
-        private double farAngle;
-
-        private LevelAngle(double closeDist, double closeAngle, double farDist, double farAngle) {
-            this.closeAngle = closeAngle;
-            this.closeDist = closeDist;
-            this.farDist = farDist;
-            this.farAngle = farAngle;
-        }
-    };
-
     public EndEffector(EndEffectorIO io) {
         this.io = io;
     }
@@ -40,32 +22,11 @@ public void periodic() {
         Logger.processInputs("EndEffector", inputs);
     }
 
-    public Command manualpivot(double desiredspeed) {
-        return null;
-    }
-
-    public Command scoreL1(DoubleSupplier robotDistance) {
-        return Commands
-                .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L1)), this)
-                .withName("Set Shoulder to L1 Scoring position");
+    public Command runIntake(DoubleSupplier desiredSpeed) {
+        return Commands.run(() -> {io.setAlgaeSpeed(desiredSpeed.getAsDouble()); io.setCoralSpeed(desiredSpeed.getAsDouble());});
     }
 
-    public Command scoreL2AndL3(DoubleSupplier robotDistance) {
-        return Commands
-                .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L23)), this)
-                .withName("Set Shoulder to L1 Scoring position");
+    public Command runUntilSpike(DoubleSupplier desiredSpeed) {
+        return Commands.none();
     }
-
-    public Command scoreL4(DoubleSupplier robotDistance) {
-        return Commands
-                .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L4)), this)
-                .withName("Set Shoulder to L1 Scoring position");
-    }
-
-    private double calculateAngleForDist(double robotDist, LevelAngle desiredLevel) {
-        return ((desiredLevel.farAngle - desiredLevel.closeAngle)
-                / (desiredLevel.farDist - desiredLevel.closeDist)) * (robotDist - desiredLevel.closeDist)
-                + desiredLevel.closeAngle;
-    }
-
 }
diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorConstants.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorConstants.java
@@ -1,11 +1,10 @@
 package frc.robot.subsystems.EndEffector;
 
 public class EndEffectorConstants {
-    public static final int algeaMotorCanId = 0;
-    public static final int coralMotorCanId = 0;
-    public static final int shoulderCanId = 0;
+    public static final int coralCanId = 9;
+    public static final int algaeCanId = 10;
     public static final int proximitySensorChannel = 0;
-    public static final double intakeMotorReduction = 1.0;
-    public static final double wristMotorReduction = 1.0;
+    public static final double coralMotorReduction = 1.0;
+    public static final double algaeMotorReduction = 1.0;
     public static final int currentLimit = 40;
 }
diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIO.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIO.java
@@ -3,25 +3,18 @@
 import org.littletonrobotics.junction.AutoLog;
 
 public interface EndEffectorIO {
-
     @AutoLog
     public static class EndEffectorIOInputs {
-        public double wristAngleRad = 0.0;
-        public double wristSpeedRadsPerSec = 0.0;
-        public double wristAppliedVoltage = 0.0;
-        public double wristCurrantAmps = 0.0;
-        public double intakeWheelsSpeedRadPerSec = 0.0;
-        public double intakeWheelsAppliedVoltage = 0.0;
-        public double intakeWheelsCurrantAmps = 0.0;
-
+        public double coralAppliedVoltage = 0.0;
+        public double coralCurrentAmps = 0.0;
+        public double algaeAppliedVoltage = 0.0;
+        public double algaeCurrentAmps = 0.0;
         public double proximitySensorDistance = 0.0;
     }
 
     public void updateInputs(EndEffectorIOInputs inputs);
 
-    public void setShoulderAngle(double angle);
-
-    public void setShoulderSpeed(double speed);
+    public void setCoralSpeed(double speed);
 
-    public void setIntakeSpeed(double speed);
-}
+    public void setAlgaeSpeed(double speed);
+}