| Sabina Davis | 8d20ca8 | 2018-02-19 13:17:45 -0800 | [diff] [blame] | 1 | #include "y2018/control_loops/superstructure/superstructure.h" |
| 2 | |
| 3 | #include "aos/common/controls/control_loops.q.h" |
| 4 | #include "aos/common/logging/logging.h" |
| 5 | #include "frc971/control_loops/control_loops.q.h" |
| 6 | #include "y2018/constants.h" |
| 7 | #include "y2018/control_loops/superstructure/intake/intake.h" |
| 8 | |
| 9 | namespace y2018 { |
| 10 | namespace control_loops { |
| 11 | namespace superstructure { |
| 12 | |
| 13 | namespace { |
| 14 | // The maximum voltage the intake roller will be allowed to use. |
| 15 | constexpr double kMaxIntakeRollerVoltage = 12.0; |
| 16 | } // namespace |
| 17 | |
| 18 | Superstructure::Superstructure( |
| 19 | control_loops::SuperstructureQueue *superstructure_queue) |
| 20 | : aos::controls::ControlLoop<control_loops::SuperstructureQueue>( |
| 21 | superstructure_queue), |
| 22 | intake_left_(constants::GetValues().left_intake.zeroing), |
| 23 | intake_right_(constants::GetValues().right_intake.zeroing) {} |
| 24 | |
| 25 | void Superstructure::RunIteration( |
| 26 | const control_loops::SuperstructureQueue::Goal *unsafe_goal, |
| 27 | const control_loops::SuperstructureQueue::Position *position, |
| 28 | control_loops::SuperstructureQueue::Output *output, |
| 29 | control_loops::SuperstructureQueue::Status *status) { |
| 30 | if (WasReset()) { |
| 31 | LOG(ERROR, "WPILib reset, restarting\n"); |
| 32 | intake_left_.Reset(); |
| 33 | intake_right_.Reset(); |
| 34 | } |
| 35 | |
| 36 | intake_left_.Iterate(unsafe_goal != nullptr |
| 37 | ? &(unsafe_goal->intake.left_intake_angle) |
| 38 | : nullptr, |
| 39 | &(position->intake.left), |
| 40 | output != nullptr ? &(output->intake.left) : nullptr, |
| 41 | &(status->left_intake)); |
| 42 | |
| 43 | intake_right_.Iterate(unsafe_goal != nullptr |
| 44 | ? &(unsafe_goal->intake.right_intake_angle) |
| 45 | : nullptr, |
| 46 | &(position->intake.right), |
| 47 | output != nullptr ? &(output->intake.right) : nullptr, |
| 48 | &(status->right_intake)); |
| 49 | |
| 50 | status->estopped = |
| 51 | status->left_intake.estopped || status->right_intake.estopped; |
| 52 | |
| 53 | status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed; |
| 54 | |
| 55 | if (output && unsafe_goal) { |
| 56 | output->intake.left.voltage_rollers = ::std::max( |
| 57 | -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage, |
| 58 | kMaxIntakeRollerVoltage)); |
| 59 | output->intake.right.voltage_rollers = ::std::max( |
| 60 | -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage, |
| 61 | kMaxIntakeRollerVoltage)); |
| 62 | } |
| 63 | } |
| 64 | |
| 65 | } // namespace superstructure |
| 66 | } // namespace control_loops |
| 67 | } // namespace y2018 |