Merge branch 'main' into se/climb

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,29 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Visualizers/Elevator": "Mechanism2d"
+    },
+    "windows": {
+      "/SmartDashboard/Visualizers/Elevator": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "Elevator": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

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

@@ -5,6 +5,8 @@
 
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.commands.elevator.ElevatorToBottom;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -15,12 +17,15 @@ public class Robot extends TimedRobot {
     private Command auto;
 
     /*************************/
-    /*** ROBOT SCHEDULEING ***/
+    /*** ROBOT SCHEDULING ***/
     /*************************/
 
     @Override
     public void robotInit() {
         robot = new RobotContainer();
+
+        // Check with Philip to automatically reset elevator encoder
+        // new ElevatorToBottom().schedule();
     }
 
     @Override

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

@@ -6,9 +6,13 @@
 package com.stuypulse.robot;
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
+import com.stuypulse.robot.commands.funnel.FunnelDefaultCommand;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.funnel.CoralFunnel;
+import com.stuypulse.robot.subsystems.lokishooter.LokiShooter;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
+import com.stuypulse.robot.subsystems.elevator.Elevator;
 
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -22,6 +26,10 @@ public class RobotContainer {
 
     // Subsystem
 
+    private final CoralFunnel funnel = CoralFunnel.getInstance();
+    private final LokiShooter shooter = LokiShooter.getInstance();
+    private final Elevator elevator = Elevator.getInstance();
+
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 
@@ -37,7 +45,9 @@ public RobotContainer() {
     /*** DEFAULTS ***/
     /****************/
 
-    private void configureDefaultCommands() {}
+    private void configureDefaultCommands() {
+        funnel.setDefaultCommand(new FunnelDefaultCommand());
+    }
 
     /***************/
     /*** BUTTONS ***/

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java

@@ -0,0 +1,24 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+
+public class ArmMoveToAngle extends InstantCommand{
+    private final Arm arm;
+    private final Rotation2d angle;
+
+    public ArmMoveToAngle(Rotation2d angle){
+        arm = Arm.getInstance();
+        this.angle = angle;
+    }
+
+    @Override
+    public void initialize(){
+        arm.setTargetAngle(angle);
+
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java

@@ -0,0 +1,14 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+
+public class ArmMoveToFunnel extends ArmMoveToAngle{
+    public ArmMoveToFunnel(){
+        super(Settings.Arm.FUNNEL_ANGLE);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Back.java

@@ -0,0 +1,13 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmMoveToL2Back extends ArmMoveToAngle{
+    public ArmMoveToL2Back(){
+        super(Settings.Arm.L2_ANGLE_BACK);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Front.java

@@ -0,0 +1,14 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+
+public class ArmMoveToL2Front extends ArmMoveToAngle{
+    public ArmMoveToL2Front(){
+        super(Settings.Arm.L2_ANGLE_FRONT);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Back.java

@@ -0,0 +1,13 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmMoveToL3Back extends ArmMoveToAngle {
+    public ArmMoveToL3Back(){
+        super(Settings.Arm.L3_ANGLE_BACK);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Front.java

@@ -0,0 +1,14 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+
+public class ArmMoveToL3Front extends ArmMoveToAngle{
+    public ArmMoveToL3Front(){
+        super(Settings.Arm.L3_ANGLE_FRONT);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Back.java

@@ -0,0 +1,13 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmMoveToL4Back extends ArmMoveToAngle {
+    public ArmMoveToL4Back(){
+        super(Settings.Arm.L4_ANGLE_BACK);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Front.java

@@ -0,0 +1,14 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+
+public class ArmMoveToL4Front extends ArmMoveToAngle{
+    public ArmMoveToL4Front(){
+        super(Settings.Arm.L4_ANGLE_FRONT);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

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

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.elevator;
+
+import com.stuypulse.robot.constants.Constants.Elevator;
+
+public class ElevatorToBottom extends ElevatorToHeight{
+    public ElevatorToBottom(){
+        super(Elevator.MIN_HEIGHT_METERS);
+    }
+}

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java

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

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

@@ -0,0 +1,29 @@
+package com.stuypulse.robot.commands.elevator;
+
+import com.stuypulse.robot.subsystems.elevator.Elevator;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand; 
+import edu.wpi.first.wpilibj2.command.Command;  
+
+public class ElevatorToHeight extends InstantCommand {
+
+    public static Command untilDone(double height) {
+        return new ElevatorToHeight(height)
+            .andThen(new WaitUntilCommand(() -> Elevator.getInstance().atTargetHeight()));
+    }
+
+    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/ElevatorToLvl2Alt.java

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

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java

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