diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp index 4d9c1ed699..c62f741e3c 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -8,13 +8,15 @@ #include "Drivetrain.h" void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { - const auto leftOutput = m_leftPIDController.Calculate( + 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 auto rightOutput = m_rightPIDController.Calculate( + const double rightOutput = m_rightPIDController.Calculate( m_rightEncoder.GetRate(), speeds.right.to()); - m_leftGroup.Set(leftOutput); - m_rightGroup.Set(rightOutput); + 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, diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp index b1e858ee47..22d1cf614d 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp @@ -1,10 +1,11 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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 @@ -20,14 +21,16 @@ class Robot : public frc::TimedRobot { 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_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + 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_controller.GetX(frc::GenericHID::kRightHand) * + const auto rot = -m_rotLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kRightHand)) * Drivetrain::kMaxAngularSpeed; m_drive.Drive(xSpeed, rot); @@ -35,6 +38,12 @@ class Robot : public frc::TimedRobot { 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; }; diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h index 3032bd9958..d93172dc5c 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -77,4 +78,8 @@ class Drivetrain { frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; frc::DifferentialDriveOdometry m_odometry{GetAngle()}; + + // 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/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp index 5e4fc8c608..79011111a4 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -15,19 +15,32 @@ frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const { } void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) { - const auto frontLeftOutput = m_frontLeftPIDController.Calculate( + const auto frontLeftFeedforward = + m_feedforward.Calculate(wheelSpeeds.frontLeft); + const auto frontRightFeedforward = + m_feedforward.Calculate(wheelSpeeds.frontRight); + const auto backLeftFeedforward = + m_feedforward.Calculate(wheelSpeeds.rearLeft); + const auto backRightFeedforward = + m_feedforward.Calculate(wheelSpeeds.rearRight); + + const double frontLeftOutput = m_frontLeftPIDController.Calculate( m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.to()); - const auto frontRightOutput = m_frontRightPIDController.Calculate( + const double frontRightOutput = m_frontRightPIDController.Calculate( m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.to()); - const auto backLeftOutput = m_backLeftPIDController.Calculate( + const double backLeftOutput = m_backLeftPIDController.Calculate( m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.to()); - const auto backRightOutput = m_backRightPIDController.Calculate( + const double backRightOutput = m_backRightPIDController.Calculate( m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.to()); - m_frontLeftMotor.Set(frontLeftOutput); - m_frontRightMotor.Set(frontRightOutput); - m_backLeftMotor.Set(backLeftOutput); - m_backRightMotor.Set(backRightOutput); + m_frontLeftMotor.SetVoltage(units::volt_t{frontLeftOutput} + + frontLeftFeedforward); + m_frontRightMotor.SetVoltage(units::volt_t{frontRightOutput} + + frontRightFeedforward); + m_backLeftMotor.SetVoltage(units::volt_t{backLeftOutput} + + backLeftFeedforward); + m_backRightMotor.SetVoltage(units::volt_t{backRightOutput} + + backRightFeedforward); } void Drivetrain::Drive(units::meters_per_second_t xSpeed, diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp index 5ff14d1970..a9695c4cd4 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp @@ -1,10 +1,11 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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 @@ -23,23 +24,32 @@ class Robot : public frc::TimedRobot { frc::XboxController m_controller{0}; Drivetrain m_mecanum; + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + frc::SlewRateLimiter m_xspeedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_yspeedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_rotLimiter{3 / 1_s}; + void DriveWithJoystick(bool fieldRelative) { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - const auto xSpeed = - -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + const auto xSpeed = -m_xspeedLimiter.Calculate( + m_controller.GetY(frc::GenericHID::kLeftHand)) * + 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. - const auto ySpeed = - -m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + const auto ySpeed = -m_yspeedLimiter.Calculate( + m_controller.GetX(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_controller.GetX(frc::GenericHID::kRightHand) * + const auto rot = -m_rotLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kRightHand)) * Drivetrain::kMaxAngularSpeed; m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative); diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h index 6550085880..41bfb80f5f 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -72,4 +73,8 @@ class Drivetrain { m_backRightLocation}; frc::MecanumDriveOdometry m_odometry{m_kinematics, GetAngle()}; + + // 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/SwerveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp index 7376e9e95a..10e550b09b 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp @@ -1,10 +1,11 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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 @@ -23,23 +24,32 @@ class Robot : public frc::TimedRobot { frc::XboxController m_controller{0}; Drivetrain m_swerve; + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + frc::SlewRateLimiter m_xspeedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_yspeedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_rotLimiter{3 / 1_s}; + void DriveWithJoystick(bool fieldRelative) { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - const auto xSpeed = - -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + const auto xSpeed = -m_xspeedLimiter.Calculate( + m_controller.GetY(frc::GenericHID::kLeftHand)) * + 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. - const auto ySpeed = - -m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed; + const auto ySpeed = -m_yspeedLimiter.Calculate( + m_controller.GetX(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_controller.GetX(frc::GenericHID::kRightHand) * + const auto rot = -m_rotLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kRightHand)) * Drivetrain::kMaxAngularSpeed; m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp index f7a11b806c..720d554a1a 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -40,11 +40,16 @@ void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) { const auto driveOutput = m_drivePIDController.Calculate( m_driveEncoder.GetRate(), state.speed.to()); + const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed); + // Calculate the turning motor output from the turning PID controller. const auto turnOutput = m_turningPIDController.Calculate( units::radian_t(m_turningEncoder.Get()), state.angle.Radians()); + const auto turnFeedforward = m_turnFeedforward.Calculate( + m_turningPIDController.GetSetpoint().velocity); + // Set the motor outputs. - m_driveMotor.Set(driveOutput); - m_turningMotor.Set(turnOutput); + m_driveMotor.SetVoltage(units::volt_t{driveOutput} + driveFeedforward); + m_turningMotor.SetVoltage(units::volt_t{turnOutput} + turnFeedforward); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h index 0dafa24929..4c281608db 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -41,4 +42,9 @@ class SwerveModule { 0.0, 0.0, {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}}; + + frc::SimpleMotorFeedforward m_driveFeedforward{1_V, + 3_V / 1_mps}; + frc::SimpleMotorFeedforward m_turnFeedforward{ + 1_V, 0.5_V / 1_rad_per_s}; }; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java index cc3d2e1f70..7e681365b0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -13,6 +13,7 @@ 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.Rotation2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; @@ -53,6 +54,9 @@ public class Drivetrain { 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. @@ -88,12 +92,15 @@ public class Drivetrain { * @param speeds The desired wheel speeds. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), + 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); - double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), + final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond); - m_leftGroup.set(leftOutput); - m_rightGroup.set(rightOutput); + m_leftGroup.setVoltage(leftOutput + leftFeedforward); + m_rightGroup.setVoltage(rightOutput + rightFeedforward); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java index f7ff493036..6e7416d8c8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj.examples.differentialdrivebot; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.SlewRateLimiter; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; @@ -15,6 +16,11 @@ 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(); @@ -25,13 +31,17 @@ public class Robot extends TimedRobot { 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_controller.getY(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed; + 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_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed; + final var rot = + -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) + * Drivetrain.kMaxAngularSpeed; m_drive.drive(xSpeed, rot); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index 6220807aa0..220fb87219 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -12,6 +12,7 @@ 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.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; @@ -56,6 +57,9 @@ public class Drivetrain { private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics, getAngle()); + // 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. */ @@ -93,23 +97,28 @@ public class Drivetrain { * @param speeds The desired wheel speeds. */ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { - final var frontLeftOutput = m_frontLeftPIDController.calculate( + 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 var frontRightOutput = m_frontRightPIDController.calculate( + final double frontRightOutput = m_frontRightPIDController.calculate( m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond ); - final var backLeftOutput = m_backLeftPIDController.calculate( + final double backLeftOutput = m_backLeftPIDController.calculate( m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond ); - final var backRightOutput = m_backRightPIDController.calculate( + final double backRightOutput = m_backRightPIDController.calculate( m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond ); - m_frontLeftMotor.set(frontLeftOutput); - m_frontRightMotor.set(frontRightOutput); - m_backLeftMotor.set(backLeftOutput); - m_backRightMotor.set(backRightOutput); + m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward); + m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward); + m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward); + m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java index aeb4681c66..d41a947cab 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj.examples.mecanumbot; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.SlewRateLimiter; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; @@ -15,6 +16,11 @@ 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); @@ -29,18 +35,24 @@ public class Robot extends TimedRobot { 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_controller.getY(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed; + 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_controller.getX(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed; + 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_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed; + final var rot = + -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) + * Drivetrain.kMaxAngularSpeed; m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java index 35afdf3d06..b8c4b8f8d6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj.examples.swervebot; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.SlewRateLimiter; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; @@ -15,6 +16,11 @@ 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); @@ -29,18 +35,24 @@ public class Robot extends TimedRobot { 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_controller.getY(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed; + 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_controller.getX(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed; + 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_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed; + 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); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java index 2969fd40d0..e13108fa74 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* 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. */ @@ -12,6 +12,7 @@ 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; @@ -36,6 +37,10 @@ public class SwerveModule { = 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. * @@ -77,16 +82,20 @@ public class SwerveModule { */ public void setDesiredState(SwerveModuleState state) { // Calculate the drive output from the drive PID controller. - final var driveOutput = m_drivePIDController.calculate( + 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 var turnOutput = m_turningPIDController.calculate( + final double turnOutput = m_turningPIDController.calculate( m_turningEncoder.get(), state.angle.getRadians() ); - // Calculate the turning motor output from the turning PID controller. - m_driveMotor.set(driveOutput); - m_turningMotor.set(turnOutput); + final double turnFeedforward = + m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity); + + m_driveMotor.setVoltage(driveOutput + driveFeedforward); + m_turningMotor.setVoltage(turnOutput + turnFeedforward); } } diff --git a/wpiutil/src/main/native/include/units/units.h b/wpiutil/src/main/native/include/units/units.h index 5164cfb1e7..ea9ce825ee 100644 --- a/wpiutil/src/main/native/include/units/units.h +++ b/wpiutil/src/main/native/include/units/units.h @@ -4854,4 +4854,5 @@ using namespace velocity; using namespace acceleration; using namespace angle; using namespace voltage; +using namespace dimensionless; } // namespace units