1 files changed, 92 insertions, 0 deletions
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
new file mode 100644
index 0000000..14c9d50
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -0,0 +1,92 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Drivetrain extends SubsystemBase 
+{
+    private AnalogGyro m_gyro = Constants.gyro;
+
+    private PWMSparkMax m_motorL_leader = Constants.motorL_leader;
+    private PWMSparkMax m_motorL_follower = Constants.motorL_follower;
+    private PWMSparkMax m_motorR_leader = Constants.motorR_leader;
+    private PWMSparkMax m_motorR_follower = Constants.motorR_follower;
+
+    private Encoder m_encoderL = Constants.encoderL;
+    private Encoder m_encoderR = Constants.encoderR;
+
+    private SimpleMotorFeedforward m_feedforward = Constants.feedforward;
+
+    private final PIDController m_pidL = new PIDController(1, 0, 0);
+    private final PIDController m_pidR = new PIDController(1, 0, 0);
+    
+    private DifferentialDriveKinematics m_kinematics = 
+        new DifferentialDriveKinematics(Constants.TrackWidth);
+
+    private DifferentialDriveOdometry m_odometry;
+
+    public Drivetrain()
+    {
+        m_gyro.reset();        
+
+        m_motorL_leader.addFollower(m_motorL_follower);
+        m_motorR_leader.addFollower(m_motorR_follower);
+
+        // perhaps invert
+        // m_motorR_leader.setInverted(true);
+
+        m_encoderL.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
+        m_encoderR.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
+        
+        m_encoderL.reset();
+        m_encoderR.reset();
+
+        m_odometry = new DifferentialDriveOdometry(
+            m_gyro.getRotation2d(),
+            Constants.encoderL.getDistance(),
+            Constants.encoderR.getDistance());
+    }
+
+    // speed - linear velocity in m/s
+    // rot   - angular velocity in rad/s
+    public void drive(double speed, double rot) 
+    {
+        speed(m_kinematics.toWheelSpeeds(new ChassisSpeeds(speed, 0.0, rot)));
+    }
+
+    public void speed(DifferentialDriveWheelSpeeds speeds) 
+    {
+        m_motorL_leader.setVoltage(
+            m_pidL.calculate(m_encoderL.getRate(), speeds.leftMetersPerSecond)
+            + m_feedforward.calculate(speeds.leftMetersPerSecond)
+        );
+
+        m_motorR_leader.setVoltage(
+            m_pidR.calculate(m_encoderR.getRate(), speeds.rightMetersPerSecond)
+            + m_feedforward.calculate(speeds.rightMetersPerSecond)
+        );
+    }
+
+    @Override
+    public void periodic() 
+    {
+        m_odometry.update(
+            Constants.gyro.getRotation2d(),
+            Constants.encoderL.getDistance(),
+            Constants.encoderR.getDistance());
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // This method will be called once per scheduler run during simulation
+    }
+}
+\ No newline at end of file