Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00: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

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