Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add feed-forward and slew rate limiting to advanced drive examples (#2301)

Browse Source
This commit is contained in:
Oblarg
2020-01-23 21:07:38 -05:00
committed by Peter Johnson
parent 1cee5ccb93
commit 0ab81d768f
16 changed files with 192 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
View File

@@ -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<double>());
const auto rightOutput = m_rightPIDController.Calculate(
const double rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), speeds.right.to<double>());
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,

17
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
View File

@@ -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 <frc/SlewRateLimiter.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
@@ -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<units::scalar> m_speedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
Drivetrain m_drive;
};

7
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
View File

@@ -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 <frc/PWMVictorSPX.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <units/units.h>
@@ -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<units::meters> m_feedforward{1_V, 3_V / 1_mps};
};

31
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -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<double>());
const auto frontRightOutput = m_frontRightPIDController.Calculate(
const double frontRightOutput = m_frontRightPIDController.Calculate(
m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.to<double>());
const auto backLeftOutput = m_backLeftPIDController.Calculate(
const double backLeftOutput = m_backLeftPIDController.Calculate(
m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.to<double>());
const auto backRightOutput = m_backRightPIDController.Calculate(
const double backRightOutput = m_backRightPIDController.Calculate(
m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.to<double>());
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,

22
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
View File

@@ -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 <frc/SlewRateLimiter.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
@@ -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<units::scalar> m_xspeedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> 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);

7
wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
View File

@@ -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 <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/MecanumDriveKinematics.h>
#include <frc/kinematics/MecanumDriveOdometry.h>
@@ -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<units::meters> m_feedforward{1_V, 3_V / 1_mps};
};

22
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
View File

@@ -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 <frc/SlewRateLimiter.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
@@ -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<units::scalar> m_xspeedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> 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);

11
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
View File

@@ -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<double>());
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);
}

8
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
View File

@@ -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 <frc/PWMVictorSPX.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <wpi/math>
@@ -41,4 +42,9 @@ class SwerveModule {
0.0,
0.0,
{kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
frc::SimpleMotorFeedforward<units::meter> m_driveFeedforward{1_V,
3_V / 1_mps};
frc::SimpleMotorFeedforward<units::radians> m_turnFeedforward{
1_V, 0.5_V / 1_rad_per_s};
};

17
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
View File

@@ -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);
}
/**

16
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
View File

@@ -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);
}

27
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -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);
}
/**

20
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
View File

@@ -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);
}

20
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
View File

@@ -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);
}

21
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
View File

@@ -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);
}
}

1
wpiutil/src/main/native/include/units/units.h
View File

@@ -4854,4 +4854,5 @@ using namespace velocity;
using namespace acceleration;
using namespace angle;
using namespace voltage;
using namespace dimensionless;
} // namespace units