Cleaned up code

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

@@ -5,7 +5,6 @@
 
 package com.stuypulse.robot;
 
-import com.stuypulse.robot.commands.SwerveDriveDrive;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.commands.auton.PreloadMobility;
 import com.stuypulse.robot.commands.launcher.LauncherHoldSpeed;
@@ -15,7 +14,7 @@
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.commands.shooter.ShootCommand;
 import com.stuypulse.robot.commands.shooter.ShootStopCommand;
-
+import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
 import com.stuypulse.robot.subsystems.Shooter;
 import com.stuypulse.robot.subsystems.TankDrive;
 import com.stuypulse.robot.subsystems.launcher.Launcher;
@@ -34,23 +33,19 @@ public class RobotContainer {
 
 
     // Gamepads
-    // public final XboxController driver = new XboxController(Ports.Gamepad.DRIVER);
-    // public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
-    // replace these with non stuylib controllers
     private final XboxController driver = new XboxController(DRIVER);
     // private final XboxController operator = new XboxController(OPERATOR);
 
     // Subsystem
-    public TankDrive drivetrain = TankDrive.getInstance();
+    private TankDrive drivetrain = TankDrive.getInstance();
     //public SwerveDrive swerve = SwerveDrive.getInstance();
     // private final Shooter shooter = Shooter.getInstance();
-    public Launcher launcher = Launcher.getInstance();
+    private Launcher launcher = Launcher.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 
     // Robot container
-
     public RobotContainer() {
         configureDefaultCommands();
         configureButtonBindings();

src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java

@@ -8,7 +8,7 @@ public class IntakeAcquire extends Command {
     private Intake intake;
 
     public IntakeAcquire() {
-        intake = intake.getInstance();
+        intake = Intake.getInstance();
         addRequirements(intake);
     }
 

src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java

@@ -9,7 +9,7 @@ public class IntakeDeacquire extends Command{
 
 
     public IntakeDeacquire(){
-        intake = intake.getInstance();
+        intake = Intake.getInstance();
         addRequirements(intake);
     }
 

src/main/java/com/stuypulse/robot/commands/SwerveDriveBack.java → src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveBack.java

@@ -1,4 +1,4 @@
-package com.stuypulse.robot.commands;
+package com.stuypulse.robot.commands.swerve;
 
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 

src/main/java/com/stuypulse/robot/commands/SwerveDriveDrive.java → src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java

@@ -1,4 +1,4 @@
-package com.stuypulse.robot.commands;
+package com.stuypulse.robot.commands.swerve;
 
 import com.stuypulse.robot.constants.Settings.Swerve.Driver.Turn;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;

src/main/java/com/stuypulse/robot/commands/tank/TankDriveDrive.java

@@ -1,7 +1,5 @@
 package com.stuypulse.robot.commands.tank;
 
-import org.opencv.features2d.MSER;
-
 import com.stuypulse.robot.subsystems.TankDrive;
 
 import edu.wpi.first.math.filter.SlewRateLimiter;
@@ -34,19 +32,16 @@ public TankDriveDrive(TankDrive TankDrive, XboxController driver) {
     }
 
     public interface Drivetrain {
-        // If speed is below this, use quick turn
-        double BASE_TURNING_SPEED = 0.45; //Driver Settings/Base Turn Speed
-
         // Low Pass Filter and deadband for Driver Controls
         double SPEED_DEADBAND = 0.02; //Driver Settings/Speed Deadband
         double ANGLE_DEADBAND = 0.05; //Driver Settings/Turn Deadband
 
         // Width of the robot
-        double TRACK_WIDTH = Units.inchesToMeters(26.9); //CHANGE
+        double TRACK_WIDTH = Units.inchesToMeters(26.9); //XXX: Robot Specfic
     }
 
     public double filterSpeed(double newSpeed) {
-        newSpeed = pow(newSpeed, 2); 
+        newSpeed = spow(newSpeed, 2); 
         newSpeed = driveSlew.calculate(newSpeed);    
         speed += (newSpeed - speed) * 0.75;   
         if (Math.abs(speed) < Drivetrain.SPEED_DEADBAND) {
@@ -57,7 +52,7 @@ public double filterSpeed(double newSpeed) {
     }
 
     public double filterTurn(double newTurn) {
-        newTurn = pow(newTurn, 1);
+        newTurn = spow(newTurn, 1);
         newTurn = turnSlew.calculate(newTurn);
         turn += (newTurn - turn) * 0.75;  
         if (Math.abs(turn) < Drivetrain.ANGLE_DEADBAND) {
@@ -67,21 +62,20 @@ public double filterTurn(double newTurn) {
         }
     }
 
-    public double pow(double value, double expo){
+    public double spow(double value, double expo){
         return Math.copySign(Math.pow(value, expo), value);
     }
 
     @Override
     public void execute() {
-        // System.out.println(driver.getRightX());
+        //XXX: joystick controls (invert as needed)
         tank.curvatureDrive(
             filterSpeed(-driver.getLeftY()),
             filterTurn(-driver.getRightX()));
-        //tank.curvatureDrive(-driver.getLeftY(), -driver.getRightX());
-    }
+
 
-    @Override
-    public boolean isFinished() {
-        return false;
+        //XXX: trigger controls (invert as needed)ß
+        // tank.curvatureDrive(
+        //     driver.getRightTriggerAxis() - driver.getLeftTriggerAxis(), -driver.getLeftY());
     }
 }

src/main/java/com/stuypulse/robot/subsystems/Intake.java

@@ -7,13 +7,19 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Intake extends SubsystemBase{
+    //TODO: Robot Specific
     private final int MOTOR_PORT = 0;
-    private final int ACQUIRE_SPEED = 0;
-
     private final CANSparkMax motor;
+
+    private final int ACQUIRE_SPEED = 1;
 
-    public Intake getInstance(){
-        return new Intake();
+    private static Intake instance;
+
+    public static Intake getInstance() {
+        if (instance == null) {
+            instance = new Intake();
+        }
+        return instance;
     }
 
     public Intake() {

src/main/java/com/stuypulse/robot/subsystems/Shooter.java

@@ -2,7 +2,7 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.CANSparkLowLevel.MotorType;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+// import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -29,8 +29,8 @@ public static Shooter getInstance() {
     private RelativeEncoder leftEncoder;
     private RelativeEncoder rightEncoder;
 
-    private SimpleMotorFeedforward leftController;
-    private SimpleMotorFeedforward rightController;
+    // private SimpleMotorFeedforward leftController;
+    // private SimpleMotorFeedforward rightController;
 
     public Shooter() {
         leftMotor = new CANSparkMax(leftMotorPort, MotorType.kBrushless);
@@ -39,8 +39,8 @@ public Shooter() {
         leftEncoder = leftMotor.getEncoder();
         rightEncoder = rightMotor.getEncoder();
 
-        leftController = new SimpleMotorFeedforward(0, 0);
-        rightController = new SimpleMotorFeedforward(0, 0);
+        // leftController = new SimpleMotorFeedforward(0, 0);
+        // rightController = new SimpleMotorFeedforward(0, 0);
     }
 
     public void shoot(double leftRPM, double rightRPM) {

src/main/java/com/stuypulse/robot/subsystems/TankDrive.java

@@ -8,9 +8,7 @@
 import com.kauailabs.navx.frc.AHRS;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
-import com.stuypulse.robot.constants.Motors.CANSparkMaxConfig;
 import com.stuypulse.robot.constants.Motors;
-//import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -50,15 +48,16 @@ public static TankDrive getInstance() {
     private final Field2d field;
     private final DifferentialDrive tankDrive;
 
-    //drivetrain ports
-    private final int LEFT_TOP = 0;
-    private final int LEFT_MIDDLE = 1;
-    private final int LEFT_BOTTOM = 2;
+    //6 motor drivetrain ports
+    // private final int LEFT_TOP = 0;
+    // private final int LEFT_MIDDLE = 1;
+    // private final int LEFT_BOTTOM = 2;
 
-    private final int RIGHT_TOP = 3;
-    private final int RIGHT_MIDDLE = 4;
-    private final int RIGHT_BOTTOM = 5;
+    // private final int RIGHT_TOP = 3;
+    // private final int RIGHT_MIDDLE = 4;
+    // private final int RIGHT_BOTTOM = 5;
 
+    //4 motors drivertrain ports
     private final int LEFTREAR = 10;
     private final int LEFTFRONT = 11;
     private final int RIGHTFRONT = 12;
@@ -107,23 +106,19 @@ public TankDrive() {
         setMotorConfig(false, true);
     }
 
-    private boolean ENCODER_INVERTED = true;
-
     private void setMotorConfig(boolean leftIsInverted, boolean rightIsInverted) {
         for(CANSparkMax motor : leftMotors) {
-            motor.setInverted(leftIsInverted);
-            motor.burnFlash();
+            Motors.TankDrive.LEFT.configure(motor);
         }
 
         for(CANSparkMax motor : rightMotors) {
-            motor.setInverted(rightIsInverted);
-            motor.burnFlash();
+            Motors.TankDrive.RIGHT.configure(motor);
         }
     }
 
     /*********************
      * ENCODER FUNCTIONS *
-     *********************/
+    *********************/
 
     public double getLeftDistance() {
         return leftEncoder.getPosition();
@@ -153,8 +148,6 @@ public double getVelocity() {
      * ROBOT ANGLE *
      ***************/
 
-    //Try to code Angle.fromDegrees without Stuylibs
-
     //Gets current Angle of the Robot as a double (contiuous / not +-180)
     public Rotation2d getRawGyroAngle() {
         return navx.getRotation2d();

src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java

@@ -2,9 +2,7 @@
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.constants.Motors;
-import com.stuypulse.robot.constants.Ports;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;

src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java

@@ -56,17 +56,19 @@ public SwerveDrive(SwerveModule... modules) {
 
     // Constants    
     public interface Swerve {
-        //TODO: Change these values to match the actual robot
+        //TODO: Robot Specific
         double WIDTH = Units.inchesToMeters(21);
         double LENGTH = Units.inchesToMeters(21);
         double CENTER_TO_INTAKE_FRONT = Units.inchesToMeters(18);
 
         double MODULE_VELOCITY_DEADBAND = 0.05; //m/s
+
+        //XXX: Motor Specfic
         double MAX_MODULE_SPEED = 5.88;
-        double MAX_MODULE_TURN = 6.28; //couldnt find 
+        double MAX_MODULE_TURN = 6.28; 
 
         public interface FrontRight {
-            //XXX: Constants to be updated
+            //XXX: Robot Specfic
             String ID = "Front Right";
             Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(-153.632812 + 180);
             Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5);
@@ -78,7 +80,7 @@ public interface FrontRight {
         }
 
         public interface FrontLeft {
-            //XXX: Constants to be updated
+            //XXX: Robot Specfic
             String ID = "Front Left";
             Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(147.919922 + 180);
             Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * +0.5);
@@ -90,7 +92,7 @@ public interface FrontLeft {
         }
 
         public interface BackLeft {
-            //XXX: Constants to be updated
+            //XXX: Robot Specfic
             String ID = "Back Left";
             Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(73.125 + 180);
             Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * +0.5);
@@ -102,7 +104,7 @@ public interface BackLeft {
         }
 
         public interface BackRight {
-            //XXX: Constants to be updated
+            //XXX: Robot Specfic
             String ID = "Back Right";
             Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(-2.02184 + 180);
             Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * -0.5);
@@ -253,7 +255,6 @@ public void updateOdomentry() {
 
     @Override
     public void periodic() {
-         // need to complete writing this without using StuyLib
         updateOdomentry();
         Pose2d pose = getPose();