diff options
Diffstat (limited to 'src/main/java/frc/robot/subsystems/Drivetrain.java')
| -rw-r--r-- | src/main/java/frc/robot/subsystems/Drivetrain.java | 55 |
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 |