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