summaryrefslogtreecommitdiff
path: root/src/main/java/frc/robot/subsystems/Drivetrain.java
diff options
context:
space:
mode:
Diffstat (limited to 'src/main/java/frc/robot/subsystems/Drivetrain.java')
-rw-r--r--src/main/java/frc/robot/subsystems/Drivetrain.java78
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
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-23 02:32:55 +0000