mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpilib] Add RamseteController examples (#2553)
This is different from the RamseteCommand examples in that they don't use the command-based library.
This commit is contained in:
Prateek Machiraju
committed by
GitHub
GitHub
parent
a3881bb452
commit
a1d2d40ad3
@@ -551,5 +551,16 @@
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "RamseteController",
|
||||
"description": "An example robot demonstrating the use of RamseteController.",
|
||||
"tags": [
|
||||
"RamseteController"
|
||||
],
|
||||
"foldername": "ramsetecontroller",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
}
|
||||
]
|
||||
|
||||
@@ -0,0 +1,131 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.ramsetecontroller;
|
||||
|
||||
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.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
|
||||
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.DifferentialDriveOdometry;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
private final DifferentialDriveOdometry m_odometry;
|
||||
|
||||
// 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();
|
||||
|
||||
m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d());
|
||||
}
|
||||
|
||||
/**
|
||||
* 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_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
|
||||
m_rightEncoder.getDistance());
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the field-relative position to a specific location.
|
||||
* @param pose The position to reset to.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the pose of the robot.
|
||||
* @return The pose of the robot.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPoseMeters();
|
||||
}
|
||||
}
|
||||
@@ -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.ramsetecontroller;
|
||||
|
||||
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,102 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.ramsetecontroller;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.SlewRateLimiter;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.controller.RamseteController;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.trajectory.Trajectory;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
|
||||
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);
|
||||
|
||||
// An example trajectory to follow during the autonomous period.
|
||||
private Trajectory m_trajectory;
|
||||
|
||||
// The Ramsete Controller to follow the trajectory.
|
||||
private RamseteController m_ramseteController;
|
||||
|
||||
// The timer to use during the autonomous period.
|
||||
private Timer m_timer;
|
||||
|
||||
@Override
|
||||
public void robotInit() {
|
||||
// Create the trajectory to follow in autonomous. It is best to initialize
|
||||
// trajectories here to avoid wasting time in autonomous.
|
||||
m_trajectory = TrajectoryGenerator.generateTrajectory(
|
||||
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
|
||||
List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
|
||||
new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
|
||||
new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0))
|
||||
);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
// Initialize the timer.
|
||||
m_timer = new Timer();
|
||||
m_timer.start();
|
||||
|
||||
// Reset the drivetrain's odometry to the starting pose of the trajectory.
|
||||
m_drive.resetOdometry(m_trajectory.getInitialPose());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void autonomousPeriodic() {
|
||||
// Update odometry.
|
||||
m_drive.updateOdometry();
|
||||
|
||||
if (m_timer.get() < m_trajectory.getTotalTimeSeconds()) {
|
||||
// Get the desired pose from the trajectory.
|
||||
var desiredPose = m_trajectory.sample(m_timer.get());
|
||||
|
||||
// Get the reference chassis speeds from the Ramsete controller.
|
||||
var refChassisSpeeds = m_ramseteController.calculate(m_drive.getPose(), desiredPose);
|
||||
|
||||
// Set the linear and angular speeds.
|
||||
m_drive.drive(refChassisSpeeds.vxMetersPerSecond, refChassisSpeeds.omegaRadiansPerSecond);
|
||||
} else {
|
||||
m_drive.drive(0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user