| Sabina Davis | edf8947 | 2020-02-17 15:27:37 -0800 | [diff] [blame^] | 1 | #include "y2020/control_loops/superstructure/shooter/shooter.h" |
| 2 | |
| 3 | #include <chrono> |
| 4 | |
| 5 | #include "aos/logging/logging.h" |
| 6 | #include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h" |
| 7 | #include "y2020/control_loops/superstructure/finisher/finisher_plant.h" |
| 8 | |
| 9 | namespace y2020 { |
| 10 | namespace control_loops { |
| 11 | namespace superstructure { |
| 12 | namespace shooter { |
| 13 | |
| 14 | Shooter::Shooter() |
| 15 | : finisher_(finisher::MakeIntegralFinisherLoop()), |
| 16 | accelerator_left_(accelerator::MakeIntegralAcceleratorLoop()), |
| 17 | accelerator_right_(accelerator::MakeIntegralAcceleratorLoop()) {} |
| 18 | |
| 19 | flatbuffers::Offset<ShooterStatus> Shooter::RunIteration( |
| 20 | const ShooterGoal *goal, const ShooterPosition *position, |
| 21 | flatbuffers::FlatBufferBuilder *fbb, OutputT *output) { |
| 22 | if (goal) { |
| 23 | // Update position/goal for our two shooter sides. |
| 24 | finisher_.set_goal(goal->velocity_finisher()); |
| 25 | accelerator_left_.set_goal(goal->velocity_accelerator()); |
| 26 | accelerator_right_.set_goal(goal->velocity_accelerator()); |
| 27 | } |
| 28 | |
| 29 | finisher_.set_position(position->theta_finisher()); |
| 30 | accelerator_left_.set_position(position->theta_accelerator_left()); |
| 31 | accelerator_right_.set_position(position->theta_accelerator_right()); |
| 32 | |
| 33 | finisher_.Update(output == nullptr); |
| 34 | accelerator_left_.Update(output == nullptr); |
| 35 | accelerator_right_.Update(output == nullptr); |
| 36 | |
| 37 | flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset = |
| 38 | finisher_.SetStatus(fbb); |
| 39 | flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset = |
| 40 | accelerator_left_.SetStatus(fbb); |
| 41 | flatbuffers::Offset<FlywheelControllerStatus> |
| 42 | accelerator_right_status_offset = accelerator_right_.SetStatus(fbb); |
| 43 | |
| 44 | ShooterStatusBuilder status_builder(*fbb); |
| 45 | |
| 46 | status_builder.add_finisher(finisher_status_offset); |
| 47 | status_builder.add_accelerator_left(accelerator_left_status_offset); |
| 48 | status_builder.add_accelerator_right(accelerator_right_status_offset); |
| 49 | |
| 50 | if (output) { |
| 51 | output->finisher_voltage = finisher_.voltage(); |
| 52 | output->accelerator_left_voltage = accelerator_left_.voltage(); |
| 53 | output->accelerator_right_voltage = accelerator_right_.voltage(); |
| 54 | } |
| 55 | |
| 56 | return status_builder.Finish(); |
| 57 | } |
| 58 | |
| 59 | } // namespace shooter |
| 60 | } // namespace superstructure |
| 61 | } // namespace control_loops |
| 62 | } // namespace y2020 |