We’re trying to get going on pose estimation and path planning with our Romi. The issue we’re facing is that our reported pose angle is decreasing at about 3 degrees/second while the Romi is at rest. There is gyro z angle negative drift but it’s orders of magnitude lower. (maybe 0.03 degrees per second) I’m assuming it’s something wrong in the pose estimator update we’re running in periodic but I can’t see where. Here’s the drivetrain subsystem where all this is managed. Any pointers on this code or to an example to review would be appreciated - thanks!
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.DoubleArrayTopic;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.DoubleTopic;
import edu.wpi.first.networktables.IntegerArrayTopic;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.romi.RomiGyro;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.function.DoubleSupplier;
public class Drive extends SubsystemBase {
private static final double kCountsPerRevolution = 1440.0;
private static final double kWheelDiameterInch = 2.75591; // 70 mm
private static final double kWheelDiameterMeter = Units.inchesToMeters(kWheelDiameterInch);
private final DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(Units.inchesToMeters(5.5));
// The Romi has the left and right motors set to
// PWM channels 0 and 1 respectively
private final Spark m_leftMotor = new Spark(0);
private final Spark m_rightMotor = new Spark(1);
// The Romi has onboard encoders that are hardcoded
// to use DIO pins 4/5 and 6/7 for the left and right
private final Encoder m_leftEncoder = new Encoder(4, 5);
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
// Set up the RomiGyro
private final RomiGyro m_gyro = new RomiGyro();
// Set up the BuiltInAccelerometer
private final BuiltInAccelerometer m_accelerometer = new BuiltInAccelerometer();
Field2d m_fieldApproximation = new Field2d(); //may need to initialize pose
// declare here for methods access, init in constructor
DifferentialDrivePoseEstimator m_poseEstimator;
// NT data for testing or tracking purposes
private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
private final NetworkTable ntDriveTable = ntInstance.getTable("drive");
DoublePublisher gyro_x = ntDriveTable.getDoubleTopic("gyro_x").publish();
DoublePublisher gyro_y =ntDriveTable.getDoubleTopic("gyro_y").publish();
DoublePublisher gyro_z =ntDriveTable.getDoubleTopic("gyro_z").publish();
DoublePublisher left_dist =ntDriveTable.getDoubleTopic("left_dist").publish();
DoublePublisher right_dist =ntDriveTable.getDoubleTopic("right_dist").publish();
DoublePublisher field_x =ntDriveTable.getDoubleTopic("field_x").publish();
DoublePublisher field_y =ntDriveTable.getDoubleTopic("field_y").publish();
DoublePublisher field_angle =ntDriveTable.getDoubleTopic("field_angle").publish();
/** Creates a new Drive subsystem. */
public Drive() {
SendableRegistry.addChild(m_drive, m_leftMotor);
SendableRegistry.addChild(m_drive, m_rightMotor);
SendableRegistry.addChild(m_drive, m_gyro);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
m_rightMotor.setSafetyEnabled(false);
m_leftMotor.setSafetyEnabled(false);
m_drive.setSafetyEnabled(false);
// Use meters as unit for encoder distances since pose estimators require
m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterMeter) / kCountsPerRevolution);
m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterMeter) / kCountsPerRevolution);
// only direct calls to reset since once pose estimator created need to use public methods
m_leftEncoder.reset();
m_rightEncoder.reset();
m_gyro.reset();
/* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
numbers used below are robot specific, and should be tuned. */
m_poseEstimator = new DifferentialDrivePoseEstimator(
m_kinematics,
new Rotation2d(getGyroAngleZ()),
m_leftEncoder.getDistance(),
m_rightEncoder.getDistance(),
new Pose2d(),
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
SmartDashboard.putData("FieldEstimation", m_fieldApproximation);
}
/**
* Returns a command that drives the robot with arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
System.out.println("Returning arcade drive");
return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
.withName("arcadeDrive");
}
/**
* Returns a command that drives the robot forward a specified distance at a specified speed.
*
* @param distanceMeters The distance to drive forward in meters
* @param speed The fraction of max speed at which to drive
*/
public Command driveDistanceCommand(double distanceMeters, double speed) {
return runOnce(
() -> {
// Reset encoders at the start of the command
resetEncoders();
})
// Drive forward at specified speed
.andThen(run(() -> m_drive.arcadeDrive(speed, 0)))
// End command when we've traveled the specified distance
.until(
() ->
Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance())
>= distanceMeters)
// Stop the drive when the command ends
.finallyDo(interrupted -> m_drive.stopMotor());
}
public Command rotateDegreesCommand(double angleDegrees, double speed) {
return runOnce(
() -> {
// Reset gyro at the start of the command
resetGyro();
})
// Rotate at specified speed
.andThen(run(() -> m_drive.arcadeDrive(0, speed)))
// End command when we've rotated the specified angle
.until(
() ->
Math.abs(getGyroAngleZ()) >= angleDegrees)
// Stop the drive when the command ends
.finallyDo(interrupted -> m_drive.stopMotor());
}
public int getLeftEncoderCount() {
return m_leftEncoder.get();
}
public int getRightEncoderCount() {
return m_rightEncoder.get();
}
public double getLeftDistanceMeter() {
return m_leftEncoder.getDistance();
}
public double getRightDistanceMeter() {
return m_rightEncoder.getDistance();
}
public double getAverageDistanceMeter() {
return (getLeftDistanceMeter() + getRightDistanceMeter()) / 2.0;
}
/**
* The acceleration in the X-axis.
*
* @return The acceleration of the Romi along the X-axis in Gs
*/
public double getAccelX() {
return m_accelerometer.getX();
}
/**
* The acceleration in the Y-axis.
*
* @return The acceleration of the Romi along the Y-axis in Gs
*/
public double getAccelY() {
return m_accelerometer.getY();
}
/**
* The acceleration in the Z-axis.
*
* @return The acceleration of the Romi along the Z-axis in Gs
*/
public double getAccelZ() {
return m_accelerometer.getZ();
}
/**
* Current angle of the Romi around the X-axis.
*
* @return The current angle of the Romi in degrees
*/
public double getGyroAngleX() {
return m_gyro.getAngleX();
}
/**
* Current angle of the Romi around the Y-axis.
*
* @return The current angle of the Romi in degrees
*/
public double getGyroAngleY() {
return m_gyro.getAngleY();
}
/**
* Current angle of the Romi around the Z-axis.
*
* @return The current angle of the Romi in degrees
*/
public double getGyroAngleZ() {
return m_gyro.getAngleZ();
}
/** Reset the gyro. Update pose estimator*/
public void resetGyro() {
Pose2d curr_pose = m_poseEstimator.getEstimatedPosition();
m_gyro.reset();
m_poseEstimator.resetPosition(
new Rotation2d(getGyroAngleZ()), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), curr_pose);
}
//** Reset the encoders, Update pose estimator */
public void resetEncoders() {
Pose2d curr_pose = m_poseEstimator.getEstimatedPosition();
m_leftEncoder.reset();
m_rightEncoder.reset();
m_poseEstimator.resetPosition(
new Rotation2d(getGyroAngleZ()), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), curr_pose);
}
/** Updates the field-relative position. */
public void updateOdometry() {
m_poseEstimator.update(
new Rotation2d(getGyroAngleZ()), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
m_fieldApproximation.setRobotPose(m_poseEstimator.getEstimatedPosition());
// Publish cameraToObject transformation to networktables --this would normally be handled by
// the
// computer vision solution.
//publishCameraToObject(
// m_objectInField, m_robotToCamera, m_cameraToObjectEntry, m_drivetrainSimulator);
// Compute the robot's field-relative position exclusively from vision measurements.
//Pose3d visionMeasurement3d =
// objectToRobotPose(m_objectInField, m_robotToCamera, m_cameraToObjectEntry);
// Convert robot pose from Pose3d to Pose2d needed to apply vision measurements.
//Pose2d visionMeasurement2d = visionMeasurement3d.toPose2d();
// Apply vision measurements. For simulation purposes only, we don't input a latency delay -- on
// a real robot, this must be calculated based either on known latency or timestamps.
//m_poseEstimator.addVisionMeasurement(visionMeasurement2d, Timer.getFPGATimestamp());
gyro_x.set(getGyroAngleX());
gyro_y.set(getGyroAngleY());
gyro_z.set(getGyroAngleZ());
left_dist.set(getLeftDistanceMeter());
right_dist.set(getRightDistanceMeter());
field_x.set(m_poseEstimator.getEstimatedPosition().getTranslation().getX());
field_y.set(m_poseEstimator.getEstimatedPosition().getTranslation().getY());
field_angle.set(m_poseEstimator.getEstimatedPosition().getRotation().getDegrees());
}
@Override
public void periodic() {
// This method will be called once per scheduler run
updateOdometry();
}
}