Advertisement
Guest User

Untitled

a guest
Oct 21st, 2019
125
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 10.57 KB | None | 0 0
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement