working gyro and subsystem

1 files changed, 28 insertions, 11 deletions

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 80ed06e..a7fe75e 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -9,8 +9,13 @@ 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.math.proto.Trajectory;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
 import frc.robot.Constants;
 import frc.robot.util.IMUGyro;
 
@@ -26,7 +31,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.5, 2.82, 0.47);
+    private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1.1, 3.16, 0.17);
 
     private final PIDController m_pidL = new PIDController(1, 0, 0);
     private final PIDController m_pidR = new PIDController(1, 0, 0);
@@ -43,10 +48,12 @@ public class Drivetrain extends SubsystemBase
 
         m_motorR_follower.follow(m_motorR_leader);
         m_motorL_follower.follow(m_motorL_leader);
+        
         m_motorR_leader.setInverted(true);
+        m_encoderL.setReverseDirection(true);
 
-        m_encoderL.setDistancePerPulse(-2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
-        m_encoderR.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
+        m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
+        m_encoderR.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
         
         m_encoderL.reset();
         m_encoderR.reset();
@@ -72,21 +79,24 @@ public class Drivetrain extends SubsystemBase
 
     public void speed(DifferentialDriveWheelSpeeds speeds) 
     {
-        m_motorL_leader.setVoltage(
+        voltage(
             m_pidL.calculate(m_encoderL.getRate(), speeds.leftMetersPerSecond)
-            + m_feedforward.calculate(speeds.leftMetersPerSecond)
-        );
-
-        m_motorR_leader.setVoltage(
+              + m_feedforward.calculate(speeds.leftMetersPerSecond),
             m_pidR.calculate(m_encoderR.getRate(), speeds.rightMetersPerSecond)
-            + m_feedforward.calculate(speeds.rightMetersPerSecond)
+              + m_feedforward.calculate(speeds.rightMetersPerSecond)
         );
     }
 
+    public void voltage(double leftVolts, double rightVolts)
+    {
+        m_motorL_leader.setVoltage(leftVolts);
+        m_motorR_leader.setVoltage(rightVolts);
+    }
+
     public Pose2d pose2d() 
     {
-        System.out.println("Distance: "+m_encoderL.getDistance());
-        System.out.println("Distance: "+m_encoderR.getDistance());
+        System.out.println("Distance: " + m_encoderL.getDistance());
+        System.out.println("Distance: " + m_encoderR.getDistance());
         return m_odometry.getPoseMeters();
     }
 
@@ -95,6 +105,7 @@ public class Drivetrain extends SubsystemBase
         m_gyro.reset();
         m_encoderL.reset();
         m_encoderR.reset();
+
         m_odometry = new DifferentialDriveOdometry(
             m_gyro.toRotation2d(),
             Constants.encoderL.getDistance(),
@@ -116,4 +127,10 @@ public class Drivetrain extends SubsystemBase
     public void simulationPeriodic() {
         // This method will be called once per scheduler run during simulation
     }
+
+    public Command trajectory(Trajectory trajectory)
+    {
+        // return this.runOnce(...)   
+        return Commands.none();
+    }
 }
\ No newline at end of file