Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Remove encoder velocities methods in DifferentialDriveOdometry (#2147)

Browse Source 
It doesn't make sense to continue to provide a less accurate method of performing odometry
when a more accurate method using distances exists.

This also removes the need to pass DifferentialDriveKinematics to the constructor.
This commit is contained in:
Prateek Machiraju
2019-12-01 02:10:29 -05:00
committed by Peter Johnson
parent b8c1024261
commit 5b73c17f25
10 changed files with 41 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
View File

@@ -51,8 +51,7 @@ public class Drivetrain {
private final DifferentialDriveKinematics m_kinematics
= new DifferentialDriveKinematics(kTrackWidth);
private final DifferentialDriveOdometry m_odometry
= new DifferentialDriveOdometry(m_kinematics, getAngle());
private final DifferentialDriveOdometry m_odometry;
/**
* Constructs a differential drive object.
@@ -66,6 +65,11 @@ public class Drivetrain {
// resolution.
m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_leftEncoder.reset();
m_rightEncoder.reset();
m_odometry = new DifferentialDriveOdometry(getAngle());
}
/**
@@ -78,15 +82,6 @@ public class Drivetrain {
return Rotation2d.fromDegrees(-m_gyro.getAngle());
}
/**
* Returns the current wheel speeds.
*
* @return The current wheel speeds.
*/
public DifferentialDriveWheelSpeeds getCurrentSpeeds() {
return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
}
/**
* Sets the desired wheel speeds.
*
@@ -117,6 +112,6 @@ public class Drivetrain {
* Updates the field-relative position.
*/
public void updateOdometry() {
m_odometry.update(getAngle(), getCurrentSpeeds());
m_odometry.update(getAngle(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
}

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
View File

@@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kGyroReversed;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderPorts;
@@ -57,8 +56,7 @@ public class DriveSubsystem extends SubsystemBase {
private final Gyro m_gyro = new ADXRS450_Gyro();
// Odometry class for tracking robot pose
DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics,
Rotation2d.fromDegrees(getHeading()));
private final DifferentialDriveOdometry m_odometry;
/**
* Creates a new DriveSubsystem.
@@ -67,12 +65,16 @@ public class DriveSubsystem extends SubsystemBase {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
resetEncoders();
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
}
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(Rotation2d.fromDegrees(getHeading()), getWheelSpeeds());
m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(),
m_rightEncoder.getDistance());
}
/**
@@ -99,6 +101,7 @@ public class DriveSubsystem extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
resetEncoders();
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
}