blob: 44ec4e824b0f81ee99901134ec6b8267a250b142 [file] [log] [blame]
Austin Schuh3bb9a442014-02-02 16:01:45 -08001#include "frc971/control_loops/claw/claw.h"
2
Austin Schuh3bb9a442014-02-02 16:01:45 -08003#include <algorithm>
4
Briana6553ed2014-04-02 21:26:46 -07005#include "aos/common/controls/control_loops.q.h"
Austin Schuh3bb9a442014-02-02 16:01:45 -08006#include "aos/common/logging/logging.h"
Brian Silvermanf48fab32014-03-09 14:32:24 -07007#include "aos/common/logging/queue_logging.h"
Brian Silverman6dd2c532014-03-29 23:34:39 -07008#include "aos/common/logging/matrix_logging.h"
Brian Silvermanad9e0002014-04-13 14:55:57 -07009#include "aos/common/commonmath.h"
Austin Schuh3bb9a442014-02-02 16:01:45 -080010
11#include "frc971/constants.h"
Austin Schuhcda86af2014-02-16 16:16:39 -080012#include "frc971/control_loops/claw/claw_motor_plant.h"
13
Austin Schuh3bb9a442014-02-02 16:01:45 -080014// Zeroing plan.
15// There are 2 types of zeros. Enabled and disabled ones.
16// Disabled ones are only valid during auto mode, and can be used to speed up
17// the enabled zero process. We need to re-zero during teleop in case the auto
18// zero was poor and causes us to miss all our shots.
19//
20// We need to be able to zero manually while disabled by moving the joint over
21// the zeros.
22// Zero on the down edge when disabled (gravity in the direction of motion)
23//
24// When enabled, zero on the up edge (gravity opposing the direction of motion)
25// The enabled sequence needs to work as follows. We can crash the claw if we
26// bring them too close to each other or too far from each other. The only safe
27// thing to do is to move them in unison.
28//
29// Start by moving them both towards the front of the bot to either find either
30// the middle hall effect on either jaw, or the front hall effect on the bottom
31// jaw. Any edge that isn't the desired edge will provide an approximate edge
32// location that can be used for the fine tuning step.
33// Once an edge is found on the front claw, move back the other way with both
34// claws until an edge is found for the other claw.
35// Now that we have an approximate zero, we can robustify the limits to keep
36// both claws safe. Then, we can move both claws to a position that is the
37// correct side of the zero and go zero.
38
39// Valid region plan.
Ben Fredrickson81ba2d52014-03-02 08:21:46 +000040// Difference between the arms has a range, and the values of each arm has a
41// range.
Austin Schuh3bb9a442014-02-02 16:01:45 -080042// If a claw runs up against a static limit, don't let the goal change outside
43// the limit.
44// If a claw runs up against a movable limit, move both claws outwards to get
45// out of the condition.
46
47namespace frc971 {
48namespace control_loops {
49
Austin Schuh01c652b2014-02-21 23:13:42 -080050static const double kZeroingVoltage = 4.0;
51static const double kMaxVoltage = 12.0;
Brian Silverman084372e2014-04-10 10:55:53 -070052const double kRezeroThreshold = 0.03;
Austin Schuhf84a1302014-02-19 00:23:30 -080053
Brian Silverman0a151c92014-05-02 15:28:44 -070054ClawLimitedLoop::ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> &&loop)
55 : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
Austin Schuh27b8fb12014-02-22 15:10:05 -080056 uncapped_average_voltage_(0.0),
James Kuszmaulf63b0ae2014-03-25 16:52:11 -070057 is_zeroing_(true),
58 U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
59 -1, 0,
60 0, 1,
61 0, -1).finished(),
62 (Eigen::Matrix<double, 4, 1>() << kMaxVoltage, kMaxVoltage,
Brian Silverman6dd2c532014-03-29 23:34:39 -070063 kMaxVoltage, kMaxVoltage).finished()),
64 U_Poly_zeroing_((Eigen::Matrix<double, 4, 2>() << 1, 0,
65 -1, 0,
66 0, 1,
67 0, -1).finished(),
68 (Eigen::Matrix<double, 4, 1>() <<
69 kZeroingVoltage, kZeroingVoltage,
70 kZeroingVoltage, kZeroingVoltage).finished()) {
James Kuszmaulf63b0ae2014-03-25 16:52:11 -070071 ::aos::controls::HPolytope<0>::Init();
72}
Austin Schuh27b8fb12014-02-22 15:10:05 -080073
James Kuszmaulf63b0ae2014-03-25 16:52:11 -070074// Caps the voltage prioritizing reducing velocity error over reducing
75// positional error.
76// Uses the polytope libararies which we used to just use for the drivetrain.
77// Uses a region representing the maximum voltage and then transforms it such
78// that the points represent different amounts of positional error and
79// constrains the region such that, if at all possible, it will maintain its
80// current efforts to reduce velocity error.
Austin Schuhcda86af2014-02-16 16:16:39 -080081void ClawLimitedLoop::CapU() {
Brian Silvermanad9e0002014-04-13 14:55:57 -070082 const Eigen::Matrix<double, 4, 1> error = R - X_hat;
Austin Schuh4cb047f2014-02-16 21:10:19 -080083
James Kuszmauld536a402014-02-18 22:32:12 -080084 double u_top = U(1, 0);
85 double u_bottom = U(0, 0);
Austin Schuhcda86af2014-02-16 16:16:39 -080086
James Kuszmaulf63b0ae2014-03-25 16:52:11 -070087 uncapped_average_voltage_ = (u_top + u_bottom) / 2;
88
89 double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
90
Brian Silvermanad9e0002014-04-13 14:55:57 -070091 if (::std::abs(u_bottom) > max_voltage || ::std::abs(u_top) > max_voltage) {
Brian Silverman6dd2c532014-03-29 23:34:39 -070092 LOG_MATRIX(DEBUG, "U at start", U);
James Kuszmaulf63b0ae2014-03-25 16:52:11 -070093 // H * U <= k
94 // U = UPos + UVel
95 // H * (UPos + UVel) <= k
96 // H * UPos <= k - H * UVel
97
98 // Now, we can do a coordinate transformation and say the following.
99
100 // UPos = position_K * position_error
101 // (H * position_K) * position_error <= k - H * UVel
102
103 Eigen::Matrix<double, 2, 2> position_K;
104 position_K << K(0, 0), K(0, 1),
105 K(1, 0), K(1, 1);
106 Eigen::Matrix<double, 2, 2> velocity_K;
107 velocity_K << K(0, 2), K(0, 3),
108 K(1, 2), K(1, 3);
109
110 Eigen::Matrix<double, 2, 1> position_error;
111 position_error << error(0, 0), error(1, 0);
112 Eigen::Matrix<double, 2, 1> velocity_error;
113 velocity_error << error(2, 0), error(3, 0);
Brian Silverman6dd2c532014-03-29 23:34:39 -0700114 LOG_MATRIX(DEBUG, "error", error);
James Kuszmaulf63b0ae2014-03-25 16:52:11 -0700115
Brian Silverman6dd2c532014-03-29 23:34:39 -0700116 const auto &poly = is_zeroing_ ? U_Poly_zeroing_ : U_Poly_;
117 const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
118 const Eigen::Matrix<double, 4, 1> pos_poly_k =
119 poly.k() - poly.H() * velocity_K * velocity_error;
120 const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
James Kuszmaulf63b0ae2014-03-25 16:52:11 -0700121
Brian Silverman6dd2c532014-03-29 23:34:39 -0700122 Eigen::Matrix<double, 2, 1> adjusted_pos_error;
123 {
124 const auto &P = position_error;
Brian Silvermanb087c0a2014-03-30 12:59:52 -0700125
126 // This line was at 45 degrees but is now at some angle steeper than the
127 // straight one between the points.
128 Eigen::Matrix<double, 1, 2> angle_45;
129 // If the top claw is above its soft upper limit, make the line actually
130 // 45 degrees to avoid smashing it into the limit in an attempt to fix the
131 // separation error faster than the bottom position one.
132 if (X_hat(0, 0) + X_hat(1, 0) >
133 constants::GetValues().claw.upper_claw.upper_limit) {
134 angle_45 << 1, 1;
135 } else {
136 // Fixing separation error half as fast as positional error works well
137 // because it means they both close evenly.
138 angle_45 << ::std::sqrt(3), 1;
139 }
140 Eigen::Matrix<double, 1, 2> L45_quadrant;
Brian Silvermanad9e0002014-04-13 14:55:57 -0700141 L45_quadrant << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
Brian Silvermanb087c0a2014-03-30 12:59:52 -0700142 const auto L45 = L45_quadrant.cwiseProduct(angle_45);
Brian Silverman6dd2c532014-03-29 23:34:39 -0700143 const double w45 = 0;
James Kuszmaulf63b0ae2014-03-25 16:52:11 -0700144
Brian Silverman6dd2c532014-03-29 23:34:39 -0700145 Eigen::Matrix<double, 1, 2> LH;
146 if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
147 LH << 0, 1;
148 } else {
149 LH << 1, 0;
150 }
151 const double wh = LH.dot(P);
James Kuszmaulf63b0ae2014-03-25 16:52:11 -0700152
Brian Silverman6dd2c532014-03-29 23:34:39 -0700153 Eigen::Matrix<double, 2, 2> standard;
154 standard << L45, LH;
155 Eigen::Matrix<double, 2, 1> W;
156 W << w45, wh;
157 const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
158
159 bool is_inside_h;
160 const auto adjusted_pos_error_h =
161 DoCoerceGoal(pos_poly, LH, wh, position_error, &is_inside_h);
162 const auto adjusted_pos_error_45 =
163 DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
164 if (pos_poly.IsInside(intersection)) {
165 adjusted_pos_error = adjusted_pos_error_h;
166 } else {
167 if (is_inside_h) {
168 if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
169 adjusted_pos_error = adjusted_pos_error_h;
170 } else {
171 adjusted_pos_error = adjusted_pos_error_45;
172 }
173 } else {
174 adjusted_pos_error = adjusted_pos_error_45;
175 }
176 }
177 }
178
179 LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
James Kuszmaulf63b0ae2014-03-25 16:52:11 -0700180 U = velocity_K * velocity_error + position_K * adjusted_pos_error;
Brian Silverman6dd2c532014-03-29 23:34:39 -0700181 LOG_MATRIX(DEBUG, "U is now", U);
Brian Silverman7b7c9072014-03-30 13:33:30 -0700182
183 {
184 const auto values = constants::GetValues().claw;
Brian Silvermaned9df2f2014-04-05 07:07:15 -0700185 if (top_known_) {
186 if (X_hat(0, 0) + X_hat(1, 0) > values.upper_claw.upper_limit &&
187 U(1, 0) > 0) {
188 LOG(WARNING, "upper claw too high and moving up\n");
189 U(1, 0) = 0;
190 } else if (X_hat(0, 0) + X_hat(1, 0) < values.upper_claw.lower_limit &&
191 U(1, 0) < 0) {
192 LOG(WARNING, "upper claw too low and moving down\n");
193 U(1, 0) = 0;
194 }
Brian Silverman7b7c9072014-03-30 13:33:30 -0700195 }
Brian Silvermaned9df2f2014-04-05 07:07:15 -0700196 if (bottom_known_) {
197 if (X_hat(0, 0) > values.lower_claw.upper_limit && U(0, 0) > 0) {
198 LOG(WARNING, "lower claw too high and moving up\n");
199 U(0, 0) = 0;
200 } else if (X_hat(0, 0) < values.lower_claw.lower_limit && U(0, 0) < 0) {
201 LOG(WARNING, "lower claw too low and moving down\n");
202 U(0, 0) = 0;
203 }
Brian Silverman7b7c9072014-03-30 13:33:30 -0700204 }
205 }
James Kuszmauld536a402014-02-18 22:32:12 -0800206 }
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800207}
208
Austin Schuh27b8fb12014-02-22 15:10:05 -0800209ZeroedStateFeedbackLoop::ZeroedStateFeedbackLoop(const char *name,
210 ClawMotor *motor)
211 : offset_(0.0),
212 name_(name),
213 motor_(motor),
214 zeroing_state_(UNKNOWN_POSITION),
215 posedge_value_(0.0),
216 negedge_value_(0.0),
217 encoder_(0.0),
218 last_encoder_(0.0) {}
219
220void ZeroedStateFeedbackLoop::SetPositionValues(const HalfClawPosition &claw) {
221 front_.Update(claw.front);
222 calibration_.Update(claw.calibration);
223 back_.Update(claw.back);
224
225 bool any_sensor_triggered = any_triggered();
226 if (any_sensor_triggered && any_triggered_last_) {
227 // We are still on the hall effect and nothing has changed.
228 min_hall_effect_on_angle_ =
229 ::std::min(min_hall_effect_on_angle_, claw.position);
230 max_hall_effect_on_angle_ =
231 ::std::max(max_hall_effect_on_angle_, claw.position);
232 } else if (!any_sensor_triggered && !any_triggered_last_) {
233 // We are still off the hall effect and nothing has changed.
234 min_hall_effect_off_angle_ =
235 ::std::min(min_hall_effect_off_angle_, claw.position);
236 max_hall_effect_off_angle_ =
237 ::std::max(max_hall_effect_off_angle_, claw.position);
238 } else if (any_sensor_triggered && !any_triggered_last_) {
239 // Saw a posedge on the hall effect. Reset the limits.
240 min_hall_effect_on_angle_ = ::std::min(claw.posedge_value, claw.position);
241 max_hall_effect_on_angle_ = ::std::max(claw.posedge_value, claw.position);
242 } else if (!any_sensor_triggered && any_triggered_last_) {
243 // Saw a negedge on the hall effect. Reset the limits.
244 min_hall_effect_off_angle_ = ::std::min(claw.negedge_value, claw.position);
245 max_hall_effect_off_angle_ = ::std::max(claw.negedge_value, claw.position);
246 }
247
248 posedge_value_ = claw.posedge_value;
249 negedge_value_ = claw.negedge_value;
250 last_encoder_ = encoder_;
251 if (front().value() || calibration().value() || back().value()) {
252 last_on_encoder_ = encoder_;
253 } else {
254 last_off_encoder_ = encoder_;
255 }
256 encoder_ = claw.position;
257 any_triggered_last_ = any_sensor_triggered;
258}
259
260void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition &claw) {
261 set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
262
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000263 front_.Reset(claw.front);
264 calibration_.Reset(claw.calibration);
265 back_.Reset(claw.back);
Austin Schuh27b8fb12014-02-22 15:10:05 -0800266 // close up the min and max edge positions as they are no longer valid and
267 // will be expanded in future iterations
268 min_hall_effect_on_angle_ = claw.position;
269 max_hall_effect_on_angle_ = claw.position;
270 min_hall_effect_off_angle_ = claw.position;
271 max_hall_effect_off_angle_ = claw.position;
272 any_triggered_last_ = any_triggered();
273}
274
275bool TopZeroedStateFeedbackLoop::SetCalibrationOnEdge(
276 const constants::Values::Claws::Claw &claw_values,
277 JointZeroingState zeroing_state) {
278 double edge_encoder;
279 double edge_angle;
280 if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
281 LOG(INFO, "Calibration edge edge should be %f.\n", edge_angle);
282 SetCalibration(edge_encoder, edge_angle);
283 set_zeroing_state(zeroing_state);
284 return true;
285 }
286 return false;
287}
288
Brian Silverman084372e2014-04-10 10:55:53 -0700289void TopZeroedStateFeedbackLoop::HandleCalibrationError(
290 const constants::Values::Claws::Claw &claw_values) {
291 double edge_encoder;
292 double edge_angle;
293 if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
294 const double calibration_error =
295 ComputeCalibrationChange(edge_encoder, edge_angle);
296 LOG(INFO, "Top calibration error is %f\n", calibration_error);
297 if (::std::abs(calibration_error) > kRezeroThreshold) {
Brian Silvermane4c701d2014-04-10 19:29:25 -0700298 LOG(WARNING, "rezeroing top\n");
Brian Silverman084372e2014-04-10 10:55:53 -0700299 SetCalibration(edge_encoder, edge_angle);
Brian Silverman640f97a2014-04-19 16:07:55 -0700300 set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
Brian Silverman084372e2014-04-10 10:55:53 -0700301 }
302 }
303}
304
305
306void BottomZeroedStateFeedbackLoop::HandleCalibrationError(
307 const constants::Values::Claws::Claw &claw_values) {
308 double edge_encoder;
309 double edge_angle;
310 if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
311 const double calibration_error =
312 ComputeCalibrationChange(edge_encoder, edge_angle);
313 LOG(INFO, "Bottom calibration error is %f\n", calibration_error);
314 if (::std::abs(calibration_error) > kRezeroThreshold) {
Brian Silvermane4c701d2014-04-10 19:29:25 -0700315 LOG(WARNING, "rezeroing bottom\n");
Brian Silverman084372e2014-04-10 10:55:53 -0700316 SetCalibration(edge_encoder, edge_angle);
Brian Silverman640f97a2014-04-19 16:07:55 -0700317 set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
Brian Silverman084372e2014-04-10 10:55:53 -0700318 }
319 }
320}
321
Austin Schuh27b8fb12014-02-22 15:10:05 -0800322bool BottomZeroedStateFeedbackLoop::SetCalibrationOnEdge(
323 const constants::Values::Claws::Claw &claw_values,
324 JointZeroingState zeroing_state) {
325 double edge_encoder;
326 double edge_angle;
327 if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
328 LOG(INFO, "Calibration edge.\n");
329 SetCalibration(edge_encoder, edge_angle);
330 set_zeroing_state(zeroing_state);
331 return true;
332 }
333 return false;
334}
335
Austin Schuhcc0bf312014-02-09 00:39:29 -0800336ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
Brian Silverman38111502014-04-10 12:36:26 -0700337 : aos::controls::ControlLoop<control_loops::ClawGroup, true, true,
Brian Silverman71fbee02014-03-13 17:24:54 -0700338 false>(my_claw),
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800339 has_top_claw_goal_(false),
340 top_claw_goal_(0.0),
Austin Schuhcda86af2014-02-16 16:16:39 -0800341 top_claw_(this),
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800342 has_bottom_claw_goal_(false),
343 bottom_claw_goal_(0.0),
Austin Schuhcda86af2014-02-16 16:16:39 -0800344 bottom_claw_(this),
345 claw_(MakeClawLoop()),
Ben Fredrickson9b388422014-02-13 06:15:31 +0000346 was_enabled_(false),
Austin Schuh4cb047f2014-02-16 21:10:19 -0800347 doing_calibration_fine_tune_(false),
Austin Schuhe7f90d12014-02-17 00:48:25 -0800348 capped_goal_(false),
349 mode_(UNKNOWN_LOCATION) {}
Austin Schuh3bb9a442014-02-02 16:01:45 -0800350
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800351const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
Austin Schuh3bb9a442014-02-02 16:01:45 -0800352
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000353bool ZeroedStateFeedbackLoop::SawFilteredPosedge(
354 const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
355 const HallEffectTracker &sensorB) {
356 if (posedge_filter_ == nullptr && this_sensor.posedge_count_changed() &&
357 !sensorA.posedge_count_changed() && !sensorB.posedge_count_changed() &&
358 this_sensor.value() && !this_sensor.last_value()) {
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000359 posedge_filter_ = &this_sensor;
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000360 } else if (posedge_filter_ == &this_sensor &&
361 !this_sensor.posedge_count_changed() &&
362 !sensorA.posedge_count_changed() &&
363 !sensorB.posedge_count_changed() && this_sensor.value()) {
364 posedge_filter_ = nullptr;
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000365 return true;
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000366 } else if (posedge_filter_ == &this_sensor) {
367 posedge_filter_ = nullptr;
368 }
369 return false;
370}
371
372bool ZeroedStateFeedbackLoop::SawFilteredNegedge(
373 const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
374 const HallEffectTracker &sensorB) {
375 if (negedge_filter_ == nullptr && this_sensor.negedge_count_changed() &&
376 !sensorA.negedge_count_changed() && !sensorB.negedge_count_changed() &&
377 !this_sensor.value() && this_sensor.last_value()) {
378 negedge_filter_ = &this_sensor;
379 } else if (negedge_filter_ == &this_sensor &&
380 !this_sensor.negedge_count_changed() &&
381 !sensorA.negedge_count_changed() &&
382 !sensorB.negedge_count_changed() && !this_sensor.value()) {
383 negedge_filter_ = nullptr;
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000384 return true;
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000385 } else if (negedge_filter_ == &this_sensor) {
386 negedge_filter_ = nullptr;
387 }
388 return false;
389}
390
Brian Silvermane0a95462014-02-17 00:41:09 -0800391bool ZeroedStateFeedbackLoop::DoGetPositionOfEdge(
392 const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000393 double *edge_angle, const HallEffectTracker &this_sensor,
394 const HallEffectTracker &sensorA, const HallEffectTracker &sensorB,
Brian Silvermane0a95462014-02-17 00:41:09 -0800395 const char *hall_effect_name) {
Austin Schuhf84a1302014-02-19 00:23:30 -0800396 bool found_edge = false;
Austin Schuh27b8fb12014-02-22 15:10:05 -0800397
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000398 if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
Austin Schuh27b8fb12014-02-22 15:10:05 -0800399 if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
Brian Silvermanf48fab32014-03-09 14:32:24 -0700400 LOG(WARNING, "%s: Uncertain which side, rejecting posedge\n", name_);
Brian Silvermane0a95462014-02-17 00:41:09 -0800401 } else {
Austin Schuh27b8fb12014-02-22 15:10:05 -0800402 const double average_last_encoder =
403 (min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
404 if (posedge_value_ < average_last_encoder) {
405 *edge_angle = angles.upper_decreasing_angle;
406 LOG(INFO, "%s Posedge upper of %s -> %f posedge: %f avg_encoder: %f\n",
407 name_, hall_effect_name, *edge_angle, posedge_value_,
408 average_last_encoder);
409 } else {
410 *edge_angle = angles.lower_angle;
411 LOG(INFO, "%s Posedge lower of %s -> %f posedge: %f avg_encoder: %f\n",
412 name_, hall_effect_name, *edge_angle, posedge_value_,
413 average_last_encoder);
414 }
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000415 *edge_encoder = posedge_value_;
Austin Schuh27b8fb12014-02-22 15:10:05 -0800416 found_edge = true;
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000417 }
418 }
419
420 if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
421 if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
Brian Silvermanf48fab32014-03-09 14:32:24 -0700422 LOG(WARNING, "%s: Uncertain which side, rejecting negedge\n", name_);
Brian Silvermane0a95462014-02-17 00:41:09 -0800423 } else {
Austin Schuh27b8fb12014-02-22 15:10:05 -0800424 const double average_last_encoder =
425 (min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
426 if (negedge_value_ > average_last_encoder) {
427 *edge_angle = angles.upper_angle;
428 LOG(INFO, "%s Negedge upper of %s -> %f negedge: %f avg_encoder: %f\n",
429 name_, hall_effect_name, *edge_angle, negedge_value_,
430 average_last_encoder);
431 } else {
432 *edge_angle = angles.lower_decreasing_angle;
433 LOG(INFO, "%s Negedge lower of %s -> %f negedge: %f avg_encoder: %f\n",
434 name_, hall_effect_name, *edge_angle, negedge_value_,
435 average_last_encoder);
436 }
437 *edge_encoder = negedge_value_;
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000438 found_edge = true;
Brian Silvermane0a95462014-02-17 00:41:09 -0800439 }
Austin Schuh27b8fb12014-02-22 15:10:05 -0800440 }
441
Austin Schuhf84a1302014-02-19 00:23:30 -0800442 return found_edge;
Brian Silvermane0a95462014-02-17 00:41:09 -0800443}
444
Austin Schuhf9286cd2014-02-11 00:51:09 -0800445bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
Austin Schuhd27931c2014-02-16 19:18:20 -0800446 const constants::Values::Claws::Claw &claw_values, double *edge_encoder,
Austin Schuhf9286cd2014-02-11 00:51:09 -0800447 double *edge_angle) {
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000448 if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle, front_,
449 calibration_, back_, "front")) {
Austin Schuhf9286cd2014-02-11 00:51:09 -0800450 return true;
451 }
Brian Silvermane0a95462014-02-17 00:41:09 -0800452 if (DoGetPositionOfEdge(claw_values.calibration, edge_encoder, edge_angle,
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000453 calibration_, front_, back_, "calibration")) {
Austin Schuhf9286cd2014-02-11 00:51:09 -0800454 return true;
455 }
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000456 if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle, back_,
457 calibration_, front_, "back")) {
Austin Schuhf9286cd2014-02-11 00:51:09 -0800458 return true;
459 }
460 return false;
461}
462
Austin Schuhcda86af2014-02-16 16:16:39 -0800463void TopZeroedStateFeedbackLoop::SetCalibration(double edge_encoder,
464 double edge_angle) {
465 double old_offset = offset_;
466 offset_ = edge_angle - edge_encoder;
467 const double doffset = offset_ - old_offset;
468 motor_->ChangeTopOffset(doffset);
469}
470
Brian Silverman084372e2014-04-10 10:55:53 -0700471double TopZeroedStateFeedbackLoop::ComputeCalibrationChange(double edge_encoder,
472 double edge_angle) {
473 const double offset = edge_angle - edge_encoder;
474 const double doffset = offset - offset_;
475 return doffset;
476}
477
Austin Schuhcda86af2014-02-16 16:16:39 -0800478void BottomZeroedStateFeedbackLoop::SetCalibration(double edge_encoder,
479 double edge_angle) {
480 double old_offset = offset_;
481 offset_ = edge_angle - edge_encoder;
482 const double doffset = offset_ - old_offset;
483 motor_->ChangeBottomOffset(doffset);
484}
485
Brian Silverman084372e2014-04-10 10:55:53 -0700486double BottomZeroedStateFeedbackLoop::ComputeCalibrationChange(
487 double edge_encoder, double edge_angle) {
488 const double offset = edge_angle - edge_encoder;
489 const double doffset = offset - offset_;
490 return doffset;
491}
492
Austin Schuhcda86af2014-02-16 16:16:39 -0800493void ClawMotor::ChangeTopOffset(double doffset) {
494 claw_.ChangeTopOffset(doffset);
495 if (has_top_claw_goal_) {
496 top_claw_goal_ += doffset;
497 }
498}
499
500void ClawMotor::ChangeBottomOffset(double doffset) {
501 claw_.ChangeBottomOffset(doffset);
502 if (has_bottom_claw_goal_) {
503 bottom_claw_goal_ += doffset;
504 }
505}
506
507void ClawLimitedLoop::ChangeTopOffset(double doffset) {
508 Y_(1, 0) += doffset;
509 X_hat(1, 0) += doffset;
510 LOG(INFO, "Changing top offset by %f\n", doffset);
511}
512void ClawLimitedLoop::ChangeBottomOffset(double doffset) {
513 Y_(0, 0) += doffset;
514 X_hat(0, 0) += doffset;
515 X_hat(1, 0) -= doffset;
516 LOG(INFO, "Changing bottom offset by %f\n", doffset);
517}
joe7376ff52014-02-16 18:28:42 -0800518
Austin Schuh069143b2014-02-17 02:46:26 -0800519void LimitClawGoal(double *bottom_goal, double *top_goal,
520 const frc971::constants::Values &values) {
521 // first update position based on angle limit
Austin Schuh069143b2014-02-17 02:46:26 -0800522 const double separation = *top_goal - *bottom_goal;
Brian Silverman06374312014-04-12 16:09:28 -0700523 if (separation > values.claw.soft_max_separation) {
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700524 LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
Brian Silverman06374312014-04-12 16:09:28 -0700525 const double dsep = (separation - values.claw.soft_max_separation) / 2.0;
Austin Schuh069143b2014-02-17 02:46:26 -0800526 *bottom_goal += dsep;
527 *top_goal -= dsep;
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700528 LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800529 }
Brian Silverman06374312014-04-12 16:09:28 -0700530 if (separation < values.claw.soft_min_separation) {
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700531 LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
Brian Silverman06374312014-04-12 16:09:28 -0700532 const double dsep = (separation - values.claw.soft_min_separation) / 2.0;
Austin Schuh069143b2014-02-17 02:46:26 -0800533 *bottom_goal += dsep;
534 *top_goal -= dsep;
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700535 LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800536 }
537
538 // now move both goals in unison
539 if (*bottom_goal < values.claw.lower_claw.lower_limit) {
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700540 LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800541 *top_goal += values.claw.lower_claw.lower_limit - *bottom_goal;
542 *bottom_goal = values.claw.lower_claw.lower_limit;
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700543 LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800544 }
545 if (*bottom_goal > values.claw.lower_claw.upper_limit) {
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700546 LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800547 *top_goal -= *bottom_goal - values.claw.lower_claw.upper_limit;
548 *bottom_goal = values.claw.lower_claw.upper_limit;
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700549 LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800550 }
551
552 if (*top_goal < values.claw.upper_claw.lower_limit) {
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700553 LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800554 *bottom_goal += values.claw.upper_claw.lower_limit - *top_goal;
555 *top_goal = values.claw.upper_claw.lower_limit;
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700556 LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800557 }
558 if (*top_goal > values.claw.upper_claw.upper_limit) {
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700559 LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800560 *bottom_goal -= *top_goal - values.claw.upper_claw.upper_limit;
561 *top_goal = values.claw.upper_claw.upper_limit;
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700562 LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
Austin Schuh069143b2014-02-17 02:46:26 -0800563 }
564}
Austin Schuhcda86af2014-02-16 16:16:39 -0800565
Austin Schuhe7f90d12014-02-17 00:48:25 -0800566bool ClawMotor::is_ready() const {
567 return (
568 (top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
569 bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
Brian Silverman71fbee02014-03-13 17:24:54 -0700570 (((::aos::robot_state.get() == NULL) ? true
571 : ::aos::robot_state->autonomous) &&
Austin Schuhe7f90d12014-02-17 00:48:25 -0800572 ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
573 top_claw_.zeroing_state() ==
574 ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
575 (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
576 bottom_claw_.zeroing_state() ==
577 ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))));
578}
579
580bool ClawMotor::is_zeroing() const { return !is_ready(); }
581
Austin Schuh3bb9a442014-02-02 16:01:45 -0800582// Positive angle is up, and positive power is up.
Austin Schuhcc0bf312014-02-09 00:39:29 -0800583void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
584 const control_loops::ClawGroup::Position *position,
585 control_loops::ClawGroup::Output *output,
James Kuszmaul9ead1de2014-02-28 21:24:39 -0800586 control_loops::ClawGroup::Status *status) {
Austin Schuhf9286cd2014-02-11 00:51:09 -0800587 constexpr double dt = 0.01;
Austin Schuh3bb9a442014-02-02 16:01:45 -0800588
589 // Disable the motors now so that all early returns will return with the
590 // motors disabled.
591 if (output) {
592 output->top_claw_voltage = 0;
593 output->bottom_claw_voltage = 0;
594 output->intake_voltage = 0;
Ben Fredrickson61893d52014-03-02 09:43:23 +0000595 output->tusk_voltage = 0;
596 }
597
Brian Silverman71fbee02014-03-13 17:24:54 -0700598 if (goal) {
599 if (::std::isnan(goal->bottom_angle) ||
600 ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
601 ::std::isnan(goal->centering)) {
602 return;
603 }
Austin Schuh3bb9a442014-02-02 16:01:45 -0800604 }
605
Austin Schuh1a499942014-02-17 01:51:58 -0800606 if (reset()) {
Austin Schuh27b8fb12014-02-22 15:10:05 -0800607 top_claw_.Reset(position->top);
608 bottom_claw_.Reset(position->bottom);
Austin Schuh1a499942014-02-17 01:51:58 -0800609 }
610
Austin Schuhf9286cd2014-02-11 00:51:09 -0800611 const frc971::constants::Values &values = constants::GetValues();
612
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800613 if (position) {
Austin Schuhcda86af2014-02-16 16:16:39 -0800614 Eigen::Matrix<double, 2, 1> Y;
615 Y << position->bottom.position + bottom_claw_.offset(),
616 position->top.position + top_claw_.offset();
617 claw_.Correct(Y);
618
Austin Schuhf9286cd2014-02-11 00:51:09 -0800619 top_claw_.SetPositionValues(position->top);
620 bottom_claw_.SetPositionValues(position->bottom);
621
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800622 if (!has_top_claw_goal_) {
623 has_top_claw_goal_ = true;
Austin Schuhcda86af2014-02-16 16:16:39 -0800624 top_claw_goal_ = top_claw_.absolute_position();
Austin Schuhe7f90d12014-02-17 00:48:25 -0800625 initial_separation_ =
Austin Schuhcda86af2014-02-16 16:16:39 -0800626 top_claw_.absolute_position() - bottom_claw_.absolute_position();
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800627 }
628 if (!has_bottom_claw_goal_) {
629 has_bottom_claw_goal_ = true;
Austin Schuhcda86af2014-02-16 16:16:39 -0800630 bottom_claw_goal_ = bottom_claw_.absolute_position();
Austin Schuhe7f90d12014-02-17 00:48:25 -0800631 initial_separation_ =
Austin Schuhcda86af2014-02-16 16:16:39 -0800632 top_claw_.absolute_position() - bottom_claw_.absolute_position();
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800633 }
Brian Silvermanf48fab32014-03-09 14:32:24 -0700634 LOG_STRUCT(DEBUG, "absolute position",
635 ClawPositionToLog(top_claw_.absolute_position(),
636 bottom_claw_.absolute_position()));
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800637 }
638
Brian Silverman71fbee02014-03-13 17:24:54 -0700639 bool autonomous, enabled;
640 if (::aos::robot_state.get() == nullptr) {
641 autonomous = true;
642 enabled = false;
643 } else {
644 autonomous = ::aos::robot_state->autonomous;
645 enabled = ::aos::robot_state->enabled;
646 }
Austin Schuh069143b2014-02-17 02:46:26 -0800647
648 double bottom_claw_velocity_ = 0.0;
649 double top_claw_velocity_ = 0.0;
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800650
Brian Silverman71fbee02014-03-13 17:24:54 -0700651 if (goal != NULL &&
652 ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
653 bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
654 (autonomous &&
655 ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
656 top_claw_.zeroing_state() ==
657 ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
658 (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
659 bottom_claw_.zeroing_state() ==
660 ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800661 // Ready to use the claw.
662 // Limit the goals here.
Austin Schuhf9286cd2014-02-11 00:51:09 -0800663 bottom_claw_goal_ = goal->bottom_angle;
Brian Silverman7c021c42014-02-17 15:15:56 -0800664 top_claw_goal_ = goal->bottom_angle + goal->separation_angle;
Austin Schuhcda86af2014-02-16 16:16:39 -0800665 has_bottom_claw_goal_ = true;
666 has_top_claw_goal_ = true;
667 doing_calibration_fine_tune_ = false;
Austin Schuhe7f90d12014-02-17 00:48:25 -0800668 mode_ = READY;
Brian Silverman084372e2014-04-10 10:55:53 -0700669
670 bottom_claw_.HandleCalibrationError(values.claw.lower_claw);
671 top_claw_.HandleCalibrationError(values.claw.upper_claw);
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800672 } else if (top_claw_.zeroing_state() !=
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000673 ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800674 bottom_claw_.zeroing_state() !=
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000675 ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800676 // Time to fine tune the zero.
677 // Limit the goals here.
Austin Schuh0c733422014-02-17 01:17:12 -0800678 if (!enabled) {
679 // If we are disabled, start the fine tune process over again.
680 doing_calibration_fine_tune_ = false;
681 }
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800682 if (bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::CALIBRATED) {
Austin Schuhcda86af2014-02-16 16:16:39 -0800683 // always get the bottom claw to calibrated first
684 LOG(DEBUG, "Calibrating the bottom of the claw\n");
685 if (!doing_calibration_fine_tune_) {
686 if (::std::abs(bottom_absolute_position() -
Austin Schuhd27931c2014-02-16 19:18:20 -0800687 values.claw.start_fine_tune_pos) <
688 values.claw.claw_unimportant_epsilon) {
Austin Schuhcda86af2014-02-16 16:16:39 -0800689 doing_calibration_fine_tune_ = true;
Austin Schuhd27931c2014-02-16 19:18:20 -0800690 bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
Austin Schuh069143b2014-02-17 02:46:26 -0800691 top_claw_velocity_ = bottom_claw_velocity_ =
692 values.claw.claw_zeroing_speed;
Austin Schuhcda86af2014-02-16 16:16:39 -0800693 LOG(DEBUG, "Ready to fine tune the bottom\n");
Austin Schuhe7f90d12014-02-17 00:48:25 -0800694 mode_ = FINE_TUNE_BOTTOM;
Austin Schuhcda86af2014-02-16 16:16:39 -0800695 } else {
696 // send bottom to zeroing start
Austin Schuhd27931c2014-02-16 19:18:20 -0800697 bottom_claw_goal_ = values.claw.start_fine_tune_pos;
Austin Schuhcda86af2014-02-16 16:16:39 -0800698 LOG(DEBUG, "Going to the start position for the bottom\n");
Austin Schuhe7f90d12014-02-17 00:48:25 -0800699 mode_ = PREP_FINE_TUNE_BOTTOM;
Austin Schuhcda86af2014-02-16 16:16:39 -0800700 }
701 } else {
Austin Schuhe7f90d12014-02-17 00:48:25 -0800702 mode_ = FINE_TUNE_BOTTOM;
Austin Schuhd27931c2014-02-16 19:18:20 -0800703 bottom_claw_goal_ += values.claw.claw_zeroing_speed * dt;
Austin Schuh069143b2014-02-17 02:46:26 -0800704 top_claw_velocity_ = bottom_claw_velocity_ =
705 values.claw.claw_zeroing_speed;
Brian Silvermane0a95462014-02-17 00:41:09 -0800706 if (top_claw_.front_or_back_triggered() ||
707 bottom_claw_.front_or_back_triggered()) {
Austin Schuhcda86af2014-02-16 16:16:39 -0800708 // We shouldn't hit a limit, but if we do, go back to the zeroing
709 // point and try again.
710 doing_calibration_fine_tune_ = false;
Austin Schuhd27931c2014-02-16 19:18:20 -0800711 bottom_claw_goal_ = values.claw.start_fine_tune_pos;
Austin Schuh069143b2014-02-17 02:46:26 -0800712 top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
Austin Schuhcda86af2014-02-16 16:16:39 -0800713 LOG(DEBUG, "Found a limit, starting over.\n");
Austin Schuhe7f90d12014-02-17 00:48:25 -0800714 mode_ = PREP_FINE_TUNE_BOTTOM;
Austin Schuhcda86af2014-02-16 16:16:39 -0800715 }
Austin Schuh288c8c32014-02-16 17:20:17 -0800716
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000717 if (position && bottom_claw_.SawFilteredPosedge(
718 bottom_claw_.calibration(), bottom_claw_.front(),
719 bottom_claw_.back())) {
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000720 // do calibration
721 bottom_claw_.SetCalibration(
722 position->bottom.posedge_value,
723 values.claw.lower_claw.calibration.lower_angle);
724 bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
725 // calibrated so we are done fine tuning bottom
726 doing_calibration_fine_tune_ = false;
727 LOG(DEBUG, "Calibrated the bottom correctly!\n");
728 } else if (bottom_claw_.calibration().last_value()) {
Brian Silvermane4c701d2014-04-10 19:29:25 -0700729 LOG(DEBUG, "Aborting bottom fine tune because sensor triggered\n");
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000730 doing_calibration_fine_tune_ = false;
Brian Silvermancc3844a2014-04-10 21:15:07 -0700731 bottom_claw_.set_zeroing_state(
732 ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
Austin Schuhcda86af2014-02-16 16:16:39 -0800733 } else {
734 LOG(DEBUG, "Fine tuning\n");
735 }
736 }
737 // now set the top claw to track
738
Austin Schuhd27931c2014-02-16 19:18:20 -0800739 top_claw_goal_ = bottom_claw_goal_ + values.claw.claw_zeroing_separation;
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800740 } else {
Austin Schuhcda86af2014-02-16 16:16:39 -0800741 // bottom claw must be calibrated, start on the top
742 if (!doing_calibration_fine_tune_) {
Austin Schuhd27931c2014-02-16 19:18:20 -0800743 if (::std::abs(top_absolute_position() -
744 values.claw.start_fine_tune_pos) <
745 values.claw.claw_unimportant_epsilon) {
Austin Schuhcda86af2014-02-16 16:16:39 -0800746 doing_calibration_fine_tune_ = true;
Austin Schuhd27931c2014-02-16 19:18:20 -0800747 top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
Austin Schuh069143b2014-02-17 02:46:26 -0800748 top_claw_velocity_ = bottom_claw_velocity_ =
749 values.claw.claw_zeroing_speed;
Austin Schuhcda86af2014-02-16 16:16:39 -0800750 LOG(DEBUG, "Ready to fine tune the top\n");
Austin Schuhe7f90d12014-02-17 00:48:25 -0800751 mode_ = FINE_TUNE_TOP;
Austin Schuhcda86af2014-02-16 16:16:39 -0800752 } else {
753 // send top to zeroing start
Austin Schuhd27931c2014-02-16 19:18:20 -0800754 top_claw_goal_ = values.claw.start_fine_tune_pos;
Austin Schuhcda86af2014-02-16 16:16:39 -0800755 LOG(DEBUG, "Going to the start position for the top\n");
Austin Schuhe7f90d12014-02-17 00:48:25 -0800756 mode_ = PREP_FINE_TUNE_TOP;
Austin Schuhcda86af2014-02-16 16:16:39 -0800757 }
758 } else {
Austin Schuhe7f90d12014-02-17 00:48:25 -0800759 mode_ = FINE_TUNE_TOP;
Austin Schuhd27931c2014-02-16 19:18:20 -0800760 top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
Austin Schuh069143b2014-02-17 02:46:26 -0800761 top_claw_velocity_ = bottom_claw_velocity_ =
762 values.claw.claw_zeroing_speed;
Brian Silvermane0a95462014-02-17 00:41:09 -0800763 if (top_claw_.front_or_back_triggered() ||
764 bottom_claw_.front_or_back_triggered()) {
Austin Schuhcda86af2014-02-16 16:16:39 -0800765 // this should not happen, but now we know it won't
766 doing_calibration_fine_tune_ = false;
Austin Schuhd27931c2014-02-16 19:18:20 -0800767 top_claw_goal_ = values.claw.start_fine_tune_pos;
Austin Schuh069143b2014-02-17 02:46:26 -0800768 top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
Austin Schuhcda86af2014-02-16 16:16:39 -0800769 LOG(DEBUG, "Found a limit, starting over.\n");
Austin Schuhe7f90d12014-02-17 00:48:25 -0800770 mode_ = PREP_FINE_TUNE_TOP;
Austin Schuhcda86af2014-02-16 16:16:39 -0800771 }
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000772
773 if (position &&
774 top_claw_.SawFilteredPosedge(top_claw_.calibration(),
775 top_claw_.front(), top_claw_.back())) {
776 // do calibration
777 top_claw_.SetCalibration(
778 position->top.posedge_value,
779 values.claw.upper_claw.calibration.lower_angle);
780 top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
781 // calibrated so we are done fine tuning top
782 doing_calibration_fine_tune_ = false;
783 LOG(DEBUG, "Calibrated the top correctly!\n");
784 } else if (top_claw_.calibration().last_value()) {
Brian Silvermane4c701d2014-04-10 19:29:25 -0700785 LOG(DEBUG, "Aborting top fine tune because sensor triggered\n");
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000786 doing_calibration_fine_tune_ = false;
Brian Silvermancc3844a2014-04-10 21:15:07 -0700787 top_claw_.set_zeroing_state(
788 ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
Austin Schuhcda86af2014-02-16 16:16:39 -0800789 }
790 }
791 // now set the bottom claw to track
Austin Schuhd27931c2014-02-16 19:18:20 -0800792 bottom_claw_goal_ = top_claw_goal_ - values.claw.claw_zeroing_separation;
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800793 }
794 } else {
Austin Schuhcda86af2014-02-16 16:16:39 -0800795 doing_calibration_fine_tune_ = false;
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800796 if (!was_enabled_ && enabled) {
Austin Schuhf9286cd2014-02-11 00:51:09 -0800797 if (position) {
Brian Silverman72035692014-03-14 10:18:27 -0700798 top_claw_goal_ = position->top.position + top_claw_.offset();
799 bottom_claw_goal_ = position->bottom.position + bottom_claw_.offset();
Austin Schuhe7f90d12014-02-17 00:48:25 -0800800 initial_separation_ =
Austin Schuhcda86af2014-02-16 16:16:39 -0800801 position->top.position - position->bottom.position;
Austin Schuhf9286cd2014-02-11 00:51:09 -0800802 } else {
803 has_top_claw_goal_ = false;
804 has_bottom_claw_goal_ = false;
805 }
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800806 }
Austin Schuhf9286cd2014-02-11 00:51:09 -0800807
Austin Schuh4cb047f2014-02-16 21:10:19 -0800808 if ((bottom_claw_.zeroing_state() !=
809 ZeroedStateFeedbackLoop::UNKNOWN_POSITION ||
Brian Silvermane0a95462014-02-17 00:41:09 -0800810 bottom_claw_.front().value() || top_claw_.front().value()) &&
811 !top_claw_.back().value() && !bottom_claw_.back().value()) {
Austin Schuhf9286cd2014-02-11 00:51:09 -0800812 if (enabled) {
813 // Time to slowly move back up to find any position to narrow down the
814 // zero.
Austin Schuhd27931c2014-02-16 19:18:20 -0800815 top_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
816 bottom_claw_goal_ += values.claw.claw_zeroing_off_speed * dt;
Austin Schuh069143b2014-02-17 02:46:26 -0800817 top_claw_velocity_ = bottom_claw_velocity_ =
818 values.claw.claw_zeroing_off_speed;
Austin Schuhcda86af2014-02-16 16:16:39 -0800819 LOG(DEBUG, "Bottom is known.\n");
Austin Schuhf9286cd2014-02-11 00:51:09 -0800820 }
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800821 } else {
822 // We don't know where either claw is. Slowly start moving down to find
823 // any hall effect.
Austin Schuhf9286cd2014-02-11 00:51:09 -0800824 if (enabled) {
Austin Schuhd27931c2014-02-16 19:18:20 -0800825 top_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
826 bottom_claw_goal_ -= values.claw.claw_zeroing_off_speed * dt;
Austin Schuh069143b2014-02-17 02:46:26 -0800827 top_claw_velocity_ = bottom_claw_velocity_ =
828 -values.claw.claw_zeroing_off_speed;
Austin Schuhcda86af2014-02-16 16:16:39 -0800829 LOG(DEBUG, "Both are unknown.\n");
Austin Schuhf9286cd2014-02-11 00:51:09 -0800830 }
831 }
832
Ben Fredricksoneaecbbb2014-02-23 12:12:03 +0000833 if (position) {
834 if (enabled) {
835 top_claw_.SetCalibrationOnEdge(
836 values.claw.upper_claw,
837 ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
838 bottom_claw_.SetCalibrationOnEdge(
839 values.claw.lower_claw,
840 ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
841 } else {
842 // TODO(austin): Only calibrate on the predetermined edge.
843 // We might be able to just ignore this since the backlash is soooo
844 // low.
845 // :)
846 top_claw_.SetCalibrationOnEdge(
847 values.claw.upper_claw,
848 ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
849 bottom_claw_.SetCalibrationOnEdge(
850 values.claw.lower_claw,
851 ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
852 }
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800853 }
Austin Schuhe7f90d12014-02-17 00:48:25 -0800854 mode_ = UNKNOWN_LOCATION;
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800855 }
856
Austin Schuh069143b2014-02-17 02:46:26 -0800857 // Limit the goals if both claws have been (mostly) found.
858 if (mode_ != UNKNOWN_LOCATION) {
859 LimitClawGoal(&bottom_claw_goal_, &top_claw_goal_, values);
860 }
861
Brian Silvermaned9df2f2014-04-05 07:07:15 -0700862 claw_.set_positions_known(top_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION, bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
Austin Schuhf9286cd2014-02-11 00:51:09 -0800863 if (has_top_claw_goal_ && has_bottom_claw_goal_) {
Austin Schuh069143b2014-02-17 02:46:26 -0800864 claw_.R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
865 bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
Brian Silverman11c6f7f2014-04-30 17:38:21 -0700866 LOG_MATRIX(DEBUG, "actual goal", claw_.R);
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800867
Austin Schuh01c652b2014-02-21 23:13:42 -0800868 // Only cap power when one of the halves of the claw is moving slowly and
869 // could wind up.
Austin Schuhe7f90d12014-02-17 00:48:25 -0800870 claw_.set_is_zeroing(mode_ == UNKNOWN_LOCATION || mode_ == FINE_TUNE_TOP ||
871 mode_ == FINE_TUNE_BOTTOM);
Austin Schuhcda86af2014-02-16 16:16:39 -0800872 claw_.Update(output == nullptr);
Austin Schuhf9286cd2014-02-11 00:51:09 -0800873 } else {
Austin Schuhcda86af2014-02-16 16:16:39 -0800874 claw_.Update(true);
Austin Schuhf9286cd2014-02-11 00:51:09 -0800875 }
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800876
Austin Schuh4cb047f2014-02-16 21:10:19 -0800877 capped_goal_ = false;
Austin Schuhe7f90d12014-02-17 00:48:25 -0800878 switch (mode_) {
Austin Schuhcda86af2014-02-16 16:16:39 -0800879 case READY:
Austin Schuhe7f90d12014-02-17 00:48:25 -0800880 case PREP_FINE_TUNE_TOP:
881 case PREP_FINE_TUNE_BOTTOM:
Austin Schuhcda86af2014-02-16 16:16:39 -0800882 break;
Austin Schuhe7f90d12014-02-17 00:48:25 -0800883 case FINE_TUNE_BOTTOM:
884 case FINE_TUNE_TOP:
Austin Schuh4cb047f2014-02-16 21:10:19 -0800885 case UNKNOWN_LOCATION: {
886 if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
Brian Silverman6dd2c532014-03-29 23:34:39 -0700887 double dx_bot = (claw_.U_uncapped(0, 0) -
Austin Schuh4cb047f2014-02-16 21:10:19 -0800888 values.claw.max_zeroing_voltage) /
889 claw_.K(0, 0);
Brian Silverman6dd2c532014-03-29 23:34:39 -0700890 double dx_top = (claw_.U_uncapped(1, 0) -
James Kuszmaul0e866512014-02-21 13:12:52 -0800891 values.claw.max_zeroing_voltage) /
892 claw_.K(0, 0);
893 double dx = ::std::max(dx_top, dx_bot);
Austin Schuh4cb047f2014-02-16 21:10:19 -0800894 bottom_claw_goal_ -= dx;
895 top_claw_goal_ -= dx;
James Kuszmaul0e866512014-02-21 13:12:52 -0800896 Eigen::Matrix<double, 4, 1> R;
897 R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
Brian Silverman6dd2c532014-03-29 23:34:39 -0700898 claw_.U = claw_.K() * (R - claw_.X_hat);
Austin Schuhcda86af2014-02-16 16:16:39 -0800899 capped_goal_ = true;
Brian Silvermanf48fab32014-03-09 14:32:24 -0700900 LOG(DEBUG, "Moving the goal by %f to prevent windup."
901 " Uncapped is %f, max is %f, difference is %f\n",
902 dx,
Austin Schuhe7f90d12014-02-17 00:48:25 -0800903 claw_.uncapped_average_voltage(), values.claw.max_zeroing_voltage,
904 (claw_.uncapped_average_voltage() -
905 values.claw.max_zeroing_voltage));
Austin Schuh4cb047f2014-02-16 21:10:19 -0800906 } else if (claw_.uncapped_average_voltage() <
907 -values.claw.max_zeroing_voltage) {
Brian Silverman6dd2c532014-03-29 23:34:39 -0700908 double dx_bot = (claw_.U_uncapped(0, 0) +
Austin Schuh4cb047f2014-02-16 21:10:19 -0800909 values.claw.max_zeroing_voltage) /
910 claw_.K(0, 0);
Brian Silverman6dd2c532014-03-29 23:34:39 -0700911 double dx_top = (claw_.U_uncapped(1, 0) +
James Kuszmaul0e866512014-02-21 13:12:52 -0800912 values.claw.max_zeroing_voltage) /
913 claw_.K(0, 0);
914 double dx = ::std::min(dx_top, dx_bot);
Austin Schuh4cb047f2014-02-16 21:10:19 -0800915 bottom_claw_goal_ -= dx;
916 top_claw_goal_ -= dx;
James Kuszmaul0e866512014-02-21 13:12:52 -0800917 Eigen::Matrix<double, 4, 1> R;
918 R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
Brian Silverman6dd2c532014-03-29 23:34:39 -0700919 claw_.U = claw_.K() * (R - claw_.X_hat);
Austin Schuhcda86af2014-02-16 16:16:39 -0800920 capped_goal_ = true;
Austin Schuh4cb047f2014-02-16 21:10:19 -0800921 LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
Austin Schuhcda86af2014-02-16 16:16:39 -0800922 }
Austin Schuh4cb047f2014-02-16 21:10:19 -0800923 } break;
Austin Schuh3bb9a442014-02-02 16:01:45 -0800924 }
925
926 if (output) {
Ben Fredrickson2f76ddf2014-02-23 05:58:23 +0000927 if (goal) {
928 //setup the intake
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000929 output->intake_voltage =
930 (goal->intake > 12.0) ? 12 : (goal->intake < -12.0) ? -12.0
931 : goal->intake;
Ben Fredrickson2f76ddf2014-02-23 05:58:23 +0000932 output->tusk_voltage = goal->centering;
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000933 output->tusk_voltage =
934 (goal->centering > 12.0) ? 12 : (goal->centering < -12.0)
935 ? -12.0
936 : goal->centering;
Ben Fredrickson2f76ddf2014-02-23 05:58:23 +0000937 }
James Kuszmauld536a402014-02-18 22:32:12 -0800938 output->top_claw_voltage = claw_.U(1, 0);
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000939 output->bottom_claw_voltage = claw_.U(0, 0);
Austin Schuhf84a1302014-02-19 00:23:30 -0800940
941 if (output->top_claw_voltage > kMaxVoltage) {
942 output->top_claw_voltage = kMaxVoltage;
943 } else if (output->top_claw_voltage < -kMaxVoltage) {
944 output->top_claw_voltage = -kMaxVoltage;
945 }
946
947 if (output->bottom_claw_voltage > kMaxVoltage) {
948 output->bottom_claw_voltage = kMaxVoltage;
949 } else if (output->bottom_claw_voltage < -kMaxVoltage) {
950 output->bottom_claw_voltage = -kMaxVoltage;
951 }
Austin Schuh3bb9a442014-02-02 16:01:45 -0800952 }
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800953
James Kuszmaul4abaf482014-02-26 21:16:35 -0800954 status->bottom = bottom_absolute_position();
955 status->separation = top_absolute_position() - bottom_absolute_position();
956 status->bottom_velocity = claw_.X_hat(2, 0);
957 status->separation_velocity = claw_.X_hat(3, 0);
Austin Schuh4b7b5d02014-02-10 21:20:34 -0800958
Brian Silverman71fbee02014-03-13 17:24:54 -0700959 if (goal) {
960 bool bottom_done =
961 ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
962 bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
963 bool separation_done =
964 ::std::abs((top_absolute_position() - bottom_absolute_position()) -
965 goal->separation_angle) < 0.020;
966 bool separation_done_with_ball =
967 ::std::abs((top_absolute_position() - bottom_absolute_position()) -
968 goal->separation_angle) < 0.06;
969 status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
970 status->done_with_ball =
971 is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
972 } else {
973 status->done = status->done_with_ball = false;
974 }
Austin Schuha556b012014-03-02 11:55:52 -0800975
Austin Schuh4f8633f2014-03-02 13:59:46 -0800976 status->zeroed = is_ready();
Brian Silverman80fc94c2014-03-09 16:56:01 -0700977 status->zeroed_for_auto =
978 (top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
979 top_claw_.zeroing_state() ==
980 ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
981 (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
982 bottom_claw_.zeroing_state() ==
983 ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
Austin Schuh4f8633f2014-03-02 13:59:46 -0800984
Brian Silverman71fbee02014-03-13 17:24:54 -0700985 was_enabled_ = enabled;
Austin Schuh3bb9a442014-02-02 16:01:45 -0800986}
987
988} // namespace control_loops
989} // namespace frc971
Ben Fredrickson81ba2d52014-03-02 08:21:46 +0000990