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 holonomic follower examples (#2052)

Browse Source
This commit is contained in:
CTT
2019-11-21 19:52:56 -08:00
committed by Peter Johnson
parent 9a8067465c
commit a58dbec8aa
51 changed files with 4793 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

187
wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp Normal file
View File

@@ -0,0 +1,187 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc2/command/MecanumControllerCommand.h"
using namespace frc2;
using namespace units;
MecanumControllerCommand::MecanumControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
frc2::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
units::meters_per_second_t maxWheelVelocity,
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
frc2::PIDController frontLeftController,
frc2::PIDController rearLeftController,
frc2::PIDController frontRightController,
frc2::PIDController rearRightController,
std::function<void(units::volt_t, units::volt_t, units::volt_t,
units::volt_t)>
output,
std::initializer_list<Subsystem*> requirements)
: m_trajectory(trajectory),
m_pose(pose),
m_feedforward(feedforward),
m_kinematics(kinematics),
m_xController(std::make_unique<frc2::PIDController>(xController)),
m_yController(std::make_unique<frc2::PIDController>(yController)),
m_thetaController(
std::make_unique<frc::ProfiledPIDController<units::radians>>(
thetaController)),
m_maxWheelVelocity(maxWheelVelocity),
m_frontLeftController(
std::make_unique<frc2::PIDController>(frontLeftController)),
m_rearLeftController(
std::make_unique<frc2::PIDController>(rearLeftController)),
m_frontRightController(
std::make_unique<frc2::PIDController>(frontRightController)),
m_rearRightController(
std::make_unique<frc2::PIDController>(rearRightController)),
m_currentWheelSpeeds(currentWheelSpeeds),
m_outputVolts(output),
m_usePID(true) {
AddRequirements(requirements);
}
MecanumControllerCommand::MecanumControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
frc2::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
units::meters_per_second_t maxWheelVelocity,
std::function<void(units::meters_per_second_t, units::meters_per_second_t,
units::meters_per_second_t, units::meters_per_second_t)>
output,
std::initializer_list<Subsystem*> requirements)
: m_trajectory(trajectory),
m_pose(pose),
m_kinematics(kinematics),
m_xController(std::make_unique<frc2::PIDController>(xController)),
m_yController(std::make_unique<frc2::PIDController>(yController)),
m_thetaController(
std::make_unique<frc::ProfiledPIDController<units::radians>>(
thetaController)),
m_maxWheelVelocity(maxWheelVelocity),
m_outputVel(output),
m_usePID(false) {
AddRequirements(requirements);
}
void MecanumControllerCommand::Initialize() {
m_prevTime = 0_s;
auto initialState = m_trajectory.Sample(0_s);
auto initialXVelocity =
initialState.velocity * initialState.pose.Rotation().Cos();
auto initialYVelocity =
initialState.velocity * initialState.pose.Rotation().Sin();
m_prevSpeeds = m_kinematics.ToWheelSpeeds(
frc::ChassisSpeeds{initialXVelocity, initialYVelocity, 0_rad_per_s});
m_timer.Reset();
m_timer.Start();
if (m_usePID) {
m_frontLeftController->Reset();
m_rearLeftController->Reset();
m_frontRightController->Reset();
m_rearRightController->Reset();
}
}
void MecanumControllerCommand::Execute() {
auto curTime = second_t(m_timer.Get());
auto dt = curTime - m_prevTime;
auto m_desiredState = m_trajectory.Sample(curTime);
auto m_desiredPose = m_desiredState.pose;
auto m_poseError = m_desiredPose.RelativeTo(m_pose());
auto targetXVel = meters_per_second_t(
m_xController->Calculate((m_pose().Translation().X().to<double>()),
(m_desiredPose.Translation().X().to<double>())));
auto targetYVel = meters_per_second_t(
m_yController->Calculate((m_pose().Translation().Y().to<double>()),
(m_desiredPose.Translation().Y().to<double>())));
// Profiled PID Controller only takes meters as setpoint and measurement
// The robot will go to the desired rotation of the final pose in the
// trajectory, not following the poses at individual states.
auto targetAngularVel = radians_per_second_t(m_thetaController->Calculate(
m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
auto vRef = m_desiredState.velocity;
targetXVel += vRef * m_poseError.Rotation().Cos();
targetYVel += vRef * m_poseError.Rotation().Sin();
auto targetChassisSpeeds =
frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
targetWheelSpeeds.Normalize(m_maxWheelVelocity);
auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;
auto frontRightSpeedSetpoint = targetWheelSpeeds.frontRight;
auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;
if (m_usePID) {
auto frontLeftFeedforward = m_feedforward.Calculate(
frontLeftSpeedSetpoint,
(frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt);
auto rearLeftFeedforward = m_feedforward.Calculate(
rearLeftSpeedSetpoint,
(rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt);
auto frontRightFeedforward = m_feedforward.Calculate(
frontRightSpeedSetpoint,
(frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt);
auto rearRightFeedforward = m_feedforward.Calculate(
rearRightSpeedSetpoint,
(rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt);
auto frontLeftOutput = volt_t(m_frontLeftController->Calculate(
m_currentWheelSpeeds().frontLeft.to<double>(),
frontLeftSpeedSetpoint.to<double>())) +
frontLeftFeedforward;
auto rearLeftOutput = volt_t(m_rearLeftController->Calculate(
m_currentWheelSpeeds().rearLeft.to<double>(),
rearLeftSpeedSetpoint.to<double>())) +
rearLeftFeedforward;
auto frontRightOutput = volt_t(m_frontRightController->Calculate(
m_currentWheelSpeeds().frontRight.to<double>(),
frontRightSpeedSetpoint.to<double>())) +
frontRightFeedforward;
auto rearRightOutput = volt_t(m_rearRightController->Calculate(
m_currentWheelSpeeds().rearRight.to<double>(),
rearRightSpeedSetpoint.to<double>())) +
rearRightFeedforward;
m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput,
rearRightOutput);
} else {
m_outputVel(frontLeftSpeedSetpoint, rearLeftSpeedSetpoint,
frontRightSpeedSetpoint, rearRightSpeedSetpoint);
m_prevTime = curTime;
m_prevSpeeds = targetWheelSpeeds;
}
}
void MecanumControllerCommand::End(bool interrupted) { m_timer.Stop(); }
bool MecanumControllerCommand::IsFinished() {
return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
}