diff options
Diffstat (limited to 'src/main/java/frc/robot/subsystems/Drivetrain.java')
-rw-r--r-- | src/main/java/frc/robot/subsystems/Drivetrain.java | 36 |
1 files changed, 30 insertions, 6 deletions
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 8f1b975..80ed06e 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -4,6 +4,7 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; @@ -25,7 +26,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, 1); + private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1.5, 2.82, 0.47); private final PIDController m_pidL = new PIDController(1, 0, 0); private final PIDController m_pidR = new PIDController(1, 0, 0); @@ -38,14 +39,13 @@ public class Drivetrain extends SubsystemBase public Drivetrain() { m_gyro.reset(); + m_gyro.calibrateGyro(10000); m_motorR_follower.follow(m_motorR_leader); m_motorL_follower.follow(m_motorL_leader); + m_motorR_leader.setInverted(true); - // perhaps invert - // m_motorR_leader.setInverted(true); - - m_encoderL.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution); + m_encoderL.setDistancePerPulse(-2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution); m_encoderR.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution); m_encoderL.reset(); @@ -59,11 +59,17 @@ public class Drivetrain extends SubsystemBase // speed - linear velocity in m/s // rot - angular velocity in rad/s - public void drive(double speed, double rot) + public void arcade(double speed, double rot) { speed(m_kinematics.toWheelSpeeds(new ChassisSpeeds(speed, 0.0, rot))); } + // in m/s + public void tank(double left, double right) + { + speed(new DifferentialDriveWheelSpeeds(left, right)); + } + public void speed(DifferentialDriveWheelSpeeds speeds) { m_motorL_leader.setVoltage( @@ -77,6 +83,24 @@ 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(); + } + + public void reset() + { + m_gyro.reset(); + m_encoderL.reset(); + m_encoderR.reset(); + m_odometry = new DifferentialDriveOdometry( + m_gyro.toRotation2d(), + Constants.encoderL.getDistance(), + Constants.encoderR.getDistance()); + } + @Override public void periodic() { |