Pivot

.vscode/settings.json

@@ -58,5 +58,5 @@
     "edu.wpi.first.math.**.struct.*",
   ],
   "wpilib.autoStartRioLog": true,
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable"
 }

src/main/java/frc/robot/Commands/AutoAlignCommand.java

@@ -1,4 +1,4 @@
-package frc.robot.Commands;
+package frc.robot.commands;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 
 

src/main/java/frc/robot/Constants.java

@@ -4,15 +4,32 @@
 
 package frc.robot;
 
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import java.util.OptionalInt;
+import java.util.Optional;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.config.ClosedLoopConfig;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.MAXMotionConfig;
+import com.revrobotics.spark.config.SoftLimitConfig;
+
+import java.util.OptionalInt;
+import java.util.Optional;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.config.ClosedLoopConfig;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.MAXMotionConfig;
+import com.revrobotics.spark.config.SoftLimitConfig;
 
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import frc.robot.subsystems.Vision.CameraConfig;
 import frc.robot.util.PolynomialRegression;
+import frc.robot.util.Motors.LoggedSparkMaxConfig;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -78,8 +95,15 @@ public static class SwerveConstants {
     public static final boolean STATE_DEBUG = false;
   }
 
+  public static class DebugConstants{
+    public static final Boolean REV_DEBUG = false;
+    public static final Boolean CTRE_DEBUG = false;
+  }
+
   public static class LoggingConstants{
     public static final String SWERVE_TABLE = "SwerveStats";
+    public static final String REV_TABLE = "MotorStats";
+    public static final String CTRE_TABLE = "MotorStats";
   }
 
   public static class VisionConstants{
@@ -150,4 +174,42 @@ public static class VisionConstants{
     public static final PolynomialRegression oStdDevModel = new PolynomialRegression(
       VisionConstants.STD_DEV_DIST,VisionConstants.O_STD_DEV,1);
   }
+
+  public static class IntakeConstants {
+    public static final int PIVOT_ID = 14;
+    public static final int ROLLER_ID = 16;
+    public static final int INTAKE_SENSOR_ID = 1;
+
+    public static final double PIVOT_CONVERSION_FACTOR = (2 * Math.PI) / 30.;
+
+    public static final double ZERO_POSITION = 0;
+    public static final double SOURCE_POSITION = Units.degreesToRadians(45); //TODO: change
+    public static final double OUTTAKE_POSITION = Units.degreesToRadians(-45); //TODO: change
+    public static final double VERTICAL_POSITION = Units.degreesToRadians(82); //TODO: change
+
+    public static final double PIVOT_TOLERANCE = .08; //TODO: change
+
+    public static final double PIVOT_KG = 3;
+    public static final double PIVOT_KS = 0;
+    public static final double PIVOT_KV = 0;
+
+    public static final double PIVOT_P = .3;
+    public static final double PIVOT_I = 0;
+    public static final double PIVOT_D = 0.3;
+
+	public static final LoggedSparkMaxConfig PivotMotorLoggedSparkMaxConfig = new LoggedSparkMaxConfig(
+		PIVOT_ID,
+		new ClosedLoopConfig().pid(PIVOT_P, PIVOT_I, PIVOT_D, ClosedLoopSlot.kSlot0),
+		new EncoderConfig().positionConversionFactor(PIVOT_CONVERSION_FACTOR),
+		OptionalInt.empty(),
+		Optional.of(
+      new SoftLimitConfig()
+			.forwardSoftLimitEnabled(true)
+			.forwardSoftLimit(Units.degreesToRadians(85))
+			.reverseSoftLimitEnabled(true)
+			.reverseSoftLimit(Units.degreesToRadians(-50))
+    )
+	);
+
+  }
 }

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

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+// import frc.robot.commands.pivot.SetPivotVerticalCommand;
+// import frc.robot.commands.pivot.SetPivotZeroCommand;
 import frc.robot.controllers.BaseDriveController;
 import frc.robot.controllers.DualJoystickDriveController;
 import frc.robot.controllers.PS5DriveController;
@@ -13,13 +15,25 @@
 
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.PS5Controller;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.FieldManagementSubsystem.FieldManagementSubsystem;
 import frc.robot.subsystems.Vision.VisionSubsystem;
