useful message

1 files changed, 52 insertions, 3 deletions

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index a7fe75e..c149477 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -10,11 +10,23 @@ 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.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;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Volts;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
 
 import frc.robot.Constants;
 import frc.robot.util.IMUGyro;
@@ -41,6 +53,34 @@ public class Drivetrain extends SubsystemBase
 
     private DifferentialDriveOdometry m_odometry;
 
+    // For system identification
+    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));
+            },
+            log -> {
+                log.motor("drive-left")
+                    .voltage(
+                        m_voltage.mut_replace(
+                            m_motorL_leader.get() * RobotController.getBatteryVoltage(), Volts)
+                    )
+                    .linearPosition(m_distance.mut_replace(m_encoderL.getDistance(), Meters))
+                    .linearVelocity(m_velocity.mut_replace(m_encoderL.getRate(), MetersPerSecond));
+                log.motor("driver-right")
+                .voltage(
+                        m_voltage.mut_replace(
+                            m_motorR_leader.get() * RobotController.getBatteryVoltage(), Volts)
+                    )
+                    .linearPosition(m_distance.mut_replace(m_encoderR.getDistance(), Meters))
+                    .linearVelocity(m_velocity.mut_replace(m_encoderR.getRate(), MetersPerSecond));
+            }, this));
+
     public Drivetrain()
     {
         m_gyro.reset();
@@ -95,8 +135,6 @@ 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();
     }
 
@@ -130,7 +168,18 @@ public class Drivetrain extends SubsystemBase
 
     public Command trajectory(Trajectory trajectory)
     {
+        // TOOD: Implement
         // return this.runOnce(...)   
         return Commands.none();
     }
+
+    public Command sysIdQausistatic(SysIdRoutine.Direction direction)
+    {
+        return m_sysIdRoutine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction)
+    {
+        return m_sysIdRoutine.dynamic(direction);
+    }
 }
\ No newline at end of file