From 558e37c412e2df24dd4b101194c0fabcc4187fbb Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Tue, 8 Dec 2020 01:32:42 -0500 Subject: [PATCH] [examples] Add simple differential drive simulation example (#2918) This provides an example of using the differential drive simulator without needing to use the command-based library. --- .../cpp/Drivetrain.cpp | 65 ++++++++ .../cpp/Robot.cpp | 76 +++++++++ .../include/Drivetrain.h | 107 +++++++++++++ .../src/main/cpp/examples/examples.json | 18 +++ .../wpi/first/wpilibj/examples/examples.json | 17 +++ .../Drivetrain.java | 144 ++++++++++++++++++ .../Main.java | 29 ++++ .../Robot.java | 83 ++++++++++ 8 files changed, 539 insertions(+) create mode 100644 wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp new file mode 100644 index 0000000000..1a005898eb --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* 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" + +#include + +void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { + auto leftFeedforward = m_feedforward.Calculate(speeds.left); + auto rightFeedforward = m_feedforward.Calculate(speeds.right); + double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(), + speeds.left.to()); + 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_leftEncoder.Reset(); + m_rightEncoder.Reset(); + m_drivetrainSimulator.SetPose(pose); + m_odometry.ResetPosition(pose, m_gyro.GetRotation2d()); +} + +void Drivetrain::SimulationPeriodic() { + // To update our simulation, we set motor voltage inputs, update the + // simulation, and write the simulated positions and velocities to our + // simulated encoder and gyro. We negate the right side so that positive + // voltages make the right side move forward. + m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} * + frc::RobotController::GetInputVoltage(), + units::volt_t{-m_rightLeader.Get()} * + frc::RobotController::GetInputVoltage()); + m_drivetrainSimulator.Update(20_ms); + + m_leftEncoderSim.SetDistance( + m_drivetrainSimulator.GetLeftPosition().to()); + m_leftEncoderSim.SetRate( + m_drivetrainSimulator.GetLeftVelocity().to()); + m_rightEncoderSim.SetDistance( + m_drivetrainSimulator.GetRightPosition().to()); + m_rightEncoderSim.SetRate( + m_drivetrainSimulator.GetRightVelocity().to()); + m_gyroSim.SetAngle( + -m_drivetrainSimulator.GetHeading().Degrees().to()); + + m_fieldSim.SetRobotPose(m_odometry.GetPose()); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp new file mode 100644 index 0000000000..c205896370 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* 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 RobotInit() override { + m_trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + frc::Pose2d(2_m, 2_m, 0_rad), {}, frc::Pose2d(6_m, 4_m, 0_rad), + frc::TrajectoryConfig(2_mps, 2_mps_sq)); + } + + void AutonomousInit() override { + m_timer.Reset(); + m_timer.Start(); + m_drive.ResetOdometry(m_trajectory.InitialPose()); + } + + void AutonomousPeriodic() override { + auto elapsed = m_timer.Get(); + auto reference = m_trajectory.Sample(elapsed); + auto speeds = m_ramsete.Calculate(m_drive.GetPose(), reference); + m_drive.Drive(speeds.vx, speeds.omega); + + m_drive.UpdateOdometry(); + } + + 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. + auto rot = -m_rotLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kRightHand)) * + Drivetrain::kMaxAngularSpeed; + + m_drive.Drive(xSpeed, rot); + } + + void SimulationPeriodic() override { m_drive.SimulationPeriodic(); } + + 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; + frc::Trajectory m_trajectory; + frc::RamseteController m_ramsete; + frc2::Timer m_timer; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h new file mode 100644 index 0000000000..e01fd823d0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h @@ -0,0 +1,107 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#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(); + + m_rightGroup.SetInverted(true); + + frc::SmartDashboard::PutData("Field", &m_fieldSim); + } + + 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 { return m_odometry.GetPose(); } + + void SimulationPeriodic(); + + 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{8.5, 0.0, 0.0}; + frc2::PIDController m_rightPIDController{8.5, 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}; + + // Simulation classes help us simulate our robot + frc::sim::AnalogGyroSim m_gyroSim{m_gyro}; + frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder}; + frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; + frc::Field2d m_fieldSim; + frc::LinearSystem<2, 2, 2> m_drivetrainSystem = + frc::LinearSystemId::IdentifyDrivetrainSystem( + 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_rad_per_s, + 0.3_V / 1_rad_per_s_sq); + frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ + m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 138843a64a..50fcb77939 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -676,6 +676,24 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "SimpleDifferentialDriveSimulation", + "description": "An example of a minimal drivetrain simulation project without the command-based library.", + "tags": [ + "Differential Drive", + "State space", + "Digital", + "Sensors", + "Simulation", + "Physics", + "Drivetrain", + "Field2d" + ], + "foldername": "SimpleDifferentialDriveSimulation", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "StateSpaceDriveSimulation", "description": "Demonstrates the use of physics simulation with a differential drivetrain and the Field2d class.", 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 0b714b0c05..f188f103ea 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 @@ -632,6 +632,23 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "SimpleDifferentialDriveSimulation", + "description": "An example of a minimal drivetrain simulation project without the command-based library.", + "tags": [ + "Drivetrain", + "State space", + "Digital", + "Sensors", + "Actuators", + "Joystick", + "Simulation" + ], + "foldername": "simpledifferentialdrivesimulation", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "StateSpaceDriveSimulation", "description": "An example of drivetrain simulation in combination with a RAMSETE path following controller and the Field2d class.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java new file mode 100644 index 0000000000..f3ddcf0216 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -0,0 +1,144 @@ +/*----------------------------------------------------------------------------*/ +/* 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.simpledifferentialdrivesimulation; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.RobotController; +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; +import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpiutil.math.numbers.N2; + +@SuppressWarnings("PMD.TooManyFields") +public class Drivetrain { + // 3 meters per second. + public static final double kMaxSpeed = 3.0; + // 1/2 rotation per second. + public static final double kMaxAngularSpeed = Math.PI; + + private static final double kTrackWidth = 0.381 * 2; + private static final double kWheelRadius = 0.0508; + private static final int kEncoderResolution = -4096; + + private final PWMVictorSPX m_leftLeader = new PWMVictorSPX(1); + private final PWMVictorSPX m_leftFollower = new PWMVictorSPX(2); + private final PWMVictorSPX m_rightLeader = new PWMVictorSPX(3); + private final PWMVictorSPX m_rightFollower = new PWMVictorSPX(4); + + private final SpeedControllerGroup m_leftGroup + = new SpeedControllerGroup(m_leftLeader, m_leftFollower); + private final SpeedControllerGroup m_rightGroup + = new SpeedControllerGroup(m_rightLeader, m_rightFollower); + + private final Encoder m_leftEncoder = new Encoder(0, 1); + private final Encoder m_rightEncoder = new Encoder(2, 3); + + private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0); + private final PIDController m_rightPIDController = new PIDController(8.5, 0, 0); + + private final AnalogGyro m_gyro = new AnalogGyro(0); + + private final DifferentialDriveKinematics m_kinematics + = new DifferentialDriveKinematics(kTrackWidth); + private final DifferentialDriveOdometry m_odometry + = new DifferentialDriveOdometry(m_gyro.getRotation2d()); + + // Gains are for example purposes only - must be determined for your own + // robot! + private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); + + // Simulation classes help us simulate our robot + private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro); + private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder); + private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); + private final Field2d m_fieldSim = new Field2d(); + private final LinearSystem m_drivetrainSystem + = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + private final DifferentialDrivetrainSim m_drivetrainSimulator + = new DifferentialDrivetrainSim(m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, + kWheelRadius, null); + + public Drivetrain() { + // 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_rightGroup.setInverted(true); + SmartDashboard.putData("Field", m_fieldSim); + } + + public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { + var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); + double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), + speeds.leftMetersPerSecond); + double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), + speeds.rightMetersPerSecond); + + m_leftGroup.setVoltage(leftOutput + leftFeedforward); + m_rightGroup.setVoltage(rightOutput + rightFeedforward); + } + + public void drive(double xSpeed, double rot) { + setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot))); + } + + public void updateOdometry() { + m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(), + m_rightEncoder.getDistance()); + } + + public void resetOdometry(Pose2d pose) { + m_leftEncoder.reset(); + m_rightEncoder.reset(); + m_drivetrainSimulator.setPose(pose); + m_odometry.resetPosition(pose, m_gyro.getRotation2d()); + } + + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } + + public void simulationPeriodic() { + // To update our simulation, we set motor voltage inputs, update the + // simulation, and write the simulated positions and velocities to our + // simulated encoder and gyro. We negate the right side so that positive + // voltages make the right side move forward. + m_drivetrainSimulator.setInputs(m_leftLeader.get() * RobotController.getInputVoltage(), + -m_rightLeader.get() * RobotController.getInputVoltage()); + m_drivetrainSimulator.update(0.02); + + m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); + m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); + m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); + m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); + m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); + + m_fieldSim.setRobotPose(m_odometry.getPoseMeters()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java new file mode 100644 index 0000000000..e6b34ad2ad --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* 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.simpledifferentialdrivesimulation; + +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/simpledifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java new file mode 100644 index 0000000000..f218fcd5b2 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java @@ -0,0 +1,83 @@ +/*----------------------------------------------------------------------------*/ +/* 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.simpledifferentialdrivesimulation; + +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.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; + +public class Robot extends TimedRobot { + private final XboxController m_controller = new XboxController(0); + + // 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); + + private final Drivetrain m_drive = new Drivetrain(); + private final RamseteController m_ramsete = new RamseteController(); + private final Timer m_timer = new Timer(); + private Trajectory m_trajectory; + + @Override + public void robotInit() { + m_trajectory = TrajectoryGenerator.generateTrajectory( + new Pose2d(2, 2, new Rotation2d()), List.of(), new Pose2d(6, 4, new Rotation2d()), + new TrajectoryConfig(2, 2) + ); + } + + @Override + public void autonomousInit() { + m_timer.reset(); + m_timer.start(); + m_drive.resetOdometry(m_trajectory.getInitialPose()); + } + + @Override + public void autonomousPeriodic() { + double elapsed = m_timer.get(); + Trajectory.State reference = m_trajectory.sample(elapsed); + ChassisSpeeds speeds = m_ramsete.calculate(m_drive.getPose(), reference); + m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); + m_drive.updateOdometry(); + } + + @Override + @SuppressWarnings("LocalVariableName") + public void teleopPeriodic() { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + double 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. + double rot = -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) + * Drivetrain.kMaxAngularSpeed; + m_drive.drive(xSpeed, rot); + } + + @Override + public void simulationPeriodic() { + m_drive.simulationPeriodic(); + } +}