From e139fb2e9e0e6ee86cf2c451ffdd8853b564b7cb Mon Sep 17 00:00:00 2001
From: BenG49 <ben@goldfisher.com>
Date: Tue, 9 Jan 2024 23:07:37 -0500
Subject: [PATCH] Fix up drivetrain

---
 .../com/stuypulse/robot/constants/Motors.java |  5 ++
 .../stuypulse/robot/constants/Settings.java   |  2 +-
 .../robot/subsystems/Drivetrain.java          | 56 +++++++++----------
 3 files changed, 34 insertions(+), 29 deletions(-)

diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java
index e3411f3..34b5226 100644
--- a/src/main/java/com/stuypulse/robot/constants/Motors.java
+++ b/src/main/java/com/stuypulse/robot/constants/Motors.java
@@ -22,6 +22,11 @@
  */
 public interface Motors {
 
+    public interface Drivetrain {
+        CANSparkMaxConfig LEFT = new CANSparkMaxConfig(false, IdleMode.kBrake, 60);
+        CANSparkMaxConfig RIGHT = new CANSparkMaxConfig(true, IdleMode.kBrake, 60);
+    }
+
     /** Classes to store all of the values a motor needs */
 
     public static class TalonSRXConfig {
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
index 59d620a..044c63f 100644
--- a/src/main/java/com/stuypulse/robot/constants/Settings.java
+++ b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -16,7 +16,7 @@
  */
 public interface Settings {
     public interface Drivetrain {
-        int CURRENT_LIMIT = 60;
+        
     }
 
     public interface Launcher {
diff --git a/src/main/java/com/stuypulse/robot/subsystems/Drivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/Drivetrain.java
index aca6d8e..7528cd5 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/Drivetrain.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/Drivetrain.java
@@ -2,48 +2,51 @@
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.stuypulse.robot.constants.Motors;
 import com.stuypulse.robot.constants.Ports;
-import com.stuypulse.robot.constants.Settings;
 
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class Drivetrain extends SubsystemBase{
-    private static Drivetrain instance = new Drivetrain();
+public class Drivetrain extends SubsystemBase {
+
+    private static final Drivetrain instance;
+
+    static {
+        instance = new Drivetrain();
+    }
 
     public static Drivetrain getInstance() {
         return instance;
     }
 
-    private DifferentialDrive drivetrain;
+    private final DifferentialDrive drivetrain;
 
-    CANSparkMax leftFront;
-    CANSparkMax leftBack;
-    CANSparkMax rightFront;
-    CANSparkMax rightBack;
+    private final CANSparkMax leftFront;
+    private final CANSparkMax leftBack;
+    private final CANSparkMax rightFront;
+    private final CANSparkMax rightBack;
 
     public Drivetrain() {
         leftFront = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed);
-        leftBack = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed);
-        rightFront = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed);
-        rightBack = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed);
+        leftBack = new CANSparkMax(Ports.Drivetrain.LEFTREAR, MotorType.kBrushed);
+        rightFront = new CANSparkMax(Ports.Drivetrain.RIGHTFRONT, MotorType.kBrushed);
+        rightBack = new CANSparkMax(Ports.Drivetrain.RIGHTREAR, MotorType.kBrushed);
+
+        Motors.Drivetrain.LEFT.configure(leftFront);
+        Motors.Drivetrain.LEFT.configure(leftBack);
+
+        Motors.Drivetrain.RIGHT.configure(rightFront);
+        Motors.Drivetrain.RIGHT.configure(rightBack);
    
         leftBack.follow(leftFront);
         rightBack.follow(rightFront);
         
-        leftFront.setInverted(true);
-        rightFront.setInverted(false);
-
-        leftFront.setSmartCurrentLimit(Settings.Drivetrain.kCurrentLimit);
-        leftBack.setSmartCurrentLimit(Settings.Drivetrain.kCurrentLimit);
-        rightFront.setSmartCurrentLimit(Settings.Drivetrain.kCurrentLimit);
-        leftBack.setSmartCurrentLimit(Settings.Drivetrain.kCurrentLimit);
-        
         drivetrain = new DifferentialDrive(leftFront, rightFront);
     }
 
-    //********** GETTERS **********
+    //********** GETTERS **********//
     public double getLeftSpeed() {
         return leftFront.getEncoder().getVelocity();
     }
@@ -60,7 +63,7 @@ public double getRightVoltage() {
         return rightFront.getAppliedOutput();
     }
 
-    //********** Drive Methods **********
+    //********** Drive Methods **********//
     public void tankDrive(double leftSpeed, double rightSpeed) {
         drivetrain.tankDrive(leftSpeed, rightSpeed);
     }
@@ -79,12 +82,9 @@ public void stop() {
 
     @Override
     public void periodic() {
-        SmartDashboard.putNumber("Drivetrain/ Left Speed", getLeftSpeed());
-        SmartDashboard.putNumber("Drivetrain/ Right Speed", getRightSpeed());
-        SmartDashboard.putNumber("Drivetrain/ Left Voltage", getLeftVoltage());
-        SmartDashboard.putNumber("Drivetrain/ Right Voltage", getRightVoltage());
-
+        SmartDashboard.putNumber("Drivetrain/Left Speed", getLeftSpeed());
+        SmartDashboard.putNumber("Drivetrain/Right Speed", getRightSpeed());
+        SmartDashboard.putNumber("Drivetrain/Left Voltage", getLeftVoltage());
+        SmartDashboard.putNumber("Drivetrain/Right Voltage", getRightVoltage());
     }
-
-   
 }
\ No newline at end of file