blob: 58f92a21ac148922d541ef0a78000e6bb55cdb51 [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
Brian Silvermanb407c672014-04-09 11:58:37 -07004#include <string.h>
Austin Schuhb58ceb62017-02-05 14:21:57 -08005#include <atomic>
brians343bc112013-02-10 01:53:46 +00006
Austin Schuha1654ed2019-01-27 17:24:54 -08007#include "aos/events/event-loop.h"
8#include "aos/events/shm-event-loop.h"
John Park33858a32018-09-28 23:05:48 -07009#include "aos/queue.h"
Austin Schuheeec74a2019-01-27 20:58:59 -080010#include "aos/robot_state/robot_state.q.h"
John Park33858a32018-09-28 23:05:48 -070011#include "aos/time/time.h"
12#include "aos/type_traits/type_traits.h"
13#include "aos/util/log_interval.h"
brians343bc112013-02-10 01:53:46 +000014
15namespace aos {
Brian Silverman38111502014-04-10 12:36:26 -070016namespace controls {
brians343bc112013-02-10 01:53:46 +000017
18// Interface to describe runnable jobs.
19class Runnable {
20 public:
21 virtual ~Runnable() {}
22 // Runs forever.
23 virtual void Run() = 0;
24 // Does one quick piece of work and return. Does _not_ block.
25 virtual void Iterate() = 0;
26};
27
Brian Silverman3204dd82013-03-12 18:42:01 -070028// Control loops run this often, "starting" at time 0.
Austin Schuh214e9c12016-11-25 17:26:20 -080029constexpr ::std::chrono::nanoseconds kLoopFrequency =
30 ::std::chrono::milliseconds(5);
Brian Silverman3204dd82013-03-12 18:42:01 -070031
brians343bc112013-02-10 01:53:46 +000032// Provides helper methods to assist in writing control loops.
33// This template expects to be constructed with a queue group as an argument
34// that has a goal, position, status, and output queue.
35// It will then call the RunIteration method every cycle that it has enough
36// valid data for the control loop to run.
Brian Silverman089f5812015-02-15 01:58:19 -050037template <class T>
Austin Schuhfd6e16c2019-01-27 12:22:47 -080038class ControlLoop : public Runnable {
brians343bc112013-02-10 01:53:46 +000039 public:
brians343bc112013-02-10 01:53:46 +000040 // Create some convenient typedefs to reference the Goal, Position, Status,
41 // and Output structures.
42 typedef typename std::remove_reference<
43 decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
44 GoalType;
45 typedef typename std::remove_reference<
46 decltype(*(static_cast<T *>(NULL)->position.MakeMessage().get()))>::type
47 PositionType;
48 typedef typename std::remove_reference<
49 decltype(*(static_cast<T *>(NULL)->status.MakeMessage().get()))>::type
50 StatusType;
51 typedef typename std::remove_reference<
52 decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
53 OutputType;
54
Austin Schuhd3cd6992019-01-27 22:44:07 -080055 ControlLoop(EventLoop *event_loop, const ::std::string &name)
56 : event_loop_(event_loop), name_(name) {
Austin Schuheeec74a2019-01-27 20:58:59 -080057 output_sender_ = event_loop_->MakeSender<OutputType>(name_ + ".output");
58 status_sender_ = event_loop_->MakeSender<StatusType>(name_ + ".status");
59 goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(name_ + ".goal");
60 robot_state_fetcher_ =
61 event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state");
62 joystick_state_fetcher_ =
63 event_loop_->MakeFetcher<::aos::JoystickState>(".aos.joystick_state");
64 }
65
66 const ::aos::RobotState &robot_state() const { return *robot_state_fetcher_; }
67 bool has_joystick_state() const { return joystick_state_fetcher_.get(); }
68 const ::aos::JoystickState &joystick_state() const {
69 return *joystick_state_fetcher_;
Austin Schuha1654ed2019-01-27 17:24:54 -080070 }
Brian Silverman699f0cb2015-02-05 19:45:01 -050071
72 // Returns true if all the counters etc in the sensor data have been reset.
73 // This will return true only a single time per reset.
74 bool WasReset() {
75 if (reset_) {
76 reset_ = false;
77 return true;
78 } else {
79 return false;
80 }
81 }
82
Brian Silvermand1e65b92014-03-08 17:07:14 -080083 // Constructs and sends a message on the output queue which sets everything to
Austin Schuhaebfb4f2019-01-27 13:26:38 -080084 // a safe state. Default is to set everything to zero. Override Zero below
85 // to change that behavior.
86 void ZeroOutputs();
brians343bc112013-02-10 01:53:46 +000087
88 // Sets the output to zero.
Austin Schuhaebfb4f2019-01-27 13:26:38 -080089 // Override this if a value of zero (or false) is not "off" for this
90 // subsystem.
brians343bc112013-02-10 01:53:46 +000091 virtual void Zero(OutputType *output) { output->Zero(); }
92
93 // Runs the loop forever.
Austin Schuh520f33d2019-01-27 22:38:01 -080094 // TODO(austin): This should move to the event loop once it gets hoisted out.
Brian Silverman699f0cb2015-02-05 19:45:01 -050095 void Run() override;
brians343bc112013-02-10 01:53:46 +000096
97 // Runs one cycle of the loop.
Austin Schuh520f33d2019-01-27 22:38:01 -080098 // TODO(austin): This should go away when all the tests use event loops
99 // directly.
Brian Silverman699f0cb2015-02-05 19:45:01 -0500100 void Iterate() override;
brians343bc112013-02-10 01:53:46 +0000101
brians343bc112013-02-10 01:53:46 +0000102 protected:
Austin Schuha1654ed2019-01-27 17:24:54 -0800103 void IteratePosition(const PositionType &position);
104
Austin Schuhb58ceb62017-02-05 14:21:57 -0800105 static void Quit(int /*signum*/) {
106 run_ = false;
107 }
108
brians343bc112013-02-10 01:53:46 +0000109 // Runs an iteration of the control loop.
Brian Silverman089f5812015-02-15 01:58:19 -0500110 // goal is the last goal that was sent. It might be any number of cycles old
111 // or nullptr if we haven't ever received a goal.
112 // position is the current position, or nullptr if we didn't get a position
113 // this cycle.
114 // output is the values to be sent to the motors. This is nullptr if the
115 // output is going to be ignored and set to 0.
brians343bc112013-02-10 01:53:46 +0000116 // status is the status of the control loop.
117 // Both output and status should be filled in by the implementation.
118 virtual void RunIteration(const GoalType *goal,
119 const PositionType *position,
120 OutputType *output,
121 StatusType *status) = 0;
122
123 private:
Austin Schuh61bdc602016-12-04 19:10:10 -0800124 static constexpr ::std::chrono::milliseconds kStaleLogInterval =
125 ::std::chrono::milliseconds(100);
Brian Silverman699f0cb2015-02-05 19:45:01 -0500126 // The amount of time after the last PWM pulse we consider motors enabled for.
127 // 100ms is the result of using an oscilliscope to look at the input and
128 // output of a Talon. The Info Sheet also lists 100ms for Talon SR, Talon SRX,
129 // and Victor SP.
Austin Schuh61bdc602016-12-04 19:10:10 -0800130 static constexpr ::std::chrono::milliseconds kPwmDisableTime =
131 ::std::chrono::milliseconds(100);
Brian Silverman699f0cb2015-02-05 19:45:01 -0500132
brians343bc112013-02-10 01:53:46 +0000133 // Pointer to the queue group
Austin Schuheeec74a2019-01-27 20:58:59 -0800134 ::std::unique_ptr<ShmEventLoop> shm_event_loop_;
135 EventLoop *event_loop_;
136 ::std::string name_;
Brian Silverman50a9d032014-02-16 17:20:57 -0800137
Austin Schuha1654ed2019-01-27 17:24:54 -0800138 ::aos::Sender<OutputType> output_sender_;
139 ::aos::Sender<StatusType> status_sender_;
140 ::aos::Fetcher<GoalType> goal_fetcher_;
Austin Schuheeec74a2019-01-27 20:58:59 -0800141 ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
142 ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
143
144 // Fetcher only to be used for the Iterate method. If Iterate is called, Run
145 // can't be called.
146 bool has_iterate_fetcher_ = false;
147 ::aos::Fetcher<PositionType> iterate_position_fetcher_;
Austin Schuha1654ed2019-01-27 17:24:54 -0800148
Brian Silverman699f0cb2015-02-05 19:45:01 -0500149 bool reset_ = false;
150 int32_t sensor_reader_pid_ = 0;
151
Austin Schuh61bdc602016-12-04 19:10:10 -0800152 ::aos::monotonic_clock::time_point last_pwm_sent_ =
153 ::aos::monotonic_clock::min_time;
Austin Schuh3d6e3df2014-02-17 01:51:03 -0800154
Brian Silverman50a9d032014-02-16 17:20:57 -0800155 typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
Brian Silverman699f0cb2015-02-05 19:45:01 -0500156 SimpleLogInterval no_sensor_state_ =
157 SimpleLogInterval(kStaleLogInterval, ERROR, "no sensor state");
Brian Silverman2704ecf2014-04-09 20:24:03 -0700158 SimpleLogInterval motors_off_log_ =
159 SimpleLogInterval(kStaleLogInterval, WARNING, "motors disabled");
Brian Silverman699f0cb2015-02-05 19:45:01 -0500160 SimpleLogInterval no_goal_ =
161 SimpleLogInterval(kStaleLogInterval, ERROR, "no goal");
Austin Schuhb58ceb62017-02-05 14:21:57 -0800162
163 static ::std::atomic<bool> run_;
brians343bc112013-02-10 01:53:46 +0000164};
165
Brian Silverman38111502014-04-10 12:36:26 -0700166} // namespace controls
brians343bc112013-02-10 01:53:46 +0000167} // namespace aos
168
John Park33858a32018-09-28 23:05:48 -0700169#include "aos/controls/control_loop-tmpl.h" // IWYU pragma: export
brians343bc112013-02-10 01:53:46 +0000170
171#endif