cam constants added

.OutlineViewer/outlineviewer.json

@@ -4,8 +4,37 @@
     "serverTeam": "192"
   },
   "transitory": {
+    "MotorStats": {
+      "open": true
+    },
+    "PathPlanner": {
+      "open": true
+    },
+    "Shuffleboard": {
+      "open": true
+    },
+    "SmartDashboard": {
+      "open": true
+    },
     "Swerve": {
       "open": true
+    },
+    "SwerveStats": {
+      "Pose2d##v_/SwerveStats/estimatedPose": {
+        "Rotation2d##v_rotation": {
+          "open": true
+        },
+        "Translation2d##v_translation": {
+          "open": true
+        },
+        "open": true
+      },
+      "open": true
+    },
+    "photonvision": {
+      "Arducam_OV9281_USB_Camera": {
+        "open": true
+      }
     }
   }
 }

src/main/java/frc/robot/Commands/Elevator/ElevatorToGroundCommand.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ElevatorConstants;
+import frc.robot.subsystems.Elevator.ElevatorSubsystem;
+
+public class ElevatorToGroundCommand extends Command {
+    private ElevatorSubsystem elevatorSubsystem;
+
+    public ElevatorToGroundCommand(ElevatorSubsystem elevatorSubsystem) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        addRequirements(elevatorSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        elevatorSubsystem.setPositionReference(ElevatorConstants.GROUND_POS);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return elevatorSubsystem.getClosedLoopError()
+            < ElevatorConstants.ELEVATOR_TOLERANCE;
+    }
+}

src/main/java/frc/robot/Commands/Elevator/ElevatorToL1Command.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ElevatorConstants;
+import frc.robot.subsystems.Elevator.ElevatorSubsystem;
+
+public class ElevatorToL1Command extends Command {
+    private ElevatorSubsystem elevatorSubsystem;
+
+    public ElevatorToL1Command(ElevatorSubsystem elevatorSubsystem) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        addRequirements(elevatorSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        elevatorSubsystem.setPositionReference(ElevatorConstants.L1_POS);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return elevatorSubsystem.getClosedLoopError()
+            < ElevatorConstants.ELEVATOR_TOLERANCE;
+    }
+}

src/main/java/frc/robot/Commands/Elevator/ElevatorToL2Command.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ElevatorConstants;
+import frc.robot.subsystems.Elevator.ElevatorSubsystem;
+
+public class ElevatorToL2Command extends Command {
+    private ElevatorSubsystem elevatorSubsystem;
+
+    public ElevatorToL2Command(ElevatorSubsystem elevatorSubsystem) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        addRequirements(elevatorSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        elevatorSubsystem.setPositionReference(ElevatorConstants.L2_POS);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return elevatorSubsystem.getClosedLoopError()
+            < ElevatorConstants.ELEVATOR_TOLERANCE;
+    }
+}

src/main/java/frc/robot/Commands/Elevator/ElevatorToL3Command.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ElevatorConstants;
+import frc.robot.subsystems.Elevator.ElevatorSubsystem;
+
+public class ElevatorToL3Command extends Command {
+    private ElevatorSubsystem elevatorSubsystem;
+
+    public ElevatorToL3Command(ElevatorSubsystem elevatorSubsystem) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        addRequirements(elevatorSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        elevatorSubsystem.setPositionReference(ElevatorConstants.L3_POS);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return elevatorSubsystem.getClosedLoopError()
+            < ElevatorConstants.ELEVATOR_TOLERANCE;
+    }
+}

src/main/java/frc/robot/Commands/Elevator/ElevatorToL4Command.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ElevatorConstants;
+import frc.robot.subsystems.Elevator.ElevatorSubsystem;
+
+public class ElevatorToL4Command extends Command {
+    private ElevatorSubsystem elevatorSubsystem;
+
+    public ElevatorToL4Command(ElevatorSubsystem elevatorSubsystem) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        addRequirements(elevatorSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        elevatorSubsystem.setPositionReference(ElevatorConstants.L4_POS);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return elevatorSubsystem.getClosedLoopError()
+            < ElevatorConstants.ELEVATOR_TOLERANCE;
+    }
+}

src/main/java/frc/robot/Commands/Elevator/ElevatorToLimitSwitchCommand.java

@@ -0,0 +1,31 @@
+package frc.robot.Commands.Elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ElevatorConstants;
+import frc.robot.subsystems.Elevator.ElevatorSubsystem;
+
+public class ElevatorToLimitSwitchCommand extends Command{
+    private ElevatorSubsystem elevatorSubsystem;
+
+    public ElevatorToLimitSwitchCommand(ElevatorSubsystem elevatorSubsystem) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        addRequirements(elevatorSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        elevatorSubsystem.setDutyCycle(
+            ElevatorConstants.DUTY_CYCLE_TO_GROUND_SPEED
+        );
+    }
+
+    @Override
+    public boolean isFinished(){
+        return elevatorSubsystem.getLimitSwitch();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        elevatorSubsystem.setDutyCycle(0);
+    }
+}

src/main/java/frc/robot/Commands/Elevator/ElevatorToSourceCommand.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ElevatorConstants;
+import frc.robot.subsystems.Elevator.ElevatorSubsystem;
+
+public class ElevatorToSourceCommand extends Command {
+    private ElevatorSubsystem elevatorSubsystem;
+
+    public ElevatorToSourceCommand(ElevatorSubsystem elevatorSubsystem) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        addRequirements(elevatorSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        elevatorSubsystem.setPositionReference(ElevatorConstants.SOURCE_POS);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return elevatorSubsystem.getClosedLoopError()
+            < ElevatorConstants.ELEVATOR_TOLERANCE;
+    }
+}

src/main/java/frc/robot/Commands/Intake/Pivot/PivotToHorizontalCommand.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Intake.Pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.IntakeConstans.PivotConstants;
+import frc.robot.subsystems.Intake.Pivot.PivotSubsystem;
+
+public class PivotToHorizontalCommand extends Command{
+   private final PivotSubsystem pivotSubsystem; 
+
+   public PivotToHorizontalCommand(PivotSubsystem pivotSubsystem){
+       this.pivotSubsystem = pivotSubsystem;
+       this.addRequirements(pivotSubsystem);
+   }
+
+   @Override
+   public void initialize(){
+       pivotSubsystem.setPositionReferenceWithVoltage(PivotConstants.PIVOT_HORIZONTAL);
+   }
+
+   @Override
+   public boolean isFinished(){
+        return Math.abs(pivotSubsystem.getClosedLoopError())
+            < PivotConstants.PIVOT_TOLERANCE;
+   }
+}

src/main/java/frc/robot/Commands/Intake/Pivot/PivotToOuttakeCommand.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Intake.Pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.IntakeConstans.PivotConstants;
+import frc.robot.subsystems.Intake.Pivot.PivotSubsystem;
+
+public class PivotToOuttakeCommand extends Command{
+   private final PivotSubsystem pivotSubsystem; 
+
+   public PivotToOuttakeCommand(PivotSubsystem pivotSubsystem){
+       this.pivotSubsystem = pivotSubsystem;
+       this.addRequirements(pivotSubsystem);
+   }
+
+   @Override
+   public void initialize(){
+       pivotSubsystem.setPositionReferenceWithVoltage(PivotConstants.OUTTAKE_POS);
+   }
+
+   @Override
+   public boolean isFinished(){
+        return Math.abs(pivotSubsystem.getClosedLoopError())
+            < PivotConstants.PIVOT_TOLERANCE;
+   }
+}

src/main/java/frc/robot/Commands/Intake/Pivot/PivotToSourceCommand.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Intake.Pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.IntakeConstans.PivotConstants;
+import frc.robot.subsystems.Intake.Pivot.PivotSubsystem;
+
+public class PivotToSourceCommand extends Command{
+   private final PivotSubsystem pivotSubsystem; 
+
+   public PivotToSourceCommand(PivotSubsystem pivotSubsystem){
+       this.pivotSubsystem = pivotSubsystem;
+       this.addRequirements(pivotSubsystem);
+   }
+
+   @Override
+   public void initialize(){
+       pivotSubsystem.setPositionReferenceWithVoltage(PivotConstants.SOURCE_POS);
+   }
+
+   @Override
+   public boolean isFinished(){
+        return Math.abs(pivotSubsystem.getClosedLoopError())
+            < PivotConstants.PIVOT_TOLERANCE;
+   }
+}

src/main/java/frc/robot/Commands/Intake/Pivot/PivotUp90Command.java

@@ -0,0 +1,25 @@
+package frc.robot.Commands.Intake.Pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.IntakeConstans.PivotConstants;
+import frc.robot.subsystems.Intake.Pivot.PivotSubsystem;
+
+public class PivotUp90Command extends Command{
+   private final PivotSubsystem pivotSubsystem; 
+
+   public PivotUp90Command(PivotSubsystem pivotSubsystem){
+       this.pivotSubsystem = pivotSubsystem;
+       this.addRequirements(pivotSubsystem);
+   }
+
+   @Override
+   public void initialize(){
+       pivotSubsystem.setPositionReferenceWithVoltage(PivotConstants.PIVOT_MAX_POS);
+   }
+
+   @Override
+   public boolean isFinished(){
+        return Math.abs(pivotSubsystem.getClosedLoopError())
+            < PivotConstants.PIVOT_TOLERANCE;
+   }
+}

src/main/java/frc/robot/Commands/Intake/Pivot/PivotZeroTo90Command.java

@@ -0,0 +1,24 @@
+package frc.robot.Commands.Intake.Pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.IntakeConstans.PivotConstants;
+import frc.robot.subsystems.Intake.Pivot.PivotSubsystem;
+
+public class PivotZeroTo90Command extends Command{
+   private final PivotSubsystem pivotSubsystem; 
+
+   public PivotZeroTo90Command(PivotSubsystem pivotSubsystem){
+       this.pivotSubsystem = pivotSubsystem;
+       this.addRequirements(pivotSubsystem);
+   }
+
+   @Override
+   public void initialize(){
+       pivotSubsystem.setPosition(Math.PI / 2.);
+   }
+
+   @Override
+   public boolean isFinished(){
+    return true;
+   }
+}

src/main/java/frc/robot/Commands/Intake/Roller/RollerInCommand.java

@@ -0,0 +1,27 @@
+package frc.robot.Commands.Intake.Roller;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.IntakeConstans.RollerConstants;
+import frc.robot.subsystems.Intake.Roller.RollerSubsystem;
+
+public class RollerInCommand extends Command{
+
+    private final RollerSubsystem rollerSubsystem;
+
+    public RollerInCommand(RollerSubsystem rollerSubsystem){
+        this.rollerSubsystem = rollerSubsystem;
+        this.addRequirements(rollerSubsystem);
+    }
+
+    @Override
+    public void initialize(){
+        rollerSubsystem.setRollerSpeed(RollerConstants.ROLLER_IN_SPEED);
+    }
+
+    @Override
+    public boolean isFinished(){
+        return Math.abs(rollerSubsystem.getClosedLoopError())
+            < RollerConstants.ROLLER_TOLERANCE
+            || rollerSubsystem.getIntakeSensor();
+    }
+}