sysid and trajectory things

1 files changed, 44 insertions, 34 deletions

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 5756650..5f34119 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -2,7 +2,9 @@ package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.RamseteController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -10,15 +12,17 @@ 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.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Velocity;
 import edu.wpi.first.units.Voltage;
 import edu.wpi.first.units.MutableMeasure;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.RobotController;
 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 edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
@@ -47,8 +51,10 @@ public class Drivetrain extends SubsystemBase
 
     private SimpleMotorFeedforward m_feedforward = Constants.motor_feedforward; 
 
-    private ProfiledPIDController m_pidL = new ProfiledPIDController(0.2, 0, 0, Constants.motor_constraints);
-    private ProfiledPIDController m_pidR = new ProfiledPIDController(0.2, 0, 0, Constants.motor_constraints);
+    private final double kP = 0.363;
+
+    private ProfiledPIDController m_pidL = new ProfiledPIDController(kP, 0, 0, Constants.motor_constraints);
+    private ProfiledPIDController m_pidR = new ProfiledPIDController(kP, 0, 0, Constants.motor_constraints);
     
     private DifferentialDriveKinematics m_kinematics = 
         new DifferentialDriveKinematics(Constants.TrackWidth);
@@ -59,30 +65,39 @@ public class Drivetrain extends SubsystemBase
     private final MutableMeasure<Voltage> m_voltage = mutable(Volts.of(0));
     private final MutableMeasure<Distance> m_distance = mutable(Meters.of(0));
     private final MutableMeasure<Velocity<Distance>> m_velocity = mutable(MetersPerSecond.of(0));
+
     private final SysIdRoutine m_sysIdRoutine = new SysIdRoutine(
         new SysIdRoutine.Config(), 
         new SysIdRoutine.Mechanism(
             (Measure<Voltage> volts) -> {
-                m_motorL_leader.set(-volts.in(Volts));
-                m_motorR_leader.set(-volts.in(Volts));
+                m_motorL_leader.setVoltage(volts.in(Volts));
+                m_motorR_leader.setVoltage(volts.in(Volts));
             },
             log -> {
                 log.motor("drive-left")
                     .voltage(
                         m_voltage.mut_replace(
-                            m_motorL_leader.get() * RobotController.getBatteryVoltage(), Volts)
+                            m_motorL_leader.getAppliedOutput() * m_motorL_leader.getBusVoltage(), Volts)
                     )
                     .linearPosition(m_distance.mut_replace(m_encoderL.getDistance(), Meters))
                     .linearVelocity(m_velocity.mut_replace(m_encoderL.getRate(), MetersPerSecond));
                 log.motor("driver-right")
-                .voltage(
+                    .voltage(
                         m_voltage.mut_replace(
-                            m_motorR_leader.get() * RobotController.getBatteryVoltage(), Volts)
+                            m_motorR_leader.getAppliedOutput() * m_motorR_follower.getBusVoltage(), Volts)
                     )
                     .linearPosition(m_distance.mut_replace(m_encoderR.getDistance(), Meters))
                     .linearVelocity(m_velocity.mut_replace(m_encoderR.getRate(), MetersPerSecond));
+                
             }, this));
 
+    public final TrajectoryConfig trajectoyConfig = 
+        new TrajectoryConfig(
+            Constants.AutonomousMaxSpeed,
+            Constants.AutonomousMaxAcceleration)
+        .setKinematics(m_kinematics);
+
+
     public Drivetrain()
     {
         m_gyro.reset();
@@ -94,8 +109,8 @@ public class Drivetrain extends SubsystemBase
         m_motorL_leader.setInverted(true);
         m_encoderL.setReverseDirection(true);
 
-        m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
-        m_encoderR.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
+        m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / 10.71 / Constants.EncoderResolution);
+        m_encoderR.setDistancePerPulse(Math.PI * Constants.WheelDiameter / 10.71 / Constants.EncoderResolution);
         
         m_encoderL.reset();
         m_encoderR.reset();
@@ -170,41 +185,36 @@ public class Drivetrain extends SubsystemBase
 
         Logger.recordOutput(getName() + "/gyro", m_gyro.toRotation2d().getDegrees());
         Logger.recordOutput(getName() + "/pose", pose2d());
-
-        Logger.recordOutput(getName() + "/encoderL", m_encoderL.getRate());
-        Logger.recordOutput(getName() + "/encoderR", m_encoderR.getRate());
+ 
+        Logger.recordOutput(getName() + "/encoderRateL", m_encoderL.getRate());
+        Logger.recordOutput(getName() + "/encoderRateR", m_encoderR.getRate());
+        Logger.recordOutput(getName() + "/encoderDL", m_encoderL.getDistance());
+        Logger.recordOutput(getName() + "/encoderDR", m_encoderR.getDistance());
     }
 
     public Command followTrajectory(Trajectory trajectory)
     {
-        // RamseteCommand ramsete = new RamseteCommand(
-        //     trajectory, this::pose2d, 
-        //     new RamseteController(0, 0), 
-        //     m_feedforward, m_kinematics, 
-        //     () -> new DifferentialDriveWheelSpeeds(m_encoderL.getRate(), m_encoderR.getRate()), 
-        //     m_pidR, m_pidL, this::voltage, this);
-
-        // return Commands.runOnce(() -> this.reset())
-        //                .andThen(ramsete)
-        //                .andThen(Commands.runOnce(() -> voltage(0, 0)));
-        return Commands.none();
+        RamseteCommand ramsete = new RamseteCommand(
+            trajectory, this::pose2d, 
+            new RamseteController(2.0, 0.7), 
+            m_feedforward, m_kinematics, 
+            () -> new DifferentialDriveWheelSpeeds(m_encoderL.getRate(), m_encoderR.getRate()), 
+            new PIDController(kP, 0, 0),
+            new PIDController(kP, 0, 0), 
+            this::voltage, this);
+
+        return Commands.runOnce(() -> this.reset())
+                       .andThen(ramsete)
+                       .andThen(Commands.runOnce(() -> voltage(0, 0)));
     }
 
     public Command sysIdQausistatic(SysIdRoutine.Direction direction)
     {
-        return Commands.race(m_sysIdRoutine.quasistatic(direction),
-            this.runOnce(() -> {
-                while(m_encoderL.getDistance() < 6 && 
-                      m_encoderR.getDistance() < 6);
-            }));
+        return m_sysIdRoutine.quasistatic(direction);
     }
 
     public Command sysIdDynamic(SysIdRoutine.Direction direction)
     {
-        return Commands.race(m_sysIdRoutine.dynamic(direction),
-        this.runOnce(() -> {
-            while(m_encoderL.getDistance() < 6 && 
-                  m_encoderR.getDistance() < 6);
-        }));
+        return m_sysIdRoutine.dynamic(direction);
     }
 }
\ No newline at end of file