+import frc.robot.subsystems.intake.pivot.PivotState;
+import frc.robot.subsystems.intake.pivot.PivotSubsystem;
+import frc.robot.subsystems.intake.rollers.RollerSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.Constants.VisionConstants;
+import frc.robot.commands.pivot.SetPivotOuttakeCommand;
+import frc.robot.commands.pivot.SetPivotSourceCommand;
+import frc.robot.commands.pivot.SetPivotVerticalCommand;
+import frc.robot.commands.pivot.SetPivotZeroCommand;
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
@@ -31,6 +45,8 @@ public class RobotContainer {
   private BaseDriveController driveController;
 
   private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
+  private final PivotSubsystem pivotSubsystem = new PivotSubsystem();
+  private final RollerSubsystem rollerSubsystem = new RollerSubsystem();
   private final VisionSubsystem visionSubsystem2 = new VisionSubsystem(
     VisionConstants.cameraConfigs[1]
   );
@@ -41,8 +57,19 @@ public class RobotContainer {
     VisionConstants.cameraConfigs[3]
   );
 
+  private CommandPS5Controller mechController;
+  private Trigger aButton, lTrigger, rTrigger, lBumper, rBumper;
+
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    mechController = new CommandPS5Controller(1);
+    aButton = new Trigger(mechController.cross());
+    lTrigger = new Trigger(mechController.L2());
+    rTrigger = new Trigger(mechController.R2());
+
+    lBumper = new Trigger(mechController.L1());
+    rBumper = new Trigger(mechController.R1());
+
     constructDriveController(); 
     // startLog();
     setVisionDataInterface();
@@ -63,25 +90,58 @@ private void configureBindings() {
       * the robot is controlled along its own axes, otherwise controls apply to the field axes by default. If the
       * swerve aim button is held down, the robot will rotate automatically to always face a target, and only
       * translation will be manually controllable. */
-    swerveSubsystem.setDefaultCommand(
-      new RunCommand(() -> {
-        swerveSubsystem.setDrivePowers(
-          driveController.getForwardPower(),
-          driveController.getLeftPower(),
-          driveController.getRotatePower()
-        );
-        }, 
-        swerveSubsystem
-      )
+    // swerveSubsystem.setDefaultCommand(
+    //   new RunCommand(() -> {
+    //     swerveSubsystem.setDrivePowers(
+    //       driveController.getForwardPower(),
+    //       driveController.getLeftPower(),
+    //       driveController.getRotatePower()
+    //     );
+    //     }, 
+    //     swerveSubsystem
+    //   )
+    // );
+
+    rBumper.onTrue(
+      new ConditionalCommand(
+        new SetPivotOuttakeCommand(pivotSubsystem),
+        new ConditionalCommand(
+          new SetPivotZeroCommand(pivotSubsystem).andThen(new SetPivotSourceCommand(pivotSubsystem)), 
+          new SetPivotSourceCommand(pivotSubsystem), 
+          () -> (pivotSubsystem.getCurrentAngle() < PivotState.ZERO.getTargetAngle())),
+        () -> (pivotSubsystem.getTargetState() == PivotState.SOURCE)
+        )
     );
 
-    /* Pressing the button resets the field axes to the current robot axes. */
-    driveController.bindDriverHeadingReset(
-      () ->{
-        swerveSubsystem.resetDriverHeading();
-      },
-      swerveSubsystem
+    lBumper.onTrue(
+      new SetPivotVerticalCommand(pivotSubsystem).withTimeout(2.5)
     );
+
+    rollerSubsystem.setDefaultCommand(new ConditionalCommand(
+      new InstantCommand( () -> {
+        //ps5 trigger's range is -1 to 1, with non-input position being -1. This maps the range -1 to 1 to 0 to 1.
+        rollerSubsystem.setRollerPower(.25 * (mechController.getR2Axis() + 1.) / 2.); 
+      }, rollerSubsystem), 
+      new InstantCommand( () -> {
+        rollerSubsystem.setRollerPower(.25 * (mechController.getR2Axis() - mechController.getL2Axis()));
+      }, rollerSubsystem), 
+      () -> rollerSubsystem.getIntakeSensor()));
+
+    // aButton.onTrue(
+    //     new SetPivotVerticalCommand(pivotSubsystem)
+    // );
+
+    // rollerSubsystem.setDefaultCommand(new InstantCommand( () -> {
+    //   rollerSubsystem.setRollerPower(mechController.getL2Axis() - mechController.getR2Axis());
+    // }, rollerSubsystem));
+
+    // /* Pressing the button resets the field axes to the current robot axes. */
+    // driveController.bindDriverHeadingReset(
+    //   () ->{
+    //     swerveSubsystem.resetDriverHeading();
+    //   },
+    //   swerveSubsystem
+    // );
   }
 
   /**

src/main/java/frc/robot/commands/pivot/SetPivotOuttakeCommand.java

@@ -0,0 +1,24 @@
+package frc.robot.commands.pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.pivot.PivotState;
+import frc.robot.subsystems.intake.pivot.PivotSubsystem;
+
+public class SetPivotOuttakeCommand extends Command{
+    private PivotSubsystem pivotSubsystem;
+
+    public SetPivotOuttakeCommand(PivotSubsystem pivotSubsystem) {
+        this.addRequirements(pivotSubsystem);
+        this.pivotSubsystem = pivotSubsystem;
+    }
+
+    @Override
+    public void execute() {
+        this.pivotSubsystem.setState(PivotState.OUTTAKE);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return pivotSubsystem.atState(PivotState.OUTTAKE);
+    }
+}

src/main/java/frc/robot/commands/pivot/SetPivotSourceCommand.java

@@ -0,0 +1,24 @@
+package frc.robot.commands.pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.pivot.PivotState;
+import frc.robot.subsystems.intake.pivot.PivotSubsystem;
+
+public class SetPivotSourceCommand extends Command{
+    private PivotSubsystem pivotSubsystem;
+
+    public SetPivotSourceCommand(PivotSubsystem pivotSubsystem) {
+        this.addRequirements(pivotSubsystem);
+        this.pivotSubsystem = pivotSubsystem;
+    }
+
+    @Override
+    public void execute() {
+        this.pivotSubsystem.setState(PivotState.SOURCE);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return pivotSubsystem.atState(PivotState.SOURCE);
+    }
+}

src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java

@@ -0,0 +1,30 @@
+package frc.robot.commands.pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.pivot.PivotState;
+import frc.robot.subsystems.intake.pivot.PivotSubsystem;
+
+public class SetPivotVerticalCommand extends Command{
+        private PivotSubsystem pivotSubsystem;
+
+    public SetPivotVerticalCommand(PivotSubsystem pivotSubsystem) {
+        this.addRequirements(pivotSubsystem);
+        this.pivotSubsystem = pivotSubsystem;
+    }
+
+    @Override
+    public void execute() {
+        this.pivotSubsystem.setState(PivotState.VERTICAL);
+        // System.out.println("vertical" + pivotSubsystem.getPositionError());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return pivotSubsystem.atState(PivotState.VERTICAL);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        pivotSubsystem.setSpeed(0);
+    }
+}

src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java

@@ -0,0 +1,30 @@
+package frc.robot.commands.pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.pivot.PivotState;
+import frc.robot.subsystems.intake.pivot.PivotSubsystem;
+
+public class SetPivotZeroCommand extends Command{
+    private PivotSubsystem pivotSubsystem;
+
+    public SetPivotZeroCommand(PivotSubsystem pivotSubsystem) {
+        this.addRequirements(pivotSubsystem);
+        this.pivotSubsystem = pivotSubsystem;
+    }
+
+    @Override
+    public void execute() {
+        this.pivotSubsystem.setState(PivotState.ZERO);
+        // System.out.println("horizontal" + pivotSubsystem.getPositionError());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return pivotSubsystem.atState(PivotState.ZERO);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        pivotSubsystem.setSpeed(0);
+    }
+}

src/main/java/frc/robot/subsystems/intake/pivot/PivotState.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.intake.pivot;
+
+import frc.robot.Constants;
+
+public enum PivotState {
+    ZERO(Constants.IntakeConstants.ZERO_POSITION),
+    OUTTAKE(Constants.IntakeConstants.OUTTAKE_POSITION),
+    SOURCE(Constants.IntakeConstants.SOURCE_POSITION),
+    VERTICAL(Constants.IntakeConstants.VERTICAL_POSITION);
+
+    private final double targetAngle;
+
+    private PivotState(double targetAngle) {
+        this.targetAngle = targetAngle;
+    }
+
+    public double getTargetAngle() {
+        return targetAngle;
+    }
+
+}