endeffector

README.md

@@ -1 +1 @@
-BitBuckets robot code for 2025
+BitBuckets robot code for 2025

src/main/java/frc/robot/OperatorInput.java

@@ -27,6 +27,9 @@ public class OperatorInput {
 
   final Trigger resetEncoder = operatorControl.leftBumper();
 
+  final Trigger openClaw = driver.leftStick();
+  final Trigger closeClaw = driver.rightStick();
+
   final Trigger IntakeOn = driver.a();
 
   final Trigger xNotDesired =

src/main/java/frc/robot/RobotContainer.java

@@ -28,20 +28,22 @@
 import frc.robot.commands.ElevatorCommands.ResetElevatorEncoderCommand;
 import frc.robot.constants.Constants;
 import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.Constants;
 import frc.robot.constants.DriveConstants;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.AlgaeManagementSubsystem.AlgaeManagementSubsystem;
 import frc.robot.subsystems.Auto.AutoSubsystem;
 import frc.robot.subsystems.ClawSubsystem.ClawSubsystem;
+import frc.robot.subsystems.ClawSubsystem.*;
 import frc.robot.subsystems.DriveSubsystem.*;
 import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
 import frc.robot.subsystems.DriveSubsystem.GyroIO;
 import frc.robot.subsystems.DriveSubsystem.GyroIOPigeon2;
 import frc.robot.subsystems.DriveSubsystem.ModuleIO;
 import frc.robot.subsystems.DriveSubsystem.ModuleIOSim;
 import frc.robot.subsystems.ElevatorSubsystem.*;
-import frc.robot.subsystems.GroundIntakeSubsystem.GroundIntakeSubsystem;
+import frc.robot.subsystems.AlgaeIntakeSubsystem.AlgaeIntakeSubsystem;
+import frc.robot.subsystems.AlgaeIntakeSubsystem.IntakeIOSparkMax;
+import frc.robot.subsystems.AlgaeIntakeSubsystem.IntakeIOSim;
 import frc.robot.subsystems.LEDSubsytem.LEDSubsystem;
 import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmSubsystem;
 import frc.robot.subsystems.VisionSubsystem.VisionIO;
@@ -50,10 +52,10 @@
 import frc.robot.subsystems.VisionSubsystem.VisionSubsystem;
 import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
+import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoral;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
-import frc.robot.commands.*;
 
 import java.util.function.DoubleSupplier;
 
@@ -65,7 +67,7 @@ public class RobotContainer {
   private final ElevatorSubsystem elevatorSubsystem;
   private final AlgaeManagementSubsystem algaeManagementSubsystem;
   private final ClawSubsystem clawSubsystem;
-  private final GroundIntakeSubsystem groundIntakeSubsystem;
+  private final AlgaeIntakeSubsystem groundIntakeSubsystem;
   private final LEDSubsystem ledSubsystem;
   private final SingleJointedArmSubsystem singleJointedArmSubsystem;
   private final VisionSubsystem visionSubsystem;
@@ -104,9 +106,9 @@ public RobotContainer() {
         algaeManagementSubsystem =
                 new AlgaeManagementSubsystem(); //TODO
         clawSubsystem =
-                new ClawSubsystem(); //TODO
+                new ClawSubsystem(new EndEffectorIOSparkMax(4, 5, new EndEffectorEncoderIOSim()));
         groundIntakeSubsystem =
-                new GroundIntakeSubsystem(); //TODO
+                new AlgaeIntakeSubsystem(new IntakeIOSparkMax(13, 12)); //TODO replace placeholder
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
@@ -138,9 +140,9 @@ public RobotContainer() {
         algaeManagementSubsystem =
                 new AlgaeManagementSubsystem(); //TODO
         clawSubsystem =
-                new ClawSubsystem(); //TODO
+                new ClawSubsystem(new EndEffectorIOSim());
         groundIntakeSubsystem =
-                new GroundIntakeSubsystem(); //TODO
+                new AlgaeIntakeSubsystem(new IntakeIOSim(driveSimulation)); //TODO
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
@@ -163,9 +165,9 @@ public RobotContainer() {
         algaeManagementSubsystem =
                 new AlgaeManagementSubsystem(); //TODO
         clawSubsystem =
-                new ClawSubsystem(); //TODO
+                new ClawSubsystem(new EndEffectorIOSparkMax(4, 5, new EndEffectorEncoderIOSim()));
         groundIntakeSubsystem =
-                new GroundIntakeSubsystem(); //TODO
+                new AlgaeIntakeSubsystem(new IntakeIOSparkMax(13, 12)); //TODO replace with real intake
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
@@ -220,6 +222,9 @@ public double getAsDouble() {
     }));
 
 
+    operatorInput.openClaw.onTrue(new OpenClawCommand(clawSubsystem));
+    operatorInput.closeClaw.onTrue(new CloseClawCommand(clawSubsystem));
+
     operatorInput.movementDesired.whileTrue(
             new BaseDriveCommand(
                 drive,
@@ -244,9 +249,20 @@ public void resetSimulationField() {
     SimulatedArena.getInstance().resetFieldForAuto();
   }
 
+  public void addCoral() {
+    SimulatedArena.getInstance().addGamePiece(new ReefscapeCoral(new Pose2d(2, 2, Rotation2d.fromDegrees(90))));
+    Logger.recordOutput("FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral"));
+  }
+
   public void displaySimFieldToAdvantageScope() {
     if (Constants.currentMode != Constants.Mode.SIM) return;
 
     Logger.recordOutput("FieldSimulation/RobotPosition", driveSimulation.getSimulatedDriveTrainPose());
   }
+
+  public void periodic() {
+    Pose3d[] coralPoses = SimulatedArena.getInstance()
+            .getGamePiecesArrayByType("Coral");
+    Logger.recordOutput("FieldSimulation/CoralPositions", coralPoses);
+  }
 }

src/main/java/frc/robot/commands/CloseClawCommand.java

@@ -0,0 +1,21 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ClawSubsystem.ClawSubsystem;
+
+public class CloseClawCommand extends Command {
+    private final ClawSubsystem clawSubsystem;
+
+    public CloseClawCommand(ClawSubsystem clawSubsystem) {
+        this.clawSubsystem = clawSubsystem;
+        addRequirements(clawSubsystem);
+    }
+
+    public void initialize() {
+        clawSubsystem.close();
+    }
+
+    public boolean isFinished() {
+        return true;
+    }
+}

src/main/java/frc/robot/commands/IntakePickupCommand.java

@@ -0,0 +1,21 @@
+package frc.robot.commands;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AlgaeIntakeSubsystem.AlgaeIntakeSubsystem;
+
+public class IntakePickupCommand extends Command {
+    private final AlgaeIntakeSubsystem intakeSubsystem;
+
+    public IntakePickupCommand(AlgaeIntakeSubsystem intakeSubsystem) {
+        this.intakeSubsystem = intakeSubsystem;
+        addRequirements(intakeSubsystem);
+    }
+
+    public void initialize() {
+        intakeSubsystem.pickup();
+    }
+
+    public boolean isFinished() {
+        return true;
+    }
+}
+

src/main/java/frc/robot/commands/OpenClawCommand.java

@@ -0,0 +1,22 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ClawSubsystem.ClawSubsystem;
+
+public class OpenClawCommand extends Command {
+    private final ClawSubsystem clawSubsystem;
+
+    public OpenClawCommand(ClawSubsystem clawSubsystem) {
+        this.clawSubsystem = clawSubsystem;
+        addRequirements(clawSubsystem);
+    }
+
+    public void initialize() {
+        clawSubsystem.open();
+    }
+
+    public boolean isFinished() {
+        return true;
+    }
+}
+

src/main/java/frc/robot/constants/ClawConstants.java

@@ -0,0 +1,13 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+
+public class ClawConstants {
+    public static DCMotor bigGearBox = new DCMotor(1, 0.5, 0.5, 0.5, 0.2, 1);
+    public static DCMotor smallGearBox = new DCMotor(1, 0.5, 0.5, 0.5, 0.2, 1);
+    public static double gearing = 0.1;
+    public static double kP = 0.0;
+    public static double kI = 0.0;
+    public static double kD = 0.0;
+    public static double mainSetpoint = 1.0;
+}

src/main/java/frc/robot/constants/DriveConstants.java

@@ -65,6 +65,14 @@ public class DriveConstants {
     public static final int frontRightTurnCanId = 6;
     public static final int backRightTurnCanId = 8;
 
+    // Encoder Ports
+
+    public static final int frontLeftEncoderPort = 3;
+    public static final int frontRightEncoderPort = 2;
+    public static final int backLeftEncoderPort = 0;
+    public static final int backRightEncoderPort = 1;
+
+
     // Drive motor configuration
     public static final int driveMotorCurrentLimit = 50;
     public static final double wheelRadiusMeters = TunerConstants.FrontLeft.WheelRadius;

src/main/java/frc/robot/constants/IntakeConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+
+public class IntakeConstants {
+    public static DCMotor pivotGearBox = new DCMotor(1, 0.5, 0.5, 0.5, 0.2, 1);
+    public static DCMotor rollersGearBox = new DCMotor(1, 0.5, 0.5, 0.5, 0.2, 1);
+    public static double gearing = 0.1;
+    public static double pivotRotation = 0.3;
+    public static double kP = 0.0;
+    public static double kI = 0.0;
+    public static double kD = 0.0;
+    public static double rollerVoltsTarget = 1.0;
+    public static double pivotVoltsTarget = 1.0;
+}

src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/AlgaeIntakeSubsystem.java

@@ -0,0 +1,18 @@
+package frc.robot.subsystems.AlgaeIntakeSubsystem;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class AlgaeIntakeSubsystem extends SubsystemBase {
+    private final IntakeIO intake;
+
+    public AlgaeIntakeSubsystem(IntakeIO intake){
+        this.intake = intake;
+    }
+
+    public void pickup() {
+        intake.pivotDown();
+        intake.pivotUp();
+    }
+
+
+}

src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeEncoderIO.java

@@ -0,0 +1,12 @@
+package frc.robot.subsystems.AlgaeIntakeSubsystem;
+
+public interface IntakeEncoderIO {
+    public class IntakeEncoderInputs {
+        public static double velocityUnitsPerSec = 0.0;
+        public static double distance = 0.0;
+    }
+
+    public default double getPivotDistance() { return 0.0; }
+
+
+}

src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeEncoderIOSim.java

@@ -0,0 +1,14 @@
+package frc.robot.subsystems.AlgaeIntakeSubsystem;
+
+public class IntakeEncoderIOSim implements IntakeEncoderIO {
+    public IntakeEncoderIOSim() {
+
+    }
+
+    @Override
+    public double getPivotDistance() {
+        return 0.0;
+    }
+
+}
+

src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIO.java

@@ -0,0 +1,23 @@
+package frc.robot.subsystems.AlgaeIntakeSubsystem;
+
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.subsystems.ClawSubsystem.EndEffectorEncoderIO;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+    @AutoLog
+    public class IntakeInputsAutoLogged {
+        public boolean hasCoral = false;
+        public boolean isRunning = false;
+        public double rollersVoltage = 0.0;
+        public double pivotVoltage = 0.0;
+    }
+
+    public default void setRunning(boolean state) {}
+    public default void pivotDown() {}
+    public default void pivotUp() {}
+    public default boolean getIsRunning() { return false; }
+    public default boolean coralInside() { return false; }
+    public default void updateInputs(IntakeInputsAutoLogged inputs) {}
+
+}

src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSim.java

@@ -0,0 +1,83 @@
+package frc.robot.subsystems.AlgaeIntakeSubsystem;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import frc.robot.constants.IntakeConstants;
+import org.ironmaple.simulation.IntakeSimulation;
+import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
+
+import static edu.wpi.first.units.Units.Meters;
+
+public class IntakeIOSim implements IntakeIO{
+    private final SwerveDriveSimulation driveSim;
+    private final IntakeSimulation intakeSim;
+    private boolean isRunning = false;
+    private final IntakeEncoderIOSim encoder = new IntakeEncoderIOSim();
+
+    private final DCMotorSim pivotWheel = new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(IntakeConstants.pivotGearBox, 0.1, IntakeConstants.gearing),
+            IntakeConstants.pivotGearBox
+    );
+
+    private final DCMotorSim rollers = new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(IntakeConstants.rollersGearBox, 0.1, IntakeConstants.gearing),
+            IntakeConstants.rollersGearBox
+    );
+
+    public IntakeIOSim(SwerveDriveSimulation driveSim) {
+        this.driveSim = driveSim;
+        this.intakeSim = IntakeSimulation.OverTheBumperIntake(
+                "Coral",
+                driveSim,
+                Meters.of(0.4),
+                Meters.of(0.2),
+                IntakeSimulation.IntakeSide.BACK, //where intake is attatched
+                1);
+    }
+
+    @Override
+    public void setRunning(boolean state) {
+        if (state) {
+            intakeSim.startIntake();
+            rollers.setInputVoltage(IntakeConstants.rollerVoltsTarget);
+            isRunning = true;
+        } else {
+            intakeSim.stopIntake();
+            rollers.setInputVoltage(0.0);
+            isRunning = false;
+        }
+    }
+
+    @Override
+    public void pivotDown() {
+        pivotWheel.setInputVoltage(IntakeConstants.pivotVoltsTarget);
+        setRunning(true);
+    }
+
+    @Override
+    public void pivotUp() {
+        setRunning(false);
+        pivotWheel.setInputVoltage(IntakeConstants.pivotVoltsTarget);
+    }
+
+    @Override
+    public boolean coralInside() {
+        return intakeSim.getGamePiecesAmount() != 0;
+    }
+
+    @Override
+    public boolean getIsRunning() {
+        return isRunning;
+    }
+
+    @Override
+    public void updateInputs(IntakeInputsAutoLogged inputs) {
+        inputs.hasCoral = coralInside();
+        inputs.isRunning = getIsRunning();
+        inputs.rollersVoltage = rollers.getInputVoltage();
+        inputs.pivotVoltage = pivotWheel.getInputVoltage();
+    }
+
+
+}