mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpilib] Add pose estimators (#2867)
Pose and state estimators can filter latency-compensated global measurements and fuse them with state-space drivetrain model information to estimate robot position. They are drop-in replacements for the existing odometry classes. Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com> Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com> Co-authored-by: Claudius Tewari <cttewari@gmail.com> Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Declan Freeman-Gleason
committed by
GitHub
GitHub
parent
3413bfc06a
commit
bc8f338771
@@ -0,0 +1,133 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.PWMVictorSPX;
|
||||
import edu.wpi.first.wpilibj.SpeedController;
|
||||
import edu.wpi.first.wpilibj.SpeedControllerGroup;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.wpilibj.estimator.DifferentialDrivePoseEstimator;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
|
||||
import edu.wpi.first.wpilibj.examples.swervesdriveposeestimator.ExampleGlobalMeasurementSensor;
|
||||
|
||||
/**
|
||||
* Represents a differential drive style drivetrain.
|
||||
*/
|
||||
public class Drivetrain {
|
||||
public static final double kMaxSpeed = 3.0; // meters per second
|
||||
public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
|
||||
|
||||
private static final double kTrackWidth = 0.381 * 2; // meters
|
||||
private static final double kWheelRadius = 0.0508; // meters
|
||||
private static final int kEncoderResolution = 4096;
|
||||
|
||||
private final SpeedController m_leftLeader = new PWMVictorSPX(1);
|
||||
private final SpeedController m_leftFollower = new PWMVictorSPX(2);
|
||||
private final SpeedController m_rightLeader = new PWMVictorSPX(3);
|
||||
private final SpeedController m_rightFollower = new PWMVictorSPX(4);
|
||||
|
||||
private final Encoder m_leftEncoder = new Encoder(0, 1);
|
||||
private final Encoder m_rightEncoder = new Encoder(2, 3);
|
||||
|
||||
private final SpeedControllerGroup m_leftGroup
|
||||
= new SpeedControllerGroup(m_leftLeader, m_leftFollower);
|
||||
private final SpeedControllerGroup m_rightGroup
|
||||
= new SpeedControllerGroup(m_rightLeader, m_rightFollower);
|
||||
|
||||
private final AnalogGyro m_gyro = new AnalogGyro(0);
|
||||
|
||||
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
|
||||
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
|
||||
|
||||
private final DifferentialDriveKinematics m_kinematics
|
||||
= new DifferentialDriveKinematics(kTrackWidth);
|
||||
|
||||
/* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
|
||||
numbers used below are robot specific, and should be tuned. */
|
||||
private final DifferentialDrivePoseEstimator m_poseEstimator = new DifferentialDrivePoseEstimator(
|
||||
m_gyro.getRotation2d(),
|
||||
new Pose2d(),
|
||||
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.01, 0.01),
|
||||
VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(1)),
|
||||
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
|
||||
|
||||
// Gains are for example purposes only - must be determined for your own robot!
|
||||
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
|
||||
|
||||
/**
|
||||
* Constructs a differential drive object.
|
||||
* Sets the encoder distance per pulse and resets the gyro.
|
||||
*/
|
||||
public Drivetrain() {
|
||||
m_gyro.reset();
|
||||
|
||||
// Set the distance per pulse for the drive encoders. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
|
||||
m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
|
||||
|
||||
m_leftEncoder.reset();
|
||||
m_rightEncoder.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the desired wheel speeds.
|
||||
*
|
||||
* @param speeds The desired wheel speeds.
|
||||
*/
|
||||
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
|
||||
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
|
||||
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
|
||||
|
||||
final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
|
||||
speeds.leftMetersPerSecond);
|
||||
final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
|
||||
speeds.rightMetersPerSecond);
|
||||
m_leftGroup.setVoltage(leftOutput + leftFeedforward);
|
||||
m_rightGroup.setVoltage(rightOutput + rightFeedforward);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drives the robot with the given linear velocity and angular velocity.
|
||||
*
|
||||
* @param xSpeed Linear velocity in m/s.
|
||||
* @param rot Angular velocity in rad/s.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void drive(double xSpeed, double rot) {
|
||||
var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
|
||||
setSpeeds(wheelSpeeds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the field-relative position.
|
||||
*/
|
||||
public void updateOdometry() {
|
||||
m_poseEstimator.update(m_gyro.getRotation2d(),
|
||||
new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()),
|
||||
m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
m_poseEstimator.addVisionMeasurement(
|
||||
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
|
||||
m_poseEstimator.getEstimatedPosition()),
|
||||
Timer.getFPGATimestamp() - 0.3);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
* Unless you know what you are doing, do not modify this file except to
|
||||
* change the parameter class to the startRobot call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.SlewRateLimiter;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private final XboxController m_controller = new XboxController(0);
|
||||
private final Drivetrain m_drive = new Drivetrain();
|
||||
|
||||
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
|
||||
private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
|
||||
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
|
||||
|
||||
|
||||
@Override
|
||||
public void autonomousPeriodic() {
|
||||
teleopPeriodic();
|
||||
m_drive.updateOdometry();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
final var xSpeed =
|
||||
-m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
|
||||
* Drivetrain.kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
final var rot =
|
||||
-m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
|
||||
* Drivetrain.kMaxAngularSpeed;
|
||||
|
||||
m_drive.drive(xSpeed, rot);
|
||||
}
|
||||
}
|
||||
@@ -680,5 +680,56 @@
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "DifferentialDrivePoseEstimator",
|
||||
"description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.",
|
||||
"tags": [
|
||||
"Drivetrain",
|
||||
"State space",
|
||||
"Vision",
|
||||
"Filter",
|
||||
"Odometry",
|
||||
"Pose",
|
||||
"Differential drive"
|
||||
],
|
||||
"foldername": "differentialdriveposeestimator",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "MecanumDrivePoseEstimator",
|
||||
"description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum drive odometry.",
|
||||
"tags": [
|
||||
"Drivetrain",
|
||||
"State space",
|
||||
"Vision",
|
||||
"Filter",
|
||||
"Odometry",
|
||||
"Pose",
|
||||
"Mecanum"
|
||||
],
|
||||
"foldername": "mecanumdriveposeestimator",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "SwerveDrivePoseEstimator",
|
||||
"description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for swerve drive odometry.",
|
||||
"tags": [
|
||||
"Drivetrain",
|
||||
"State space",
|
||||
"Vision",
|
||||
"Filter",
|
||||
"Odometry",
|
||||
"Pose",
|
||||
"Swerve"
|
||||
],
|
||||
"foldername": "swervesdriveposeestimator",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
}
|
||||
]
|
||||
|
||||
@@ -0,0 +1,158 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.PWMVictorSPX;
|
||||
import edu.wpi.first.wpilibj.SpeedController;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.wpilibj.estimator.MecanumDrivePoseEstimator;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
|
||||
import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
|
||||
import edu.wpi.first.wpilibj.examples.swervesdriveposeestimator.ExampleGlobalMeasurementSensor;
|
||||
|
||||
/**
|
||||
* Represents a mecanum drive style drivetrain.
|
||||
*/
|
||||
@SuppressWarnings("PMD.TooManyFields")
|
||||
public class Drivetrain {
|
||||
public static final double kMaxSpeed = 3.0; // 3 meters per second
|
||||
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
|
||||
|
||||
private final SpeedController m_frontLeftMotor = new PWMVictorSPX(1);
|
||||
private final SpeedController m_frontRightMotor = new PWMVictorSPX(2);
|
||||
private final SpeedController m_backLeftMotor = new PWMVictorSPX(3);
|
||||
private final SpeedController m_backRightMotor = new PWMVictorSPX(4);
|
||||
|
||||
private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
|
||||
private final Encoder m_frontRightEncoder = new Encoder(2, 3);
|
||||
private final Encoder m_backLeftEncoder = new Encoder(4, 5);
|
||||
private final Encoder m_backRightEncoder = new Encoder(6, 7);
|
||||
|
||||
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
|
||||
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
|
||||
private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
|
||||
private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
|
||||
|
||||
private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0);
|
||||
private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0);
|
||||
private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
|
||||
private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
|
||||
|
||||
private final AnalogGyro m_gyro = new AnalogGyro(0);
|
||||
|
||||
private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
|
||||
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
|
||||
);
|
||||
|
||||
/* Here we use MecanumDrivePoseEstimator so that we can fuse odometry readings. The numbers used
|
||||
below are robot specific, and should be tuned. */
|
||||
private final MecanumDrivePoseEstimator m_poseEstimator = new MecanumDrivePoseEstimator(
|
||||
m_gyro.getRotation2d(),
|
||||
new Pose2d(),
|
||||
m_kinematics,
|
||||
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
|
||||
VecBuilder.fill(Units.degreesToRadians(0.01)),
|
||||
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
|
||||
|
||||
// Gains are for example purposes only - must be determined for your own robot!
|
||||
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
|
||||
|
||||
/**
|
||||
* Constructs a MecanumDrive and resets the gyro.
|
||||
*/
|
||||
public Drivetrain() {
|
||||
m_gyro.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current state of the drivetrain.
|
||||
*
|
||||
* @return The current state of the drivetrain.
|
||||
*/
|
||||
public MecanumDriveWheelSpeeds getCurrentState() {
|
||||
return new MecanumDriveWheelSpeeds(
|
||||
m_frontLeftEncoder.getRate(),
|
||||
m_frontRightEncoder.getRate(),
|
||||
m_backLeftEncoder.getRate(),
|
||||
m_backRightEncoder.getRate()
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the desired speeds for each wheel.
|
||||
*
|
||||
* @param speeds The desired wheel speeds.
|
||||
*/
|
||||
public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
|
||||
final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
|
||||
final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
|
||||
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
|
||||
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
|
||||
|
||||
final double frontLeftOutput = m_frontLeftPIDController.calculate(
|
||||
m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond
|
||||
);
|
||||
final double frontRightOutput = m_frontRightPIDController.calculate(
|
||||
m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond
|
||||
);
|
||||
final double backLeftOutput = m_backLeftPIDController.calculate(
|
||||
m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond
|
||||
);
|
||||
final double backRightOutput = m_backRightPIDController.calculate(
|
||||
m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond
|
||||
);
|
||||
|
||||
m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
|
||||
m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
|
||||
m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
|
||||
m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
|
||||
}
|
||||
|
||||
/**
|
||||
* Method to drive the robot using joystick info.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction (forward).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(
|
||||
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
|
||||
xSpeed, ySpeed, rot, m_gyro.getRotation2d()
|
||||
) : new ChassisSpeeds(xSpeed, ySpeed, rot)
|
||||
);
|
||||
mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
|
||||
setSpeeds(mecanumDriveWheelSpeeds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the field relative position of the robot.
|
||||
*/
|
||||
public void updateOdometry() {
|
||||
m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState());
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
m_poseEstimator.addVisionMeasurement(
|
||||
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
|
||||
m_poseEstimator.getEstimatedPosition()),
|
||||
Timer.getFPGATimestamp() - 0.3);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
|
||||
/**
|
||||
* This dummy class represents a global measurement sensor, such as a computer vision solution.
|
||||
*/
|
||||
public final class ExampleGlobalMeasurementSensor {
|
||||
private ExampleGlobalMeasurementSensor() {
|
||||
// Utility class
|
||||
}
|
||||
|
||||
/**
|
||||
* Get a "noisy" fake global pose reading.
|
||||
*
|
||||
* @param estimatedRobotPose The robot pose.
|
||||
*/
|
||||
public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
|
||||
var rand = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5,
|
||||
Units.degreesToRadians(30)));
|
||||
return new Pose2d(estimatedRobotPose.getX() + rand.get(0, 0),
|
||||
estimatedRobotPose.getY() + rand.get(1, 0),
|
||||
estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
* Unless you know what you are doing, do not modify this file except to
|
||||
* change the parameter class to the startRobot call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.SlewRateLimiter;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private final XboxController m_controller = new XboxController(0);
|
||||
private final Drivetrain m_mecanum = new Drivetrain();
|
||||
|
||||
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
|
||||
private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
|
||||
private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
|
||||
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
|
||||
|
||||
@Override
|
||||
public void autonomousPeriodic() {
|
||||
driveWithJoystick(false);
|
||||
m_mecanum.updateOdometry();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
driveWithJoystick(true);
|
||||
}
|
||||
|
||||
private void driveWithJoystick(boolean fieldRelative) {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
final var xSpeed =
|
||||
-m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
|
||||
* Drivetrain.kMaxSpeed;
|
||||
|
||||
// Get the y speed or sideways/strafe speed. We are inverting this because
|
||||
// we want a positive value when we pull to the left. Xbox controllers
|
||||
// return positive values when you pull to the right by default.
|
||||
final var ySpeed =
|
||||
-m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
|
||||
* Drivetrain.kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
final var rot =
|
||||
-m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
|
||||
* Drivetrain.kMaxAngularSpeed;
|
||||
|
||||
m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.estimator.SwerveDrivePoseEstimator;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
|
||||
/**
|
||||
* Represents a swerve drive style drivetrain.
|
||||
*/
|
||||
public class Drivetrain {
|
||||
public static final double kMaxSpeed = 3.0; // 3 meters per second
|
||||
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
|
||||
|
||||
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
|
||||
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
|
||||
private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
|
||||
private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
|
||||
|
||||
private final SwerveModule m_frontLeft = new SwerveModule(1, 2);
|
||||
private final SwerveModule m_frontRight = new SwerveModule(3, 4);
|
||||
private final SwerveModule m_backLeft = new SwerveModule(5, 6);
|
||||
private final SwerveModule m_backRight = new SwerveModule(7, 8);
|
||||
|
||||
private final AnalogGyro m_gyro = new AnalogGyro(0);
|
||||
|
||||
private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
|
||||
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
|
||||
);
|
||||
|
||||
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
|
||||
below are robot specific, and should be tuned. */
|
||||
private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(
|
||||
m_gyro.getRotation2d(),
|
||||
new Pose2d(),
|
||||
m_kinematics,
|
||||
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
|
||||
VecBuilder.fill(Units.degreesToRadians(0.01)),
|
||||
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))
|
||||
);
|
||||
|
||||
public Drivetrain() {
|
||||
m_gyro.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Method to drive the robot using joystick info.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction (forward).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
var swerveModuleStates = m_kinematics.toSwerveModuleStates(
|
||||
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
|
||||
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeed, ySpeed, rot)
|
||||
);
|
||||
SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
|
||||
m_frontLeft.setDesiredState(swerveModuleStates[0]);
|
||||
m_frontRight.setDesiredState(swerveModuleStates[1]);
|
||||
m_backLeft.setDesiredState(swerveModuleStates[2]);
|
||||
m_backRight.setDesiredState(swerveModuleStates[3]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the field relative position of the robot.
|
||||
*/
|
||||
public void updateOdometry() {
|
||||
m_poseEstimator.update(
|
||||
m_gyro.getRotation2d(),
|
||||
m_frontLeft.getState(),
|
||||
m_frontRight.getState(),
|
||||
m_backLeft.getState(),
|
||||
m_backRight.getState()
|
||||
);
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
m_poseEstimator.addVisionMeasurement(
|
||||
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
|
||||
m_poseEstimator.getEstimatedPosition()),
|
||||
Timer.getFPGATimestamp() - 0.3);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
|
||||
/**
|
||||
* This dummy class represents a global measurement sensor, such as a computer vision solution.
|
||||
*/
|
||||
public final class ExampleGlobalMeasurementSensor {
|
||||
private ExampleGlobalMeasurementSensor() {
|
||||
// Utility class
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Get a "noisy" fake global pose reading.
|
||||
*
|
||||
* @param estimatedRobotPose The robot pose.
|
||||
*/
|
||||
public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
|
||||
var rand = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5,
|
||||
Units.degreesToRadians(30)));
|
||||
return new Pose2d(estimatedRobotPose.getX() + rand.get(0, 0),
|
||||
estimatedRobotPose.getY() + rand.get(1, 0),
|
||||
estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
* Unless you know what you are doing, do not modify this file except to
|
||||
* change the parameter class to the startRobot call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.SlewRateLimiter;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private final XboxController m_controller = new XboxController(0);
|
||||
private final Drivetrain m_swerve = new Drivetrain();
|
||||
|
||||
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
|
||||
private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
|
||||
private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
|
||||
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
|
||||
|
||||
@Override
|
||||
public void autonomousPeriodic() {
|
||||
driveWithJoystick(false);
|
||||
m_swerve.updateOdometry();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
driveWithJoystick(true);
|
||||
}
|
||||
|
||||
private void driveWithJoystick(boolean fieldRelative) {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
final var xSpeed =
|
||||
-m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
|
||||
* edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
|
||||
|
||||
// Get the y speed or sideways/strafe speed. We are inverting this because
|
||||
// we want a positive value when we pull to the left. Xbox controllers
|
||||
// return positive values when you pull to the right by default.
|
||||
final var ySpeed =
|
||||
-m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
|
||||
* edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
final var rot =
|
||||
-m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
|
||||
* edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed;
|
||||
|
||||
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,101 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
|
||||
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.PWMVictorSPX;
|
||||
import edu.wpi.first.wpilibj.SpeedController;
|
||||
import edu.wpi.first.wpilibj.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
|
||||
|
||||
public class SwerveModule {
|
||||
private static final double kWheelRadius = 0.0508;
|
||||
private static final int kEncoderResolution = 4096;
|
||||
|
||||
private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
|
||||
private static final double kModuleMaxAngularAcceleration
|
||||
= 2 * Math.PI; // radians per second squared
|
||||
|
||||
private final SpeedController m_driveMotor;
|
||||
private final SpeedController m_turningMotor;
|
||||
|
||||
private final Encoder m_driveEncoder = new Encoder(0, 1);
|
||||
private final Encoder m_turningEncoder = new Encoder(2, 3);
|
||||
|
||||
private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
|
||||
|
||||
private final ProfiledPIDController m_turningPIDController
|
||||
= new ProfiledPIDController(1, 0, 0,
|
||||
new TrapezoidProfile.Constraints(kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
|
||||
|
||||
// Gains are for example purposes only - must be determined for your own robot!
|
||||
private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3);
|
||||
private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
|
||||
|
||||
/**
|
||||
* Constructs a SwerveModule.
|
||||
*
|
||||
* @param driveMotorChannel ID for the drive motor.
|
||||
* @param turningMotorChannel ID for the turning motor.
|
||||
*/
|
||||
public SwerveModule(int driveMotorChannel, int turningMotorChannel) {
|
||||
m_driveMotor = new PWMVictorSPX(driveMotorChannel);
|
||||
m_turningMotor = new PWMVictorSPX(turningMotorChannel);
|
||||
|
||||
// Set the distance per pulse for the drive encoder. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
|
||||
|
||||
// Set the distance (in this case, angle) per pulse for the turning encoder.
|
||||
// This is the the angle through an entire rotation (2 * wpi::math::pi)
|
||||
// divided by the encoder resolution.
|
||||
m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
|
||||
|
||||
// Limit the PID Controller's input range between -pi and pi and set the input
|
||||
// to be continuous.
|
||||
m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current state of the module.
|
||||
*
|
||||
* @return The current state of the module.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the desired state for the module.
|
||||
*
|
||||
* @param state Desired state with speed and angle.
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState state) {
|
||||
// Calculate the drive output from the drive PID controller.
|
||||
final double driveOutput = m_drivePIDController.calculate(
|
||||
m_driveEncoder.getRate(), state.speedMetersPerSecond);
|
||||
|
||||
final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
|
||||
|
||||
// Calculate the turning motor output from the turning PID controller.
|
||||
final double turnOutput = m_turningPIDController.calculate(
|
||||
m_turningEncoder.get(), state.angle.getRadians()
|
||||
);
|
||||
|
||||
final double turnFeedforward =
|
||||
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
|
||||
|
||||
m_driveMotor.setVoltage(driveOutput + driveFeedforward);
|
||||
m_turningMotor.setVoltage(turnOutput + turnFeedforward);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user