blob: 812f0912fc35ff50bdc4b7a190c79af67a8345ee [file] [log] [blame]
Austin Schuhcb091712018-02-21 20:01:55 -08001#include "y2018/control_loops/superstructure/arm/arm.h"
2
3#include <chrono>
4#include <iostream>
5
6#include "aos/common/logging/logging.h"
7#include "aos/common/logging/queue_logging.h"
8#include "y2018/constants.h"
9#include "y2018/control_loops/superstructure/arm/demo_path.h"
10#include "y2018/control_loops/superstructure/arm/dynamics.h"
Austin Schuh7dfccf62018-03-03 21:28:14 -080011#include "y2018/control_loops/superstructure/arm/generated_graph.h"
Austin Schuhcb091712018-02-21 20:01:55 -080012
13namespace y2018 {
14namespace control_loops {
15namespace superstructure {
16namespace arm {
17
18namespace chrono = ::std::chrono;
19using ::aos::monotonic_clock;
20
Austin Schuhcb091712018-02-21 20:01:55 -080021Arm::Arm()
22 : proximal_zeroing_estimator_(constants::GetValues().arm_proximal.zeroing),
23 distal_zeroing_estimator_(constants::GetValues().arm_distal.zeroing),
24 alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
25 0.0, 0.0, 1.0 / kAlpha1Max())
26 .finished()),
Austin Schuh7dfccf62018-03-03 21:28:14 -080027 search_graph_(MakeSearchGraph(&trajectories_, alpha_unitizer_, kVMax())),
Austin Schuhcb091712018-02-21 20:01:55 -080028 // Go to the start of the first trajectory.
Austin Schuhb874fd32018-03-05 00:27:10 -080029 follower_(ReadyAboveBoxPoint()),
30 points_(PointList()) {
Austin Schuh7dfccf62018-03-03 21:28:14 -080031 int i = 0;
32 for (const auto &trajectory : trajectories_) {
33 LOG(INFO, "trajectory length for edge node %d: %f\n", i,
Austin Schuh41c71e42018-04-04 20:11:20 -070034 trajectory.trajectory.path().length());
Austin Schuh7dfccf62018-03-03 21:28:14 -080035 ++i;
Austin Schuhcb091712018-02-21 20:01:55 -080036 }
37}
38
39void Arm::Reset() { state_ = State::UNINITIALIZED; }
40
Austin Schuh96341532018-03-09 21:17:24 -080041void Arm::Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
Neil Balchba9cbba2018-04-06 22:26:38 -070042 bool close_claw, const control_loops::ArmPosition *position,
Austin Schuh96341532018-03-09 21:17:24 -080043 const bool claw_beambreak_triggered,
44 const bool box_back_beambreak_triggered,
Austin Schuhd76546a2018-07-08 16:05:14 -070045 const bool intake_clear_of_box, bool suicide,
46 bool trajectory_override, double *proximal_output,
Austin Schuh96341532018-03-09 21:17:24 -080047 double *distal_output, bool *release_arm_brake,
Austin Schuhd76546a2018-07-08 16:05:14 -070048 bool *claw_closed, control_loops::ArmStatus *status) {
Austin Schuhcb091712018-02-21 20:01:55 -080049 ::Eigen::Matrix<double, 2, 1> Y;
Austin Schuh96341532018-03-09 21:17:24 -080050 const bool outputs_disabled =
51 ((proximal_output == nullptr) || (distal_output == nullptr) ||
52 (release_arm_brake == nullptr) || (claw_closed == nullptr));
53
54 uint32_t filtered_goal = 0;
55 if (unsafe_goal != nullptr) {
56 filtered_goal = *unsafe_goal;
57 }
58
59 if (open_claw) {
60 claw_closed_ = false;
61 }
Neil Balchba9cbba2018-04-06 22:26:38 -070062 if (close_claw) {
Austin Schuh96341532018-03-09 21:17:24 -080063 claw_closed_ = true;
64 }
Neil Balchba9cbba2018-04-06 22:26:38 -070065 if (outputs_disabled) {
66 if (claw_closed_count_ == 0) {
67 claw_closed_ = true;
68 } else {
69 --claw_closed_count_;
70 }
71 } else {
72 // Wait this many iterations before closing the claw. That prevents
73 // brownouts from closing the claw.
74 claw_closed_count_ = 50;
75 }
Austin Schuhcb091712018-02-21 20:01:55 -080076
77 Y << position->proximal.encoder + proximal_offset_,
78 position->distal.encoder + distal_offset_;
79
80 proximal_zeroing_estimator_.UpdateEstimate(position->proximal);
81 distal_zeroing_estimator_.UpdateEstimate(position->distal);
82
83 if (proximal_output != nullptr) {
84 *proximal_output = 0.0;
85 }
86 if (distal_output != nullptr) {
87 *distal_output = 0.0;
88 }
89
90 arm_ekf_.Correct(Y, kDt());
91
92 if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= 0.05 &&
93 ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= 0.05) {
94 close_enough_for_full_power_ = true;
95 }
96 if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) >= 1.10 ||
97 ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) >= 1.10) {
98 close_enough_for_full_power_ = false;
99 }
100
101 switch (state_) {
102 case State::UNINITIALIZED:
103 // Wait in the uninitialized state until the intake is initialized.
104 LOG(DEBUG, "Uninitialized, waiting for intake\n");
105 state_ = State::ZEROING;
106 proximal_zeroing_estimator_.Reset();
107 distal_zeroing_estimator_.Reset();
Austin Schuhcb091712018-02-21 20:01:55 -0800108 break;
109
110 case State::ZEROING:
111 // Zero by not moving.
112 if (proximal_zeroing_estimator_.zeroed() &&
113 distal_zeroing_estimator_.zeroed()) {
Austin Schuh96341532018-03-09 21:17:24 -0800114 state_ = State::DISABLED;
Austin Schuhcb091712018-02-21 20:01:55 -0800115
116 proximal_offset_ = proximal_zeroing_estimator_.offset();
117 distal_offset_ = distal_zeroing_estimator_.offset();
118
119 Y << position->proximal.encoder + proximal_offset_,
120 position->distal.encoder + distal_offset_;
121
122 // TODO(austin): Offset ekf rather than reset it. Since we aren't
123 // moving at this point, it's pretty safe to do this.
124 ::Eigen::Matrix<double, 4, 1> X;
125 X << Y(0), 0.0, Y(1), 0.0;
126 arm_ekf_.Reset(X);
127 } else {
128 break;
129 }
130
Austin Schuhb874fd32018-03-05 00:27:10 -0800131 case State::DISABLED: {
132 follower_.SwitchTrajectory(nullptr);
133 close_enough_for_full_power_ = false;
Austin Schuh96341532018-03-09 21:17:24 -0800134
Austin Schuhb874fd32018-03-05 00:27:10 -0800135 const ::Eigen::Matrix<double, 2, 1> current_theta =
136 (::Eigen::Matrix<double, 2, 1>() << arm_ekf_.X_hat(0),
137 arm_ekf_.X_hat(2))
138 .finished();
139 uint32_t best_index = 0;
140 double best_distance = (points_[0] - current_theta).norm();
141 uint32_t current_index = 0;
142 for (const ::Eigen::Matrix<double, 2, 1> &point : points_) {
143 const double new_distance = (point - current_theta).norm();
144 if (new_distance < best_distance) {
145 best_distance = new_distance;
146 best_index = current_index;
147 }
148 ++current_index;
149 }
150 follower_.set_theta(points_[best_index]);
151 current_node_ = best_index;
152
153 if (!outputs_disabled) {
Austin Schuh96341532018-03-09 21:17:24 -0800154 state_ = State::GOTO_PATH;
155 } else {
156 break;
157 }
Austin Schuhb874fd32018-03-05 00:27:10 -0800158 }
Austin Schuh96341532018-03-09 21:17:24 -0800159
160 case State::GOTO_PATH:
161 if (outputs_disabled) {
162 state_ = State::DISABLED;
Austin Schuhd76546a2018-07-08 16:05:14 -0700163 } else if (trajectory_override) {
164 follower_.SwitchTrajectory(nullptr);
165 current_node_ = filtered_goal;
166 follower_.set_theta(points_[current_node_]);
167 state_ = State::GOTO_PATH;
Austin Schuh96341532018-03-09 21:17:24 -0800168 } else if (close_enough_for_full_power_) {
169 state_ = State::RUNNING;
170 grab_state_ = GrabState::NORMAL;
171 }
172 break;
173
Austin Schuhcb091712018-02-21 20:01:55 -0800174 case State::RUNNING:
175 // ESTOP if we hit the hard limits.
176 // TODO(austin): Pick some sane limits.
177 if (proximal_zeroing_estimator_.error() ||
178 distal_zeroing_estimator_.error()) {
179 LOG(ERROR, "Zeroing error ESTOP\n");
180 state_ = State::ESTOP;
Austin Schuh96341532018-03-09 21:17:24 -0800181 } else if (outputs_disabled) {
182 state_ = State::DISABLED;
Austin Schuhd76546a2018-07-08 16:05:14 -0700183 } else if (trajectory_override) {
184 follower_.SwitchTrajectory(nullptr);
185 current_node_ = filtered_goal;
186 follower_.set_theta(points_[current_node_]);
187 state_ = State::GOTO_PATH;
Austin Schuh17e484e2018-03-11 01:11:36 -0800188 } else if (suicide) {
189 state_ = State::PREP_CLIMB;
190 climb_count_ = 50;
191 }
192 break;
193
194 case State::PREP_CLIMB:
195 --climb_count_;
196 if (climb_count_ <= 0) {
197 state_ = State::ESTOP;
198 } else if (!suicide) {
199 state_ = State::RUNNING;
Austin Schuhcb091712018-02-21 20:01:55 -0800200 }
201 break;
202
203 case State::ESTOP:
204 LOG(ERROR, "Estop\n");
205 break;
206 }
207
Austin Schuh17e484e2018-03-11 01:11:36 -0800208 const bool disable = outputs_disabled || (state_ != State::RUNNING &&
209 state_ != State::GOTO_PATH &&
210 state_ != State::PREP_CLIMB);
Austin Schuhcb091712018-02-21 20:01:55 -0800211 if (disable) {
212 close_enough_for_full_power_ = false;
213 }
214
Austin Schuhb874fd32018-03-05 00:27:10 -0800215 // TODO(austin): Do we need to debounce box_back_beambreak_triggered ?
Austin Schuh96341532018-03-09 21:17:24 -0800216 if (claw_closed_) {
217 if ((filtered_goal == ReadyAboveBoxIndex()) ||
218 (filtered_goal == TallBoxGrabIndex()) ||
219 (filtered_goal == ShortBoxGrabIndex())) {
220 filtered_goal = NeutralIndex();
221 }
222 }
223
224 // TODO(austin): Do we need to debounce box_back_beambreak_triggered ?
225 switch (grab_state_) {
226 case GrabState::NORMAL:
227 if (grab_box && !claw_closed_) {
228 grab_state_ = GrabState::WAIT_FOR_BOX;
229 } else {
230 break;
231 }
232 case GrabState::WAIT_FOR_BOX:
233 if (!grab_box) {
234 grab_state_ = GrabState::NORMAL;
235 } else {
236 if (AtState(ReadyAboveBoxIndex()) && NearEnd()) {
237 // We are being asked to grab the box, and the claw is near the box.
238 if (box_back_beambreak_triggered) {
239 // And we now see the box! Try for a tall box.
240 grab_state_ = GrabState::TALL_BOX;
241 }
242 }
243 }
244 break;
245 case GrabState::TALL_BOX:
246 if (!grab_box) {
247 grab_state_ = GrabState::NORMAL;
Austin Schuh96341532018-03-09 21:17:24 -0800248 } else if (AtState(TallBoxGrabIndex()) && NearEnd()) {
249 // We are being asked to grab the box, and the claw is near the box.
250 if (claw_beambreak_triggered) {
251 grab_state_ = GrabState::CLAW_CLOSE;
252 // Snap time for the delay here.
253 claw_close_start_time_ = ::aos::monotonic_clock::now();
254 } else {
255 grab_state_ = GrabState::SHORT_BOX;
256 }
257 }
258 break;
259 case GrabState::SHORT_BOX:
260 if (!grab_box) {
261 grab_state_ = GrabState::NORMAL;
Austin Schuh96341532018-03-09 21:17:24 -0800262 } else if (AtState(ShortBoxGrabIndex()) && NearEnd()) {
263 // We are being asked to grab the box, and the claw is near the box.
264 if (claw_beambreak_triggered) {
265 grab_state_ = GrabState::CLAW_CLOSE;
266 // Snap time for the delay here.
267 claw_close_start_time_ = ::aos::monotonic_clock::now();
268 } else {
269 grab_state_ = GrabState::WAIT_FOR_BOX;
270 }
271 }
272 break;
273 case GrabState::CLAW_CLOSE:
274 if (::aos::monotonic_clock::now() >
Austin Schuhb874fd32018-03-05 00:27:10 -0800275 claw_close_start_time_ + ::std::chrono::milliseconds(300)) {
Austin Schuh96341532018-03-09 21:17:24 -0800276 grab_state_ = GrabState::OPEN_INTAKE;
277 }
278 break;
279 case GrabState::OPEN_INTAKE:
280 if (intake_clear_of_box) {
281 grab_state_ = GrabState::NORMAL;
282 }
283 break;
284 }
285
286 // Now, based out our current state, go to the right state.
287 switch (grab_state_) {
288 case GrabState::NORMAL:
289 // Don't let the intake close fully with the claw closed.
290 // TODO(austin): If we want to transfer the box from the claw to the
291 // intake, we'll need to change this.
292 if (claw_closed_) {
293 max_intake_override_ = -0.5;
294 } else {
295 max_intake_override_ = 1000.0;
296 }
297 break;
298 case GrabState::WAIT_FOR_BOX:
299 filtered_goal = ReadyAboveBoxIndex();
300 claw_closed_ = false;
301 max_intake_override_ = 1000.0;
302 break;
303 case GrabState::TALL_BOX:
304 filtered_goal = TallBoxGrabIndex();
305 claw_closed_ = false;
306 max_intake_override_ = 1000.0;
307 break;
308 case GrabState::SHORT_BOX:
309 filtered_goal = ShortBoxGrabIndex();
310 claw_closed_ = false;
311 max_intake_override_ = 1000.0;
312 break;
313 case GrabState::CLAW_CLOSE:
314 // Don't move.
315 filtered_goal = current_node_;
316 claw_closed_ = true;
317 max_intake_override_ = 1000.0;
318 break;
319 case GrabState::OPEN_INTAKE:
320 // Don't move.
321 filtered_goal = current_node_;
322 claw_closed_ = true;
323 max_intake_override_ = -0.5;
324 break;
325 }
326
327 if (state_ == State::RUNNING && unsafe_goal != nullptr) {
328 if (current_node_ != filtered_goal) {
329 LOG(INFO, "Goal is different\n");
330 if (filtered_goal >= search_graph_.num_vertexes()) {
331 LOG(ERROR, "goal node out of range ESTOP\n");
332 state_ = State::ESTOP;
333 } else if (follower_.path_distance_to_go() > 1e-3) {
334 // Still on the old path segment. Can't change yet.
335 } else {
336 search_graph_.SetGoal(filtered_goal);
337
338 size_t min_edge = 0;
339 double min_cost = ::std::numeric_limits<double>::infinity();
340 for (const SearchGraph::HalfEdge &edge :
341 search_graph_.Neighbors(current_node_)) {
342 const double cost = search_graph_.GetCostToGoal(edge.dest);
343 if (cost < min_cost) {
344 min_edge = edge.edge_id;
345 min_cost = cost;
346 }
347 }
348 // Ok, now we know which edge we are on. Figure out the path and
349 // trajectory.
350 const SearchGraph::Edge &next_edge = search_graph_.edges()[min_edge];
351 LOG(INFO, "Switching from node %d to %d along edge %d\n",
352 static_cast<int>(current_node_), static_cast<int>(next_edge.end),
353 static_cast<int>(min_edge));
Austin Schuh41c71e42018-04-04 20:11:20 -0700354 vmax_ = trajectories_[min_edge].vmax;
355 follower_.SwitchTrajectory(&trajectories_[min_edge].trajectory);
Austin Schuh96341532018-03-09 21:17:24 -0800356 current_node_ = next_edge.end;
357 }
358 }
359 }
360
Austin Schuh345a3732018-03-21 20:49:32 -0700361 const double max_operating_voltage =
362 close_enough_for_full_power_
363 ? kOperatingVoltage()
364 : (state_ == State::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
Austin Schuh41c71e42018-04-04 20:11:20 -0700365 follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
Austin Schuh345a3732018-03-21 20:49:32 -0700366 max_operating_voltage);
367 LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
Austin Schuhcb091712018-02-21 20:01:55 -0800368 status->goal_theta0 = follower_.theta(0);
369 status->goal_theta1 = follower_.theta(1);
370 status->goal_omega0 = follower_.omega(0);
371 status->goal_omega1 = follower_.omega(1);
372
373 status->theta0 = arm_ekf_.X_hat(0);
374 status->theta1 = arm_ekf_.X_hat(2);
375 status->omega0 = arm_ekf_.X_hat(1);
376 status->omega1 = arm_ekf_.X_hat(3);
377 status->voltage_error0 = arm_ekf_.X_hat(4);
378 status->voltage_error1 = arm_ekf_.X_hat(5);
379
380 if (!disable) {
381 *proximal_output = ::std::max(
382 -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(0)));
383 *distal_output = ::std::max(
384 -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
Austin Schuh17e484e2018-03-11 01:11:36 -0800385 if (state_ != State::PREP_CLIMB) {
386 *release_arm_brake = true;
387 } else {
388 *release_arm_brake = false;
389 }
Austin Schuh96341532018-03-09 21:17:24 -0800390 *claw_closed = claw_closed_;
Austin Schuhcb091712018-02-21 20:01:55 -0800391 }
392
393 status->proximal_estimator_state =
394 proximal_zeroing_estimator_.GetEstimatorState();
395 status->distal_estimator_state =
396 distal_zeroing_estimator_.GetEstimatorState();
397
398 status->path_distance_to_go = follower_.path_distance_to_go();
399 status->current_node = current_node_;
400
401 status->zeroed = (proximal_zeroing_estimator_.zeroed() &&
402 distal_zeroing_estimator_.zeroed());
403 status->estopped = (state_ == State::ESTOP);
404 status->state = static_cast<int32_t>(state_);
Austin Schuh96341532018-03-09 21:17:24 -0800405 status->grab_state = static_cast<int32_t>(grab_state_);
Austin Schuhcb091712018-02-21 20:01:55 -0800406 status->failed_solutions = follower_.failed_solutions();
407
408 arm_ekf_.Predict(follower_.U(), kDt());
409}
410
411} // namespace arm
412} // namespace superstructure
413} // namespace control_loops
414} // namespace y2018