Se/code organization

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

@@ -10,7 +10,7 @@ public AmperScoreAmp() {
         addCommands(
             new AmperToHeight(Score.AMP_SCORE_HEIGHT.get()),
             new AmperWaitToHeight(Score.AMP_SCORE_HEIGHT.get()),
-            new AmperOuttake()
+            new AmperScore()
         );
     }
 

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

@@ -10,7 +10,7 @@ public AmperScoreTrap() {
         addCommands(
             new AmperToHeight(Score.TRAP_SCORE_HEIGHT.get()),
             new AmperWaitToHeight(Score.TRAP_SCORE_HEIGHT.get()),
-            new AmperOuttake()
+            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();
+    }
+
+}

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

@@ -17,6 +17,11 @@ public AmperWaitToHeight(double height) {
         addRequirements(amper);
     }
 
+    @Override
+    public void initialize() {
+        amper.setTargetHeight(height);
+    }
+
     @Override
     public boolean isFinished() {
         return Math.abs(amper.getLiftHeight() - height) < Lift.MAX_HEIGHT_ERROR;

src/main/java/com/stuypulse/robot/commands/climber/ClimberDrive.java

@@ -27,7 +27,7 @@ public ClimberDrive(Gamepad gamepad) {
     public void execute() {
         climber.setVoltageOverride(voltage.get());
 
-        SmartDashboard.putNumber("Climber/Gamepad Velocity", voltage.get());
+        SmartDashboard.putNumber("Climber/Gamepad Voltage", voltage.get());
     }
 
     @Override

src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeight.java

@@ -1,11 +1,11 @@
 package com.stuypulse.robot.commands.climber;
 
-import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.climber.Climber;
 
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ClimberToHeight extends InstantCommand {
 
-public class ClimberToHeight extends Command {
     private final Climber climber;
     private final double height;
 
@@ -20,9 +20,4 @@ public ClimberToHeight(double height) {
     public void initialize() {
         climber.setTargetHeight(height);
     }
-
-    @Override
-    public boolean isFinished() {
-        return Math.abs(climber.getTargetHeight() - climber.getHeight()) < Settings.Climber.BangBang.THRESHOLD;
-    }
 }

src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeightInstant.java → src/main/java/com/stuypulse/robot/commands/climber/ClimberWaitForHeight.java

@@ -1,15 +1,15 @@
 package com.stuypulse.robot.commands.climber;
 
+import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.climber.Climber;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-
-public class ClimberToHeightInstant extends InstantCommand {
+import edu.wpi.first.wpilibj2.command.Command;
 
+public class ClimberWaitForHeight extends Command {
     private final Climber climber;
     private final double height;
 
-    public ClimberToHeightInstant(double height) {
+    public ClimberWaitForHeight(double height) {
         climber = Climber.getInstance();
         this.height = height;
 
@@ -20,4 +20,9 @@ public ClimberToHeightInstant(double height) {
     public void initialize() {
         climber.setTargetHeight(height);
     }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(climber.getTargetHeight() - climber.getHeight()) < Settings.Climber.BangBang.THRESHOLD;
+    }
 }

src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShoot.java

@@ -2,9 +2,9 @@
 
 import com.stuypulse.robot.subsystems.conveyor.Conveyor;
 
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-public class ConveyorShoot extends Command{
+public class ConveyorShoot extends InstantCommand {
 
     private final Conveyor conveyor;
 

src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorStop.java

@@ -2,11 +2,11 @@
 
 import com.stuypulse.robot.subsystems.conveyor.Conveyor;
 
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-public class ConveyorStop extends Command{
+public class ConveyorStop extends InstantCommand {
 
-    private Conveyor conveyor;
+    private final Conveyor conveyor;
 
     public ConveyorStop() {
         conveyor = Conveyor.getInstance();

src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmp.java

@@ -4,11 +4,11 @@
 
 import com.stuypulse.robot.subsystems.intake.Intake;
 
-import com.stuypulse.robot.subsystems.amper.Amper;
-
 import edu.wpi.first.wpilibj2.command.Command;
 
-public class ConveyorToAmp extends Command{
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+public class ConveyorToAmp extends Command {
 
     private final Conveyor conveyor;
     private final Intake intake;
@@ -26,6 +26,7 @@ public ConveyorToAmp() {
     public void execute() {
         conveyor.toAmp();
         intake.acquire();
+        amper.score();
     }
 
     @Override

src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToShooter.java

@@ -6,7 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 
-public class ConveyorToShooter extends Command{
+public class ConveyorToShooter extends Command {
 
     private final Conveyor conveyor;
     private final Intake intake;

src/main/java/com/stuypulse/robot/constants/Field.java

@@ -2,7 +2,7 @@
 
 import java.util.ArrayList;
 
-import com.stuypulse.robot.util.Fiducial;
+import com.stuypulse.robot.util.vision.Fiducial;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;

src/main/java/com/stuypulse/robot/constants/Motors.java

@@ -25,7 +25,7 @@ public interface Motors {
     /** Classes to store all of the values a motor needs */
 
     public interface Amper {
-        CANSparkMaxConfig LIFT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kCoast);
+        CANSparkMaxConfig LIFT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake);
         CANSparkMaxConfig SCORE_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake);
     }
 
@@ -45,12 +45,12 @@ public interface Shooter {
 
     public interface Conveyor {
         CANSparkMaxConfig GANDALF_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake);
-        CANSparkMaxConfig SHOOTER_FEEDER_MOTOR = new CANSparkMaxConfig(false, IdleMode.kCoast);
+        CANSparkMaxConfig SHOOTER_FEEDER_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake);
     }
 
     public interface Climber {
-        CANSparkMaxConfig LEFT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake, 80);
-        CANSparkMaxConfig RIGHT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake, 80);
+        CANSparkMaxConfig LEFT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake);
+        CANSparkMaxConfig RIGHT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake);
     }
 
     public static class TalonSRXConfig {