Merge branch 'main' into se/coral-mech

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,77 @@
+{
+  "HALProvider": {
+    "Encoders": {
+      "window": {
+        "visible": true
+      }
+    },
+    "PWM Outputs": {
+      "window": {
+        "visible": true
+      }
+    }
+  },
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Visualizers/Elevator": "Mechanism2d"
+    },
+    "windows": {
+      "/SmartDashboard/Visualizers/Elevator": {
+      "/SmartDashboard/algae": "Mechanism2d"
+    },
+    "windows": {
+      "/SmartDashboard/algae": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "FMSInfo": {
+        "open": true
+      },
+      "LiveWindow": {
+        "open": true
+      },
+      "Shuffleboard": {
+        "open": true
+      },
+      "SmartDashboard": {
+        "Elevator": {
+
+        "algae": {
+
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "Server": {
+
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      }
+    },
+    "visible": true
+  },
+  "NetworkTables View": {
+    "visible": false
+
+      "open": true
+    },
+    "visible": true
+
+  }
+}

src/main/java/com/stuypulse/robot/Robot.java

@@ -5,12 +5,23 @@
 
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.constants.Settings.RobotType;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class Robot extends TimedRobot {
 
+    public static final RobotType ROBOT;
+
+    static {
+        if (Robot.isSimulation())
+            ROBOT = RobotType.SIM;
+        else
+            ROBOT = RobotType.fromString(System.getenv("serialnum"));
+    }
+
     private RobotContainer robot;
     private Command auto;
 

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -5,27 +5,52 @@
 
 package com.stuypulse.robot;
 
+import java.lang.annotation.ElementType;
+import com.stuypulse.robot.commands.Elevator.ElevatorToBottom;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl1;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl2;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl3;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl4;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.Elevator.Elevator;
+import javax.management.openmbean.OpenType;
+
+// Algae Commands
+import com.stuypulse.robot.commands.algae.AlgaeGroundPickup;
+import com.stuypulse.robot.commands.algae.AlgaeL2;
+import com.stuypulse.robot.commands.algae.AlgaeL3;
+import com.stuypulse.robot.commands.algae.AlgaeProcessorScore;
+import com.stuypulse.robot.commands.algae.AlgaeReefKnockoff;
+import com.stuypulse.robot.commands.algae.AlgaeStopRoller;
+import com.stuypulse.robot.commands.algae.AlgaeStow;
+
+import com.stuypulse.robot.commands.auton.DoNothingAuton;
+import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.algae.Algae;
+
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
+
 public class RobotContainer {
 
     // Gamepads
     public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER);
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
-    // Subsystem
+    // Subsystems
+    public final Elevator elevator = Elevator.getInstance();
+    public final Algae algae = Algae.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 
-    // Robot container
+    // Robot Container
 
     public RobotContainer() {
         configureDefaultCommands();
@@ -43,7 +68,53 @@ private void configureDefaultCommands() {}
     /*** BUTTONS ***/
     /***************/
 
-    private void configureButtonBindings() {}
+    private void configureButtonBindings() {
+        configureDriverBindings();
+        configureOperatorBindings();
+    }
+
+    private void configureDriverBindings() {
+
+    }
+
+    private void configureOperatorBindings() {
+
+        operator.getDPadDown()
+            .onTrue(new ElevatorToLvl1());
+
+        operator.getDPadLeft()
+            .onTrue(new ElevatorToLvl2());
+
+        operator.getDPadRight()
+            .onTrue(new ElevatorToLvl3());    
+
+        operator.getDPadUp()
+            .onTrue(new ElevatorToLvl4());
+
+        operator.getLeftButton()
+            .onTrue(new ElevatorToBottom());
+
+        operator.getBottomButton()
+            .onTrue(new AlgaeStow());
+
+        operator.getTopButton()
+            .onTrue(new AlgaeGroundPickup());
+
+        operator.getLeftButton()
+            .onTrue(new AlgaeL2());
+
+        operator.getRightButton()
+            .onTrue(new AlgaeL3());
+
+        operator.getLeftBumper()
+            .onTrue(new AlgaeProcessorScore());
+
+        operator.getRightBumper()
+            .onTrue(new AlgaeReefKnockoff());
+
+        operator.getLeftMenuButton()
+            .onTrue(new AlgaeStopRoller());
+    }
 
     /**************/
     /*** AUTONS ***/

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java

@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToBottom extends ElevatorToHeight {
+
+    public ElevatorToBottom() {
+        super(Elevator.MIN_HEIGHT);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java

@@ -0,0 +1,21 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.subsystems.Elevator.Elevator;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;   
+
+public class ElevatorToHeight extends InstantCommand {
+    private final Elevator elevator;
+    private final double targetHeight;
+
+    public ElevatorToHeight(double targetHeight){
+        elevator = Elevator.getInstance();
+        this.targetHeight = targetHeight;
+
+        addRequirements(elevator);
+    }
+
+    public void initialize(){
+        elevator.setTargetHeight(targetHeight);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl1 extends ElevatorToHeight{
+    public ElevatorToLvl1(){
+        super(Elevator.L1);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl2 extends ElevatorToHeight {
+    public ElevatorToLvl2(){
+        super(Elevator.L2);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl3 extends ElevatorToHeight {
+    public ElevatorToLvl3(){
+        super(Elevator.L3);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl4 extends ElevatorToHeight{
+    public ElevatorToLvl4(){
+        super(Elevator.L4);
+    }
+}

src/main/java/com/stuypulse/robot/commands/algae/AlgaeGroundPickup.java

@@ -0,0 +1,15 @@
+package com.stuypulse.robot.commands.algae;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class AlgaeGroundPickup extends AlgaeSetPivot {
+
+    public AlgaeGroundPickup(){
+        super(Settings.Algae.GROUND_PICKUP_ANGLE);
+        algae.acquireUnder();
+    }
+    public void initialize(){
+        super.initialize();
+    }
+
+}