Implmented ZeroOuputs function for the drivetrain.
Change-Id: I0e35f34ce3c2d7548775aed20e6596168a806e88
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index aabd89d..12d2cbc 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -150,6 +150,14 @@
kf_.UpdateObserver(U);
}
+void DrivetrainLoop::Zero(
+ ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ output->left_voltage = 0;
+ output->right_voltage = 0;
+ output->left_high = dt_config_.default_high_gear;
+ output->right_high = dt_config_.default_high_gear;
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a2f2015..30548be 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -21,17 +21,20 @@
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
- explicit DrivetrainLoop(const DrivetrainConfig &dt_config,
+ explicit DrivetrainLoop(
+ const DrivetrainConfig &dt_config,
::frc971::control_loops::DrivetrainQueue *my_drivetrain =
&::frc971::control_loops::drivetrain_queue);
protected:
// Executes one cycle of the control loop.
- virtual void RunIteration(
+ void RunIteration(
const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
const ::frc971::control_loops::DrivetrainQueue::Position *position,
::frc971::control_loops::DrivetrainQueue::Output *output,
- ::frc971::control_loops::DrivetrainQueue::Status *status);
+ ::frc971::control_loops::DrivetrainQueue::Status *status) override;
+
+ void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
SimpleLogInterval no_position_ = SimpleLogInterval(
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 21dd874..a0b6ac0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -44,6 +44,10 @@
// Hall effect constants. Unused if not applicable to shifter type.
constants::ShifterHallEffect left_drive;
constants::ShifterHallEffect right_drive;
+
+ // Variable that holds the default gear ratio. We use this in ZeroOutputs().
+ // (ie. true means high gear is default).
+ bool default_high_gear;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index afa8cf1..4ceda53 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -48,7 +48,8 @@
::y2016::control_loops::drivetrain::kHighGearRatio,
::y2016::control_loops::drivetrain::kLowGearRatio,
kThreeStateDriveShifter,
- kThreeStateDriveShifter};
+ kThreeStateDriveShifter,
+ false};
return kDrivetrainConfig;
};