blob: 80b0bca0c186d9dedbeb5a909201eca456ba7417 [file] [log] [blame]
brians343bc112013-02-10 01:53:46 +00001#ifndef AOS_CONTROL_LOOP_CONTROL_LOOP_H_
2#define AOS_CONTROL_LOOP_CONTROL_LOOP_H_
3
Austin Schuhb58ceb62017-02-05 14:21:57 -08004#include <atomic>
Tyler Chatowbf0609c2021-07-31 16:13:27 -07005#include <cstring>
brians343bc112013-02-10 01:53:46 +00006
Alex Perrycb7da4b2019-08-28 19:35:56 -07007#include "aos/events/event_loop.h"
John Park33858a32018-09-28 23:05:48 -07008#include "aos/time/time.h"
9#include "aos/type_traits/type_traits.h"
10#include "aos/util/log_interval.h"
James Kuszmaul7077d342021-06-09 20:23:58 -070011#include "frc971/input/joystick_state_generated.h"
12#include "frc971/input/robot_state_generated.h"
brians343bc112013-02-10 01:53:46 +000013
James Kuszmaul61750662021-06-21 21:32:33 -070014namespace frc971 {
Brian Silverman38111502014-04-10 12:36:26 -070015namespace controls {
brians343bc112013-02-10 01:53:46 +000016
Brian Silverman3204dd82013-03-12 18:42:01 -070017// Control loops run this often, "starting" at time 0.
Austin Schuh214e9c12016-11-25 17:26:20 -080018constexpr ::std::chrono::nanoseconds kLoopFrequency =
19 ::std::chrono::milliseconds(5);
Brian Silverman3204dd82013-03-12 18:42:01 -070020
brians343bc112013-02-10 01:53:46 +000021// Provides helper methods to assist in writing control loops.
brians343bc112013-02-10 01:53:46 +000022// It will then call the RunIteration method every cycle that it has enough
23// valid data for the control loop to run.
Alex Perrycb7da4b2019-08-28 19:35:56 -070024template <class GoalType, class PositionType, class StatusType,
25 class OutputType>
Austin Schuh9fe68f72019-08-10 19:32:03 -070026class ControlLoop {
brians343bc112013-02-10 01:53:46 +000027 public:
James Kuszmaul61750662021-06-21 21:32:33 -070028 ControlLoop(aos::EventLoop *event_loop, const ::std::string &name)
Austin Schuhd3cd6992019-01-27 22:44:07 -080029 : event_loop_(event_loop), name_(name) {
Alex Perrycb7da4b2019-08-28 19:35:56 -070030 output_sender_ = event_loop_->MakeSender<OutputType>(name_);
31 status_sender_ = event_loop_->MakeSender<StatusType>(name_);
32 goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(name_);
33 robot_state_fetcher_ = event_loop_->MakeFetcher<::aos::RobotState>("/aos");
Austin Schuheeec74a2019-01-27 20:58:59 -080034 joystick_state_fetcher_ =
Alex Perrycb7da4b2019-08-28 19:35:56 -070035 event_loop_->MakeFetcher<::aos::JoystickState>("/aos");
Austin Schuh9fe68f72019-08-10 19:32:03 -070036
Alex Perrycb7da4b2019-08-28 19:35:56 -070037 event_loop_->MakeWatcher(name_, [this](const PositionType &position) {
38 this->IteratePosition(position);
39 });
Austin Schuheeec74a2019-01-27 20:58:59 -080040 }
41
42 const ::aos::RobotState &robot_state() const { return *robot_state_fetcher_; }
43 bool has_joystick_state() const { return joystick_state_fetcher_.get(); }
44 const ::aos::JoystickState &joystick_state() const {
45 return *joystick_state_fetcher_;
Austin Schuha1654ed2019-01-27 17:24:54 -080046 }
Brian Silverman699f0cb2015-02-05 19:45:01 -050047
48 // Returns true if all the counters etc in the sensor data have been reset.
49 // This will return true only a single time per reset.
50 bool WasReset() {
51 if (reset_) {
52 reset_ = false;
53 return true;
54 } else {
55 return false;
56 }
57 }
58
Brian Silvermand1e65b92014-03-08 17:07:14 -080059 // Constructs and sends a message on the output queue which sets everything to
Austin Schuhaebfb4f2019-01-27 13:26:38 -080060 // a safe state. Default is to set everything to zero. Override Zero below
61 // to change that behavior.
62 void ZeroOutputs();
brians343bc112013-02-10 01:53:46 +000063
64 // Sets the output to zero.
Austin Schuhaebfb4f2019-01-27 13:26:38 -080065 // Override this if a value of zero (or false) is not "off" for this
66 // subsystem.
Alex Perrycb7da4b2019-08-28 19:35:56 -070067 virtual flatbuffers::Offset<OutputType> Zero(
68 typename ::aos::Sender<OutputType>::Builder *builder) {
69 return builder->template MakeBuilder<OutputType>().Finish();
70 }
brians343bc112013-02-10 01:53:46 +000071
brians343bc112013-02-10 01:53:46 +000072 protected:
Austin Schuh9fe68f72019-08-10 19:32:03 -070073 // Runs one cycle of the loop.
Austin Schuha1654ed2019-01-27 17:24:54 -080074 void IteratePosition(const PositionType &position);
75
James Kuszmaul61750662021-06-21 21:32:33 -070076 aos::EventLoop *event_loop() { return event_loop_; }
Theo Bafrali3274a182019-02-17 20:01:38 -080077
Alex Perrycb7da4b2019-08-28 19:35:56 -070078 // Returns the position context. This is only valid inside the RunIteration
79 // method.
80 const aos::Context &position_context() { return event_loop_->context(); }
81
brians343bc112013-02-10 01:53:46 +000082 // Runs an iteration of the control loop.
Brian Silverman089f5812015-02-15 01:58:19 -050083 // goal is the last goal that was sent. It might be any number of cycles old
84 // or nullptr if we haven't ever received a goal.
85 // position is the current position, or nullptr if we didn't get a position
86 // this cycle.
87 // output is the values to be sent to the motors. This is nullptr if the
88 // output is going to be ignored and set to 0.
brians343bc112013-02-10 01:53:46 +000089 // status is the status of the control loop.
90 // Both output and status should be filled in by the implementation.
Alex Perrycb7da4b2019-08-28 19:35:56 -070091 virtual void RunIteration(
92 const GoalType *goal, const PositionType *position,
93 typename ::aos::Sender<OutputType>::Builder *output,
94 typename ::aos::Sender<StatusType>::Builder *status) = 0;
brians343bc112013-02-10 01:53:46 +000095
96 private:
Austin Schuh61bdc602016-12-04 19:10:10 -080097 static constexpr ::std::chrono::milliseconds kStaleLogInterval =
98 ::std::chrono::milliseconds(100);
Brian Silverman699f0cb2015-02-05 19:45:01 -050099 // The amount of time after the last PWM pulse we consider motors enabled for.
100 // 100ms is the result of using an oscilliscope to look at the input and
101 // output of a Talon. The Info Sheet also lists 100ms for Talon SR, Talon SRX,
102 // and Victor SP.
Austin Schuh61bdc602016-12-04 19:10:10 -0800103 static constexpr ::std::chrono::milliseconds kPwmDisableTime =
104 ::std::chrono::milliseconds(100);
Brian Silverman699f0cb2015-02-05 19:45:01 -0500105
brians343bc112013-02-10 01:53:46 +0000106 // Pointer to the queue group
James Kuszmaul61750662021-06-21 21:32:33 -0700107 aos::EventLoop *event_loop_;
Austin Schuheeec74a2019-01-27 20:58:59 -0800108 ::std::string name_;
Brian Silverman50a9d032014-02-16 17:20:57 -0800109
Austin Schuha1654ed2019-01-27 17:24:54 -0800110 ::aos::Sender<OutputType> output_sender_;
111 ::aos::Sender<StatusType> status_sender_;
112 ::aos::Fetcher<GoalType> goal_fetcher_;
Austin Schuheeec74a2019-01-27 20:58:59 -0800113 ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
114 ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
115
116 // Fetcher only to be used for the Iterate method. If Iterate is called, Run
117 // can't be called.
118 bool has_iterate_fetcher_ = false;
119 ::aos::Fetcher<PositionType> iterate_position_fetcher_;
Austin Schuha1654ed2019-01-27 17:24:54 -0800120
Brian Silverman699f0cb2015-02-05 19:45:01 -0500121 bool reset_ = false;
122 int32_t sensor_reader_pid_ = 0;
123
Austin Schuh61bdc602016-12-04 19:10:10 -0800124 ::aos::monotonic_clock::time_point last_pwm_sent_ =
125 ::aos::monotonic_clock::min_time;
Austin Schuh3d6e3df2014-02-17 01:51:03 -0800126
Brian Silverman50a9d032014-02-16 17:20:57 -0800127 typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
Brian Silverman699f0cb2015-02-05 19:45:01 -0500128 SimpleLogInterval no_sensor_state_ =
129 SimpleLogInterval(kStaleLogInterval, ERROR, "no sensor state");
Brian Silverman2704ecf2014-04-09 20:24:03 -0700130 SimpleLogInterval motors_off_log_ =
131 SimpleLogInterval(kStaleLogInterval, WARNING, "motors disabled");
Brian Silverman699f0cb2015-02-05 19:45:01 -0500132 SimpleLogInterval no_goal_ =
133 SimpleLogInterval(kStaleLogInterval, ERROR, "no goal");
brians343bc112013-02-10 01:53:46 +0000134};
135
Brian Silverman38111502014-04-10 12:36:26 -0700136} // namespace controls
James Kuszmaul61750662021-06-21 21:32:33 -0700137} // namespace frc971
brians343bc112013-02-10 01:53:46 +0000138
James Kuszmaul61750662021-06-21 21:32:33 -0700139#include "frc971/control_loops/control_loop-tmpl.h" // IWYU pragma: export
brians343bc112013-02-10 01:53:46 +0000140
141#endif