Se/code organization

simgui-ds.json

@@ -0,0 +1,93 @@
+{
+  "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
+    }
+  ],
+  "useEnableDisableHotkeys": true
+}

simgui.json

@@ -0,0 +1,63 @@
+{
+  "HALProvider": {
+    "Other Devices": {
+      "window": {
+        "visible": false
+      }
+    }
+  },
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Climber": "Mechanism2d",
+      "/SmartDashboard/Field": "Field2d",
+      "/SmartDashboard/Lift Mechanism": "Mechanism2d"
+    },
+    "windows": {
+      "/FMSInfo": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Autonomous": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Climber": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Field": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Lift Mechanism": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "combined": true
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "Server": {
+      "open": true
+    },
+    "visible": true
+  }
+}

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

@@ -5,8 +5,10 @@
 
 package com.stuypulse.robot;
 
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -15,13 +17,17 @@ public class Robot extends TimedRobot {
     private RobotContainer robot;
     private Command auto;
 
-    /*************************/
-    /*** ROBOT SCHEDULEING ***/
-    /*************************/
+    /************************/
+    /*** ROBOT SCHEDULING ***/
+    /************************/
 
     @Override
     public void robotInit() {
+        DataLogManager.start();
+
         robot = new RobotContainer();
+
+        SmartDashboard.putString("Robot State", "DISABLED");
     }
 
     @Override
@@ -34,7 +40,9 @@ public void robotPeriodic() {
     /*********************/
 
     @Override
-    public void disabledInit() {}
+    public void disabledInit() {
+        SmartDashboard.putString("Robot State", "DISABLED");
+    }
 
     @Override
     public void disabledPeriodic() {
@@ -62,6 +70,8 @@ public void autonomousInit() {
         if (auto != null) {
             auto.schedule();
         }
+
+        SmartDashboard.putString("Robot State", "AUTON");
     }
 
     @Override
@@ -79,6 +89,8 @@ public void teleopInit() {
         if (auto != null) {
             auto.cancel();
         }
+
+        SmartDashboard.putString("Robot State", "TELEOP");
     }
 
     @Override
@@ -94,6 +106,8 @@ public void teleopExit() {}
     @Override
     public void testInit() {
         CommandScheduler.getInstance().cancelAll();
+
+        SmartDashboard.putString("Robot State", "TEST");
     }
 
     @Override

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

@@ -5,9 +5,14 @@
 
 package com.stuypulse.robot;
 
-import com.stuypulse.robot.commands.auton.DoNothingAuton;
-import com.stuypulse.robot.commands.climber.ClimberDrive;
-import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
+import com.stuypulse.robot.commands.*;
+import com.stuypulse.robot.commands.amper.*;
+import com.stuypulse.robot.commands.auton.*;
+import com.stuypulse.robot.commands.climber.*;
+import com.stuypulse.robot.commands.swerve.*;
+import com.stuypulse.robot.commands.intake.*;
+import com.stuypulse.robot.commands.shooter.*;
+import com.stuypulse.robot.commands.conveyor.*;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.amper.Amper;
@@ -35,15 +40,16 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
-    public final Climber climber = Climber.getInstance();
-    public final Amper amper = Amper.getInstance();
-    public final SwerveDrive swerve = SwerveDrive.getInstance();
-    public final Odometry odometry = Odometry.getInstance();
     public final AprilTagVision vision = AprilTagVision.getInstance();
     public final NoteVision noteVision = NoteVision.getInstance();
+    public final Odometry odometry = Odometry.getInstance();
+
+    public final Amper amper = Amper.getInstance();
+    public final Conveyor conveyor = Conveyor.getInstance();
+    public final Climber climber = Climber.getInstance();
     public final Intake intake = Intake.getInstance();
     public final Shooter shooter = Shooter.getInstance();
-    public final Conveyor conveyor = Conveyor.getInstance();
+    public final SwerveDrive swerve = SwerveDrive.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();

src/main/java/com/stuypulse/robot/commands/amper/AmperIntake.java

@@ -18,6 +18,11 @@ public void initialize() {
         amper.intake();
     }
 
+    @Override
+    public boolean isFinished() {
+        return amper.hasNote();
+    }
+
     @Override
     public void end(boolean interrupted) {
         amper.stopRoller();

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreForever.java → src/main/java/com/stuypulse/robot/commands/amper/AmperScore.java

@@ -4,11 +4,11 @@
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-public class AmperScoreForever extends InstantCommand {
+public class AmperScore extends InstantCommand {
 
     private final Amper amper;
 
-    public AmperScoreForever() {
+    public AmperScore() {
         amper = Amper.getInstance();
         addRequirements(amper);
     } 

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreAmp.java → src/main/java/com/stuypulse/robot/commands/amper/AmperScoreAmpRoutine.java

@@ -4,13 +4,13 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
-public class AmperScoreAmp extends SequentialCommandGroup {
+public class AmperScoreAmpRoutine extends SequentialCommandGroup {
 
-    public AmperScoreAmp() {
+    public AmperScoreAmpRoutine() {
         addCommands(
             new AmperToHeight(Score.AMP_SCORE_HEIGHT.get()),
-            new AmperWaitToHeight(Score.AMP_SCORE_HEIGHT.get()),
-            new AmperOuttake()
+            new AmperToHeight(Score.AMP_SCORE_HEIGHT.get()),
+            new AmperScore()
         );
     }
 

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreTrap.java → src/main/java/com/stuypulse/robot/commands/amper/AmperScoreTrapRoutine.java

@@ -4,13 +4,13 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
-public class AmperScoreTrap extends SequentialCommandGroup {
+public class AmperScoreTrapRoutine extends SequentialCommandGroup {
 
-    public AmperScoreTrap() {
+    public AmperScoreTrapRoutine() {
         addCommands(
             new AmperToHeight(Score.TRAP_SCORE_HEIGHT.get()),
-            new AmperWaitToHeight(Score.TRAP_SCORE_HEIGHT.get()),
-            new AmperOuttake()
+            new AmperToHeight(Score.TRAP_SCORE_HEIGHT.get()),
+            new AmperScore()
         ); 
     }
 

src/main/java/com/stuypulse/robot/commands/amper/AmperStop.java

@@ -0,0 +1,21 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class AmperStop extends InstantCommand {
+
+    private final Amper amper;
+
+    public AmperStop() {
+        amper = Amper.getInstance();
+        addRequirements(amper);
+    } 
+
+    @Override
+    public void initialize() {
+        amper.stopRoller();
+    }
+
+}