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 TrapezoidProfile external PID examples (#2131)

Browse Source
This commit is contained in:
Oblarg
2019-12-07 16:37:54 -05:00
committed by Peter Johnson
parent 5c6b8a0f45
commit ccdd0fbdb2
54 changed files with 2734 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
View File

@@ -8,6 +8,9 @@
#include "RobotContainer.h"
#include <frc/shuffleboard/Shuffleboard.h>
#include <frc2/command/PIDCommand.h>
#include <frc2/command/ParallelRaceGroup.h>
#include <frc2/command/RunCommand.h>
#include <frc2/command/button/JoystickButton.h>
RobotContainer::RobotContainer() {
@@ -29,16 +32,39 @@ RobotContainer::RobotContainer() {
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
// Assorted commands to be bound to buttons
// Stabilize robot to drive straight with gyro when left bumper is held
frc2::JoystickButton(&m_driverController, 5).WhenHeld(&m_stabilizeDriving);
frc2::JoystickButton(&m_driverController, 5)
.WhenHeld(frc2::PIDCommand{
frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
dc::kStabilizationD},
// Close the loop on the turn rate
[this] { return m_drive.GetTurnRate(); },
// Setpoint is 0
0,
// Pipe the output to the turning controls
[this](double output) {
m_drive.ArcadeDrive(m_driverController.GetY(
frc::GenericHID::JoystickHand::kLeftHand),
output);
},
// Require the robot drive
{&m_drive}});
// Turn to 90 degrees when the 'X' button is pressed
frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_turnTo90);
frc2::JoystickButton(&m_driverController, 3)
.WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
// Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5
// second timeout
frc2::JoystickButton(&m_driverController, 1)
.WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
// While holding the shoulder button, drive at half speed
frc2::JoystickButton(&m_driverController, 6)
.WhenPressed(&m_driveHalfSpeed)
.WhenReleased(&m_driveFullSpeed);
.WhenPressed(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(0.5); }})
.WhenReleased(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(1); }});
}
frc2::Command* RobotContainer::GetAutonomousCommand() {

11
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
View File

@@ -11,12 +11,12 @@
using namespace DriveConstants;
TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive)
TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
: CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD),
// Close loop on heading
[drive] { return drive->GetHeading(); },
[drive] { return drive->GetHeading().to<double>(); },
// Set reference to target
targetAngleDegrees,
target.to<double>(),
// Pipe output to turn robot
[drive](double output) { drive->ArcadeDrive(0, output); },
// Require the drive
@@ -26,9 +26,10 @@ TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive)
// Set the controller tolerance - the delta tolerance ensures the robot is
// stationary at the setpoint before it is considered as having reached the
// reference
m_controller.SetTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
m_controller.SetTolerance(kTurnTolerance.to<double>(),
kTurnRateTolerance.to<double>());
AddRequirements({drive});
}
bool TurnToAngle::IsFinished() { return m_controller.AtSetpoint(); }
bool TurnToAngle::IsFinished() { return GetController().AtSetpoint(); }

39
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp Normal file
View File

@@ -0,0 +1,39 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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 "commands/TurnToAngleProfiled.h"
#include <frc/controller/ProfiledPIDController.h>
using namespace DriveConstants;
TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
DriveSubsystem* drive)
: CommandHelper(
frc::ProfiledPIDController<units::radians>(
kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}),
// Close loop on heading
[drive] { return drive->GetHeading(); },
// Set reference to target
target,
// Pipe output to turn robot
[drive](double output, auto setpointState) {
drive->ArcadeDrive(0, output);
},
// Require the drive
{drive}) {
// Set the controller to be continuous (because it is an angle controller)
GetController().EnableContinuousInput(-180_deg, 180_deg);
// Set the controller tolerance - the delta tolerance ensures the robot is
// stationary at the setpoint before it is considered as having reached the
// reference
GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance);
AddRequirements({drive});
}
bool TurnToAngleProfiled::IsFinished() { return GetController().AtGoal(); }

5
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -46,8 +46,9 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
double DriveSubsystem::GetHeading() {
return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
units::degree_t DriveSubsystem::GetHeading() {
return units::degree_t(std::remainder(m_gyro.GetAngle(), 360) *
(kGyroReversed ? -1.0 : 1.0));
}
double DriveSubsystem::GetTurnRate() {

8
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
View File

@@ -7,6 +7,7 @@
#pragma once
#include <units/units.h>
#include <wpi/math>
/**
@@ -45,8 +46,11 @@ constexpr double kTurnP = 1;
constexpr double kTurnI = 0;
constexpr double kTurnD = 0;
constexpr double kTurnToleranceDeg = 5;
constexpr double kTurnRateToleranceDegPerS = 10; // degrees per second
constexpr auto kTurnTolerance = 5_deg;
constexpr auto kTurnRateTolerance = 10_deg_per_s;
constexpr auto kMaxTurnRate = 100_deg_per_s;
constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
} // namespace DriveConstants
namespace AutoConstants {

31
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
View File

@@ -12,9 +12,6 @@
#include <frc/smartdashboard/SendableChooser.h>
#include <frc2/command/Command.h>
#include <frc2/command/InstantCommand.h>
#include <frc2/command/PIDCommand.h>
#include <frc2/command/ParallelRaceGroup.h>
#include <frc2/command/RunCommand.h>
#include "Constants.h"
#include "commands/TurnToAngle.h"
@@ -45,34 +42,6 @@ class RobotContainer {
// The robot's subsystems
DriveSubsystem m_drive;
// Assorted commands to be bound to buttons
// Turn to 90 degrees, with a 5 second timeout
frc2::ParallelRaceGroup m_turnTo90 =
TurnToAngle{90, &m_drive}.WithTimeout(5_s);
// Stabilize the robot while driving
frc2::PIDCommand m_stabilizeDriving{
frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
dc::kStabilizationD},
// Close the loop on the turn rate
[this] { return m_drive.GetTurnRate(); },
// Setpoint is 0
0,
// Pipe the output to the turning controls
[this](double output) {
m_drive.ArcadeDrive(
m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
output);
},
// Require the robot drive
{&m_drive}};
frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
// The chooser for the autonomous routines
frc::SendableChooser<frc2::Command*> m_chooser;

2
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
View File

@@ -23,7 +23,7 @@ class TurnToAngle : public frc2::CommandHelper<frc2::PIDCommand, TurnToAngle> {
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive);
TurnToAngle(units::degree_t target, DriveSubsystem* drive);
bool IsFinished() override;
};

33
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h Normal file
View File

@@ -0,0 +1,33 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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 <frc2/command/CommandHelper.h>
#include <frc2/command/ProfiledPIDCommand.h>
#include "subsystems/DriveSubsystem.h"
/**
* A command that will turn the robot to the specified angle using a motion
* profile.
*/
class TurnToAngleProfiled
: public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
TurnToAngleProfiled> {
public:
/**
* Turns to robot to the specified angle using a motion profile.
*
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
TurnToAngleProfiled(units::degree_t targetAngleDegrees,
DriveSubsystem* drive);
bool IsFinished() override;
};

3
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
View File

@@ -13,6 +13,7 @@
#include <frc/SpeedControllerGroup.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc2/command/SubsystemBase.h>
#include <units/units.h>
#include "Constants.h"
@@ -74,7 +75,7 @@ class DriveSubsystem : public frc2::SubsystemBase {
*
* @return the robot's heading in degrees, from 180 to 180
*/
double GetHeading();
units::degree_t GetHeading();
/**
* Returns the turn rate of the robot.