From a1d2d40ad38b32027b9752da76ce4914eb8ef74c Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sat, 4 Jul 2020 00:59:42 -0400 Subject: [PATCH] [wpilib] Add RamseteController examples (#2553) This is different from the RamseteCommand examples in that they don't use the command-based library. --- .../RamseteController/cpp/Drivetrain.cpp | 37 +++++ .../examples/RamseteController/cpp/Robot.cpp | 89 ++++++++++++ .../RamseteController/include/Drivetrain.h | 80 +++++++++++ .../src/main/cpp/examples/examples.json | 10 ++ .../wpi/first/wpilibj/examples/examples.json | 11 ++ .../ramsetecontroller/Drivetrain.java | 131 ++++++++++++++++++ .../examples/ramsetecontroller/Main.java | 29 ++++ .../examples/ramsetecontroller/Robot.java | 102 ++++++++++++++ 8 files changed, 489 insertions(+) create mode 100644 wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp new file mode 100644 index 0000000000..f52ddec1b1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "Drivetrain.h" + +void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { + const auto leftFeedforward = m_feedforward.Calculate(speeds.left); + const auto rightFeedforward = m_feedforward.Calculate(speeds.right); + const double leftOutput = m_leftPIDController.Calculate( + m_leftEncoder.GetRate(), speeds.left.to()); + const double rightOutput = m_rightPIDController.Calculate( + m_rightEncoder.GetRate(), speeds.right.to()); + + m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); + m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); +} + +void Drivetrain::Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot) { + SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot})); +} + +void Drivetrain::UpdateOdometry() { + m_odometry.Update(m_gyro.GetRotation2d(), + units::meter_t(m_leftEncoder.GetDistance()), + units::meter_t(m_rightEncoder.GetDistance())); +} + +void Drivetrain::ResetOdometry(const frc::Pose2d& pose) { + m_odometry.ResetPosition(pose, m_gyro.GetRotation2d()); +} + +frc::Pose2d Drivetrain::GetPose() const { return m_odometry.GetPose(); } diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp new file mode 100644 index 0000000000..61e5d857e5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp @@ -0,0 +1,89 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include + +#include "Drivetrain.h" + +class Robot : public frc::TimedRobot { + public: + void AutonomousInit() override { + // Start the timer. + m_timer.Start(); + + // Reset the drivetrain's odometry to the starting pose of the trajectory. + m_drive.ResetOdometry(m_trajectory.InitialPose()); + } + + void AutonomousPeriodic() override { + // Update odometry. + m_drive.UpdateOdometry(); + + if (m_timer.Get() < m_trajectory.TotalTime()) { + // Get the desired pose from the trajectory. + auto desiredPose = m_trajectory.Sample(m_timer.Get()); + + // Get the reference chassis speeds from the Ramsete Controller. + auto refChassisSpeeds = + m_ramseteController.Calculate(m_drive.GetPose(), desiredPose); + + // Set the linear and angular speeds. + m_drive.Drive(refChassisSpeeds.vx, refChassisSpeeds.omega); + } else { + m_drive.Drive(0_mps, 0_rad_per_s); + } + } + + void TeleopPeriodic() override { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + const auto xSpeed = -m_speedLimiter.Calculate( + m_controller.GetY(frc::GenericHID::kLeftHand)) * + 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. + const auto rot = -m_rotLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kRightHand)) * + Drivetrain::kMaxAngularSpeed; + + m_drive.Drive(xSpeed, rot); + } + + private: + frc::XboxController m_controller{0}; + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + frc::SlewRateLimiter m_speedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_rotLimiter{3 / 1_s}; + + Drivetrain m_drive; + + // An example trajectory to follow. + frc::Trajectory m_trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + frc::Pose2d(0_m, 0_m, 0_rad), + {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)}, + frc::Pose2d(3_m, 0_m, 0_rad), frc::TrajectoryConfig(3_fps, 3_fps_sq)); + + // The Ramsete Controller to follow the trajectory. + frc::RamseteController m_ramseteController; + + // The timer to use during the autonomous period. + frc2::Timer m_timer; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h new file mode 100644 index 0000000000..a7070145b9 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h @@ -0,0 +1,80 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Represents a differential drive style drivetrain. + */ +class Drivetrain { + 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 * wpi::math::pi * kWheelRadius / + kEncoderResolution); + m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + kEncoderResolution); + + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); + } + + static constexpr units::meters_per_second_t kMaxSpeed = + 3.0_mps; // 3 meters per second + static constexpr units::radians_per_second_t kMaxAngularSpeed{ + wpi::math::pi}; // 1/2 rotation per second + + void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); + void Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot); + void UpdateOdometry(); + void ResetOdometry(const frc::Pose2d& pose); + frc::Pose2d GetPose() const; + + private: + static constexpr units::meter_t kTrackWidth = 0.381_m * 2; + static constexpr double kWheelRadius = 0.0508; // meters + static constexpr int kEncoderResolution = 4096; + + frc::PWMVictorSPX m_leftLeader{1}; + frc::PWMVictorSPX m_leftFollower{2}; + frc::PWMVictorSPX m_rightLeader{3}; + frc::PWMVictorSPX m_rightFollower{4}; + + frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower}; + frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower}; + + frc::Encoder m_leftEncoder{0, 1}; + frc::Encoder m_rightEncoder{2, 3}; + + frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0}; + + frc::AnalogGyro m_gyro{0}; + + frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; + frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()}; + + // Gains are for example purposes only - must be determined for your own + // robot! + frc::SimpleMotorFeedforward m_feedforward{1_V, 3_V / 1_mps}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 2a0dd8b85e..e2b74d03b5 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -532,5 +532,15 @@ "foldername": "DriveDistanceOffboard", "gradlebase": "cpp", "commandversion": 2 + }, + { + "name": "RamseteController", + "description": "An example robot demonstrating the use of RamseteController.", + "tags": [ + "RamseteController" + ], + "foldername": "RamseteController", + "gradlebase": "cpp", + "commandversion": 2 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 149e3032e8..fd5a3a1a23 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -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 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java new file mode 100644 index 0000000000..388b32d4f7 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java @@ -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(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java new file mode 100644 index 0000000000..bd07c2e811 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java @@ -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. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java new file mode 100644 index 0000000000..6a95a24a83 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java @@ -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); + } +}