Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- diff --git a/MecanumFollowerCommand.cpp b/SwerveFollowerCommand.cpp
- index 96a11a83d..3db26ba83 100644
- --- a/MecanumFollowerCommand.cpp
- +++ b/SwerveFollowerCommand.cpp
- @@ -5,7 +5,7 @@
- /* the project. */
- /*----------------------------------------------------------------------------*/
- -#include "frc2/command/MecanumFollowerCommand.h"
- +#include "frc2/command/SwerveFollowerCommand.h"
- using namespace frc2;
- using namespace units;
- @@ -15,75 +15,35 @@ int sgn(T val) {
- return (T(0) < val) - (val < T(0));
- }
- -MecanumFollowerCommand::MecanumFollowerCommand(
- +template<int NumModules>
- +SwerveFollowerCommand<NumModules>::SwerveFollowerCommand(
- frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
- - units::voltage::volt_t ks,
- :...skipping...
- diff --git a/MecanumFollowerCommand.cpp b/SwerveFollowerCommand.cpp
- index 96a11a83d..3db26ba83 100644
- --- a/MecanumFollowerCommand.cpp
- +++ b/SwerveFollowerCommand.cpp
- @@ -5,7 +5,7 @@
- /* the project. */
- /*----------------------------------------------------------------------------*/
- -#include "frc2/command/MecanumFollowerCommand.h"
- +#include "frc2/command/SwerveFollowerCommand.h"
- using namespace frc2;
- using namespace units;
- @@ -15,75 +15,35 @@ int sgn(T val) {
- return (T(0) < val) - (val < T(0));
- }
- -MecanumFollowerCommand::MecanumFollowerCommand(
- +template<int NumModules>
- +SwerveFollowerCommand<NumModules>::SwerveFollowerCommand(
- frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
- - units::voltage::volt_t ks,
- - units::unit_t<voltsecondspermeter> kv,
- - units::unit_t<voltsecondssquaredpermeter> ka,
- - frc::MecanumDriveKinematics kinematics,
- + frc::SwerveDriveKinematics <NumModules> kinematics,
- frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController,
- - std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
- :...skipping...
- diff --git a/MecanumFollowerCommand.cpp b/SwerveFollowerCommand.cpp
- index 96a11a83d..3db26ba83 100644
- --- a/MecanumFollowerCommand.cpp
- +++ b/SwerveFollowerCommand.cpp
- @@ -5,7 +5,7 @@
- /* the project. */
- /*----------------------------------------------------------------------------*/
- -#include "frc2/command/MecanumFollowerCommand.h"
- +#include "frc2/command/SwerveFollowerCommand.h"
- using namespace frc2;
- using namespace units;
- @@ -15,75 +15,35 @@ int sgn(T val) {
- return (T(0) < val) - (val < T(0));
- }
- -MecanumFollowerCommand::MecanumFollowerCommand(
- +template<int NumModules>
- +SwerveFollowerCommand<NumModules>::SwerveFollowerCommand(
- frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
- - units::voltage::volt_t ks,
- - units::unit_t<voltsecondspermeter> kv,
- - units::unit_t<voltsecondssquaredpermeter> ka,
- - frc::MecanumDriveKinematics kinematics,
- + frc::SwerveDriveKinematics <NumModules> kinematics,
- frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController,
- - std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
- - frc2::PIDController frontLeftController,
- - frc2::PIDController rearLeftController,
- :...skipping...
- diff --git a/MecanumFollowerCommand.cpp b/SwerveFollowerCommand.cpp
- index 96a11a83d..3db26ba83 100644
- --- a/MecanumFollowerCommand.cpp
- +++ b/SwerveFollowerCommand.cpp
- @@ -5,7 +5,7 @@
- /* the project. */
- /*----------------------------------------------------------------------------*/
- -#include "frc2/command/MecanumFollowerCommand.h"
- +#include "frc2/command/SwerveFollowerCommand.h"
- using namespace frc2;
- using namespace units;
- @@ -15,75 +15,35 @@ int sgn(T val) {
- return (T(0) < val) - (val < T(0));
- }
- -MecanumFollowerCommand::MecanumFollowerCommand(
- +template<int NumModules>
- +SwerveFollowerCommand<NumModules>::SwerveFollowerCommand(
- frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
- - units::voltage::volt_t ks,
- - units::unit_t<voltsecondspermeter> kv,
- - units::unit_t<voltsecondssquaredpermeter> ka,
- - frc::MecanumDriveKinematics kinematics,
- + frc::SwerveDriveKinematics <NumModules> kinematics,
- frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController,
- - std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
- - frc2::PIDController frontLeftController,
- - frc2::PIDController rearLeftController,
- - frc2::PIDController frontRightController,
- - frc2::PIDController rearRightController,
- - std::function<void(units::voltage::volt_t, units::voltage::volt_t, units::voltage::volt_t, units::voltage::volt_t)>
- + std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
- output,
- std::initializer_list<Subsystem*> requirements)
- : m_trajectory(trajectory),
- m_pose(pose),
- - m_ks(ks),
- - m_kv(kv),
- - m_ka(ka),
- 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>(thetaController)),
- - 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_outputStates(output) {
- AddRequirements(requirements);
- }
- -MecanumFollowerCommand::MecanumFollowerCommand(
- - frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
- - frc::MecanumDriveKinematics kinematics,
- - frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController,
- - 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_ks(0),
- - m_kv(0),
- - m_ka(0),
- - 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>(thetaController)),
- - m_outputVel(output) {
- - AddRequirements(requirements);
- -}
- -
- -void MecanumFollowerCommand::Initialize() {
- - m_prevTime = 0_s;
- - auto initialState = m_trajectory.Sample(0_s);
- -
- - auto initialXVelocity = initialState.velocity * sin (initialState.pose.Rotation().Radians().to<double>());
- - auto initialYVelocity = initialState.velocity * cos (initialState.pose.Rotation().Radians().to<double>());
- -
- - m_prevSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds{initialXVelocity, initialYVelocity, initialState.curvature * initialState.velocity});
- +template<int NumModules>
- +void SwerveFollowerCommand<NumModules>::Initialize() {
- + auto m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime).pose;
- m_timer.Reset();
- m_timer.Start();
- }
- -void MecanumFollowerCommand::Execute() {
- +template<int NumModules>
- +void SwerveFollowerCommand<NumModules>::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;
- @@ -101,47 +61,16 @@ void MecanumFollowerCommand::Execute() {
- auto targetChassisSpeeds = frc::ChassisSpeeds{targetXVel, targetYVel, 0/*targetAngularVel*/};
- - auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
- -
- - auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
- - auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;
- - auto frontRightSpeedSetpoint = targetWheelSpeeds.frontRight;
- - auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;
- -
- - if (m_frontLeftController.get() != nullptr) {
- - auto frontLeftFeedforward =
- - m_ks * sgn(frontLeftSpeedSetpoint) + m_kv * frontLeftSpeedSetpoint +
- - m_ka * (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt;
- -
- - auto rearLeftFeedforward =
- - m_ks * sgn(rearLeftSpeedSetpoint) + m_kv * rearLeftSpeedSetpoint +
- - m_ka * (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt;
- -
- - auto frontRightFeedforward =
- - m_ks * sgn(frontRightSpeedSetpoint) + m_kv * frontRightSpeedSetpoint +
- - m_ka * (frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt;
- -
- - auto rearRightFeedforward =
- - m_ks * sgn(rearRightSpeedSetpoint) + m_kv * rearRightSpeedSetpoint +
- - m_ka * (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt;
- -
- - auto frontLeftOutput = voltage::volt_t(m_frontLeftController->Calculate(m_currentWheelSpeeds().frontLeft.to<double>(), frontLeftSpeedSetpoint.to<double>())) + frontLeftFeedforward;
- - auto rearLeftOutput = voltage::volt_t(m_rearLeftController->Calculate(m_currentWheelSpeeds().rearLeft.to<double>(), rearLeftSpeedSetpoint.to<double>())) + rearLeftFeedforward;
- - auto frontRightOutput = voltage::volt_t(m_frontRightController->Calculate(m_currentWheelSpeeds().frontRight.to<double>(), frontRightSpeedSetpoint.to<double>())) + frontRightFeedforward;
- - auto rearRightOutput = voltage::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);
- + auto targetModuleStates = m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
- +
- + m_outputStates(targetModuleStates);
- - m_prevTime = curTime;
- - m_prevSpeeds = targetWheelSpeeds;
- - }
- }
- -void MecanumFollowerCommand::End(bool interrupted) { m_timer.Stop(); }
- +template<int NumModules>
- +void SwerveFollowerCommand<NumModules>::End(bool interrupted) { m_timer.Stop(); }
- -bool MecanumFollowerCommand::IsFinished() {
- +template<int NumModules>
- +bool SwerveFollowerCommand<NumModules>::IsFinished() {
- return second_t(m_timer.Get()) - m_trajectory.TotalTime() >= 0_s;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement