1 files changed, 30 insertions, 6 deletions
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 8f1b975..80ed06e 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -4,6 +4,7 @@ import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
@@ -25,7 +26,7 @@ public class Drivetrain extends SubsystemBase
     private Encoder m_encoderL = Constants.encoderL;
     private Encoder m_encoderR = Constants.encoderR;
 
-    private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1);
+    private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1.5, 2.82, 0.47);
 
     private final PIDController m_pidL = new PIDController(1, 0, 0);
     private final PIDController m_pidR = new PIDController(1, 0, 0);
@@ -38,14 +39,13 @@ public class Drivetrain extends SubsystemBase
     public Drivetrain()
     {
         m_gyro.reset();
+        m_gyro.calibrateGyro(10000);
 
         m_motorR_follower.follow(m_motorR_leader);
         m_motorL_follower.follow(m_motorL_leader);
+        m_motorR_leader.setInverted(true);
 
-        // perhaps invert
-        // m_motorR_leader.setInverted(true);
-
-        m_encoderL.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
+        m_encoderL.setDistancePerPulse(-2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
         m_encoderR.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
         
         m_encoderL.reset();
@@ -59,11 +59,17 @@ public class Drivetrain extends SubsystemBase
 
     // speed - linear velocity in m/s
     // rot   - angular velocity in rad/s
-    public void drive(double speed, double rot) 
+    public void arcade(double speed, double rot) 
     {
         speed(m_kinematics.toWheelSpeeds(new ChassisSpeeds(speed, 0.0, rot)));
     }
 
+    // in m/s
+    public void tank(double left, double right) 
+    {
+        speed(new DifferentialDriveWheelSpeeds(left, right));
+    }
+
     public void speed(DifferentialDriveWheelSpeeds speeds) 
     {
         m_motorL_leader.setVoltage(
@@ -77,6 +83,24 @@ public class Drivetrain extends SubsystemBase
         );
     }
 
+    public Pose2d pose2d() 
+    {
+        System.out.println("Distance: "+m_encoderL.getDistance());
+        System.out.println("Distance: "+m_encoderR.getDistance());
+        return m_odometry.getPoseMeters();
+    }
+
+    public void reset() 
+    {
+        m_gyro.reset();
+        m_encoderL.reset();
+        m_encoderR.reset();
+        m_odometry = new DifferentialDriveOdometry(
+            m_gyro.toRotation2d(),
+            Constants.encoderL.getDistance(),
+            Constants.encoderR.getDistance());
+    }    
+
     @Override
     public void periodic() 
     {