SHARE
TWEET

Untitled

a guest Oct 21st, 2019 101 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. diff --git a/MecanumFollowerCommand.cpp b/SwerveFollowerCommand.cpp
  2. index 96a11a83d..3db26ba83 100644
  3. --- a/MecanumFollowerCommand.cpp
  4. +++ b/SwerveFollowerCommand.cpp
  5. @@ -5,7 +5,7 @@
  6.  /* the project.                                                               */
  7.  /*----------------------------------------------------------------------------*/
  8.  
  9. -#include "frc2/command/MecanumFollowerCommand.h"
  10. +#include "frc2/command/SwerveFollowerCommand.h"
  11.  
  12.  using namespace frc2;
  13.  using namespace units;
  14. @@ -15,75 +15,35 @@ int sgn(T val) {
  15.    return (T(0) < val) - (val < T(0));
  16.  }
  17.  
  18. -MecanumFollowerCommand::MecanumFollowerCommand(
  19. +template<int NumModules>
  20. +SwerveFollowerCommand<NumModules>::SwerveFollowerCommand(
  21.      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
  22. -    units::voltage::volt_t ks,
  23. :...skipping...
  24. diff --git a/MecanumFollowerCommand.cpp b/SwerveFollowerCommand.cpp
  25. index 96a11a83d..3db26ba83 100644
  26. --- a/MecanumFollowerCommand.cpp
  27. +++ b/SwerveFollowerCommand.cpp
  28. @@ -5,7 +5,7 @@
  29.  /* the project.                                                               */
  30.  /*----------------------------------------------------------------------------*/
  31.  
  32. -#include "frc2/command/MecanumFollowerCommand.h"
  33. +#include "frc2/command/SwerveFollowerCommand.h"
  34.  
  35.  using namespace frc2;
  36.  using namespace units;
  37. @@ -15,75 +15,35 @@ int sgn(T val) {
  38.    return (T(0) < val) - (val < T(0));
  39.  }
  40.  
  41. -MecanumFollowerCommand::MecanumFollowerCommand(
  42. +template<int NumModules>
  43. +SwerveFollowerCommand<NumModules>::SwerveFollowerCommand(
  44.      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
  45. -    units::voltage::volt_t ks,
  46. -    units::unit_t<voltsecondspermeter> kv,
  47. -    units::unit_t<voltsecondssquaredpermeter> ka,
  48. -    frc::MecanumDriveKinematics kinematics,
  49. +    frc::SwerveDriveKinematics <NumModules> kinematics,
  50.      frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController,
  51. -    std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
  52. :...skipping...
  53. diff --git a/MecanumFollowerCommand.cpp b/SwerveFollowerCommand.cpp
  54. index 96a11a83d..3db26ba83 100644
  55. --- a/MecanumFollowerCommand.cpp
  56. +++ b/SwerveFollowerCommand.cpp
  57. @@ -5,7 +5,7 @@
  58.  /* the project.                                                               */
  59.  /*----------------------------------------------------------------------------*/
  60.  
  61. -#include "frc2/command/MecanumFollowerCommand.h"
  62. +#include "frc2/command/SwerveFollowerCommand.h"
  63.  
  64.  using namespace frc2;
  65.  using namespace units;
  66. @@ -15,75 +15,35 @@ int sgn(T val) {
  67.    return (T(0) < val) - (val < T(0));
  68.  }
  69.  
  70. -MecanumFollowerCommand::MecanumFollowerCommand(
  71. +template<int NumModules>
  72. +SwerveFollowerCommand<NumModules>::SwerveFollowerCommand(
  73.      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
  74. -    units::voltage::volt_t ks,
  75. -    units::unit_t<voltsecondspermeter> kv,
  76. -    units::unit_t<voltsecondssquaredpermeter> ka,
  77. -    frc::MecanumDriveKinematics kinematics,
  78. +    frc::SwerveDriveKinematics <NumModules> kinematics,
  79.      frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController,
  80. -    std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
  81. -    frc2::PIDController frontLeftController,
  82. -    frc2::PIDController rearLeftController,
  83. :...skipping...
  84. diff --git a/MecanumFollowerCommand.cpp b/SwerveFollowerCommand.cpp
  85. index 96a11a83d..3db26ba83 100644
  86. --- a/MecanumFollowerCommand.cpp
  87. +++ b/SwerveFollowerCommand.cpp
  88. @@ -5,7 +5,7 @@
  89.  /* the project.                                                               */
  90.  /*----------------------------------------------------------------------------*/
  91.  
  92. -#include "frc2/command/MecanumFollowerCommand.h"
  93. +#include "frc2/command/SwerveFollowerCommand.h"
  94.  
  95.  using namespace frc2;
  96.  using namespace units;
  97. @@ -15,75 +15,35 @@ int sgn(T val) {
  98.    return (T(0) < val) - (val < T(0));
  99.  }
  100.  
  101. -MecanumFollowerCommand::MecanumFollowerCommand(
  102. +template<int NumModules>
  103. +SwerveFollowerCommand<NumModules>::SwerveFollowerCommand(
  104.      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
  105. -    units::voltage::volt_t ks,
  106. -    units::unit_t<voltsecondspermeter> kv,
  107. -    units::unit_t<voltsecondssquaredpermeter> ka,
  108. -    frc::MecanumDriveKinematics kinematics,
  109. +    frc::SwerveDriveKinematics <NumModules> kinematics,
  110.      frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController,
  111. -    std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
  112. -    frc2::PIDController frontLeftController,
  113. -    frc2::PIDController rearLeftController,
  114. -    frc2::PIDController frontRightController,
  115. -    frc2::PIDController rearRightController,
  116. -    std::function<void(units::voltage::volt_t, units::voltage::volt_t, units::voltage::volt_t, units::voltage::volt_t)>
  117. +    std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
  118.          output,
  119.      std::initializer_list<Subsystem*> requirements)
  120.      : m_trajectory(trajectory),
  121.        m_pose(pose),
  122. -      m_ks(ks),
  123. -      m_kv(kv),
  124. -      m_ka(ka),
  125.        m_kinematics(kinematics),
  126.        m_xController(std::make_unique<frc2::PIDController>(xController)),
  127.        m_yController(std::make_unique<frc2::PIDController>(yController)),
  128.        m_thetaController(std::make_unique<frc::ProfiledPIDController>(thetaController)),
  129. -      m_frontLeftController(std::make_unique<frc2::PIDController>(frontLeftController)),
  130. -      m_rearLeftController(std::make_unique<frc2::PIDController>(rearLeftController)),
  131. -      m_frontRightController(std::make_unique<frc2::PIDController>(frontRightController)),
  132. -      m_rearRightController(std::make_unique<frc2::PIDController>(rearRightController)),
  133. -      m_currentWheelSpeeds(currentWheelSpeeds),
  134. -      m_outputVolts(output) {
  135. +      m_outputStates(output) {
  136.    AddRequirements(requirements);
  137.  }
  138.  
  139. -MecanumFollowerCommand::MecanumFollowerCommand(
  140. -    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
  141. -    frc::MecanumDriveKinematics kinematics,
  142. -    frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController,
  143. -    std::function<void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)>
  144. -        output,
  145. -    std::initializer_list<Subsystem*> requirements)
  146. -    : m_trajectory(trajectory),
  147. -      m_pose(pose),
  148. -      m_ks(0),
  149. -      m_kv(0),
  150. -      m_ka(0),
  151. -      m_kinematics(kinematics),
  152. -      m_xController(std::make_unique<frc2::PIDController>(xController)),
  153. -      m_yController(std::make_unique<frc2::PIDController>(yController)),
  154. -      m_thetaController(std::make_unique<frc::ProfiledPIDController>(thetaController)),
  155. -      m_outputVel(output) {
  156. -  AddRequirements(requirements);
  157. -}
  158. -
  159. -void MecanumFollowerCommand::Initialize() {
  160. -  m_prevTime = 0_s;
  161. -  auto initialState = m_trajectory.Sample(0_s);
  162. -
  163. -  auto initialXVelocity = initialState.velocity * sin (initialState.pose.Rotation().Radians().to<double>());
  164. -  auto initialYVelocity = initialState.velocity * cos (initialState.pose.Rotation().Radians().to<double>());
  165. -
  166. -  m_prevSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds{initialXVelocity, initialYVelocity, initialState.curvature * initialState.velocity});
  167. +template<int NumModules>
  168. +void SwerveFollowerCommand<NumModules>::Initialize() {
  169. +  auto m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime).pose;
  170.  
  171.    m_timer.Reset();
  172.    m_timer.Start();
  173.  }
  174.  
  175. -void MecanumFollowerCommand::Execute() {
  176. +template<int NumModules>
  177. +void SwerveFollowerCommand<NumModules>::Execute() {
  178.    auto curTime = second_t(m_timer.Get());
  179. -  auto dt = curTime - m_prevTime;
  180.  
  181.    auto m_desiredState = m_trajectory.Sample(curTime);
  182.    auto m_desiredPose = m_desiredState.pose;
  183. @@ -101,47 +61,16 @@ void MecanumFollowerCommand::Execute() {
  184.  
  185.    auto targetChassisSpeeds = frc::ChassisSpeeds{targetXVel, targetYVel, 0/*targetAngularVel*/};
  186.  
  187. -  auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
  188. -
  189. -  auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
  190. -  auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;
  191. -  auto frontRightSpeedSetpoint = targetWheelSpeeds.frontRight;
  192. -  auto rearRightSpeedSetpoint =  targetWheelSpeeds.rearRight;
  193. -
  194. -  if (m_frontLeftController.get() != nullptr) {
  195. -    auto frontLeftFeedforward =
  196. -        m_ks * sgn(frontLeftSpeedSetpoint) + m_kv * frontLeftSpeedSetpoint +
  197. -        m_ka * (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt;
  198. -
  199. -    auto rearLeftFeedforward =
  200. -        m_ks * sgn(rearLeftSpeedSetpoint) + m_kv * rearLeftSpeedSetpoint +
  201. -        m_ka * (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt;
  202. -
  203. -    auto frontRightFeedforward =
  204. -        m_ks * sgn(frontRightSpeedSetpoint) + m_kv * frontRightSpeedSetpoint +
  205. -        m_ka * (frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt;
  206. -
  207. -    auto rearRightFeedforward =
  208. -        m_ks * sgn(rearRightSpeedSetpoint) + m_kv * rearRightSpeedSetpoint +
  209. -        m_ka * (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt;
  210. -
  211. -    auto frontLeftOutput = voltage::volt_t(m_frontLeftController->Calculate(m_currentWheelSpeeds().frontLeft.to<double>(), frontLeftSpeedSetpoint.to<double>())) + frontLeftFeedforward;
  212. -    auto rearLeftOutput = voltage::volt_t(m_rearLeftController->Calculate(m_currentWheelSpeeds().rearLeft.to<double>(), rearLeftSpeedSetpoint.to<double>())) + rearLeftFeedforward;
  213. -    auto frontRightOutput = voltage::volt_t(m_frontRightController->Calculate(m_currentWheelSpeeds().frontRight.to<double>(), frontRightSpeedSetpoint.to<double>())) + frontRightFeedforward;
  214. -    auto rearRightOutput = voltage::volt_t(m_rearRightController->Calculate(m_currentWheelSpeeds().rearRight.to<double>(), rearRightSpeedSetpoint.to<double>())) + rearRightFeedforward;
  215. -
  216. -
  217. -    m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput, rearRightOutput);
  218. -  } else {
  219. -    m_outputVel(frontLeftSpeedSetpoint, rearLeftSpeedSetpoint, frontRightSpeedSetpoint, rearRightSpeedSetpoint);
  220. +  auto targetModuleStates = m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
  221. +
  222. +  m_outputStates(targetModuleStates);
  223.  
  224. -  m_prevTime = curTime;
  225. -  m_prevSpeeds = targetWheelSpeeds;
  226. -  }
  227.  }
  228.  
  229. -void MecanumFollowerCommand::End(bool interrupted) { m_timer.Stop(); }
  230. +template<int NumModules>
  231. +void SwerveFollowerCommand<NumModules>::End(bool interrupted) { m_timer.Stop(); }
  232.  
  233. -bool MecanumFollowerCommand::IsFinished() {
  234. +template<int NumModules>
  235. +bool SwerveFollowerCommand<NumModules>::IsFinished() {
  236.    return second_t(m_timer.Get()) - m_trajectory.TotalTime() >= 0_s;
  237.  }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top