Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix NPE in RamseteCommand (#2019)

Browse Source
This commit is contained in:
Oblarg
2019-11-01 12:26:48 -04:00
committed by Peter Johnson
parent 1c1e0c9a6a
commit f33bd9f050
3 changed files with 21 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
View File

@@ -37,7 +37,8 @@ RamseteCommand::RamseteCommand(
m_rightSpeed(rightSpeed),
m_leftController(std::make_unique<frc2::PIDController>(leftController)),
m_rightController(std::make_unique<frc2::PIDController>(rightController)),
m_outputVolts(output) {
m_outputVolts(output),
m_usePID(true) {
AddRequirements(requirements);
}
@@ -55,7 +56,8 @@ RamseteCommand::RamseteCommand(
m_kv(0),
m_ka(0),
m_kinematics(kinematics),
m_outputVel(output) {
m_outputVel(output),
m_usePID(false) {
AddRequirements(requirements);
}
@@ -67,8 +69,10 @@ void RamseteCommand::Initialize() {
initialState.velocity * initialState.curvature});
m_timer.Reset();
m_timer.Start();
m_leftController->Reset();
m_rightController->Reset();
if (m_usePID) {
m_leftController->Reset();
m_rightController->Reset();
}
}
void RamseteCommand::Execute() {
@@ -78,7 +82,7 @@ void RamseteCommand::Execute() {
auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
if (m_leftController.get() != nullptr) {
if (m_usePID) {
auto leftFeedforward =
m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;