Merge branch 'dev' into gil-path-planner-swerve

src/main/java/frc/robot/RobotMap.java

@@ -3,7 +3,7 @@
 public class RobotMap {
 
     public interface CAN {
-
+      
         int FRONT_LEFT_DRIVE_TALON_FX = 1;
         int FRONT_RIGHT_DRIVE_TALON_FX = 4;
         int BACK_LEFT_DRIVE_TALON_FX = 7;
@@ -18,18 +18,33 @@ public interface CAN {
         int FRONT_RIGHT_ABSOLUTE_ENCODER = 6;
         int BACK_LEFT_ABSOLUTE_ENCODER = 9;
         int BACK_RIGHT_ABSOLUTE_ENCODER = 12;
+
+      int ELEVATOR_MASTER_SPARK = -1;
+      int ELEVATOR_SLAVE_SPARK = -1;
+      int ALGAE_JOINT_SPARK = -1;
+      int CORAL_JOINT_SPARK = -1;
+      int GRIPPER_TALON = -1;
+      int STORAGE_SPARK = -1;
     }
-    
+
     public interface DIO {
 
+        int ELEVATOR_TOP_LIMIT = -1;
+        int ELEVATOR_BOTTOM_LIMIT = -1;
+        int ALGAE_TOP_LIMIT = -1;
+        int ALGAE_BOTTOM_LIMIT = -1;
+        int CORAL_JOINT_TOP_LIMIT = -1;
+        int CORAL_JOINT_BOTTOM_LIMIT = -1;
+        int GRIPPER_LIMIT = -1;
+        int STORAGE_INFRARED = -1;
     }
-    
+
     public interface PWM {
 
     }
-    
+
     public interface AIN {
-    
+
     }
 
     public interface PCM {

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

@@ -0,0 +1,30 @@
+package frc.robot.commands;
+
+import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
+import com.spikes2212.dashboard.RootNamespace;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.Gripper;
+import java.util.function.Supplier;
+
+public class IntakeAlgae extends MoveGenericSubsystem {
+
+    private static final RootNamespace NAMESPACE = new RootNamespace("intake algae");
+    private static final Supplier<Double> INTAKE_SPEED = NAMESPACE.addConstantDouble("intake speed", 0.9);
+    private static final Supplier<Double> TIME_TO_INTAKE = NAMESPACE.addConstantDouble("time to intake", 0.2);
+
+    private double startTime;
+
+    public IntakeAlgae(Gripper gripper) {
+        super(gripper, INTAKE_SPEED);
+    }
+
+    @Override
+    public void initialize() {
+        startTime = Timer.getFPGATimestamp();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return super.isFinished() && Timer.getFPGATimestamp() - startTime >= TIME_TO_INTAKE.get();
+    }
+}

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

@@ -0,0 +1,17 @@
+package frc.robot.commands;
+
+import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
+import com.spikes2212.dashboard.RootNamespace;
+import frc.robot.subsystems.Storage;
+
+import java.util.function.Supplier;
+
+public class IntakeCoral extends MoveGenericSubsystem {
+
+    private static final RootNamespace NAMESPACE = new RootNamespace("intake coral");
+    private static final Supplier<Double> INTAKE_SPEED = NAMESPACE.addConstantDouble("intake speed", 0.5);
+
+    public IntakeCoral(Storage storage) {
+        super(storage, INTAKE_SPEED);
+    }
+}

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

@@ -0,0 +1,44 @@
+package frc.robot.commands;
+
+import com.spikes2212.command.genericsubsystem.commands.smartmotorcontrollergenericsubsystem.
+        MoveSmartMotorControllerGenericSubsystem;
+import com.spikes2212.control.FeedForwardController;
+import com.spikes2212.control.FeedForwardSettings;
+import com.spikes2212.control.PIDSettings;
+import com.spikes2212.dashboard.RootNamespace;
+import com.spikes2212.util.UnifiedControlMode;
+import frc.robot.subsystems.Elevator;
+
+import java.util.function.Supplier;
+
+public class MoveToHeight extends MoveSmartMotorControllerGenericSubsystem {
+
+    private static final RootNamespace namespace = new RootNamespace("move to height");
+    private static final PIDSettings pidSettings = namespace.addPIDNamespace("move to height");
+    private static final FeedForwardSettings feedForwardSettings = namespace.addFeedForwardNamespace("move to height",
+            FeedForwardController.ControlMode.LINEAR_POSITION);
+
+    private final Elevator elevator;
+
+    public MoveToHeight(Elevator elevator, Supplier<Double> setpoint) {
+        super(elevator, pidSettings, feedForwardSettings, UnifiedControlMode.POSITION, setpoint, true);
+        this.elevator = elevator;
+    }
+
+    public MoveToHeight(Elevator elevator, Elevator.ElevatorLevel elevatorLevel) {
+        this(elevator, () -> elevatorLevel.height);
+    }
+
+    @Override
+    public void execute() {
+        if (!isFinished()) {
+            super.execute();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        double error = setpoint.get() - elevator.getPosition();
+        return super.isFinished() || !elevator.canMove(error);
+    }
+}

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

@@ -0,0 +1,15 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.AlgaeJoint;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Gripper;
+
+public class PickAlgaeFloar extends SequentialCommandGroup {
+
+    public PickAlgaeFloar(Elevator elevator, AlgaeJoint algaeJoint, Gripper gripper) {
+        addCommands(new ParallelCommandGroup(new MoveToHeight(elevator, Elevator.ElevatorLevel.BOTTOM),
+                new RotateAlgaeJointToBottom(algaeJoint)), new IntakeAlgae(gripper));
+    }
+}

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

@@ -0,0 +1,19 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.CoralJoint;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Storage;
+
+public class PlaceOnReef extends SequentialCommandGroup {
+
+    public PlaceOnReef(Elevator elevator, CoralJoint coralJoint, Storage storage, Elevator.ElevatorLevel level) {
+        addCommands(new ParallelCommandGroup(
+                        new MoveToHeight(elevator, level),
+                        new RotateStorage(coralJoint, CoralJoint.StoragePose.PLACEMENT)),
+                new ReleaseCoral(storage), new ParallelCommandGroup(
+                        new MoveToHeight(elevator, Elevator.ElevatorLevel.FEEDER),
+                        new RotateStorage(coralJoint, CoralJoint.StoragePose.INTAKE)));
+    }
+}

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

@@ -0,0 +1,30 @@
+package frc.robot.commands;
+
+import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
+import com.spikes2212.dashboard.RootNamespace;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.Gripper;
+import java.util.function.Supplier;
+
+public class ReleaseAlgae extends MoveGenericSubsystem {
+
+    private static final RootNamespace NAMESPACE = new RootNamespace("release algae");
+    private static final Supplier<Double> RELEASE_SPEED = NAMESPACE.addConstantDouble("release speed", -0.5);
+    private static final Supplier<Double> TIME_TO_RELEASE = NAMESPACE.addConstantDouble("time to release", 0.5);
+
+    private double startTime;
+
+    public ReleaseAlgae(Gripper gripper) {
+        super(gripper, RELEASE_SPEED);
+    }
+
+    @Override
+    public void initialize() {
+        startTime = Timer.getFPGATimestamp();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Timer.getFPGATimestamp() - startTime >= TIME_TO_RELEASE.get();
+    }
+}

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

@@ -0,0 +1,31 @@
+package frc.robot.commands;
+
+import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
+import com.spikes2212.dashboard.RootNamespace;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.Storage;
+
+import java.util.function.Supplier;
+
+public class ReleaseCoral extends MoveGenericSubsystem {
+
+    private static final RootNamespace NAMESPACE = new RootNamespace("release coral");
+    private static final Supplier<Double> TIME_TO_RELEASE = NAMESPACE.addConstantDouble("time to release", 0.5);
+    private static final Supplier<Double> RELEASE_SPEED = NAMESPACE.addConstantDouble("release speed", -0.5);
+
+    private double startTime;
+
+    public ReleaseCoral(Storage storage) {
+        super(storage, RELEASE_SPEED);
+    }
+
+    @Override
+    public void initialize() {
+        startTime = Timer.getFPGATimestamp();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Timer.getFPGATimestamp() - startTime >= TIME_TO_RELEASE.get();
+    }
+}

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

@@ -0,0 +1,18 @@
+package frc.robot.commands;
+
+import com.spikes2212.command.genericsubsystem.GenericSubsystem;
+import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
+import com.spikes2212.dashboard.RootNamespace;
+import frc.robot.subsystems.AlgaeJoint;
+
+import java.util.function.Supplier;
+
+public class RotateAlgaeJointToBottom extends MoveGenericSubsystem {
+
+    private static final RootNamespace NAMESPACE = new RootNamespace("rotate algae joint to bottom");
+    private static final Supplier<Double> SPEED = NAMESPACE.addConstantDouble("intake speed", -0.5);
+
+    public RotateAlgaeJointToBottom(AlgaeJoint algaeJoint) {
+        super(algaeJoint, SPEED);
+    }
+}