blob: c8f5cc8a98ea33b5f0090cb33f399a2fb319b4ec [file] [log] [blame]
Austin Schuh00558222013-03-03 14:16:16 -08001#include "frc971/control_loops/index/index.h"
Austin Schuhd78ab542013-03-01 22:22:19 -08002
3#include <stdio.h>
4
5#include <algorithm>
6
7#include "aos/aos_core.h"
8
9#include "aos/common/messages/RobotState.q.h"
10#include "aos/common/control_loop/control_loops.q.h"
11#include "aos/common/logging/logging.h"
Brian Silverman94195052013-03-09 13:45:05 -080012#include "aos/common/inttypes.h"
Austin Schuhd78ab542013-03-01 22:22:19 -080013
14#include "frc971/constants.h"
Austin Schuh00558222013-03-03 14:16:16 -080015#include "frc971/control_loops/index/index_motor_plant.h"
Austin Schuhd78ab542013-03-01 22:22:19 -080016
17using ::aos::time::Time;
18
19namespace frc971 {
20namespace control_loops {
21
Austin Schuhdff24e22013-03-06 00:41:21 -080022double IndexMotor::Frisbee::ObserveNoTopDiscSensor(
Austin Schuh825bde92013-03-06 00:16:46 -080023 double index_position, double index_velocity) {
Austin Schuhdff24e22013-03-06 00:41:21 -080024 // The absolute disc position in meters.
Austin Schuh1b864a12013-03-07 00:46:50 -080025 double disc_position = absolute_position(index_position);
Austin Schuh825bde92013-03-06 00:16:46 -080026 if (IndexMotor::kTopDiscDetectStart <= disc_position &&
27 disc_position <= IndexMotor::kTopDiscDetectStop) {
28 // Whoops, this shouldn't be happening.
29 // Move the disc off the way that makes most sense.
Austin Schuhdff24e22013-03-06 00:41:21 -080030 double distance_to_above = IndexMotor::ConvertDiscPositionToIndex(
31 ::std::abs(disc_position - IndexMotor::kTopDiscDetectStop));
32 double distance_to_below = IndexMotor::ConvertDiscPositionToIndex(
33 ::std::abs(disc_position - IndexMotor::kTopDiscDetectStart));
Austin Schuh825bde92013-03-06 00:16:46 -080034 if (::std::abs(index_velocity) < 100) {
35 if (distance_to_above < distance_to_below) {
Austin Schuhdff24e22013-03-06 00:41:21 -080036 printf("Moving disc to top slow.\n");
Austin Schuh825bde92013-03-06 00:16:46 -080037 // Move it up.
Austin Schuhdff24e22013-03-06 00:41:21 -080038 index_start_position_ -= distance_to_above;
39 return -distance_to_above;
Austin Schuh825bde92013-03-06 00:16:46 -080040 } else {
Austin Schuhdff24e22013-03-06 00:41:21 -080041 printf("Moving disc to bottom slow.\n");
42 index_start_position_ += distance_to_below;
43 return distance_to_below;
Austin Schuh825bde92013-03-06 00:16:46 -080044 }
45 } else {
46 if (index_velocity > 0) {
47 // Now going up. If we didn't see it before, and we don't see it
48 // now but it should be in view, it must still be below. If it were
49 // above, it would be going further away from us.
Austin Schuh1b864a12013-03-07 00:46:50 -080050 printf("Moving fast up, shifting disc down. Disc was at %f\n",
51 absolute_position(index_position));
Austin Schuhdff24e22013-03-06 00:41:21 -080052 index_start_position_ += distance_to_below;
Austin Schuh1b864a12013-03-07 00:46:50 -080053 printf("Moving fast up, shifting disc down. Disc now at %f\n",
54 absolute_position(index_position));
Austin Schuhdff24e22013-03-06 00:41:21 -080055 return distance_to_below;
Austin Schuh825bde92013-03-06 00:16:46 -080056 } else {
Austin Schuh1b864a12013-03-07 00:46:50 -080057 printf("Moving fast down, shifting disc up. Disc was at %f\n",
58 absolute_position(index_position));
Austin Schuhdff24e22013-03-06 00:41:21 -080059 index_start_position_ -= distance_to_above;
Austin Schuh1b864a12013-03-07 00:46:50 -080060 printf("Moving fast down, shifting disc up. Disc now at %f\n",
61 absolute_position(index_position));
Austin Schuhdff24e22013-03-06 00:41:21 -080062 return -distance_to_above;
Austin Schuh825bde92013-03-06 00:16:46 -080063 }
64 }
65 }
Austin Schuhdff24e22013-03-06 00:41:21 -080066 return 0.0;
Austin Schuh825bde92013-03-06 00:16:46 -080067}
68
Austin Schuhd78ab542013-03-01 22:22:19 -080069IndexMotor::IndexMotor(control_loops::IndexLoop *my_index)
70 : aos::control_loops::ControlLoop<control_loops::IndexLoop>(my_index),
Austin Schuh93485832013-03-04 00:01:34 -080071 wrist_loop_(new IndexStateFeedbackLoop(MakeIndexLoop())),
Austin Schuhd78ab542013-03-01 22:22:19 -080072 hopper_disc_count_(0),
73 total_disc_count_(0),
Austin Schuh70be1ba2013-03-10 13:37:17 -070074 shot_disc_count_(0),
Austin Schuhf8c52252013-03-03 02:25:49 -080075 safe_goal_(Goal::HOLD),
76 loader_goal_(LoaderGoal::READY),
77 loader_state_(LoaderState::READY),
Austin Schuhd78ab542013-03-01 22:22:19 -080078 loader_up_(false),
79 disc_clamped_(false),
80 disc_ejected_(false),
Austin Schuhbcdb90c2013-03-03 23:24:58 -080081 last_bottom_disc_detect_(false),
Austin Schuh825bde92013-03-06 00:16:46 -080082 last_top_disc_detect_(false),
Austin Schuhbcdb90c2013-03-03 23:24:58 -080083 no_prior_position_(true),
Austin Schuh723770b2013-03-10 13:26:20 -070084 missing_position_count_(0) {
Austin Schuhd78ab542013-03-01 22:22:19 -080085}
86
Austin Schuhf8c52252013-03-03 02:25:49 -080087/*static*/ const double IndexMotor::kTransferStartPosition = 0.0;
88/*static*/ const double IndexMotor::kIndexStartPosition = 0.2159;
89/*static*/ const double IndexMotor::kIndexFreeLength =
90 IndexMotor::ConvertDiscAngleToDiscPosition((360 * 2 + 14) * M_PI / 180);
91/*static*/ const double IndexMotor::kLoaderFreeStopPosition =
92 kIndexStartPosition + kIndexFreeLength;
Austin Schuh1b864a12013-03-07 00:46:50 -080093/*static*/ const double IndexMotor::kReadyToPreload =
94 kLoaderFreeStopPosition - ConvertDiscAngleToDiscPosition(M_PI / 6.0);
Austin Schuhf8c52252013-03-03 02:25:49 -080095/*static*/ const double IndexMotor::kReadyToLiftPosition =
96 kLoaderFreeStopPosition + 0.2921;
97/*static*/ const double IndexMotor::kGrabberLength = 0.03175;
98/*static*/ const double IndexMotor::kGrabberStartPosition =
99 kReadyToLiftPosition - kGrabberLength;
Austin Schuh6328daf2013-03-05 00:53:15 -0800100/*static*/ const double IndexMotor::kGrabberMovementVelocity = 0.7;
Austin Schuhf8c52252013-03-03 02:25:49 -0800101/*static*/ const double IndexMotor::kLifterStopPosition =
102 kReadyToLiftPosition + 0.161925;
103/*static*/ const double IndexMotor::kLifterMovementVelocity = 1.0;
104/*static*/ const double IndexMotor::kEjectorStopPosition =
105 kLifterStopPosition + 0.01;
106/*static*/ const double IndexMotor::kEjectorMovementVelocity = 1.0;
Austin Schuhcc297022013-03-09 23:26:40 -0800107/*static*/ const double IndexMotor::kBottomDiscDetectStart = 0.00;
108/*static*/ const double IndexMotor::kBottomDiscDetectStop = 0.13;
109/*static*/ const double IndexMotor::kBottomDiscIndexDelay = 0.032;
Austin Schuhf8c52252013-03-03 02:25:49 -0800110
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800111// TODO(aschuh): Verify these with the sensor actually on.
Austin Schuh825bde92013-03-06 00:16:46 -0800112/*static*/ const double IndexMotor::kTopDiscDetectStart =
113 (IndexMotor::kLoaderFreeStopPosition -
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800114 IndexMotor::ConvertDiscAngleToDiscPosition(49 * M_PI / 180));
Austin Schuh825bde92013-03-06 00:16:46 -0800115/*static*/ const double IndexMotor::kTopDiscDetectStop =
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800116 (IndexMotor::kLoaderFreeStopPosition +
117 IndexMotor::ConvertDiscAngleToDiscPosition(19 * M_PI / 180));
118
119// I measured the angle between 2 discs. That then gives me the distance
120// between 2 posedges (or negedges). Then subtract off the width of the
121// positive pulse, and that gives the width of the negative pulse.
122/*static*/ const double IndexMotor::kTopDiscDetectMinSeperation =
123 (IndexMotor::ConvertDiscAngleToDiscPosition(120 * M_PI / 180) -
124 (IndexMotor::kTopDiscDetectStop - IndexMotor::kTopDiscDetectStart));
Austin Schuhf8c52252013-03-03 02:25:49 -0800125
Austin Schuhd78ab542013-03-01 22:22:19 -0800126const /*static*/ double IndexMotor::kDiscRadius = 10.875 * 0.0254 / 2;
127const /*static*/ double IndexMotor::kRollerRadius = 2.0 * 0.0254 / 2;
Austin Schuhf8c52252013-03-03 02:25:49 -0800128const /*static*/ double IndexMotor::kTransferRollerRadius = 1.25 * 0.0254 / 2;
Austin Schuhd78ab542013-03-01 22:22:19 -0800129
Austin Schuhf8c52252013-03-03 02:25:49 -0800130/*static*/ const int IndexMotor::kGrabbingDelay = 5;
131/*static*/ const int IndexMotor::kLiftingDelay = 20;
132/*static*/ const int IndexMotor::kShootingDelay = 5;
133/*static*/ const int IndexMotor::kLoweringDelay = 20;
Austin Schuhd78ab542013-03-01 22:22:19 -0800134
Austin Schuh93485832013-03-04 00:01:34 -0800135// TODO(aschuh): Tune these.
136/*static*/ const double
137 IndexMotor::IndexStateFeedbackLoop::kMinMotionVoltage = 5.0;
138/*static*/ const double
139 IndexMotor::IndexStateFeedbackLoop::kNoMotionCuttoffCount = 30;
140
Austin Schuhd78ab542013-03-01 22:22:19 -0800141// Distance to move the indexer when grabbing a disc.
142const double kNextPosition = 10.0;
143
144/*static*/ double IndexMotor::ConvertDiscAngleToIndex(const double angle) {
145 return (angle * (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
146}
147
Austin Schuhf8c52252013-03-03 02:25:49 -0800148/*static*/ double IndexMotor::ConvertDiscAngleToDiscPosition(
149 const double angle) {
Austin Schuhd78ab542013-03-01 22:22:19 -0800150 return angle * (kDiscRadius + kRollerRadius);
151}
152
Austin Schuhf8c52252013-03-03 02:25:49 -0800153/*static*/ double IndexMotor::ConvertDiscPositionToDiscAngle(
154 const double position) {
155 return position / (kDiscRadius + kRollerRadius);
156}
157
Austin Schuhd78ab542013-03-01 22:22:19 -0800158/*static*/ double IndexMotor::ConvertIndexToDiscAngle(const double angle) {
159 return (angle / (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
160}
161
162/*static*/ double IndexMotor::ConvertIndexToDiscPosition(const double angle) {
163 return IndexMotor::ConvertDiscAngleToDiscPosition(
164 ConvertIndexToDiscAngle(angle));
165}
166
Austin Schuhf8c52252013-03-03 02:25:49 -0800167/*static*/ double IndexMotor::ConvertTransferToDiscPosition(
168 const double angle) {
169 const double gear_ratio = (1 + (kDiscRadius * 2 + kTransferRollerRadius) /
170 kTransferRollerRadius);
171 return angle / gear_ratio * (kDiscRadius + kTransferRollerRadius);
172}
173
174/*static*/ double IndexMotor::ConvertDiscPositionToIndex(
175 const double position) {
176 return IndexMotor::ConvertDiscAngleToIndex(
177 ConvertDiscPositionToDiscAngle(position));
178}
179
Austin Schuh1b864a12013-03-07 00:46:50 -0800180bool IndexMotor::MinDiscPosition(double *disc_position, Frisbee **found_disc) {
Austin Schuhf8c52252013-03-03 02:25:49 -0800181 bool found_start = false;
182 for (unsigned int i = 0; i < frisbees_.size(); ++i) {
Austin Schuh1b864a12013-03-07 00:46:50 -0800183 Frisbee &frisbee = frisbees_[i];
Austin Schuhf8c52252013-03-03 02:25:49 -0800184 if (!found_start) {
185 if (frisbee.has_position()) {
186 *disc_position = frisbee.position();
Austin Schuh1b864a12013-03-07 00:46:50 -0800187 if (found_disc) {
188 *found_disc = &frisbee;
189 }
Austin Schuhf8c52252013-03-03 02:25:49 -0800190 found_start = true;
191 }
192 } else {
Austin Schuh1b864a12013-03-07 00:46:50 -0800193 if (frisbee.position() <= *disc_position) {
194 *disc_position = frisbee.position();
195 if (found_disc) {
196 *found_disc = &frisbee;
197 }
198 }
Austin Schuhf8c52252013-03-03 02:25:49 -0800199 }
200 }
201 return found_start;
202}
203
Austin Schuh1b864a12013-03-07 00:46:50 -0800204bool IndexMotor::MaxDiscPosition(double *disc_position, Frisbee **found_disc) {
Austin Schuhf8c52252013-03-03 02:25:49 -0800205 bool found_start = false;
206 for (unsigned int i = 0; i < frisbees_.size(); ++i) {
Austin Schuh1b864a12013-03-07 00:46:50 -0800207 Frisbee &frisbee = frisbees_[i];
Austin Schuhf8c52252013-03-03 02:25:49 -0800208 if (!found_start) {
209 if (frisbee.has_position()) {
210 *disc_position = frisbee.position();
Austin Schuh1b864a12013-03-07 00:46:50 -0800211 if (found_disc) {
212 *found_disc = &frisbee;
213 }
Austin Schuhf8c52252013-03-03 02:25:49 -0800214 found_start = true;
215 }
216 } else {
Austin Schuh1b864a12013-03-07 00:46:50 -0800217 if (frisbee.position() > *disc_position) {
218 *disc_position = frisbee.position();
219 if (found_disc) {
220 *found_disc = &frisbee;
221 }
222 }
Austin Schuhf8c52252013-03-03 02:25:49 -0800223 }
224 }
225 return found_start;
226}
227
Austin Schuh93485832013-03-04 00:01:34 -0800228void IndexMotor::IndexStateFeedbackLoop::CapU() {
229 // If the voltage has been low for a large number of cycles, cut the motor
230 // power. This is generally very bad controls practice since this isn't LTI,
231 // but we don't really care about tracking anything other than large step
232 // inputs, and the loader doesn't need to be that accurate.
233 if (::std::abs(U(0, 0)) < kMinMotionVoltage) {
234 ++low_voltage_count_;
235 if (low_voltage_count_ > kNoMotionCuttoffCount) {
236 printf("Limiting power from %f to 0\n", U(0, 0));
237 U(0, 0) = 0.0;
238 }
239 } else {
240 low_voltage_count_ = 0;
241 }
242
243 for (int i = 0; i < kNumOutputs; ++i) {
244 if (U[i] > plant.U_max[i]) {
245 U[i] = plant.U_max[i];
246 } else if (U[i] < plant.U_min[i]) {
247 U[i] = plant.U_min[i];
248 }
249 }
250}
251
252
Austin Schuhd78ab542013-03-01 22:22:19 -0800253// Positive angle is towards the shooter, and positive power is towards the
254// shooter.
255void IndexMotor::RunIteration(
256 const control_loops::IndexLoop::Goal *goal,
257 const control_loops::IndexLoop::Position *position,
258 control_loops::IndexLoop::Output *output,
259 control_loops::IndexLoop::Status *status) {
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800260 // Make goal easy to work with and sanity check it.
Austin Schuhd78ab542013-03-01 22:22:19 -0800261 Goal goal_enum = static_cast<Goal>(goal->goal_state);
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800262 if (goal->goal_state < 0 || goal->goal_state > 4) {
Brian Silverman94195052013-03-09 13:45:05 -0800263 LOG(ERROR, "Goal state is %"PRId32" which is out of range. Going to HOLD.\n",
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800264 goal->goal_state);
265 goal_enum = Goal::HOLD;
266 }
Austin Schuhd78ab542013-03-01 22:22:19 -0800267
268 // Disable the motors now so that all early returns will return with the
269 // motors disabled.
Austin Schuhb6d898b2013-03-03 15:34:35 -0800270 double intake_voltage = 0.0;
Austin Schuhf8c52252013-03-03 02:25:49 -0800271 double transfer_voltage = 0.0;
Austin Schuhd78ab542013-03-01 22:22:19 -0800272 if (output) {
Austin Schuhb6d898b2013-03-03 15:34:35 -0800273 output->intake_voltage = 0.0;
Austin Schuhd78ab542013-03-01 22:22:19 -0800274 output->transfer_voltage = 0.0;
275 output->index_voltage = 0.0;
276 }
277
278 status->ready_to_intake = false;
279
Austin Schuhf8c52252013-03-03 02:25:49 -0800280 // Compute a safe index position that we can use.
Austin Schuhd78ab542013-03-01 22:22:19 -0800281 if (position) {
282 wrist_loop_->Y << position->index_position;
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800283 // Set the goal to be the current position if this is the first time through
284 // so we don't always spin the indexer to the 0 position before starting.
285 if (no_prior_position_) {
286 wrist_loop_->R << wrist_loop_->Y(0, 0), 0.0;
Austin Schuhc5ef1bb2013-03-10 00:42:05 -0800287 wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800288 no_prior_position_ = false;
Austin Schuh6328daf2013-03-05 00:53:15 -0800289 last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
290 last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
291 last_bottom_disc_negedge_wait_count_ =
292 position->bottom_disc_negedge_wait_count;
Austin Schuh825bde92013-03-06 00:16:46 -0800293 last_top_disc_posedge_count_ = position->top_disc_posedge_count;
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800294 last_top_disc_negedge_count_ = position->top_disc_negedge_count;
295 // The open positions for the upper is right here and isn't a hard edge.
Austin Schuh723770b2013-03-10 13:26:20 -0700296 upper_open_region_.Restart(wrist_loop_->Y(0, 0));
297 lower_open_region_.Restart(wrist_loop_->Y(0, 0));
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800298 }
299
Austin Schuh1b864a12013-03-07 00:46:50 -0800300 // If the cRIO is gone for over 1/2 of a second, assume that it rebooted.
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800301 if (missing_position_count_ > 50) {
Austin Schuh6328daf2013-03-05 00:53:15 -0800302 last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
303 last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
304 last_bottom_disc_negedge_wait_count_ =
305 position->bottom_disc_negedge_wait_count;
Austin Schuh825bde92013-03-06 00:16:46 -0800306 last_top_disc_posedge_count_ = position->top_disc_posedge_count;
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800307 last_top_disc_negedge_count_ = position->top_disc_negedge_count;
308 // We can't really trust the open range any more if the crio rebooted.
Austin Schuh723770b2013-03-10 13:26:20 -0700309 upper_open_region_.Restart(wrist_loop_->Y(0, 0));
310 lower_open_region_.Restart(wrist_loop_->Y(0, 0));
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800311 // Adjust the disc positions so that they don't have to move.
312 const double disc_offset =
313 position->index_position - wrist_loop_->X_hat(0, 0);
314 for (auto frisbee = frisbees_.begin();
315 frisbee != frisbees_.end(); ++frisbee) {
316 frisbee->OffsetDisc(disc_offset);
317 }
Austin Schuhc5ef1bb2013-03-10 00:42:05 -0800318 wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800319 }
320 missing_position_count_ = 0;
321 } else {
322 ++missing_position_count_;
Austin Schuhd78ab542013-03-01 22:22:19 -0800323 }
324 const double index_position = wrist_loop_->X_hat(0, 0);
325
Austin Schuh825bde92013-03-06 00:16:46 -0800326 if (position) {
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800327 // Reset the open region if we saw a negedge.
Austin Schuh723770b2013-03-10 13:26:20 -0700328 if (position->bottom_disc_negedge_wait_count !=
329 last_bottom_disc_negedge_wait_count_) {
330 // Saw a negedge, must be a new region.
331 lower_open_region_.Restart(position->bottom_disc_negedge_wait_position);
332 }
333 // Reset the open region if we saw a negedge.
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800334 if (position->top_disc_negedge_count != last_top_disc_negedge_count_) {
335 // Saw a negedge, must be a new region.
Austin Schuh723770b2013-03-10 13:26:20 -0700336 upper_open_region_.Restart(position->top_disc_negedge_position);
337 }
338
339 // No disc. Expand the open region.
340 if (!position->bottom_disc_detect) {
341 lower_open_region_.Expand(index_position);
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800342 }
343
344 // No disc. Expand the open region.
345 if (!position->top_disc_detect) {
Austin Schuh723770b2013-03-10 13:26:20 -0700346 upper_open_region_.Expand(index_position);
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800347 }
348
Austin Schuh825bde92013-03-06 00:16:46 -0800349 if (!position->top_disc_detect) {
350 // We don't see a disc. Verify that there are no discs that we should be
351 // seeing.
Austin Schuh1b864a12013-03-07 00:46:50 -0800352 // Assume that discs will move slow enough that we won't miss one as it
353 // goes by. They will either pile up above or below the sensor.
Austin Schuhdff24e22013-03-06 00:41:21 -0800354
355 double cumulative_offset = 0.0;
356 for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
357 frisbee != rend; ++frisbee) {
358 frisbee->OffsetDisc(cumulative_offset);
359 double amount_moved = frisbee->ObserveNoTopDiscSensor(
Austin Schuh825bde92013-03-06 00:16:46 -0800360 wrist_loop_->X_hat(0, 0), wrist_loop_->X_hat(1, 0));
Austin Schuhdff24e22013-03-06 00:41:21 -0800361 cumulative_offset += amount_moved;
Austin Schuh825bde92013-03-06 00:16:46 -0800362 }
363 }
Austin Schuh1b864a12013-03-07 00:46:50 -0800364
Austin Schuh825bde92013-03-06 00:16:46 -0800365 if (position->top_disc_posedge_count != last_top_disc_posedge_count_) {
Austin Schuh1b864a12013-03-07 00:46:50 -0800366 const double index_position = wrist_loop_->X_hat(0, 0) -
367 position->index_position + position->top_disc_posedge_position;
Austin Schuh825bde92013-03-06 00:16:46 -0800368 // TODO(aschuh): Sanity check this number...
369 // Requires storing when the disc was last seen with the sensor off, and
370 // figuring out what to do if things go south.
371
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800372 // 1 if discs are going up, 0 if we have no clue, and -1 if they are going
373 // down.
374 int disc_direction = 0;
Austin Schuh825bde92013-03-06 00:16:46 -0800375 if (wrist_loop_->X_hat(1, 0) > 50.0) {
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800376 disc_direction = 1;
377 } else if (wrist_loop_->X_hat(1, 0) < -50.0) {
378 disc_direction = -1;
379 } else {
380 // Save the upper and lower positions that we last saw a disc at.
381 // If there is a big buffer above, must be a disc from below.
382 // If there is a big buffer below, must be a disc from above.
383 // This should work to replace the velocity threshold above.
384
Austin Schuh723770b2013-03-10 13:26:20 -0700385 const double open_width = upper_open_region_.width();
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800386 const double relative_upper_open_precentage =
Austin Schuh723770b2013-03-10 13:26:20 -0700387 (upper_open_region_.upper_bound() - index_position) / open_width;
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800388 const double relative_lower_open_precentage =
Austin Schuh723770b2013-03-10 13:26:20 -0700389 (index_position - upper_open_region_.lower_bound()) / open_width;
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800390 printf("Width %f upper %f lower %f\n",
391 open_width, relative_upper_open_precentage,
392 relative_lower_open_precentage);
393
394 if (ConvertIndexToDiscPosition(open_width) <
395 kTopDiscDetectMinSeperation * 0.9) {
396 LOG(ERROR, "Discs are way too close to each other. Doing nothing\n");
397 } else if (relative_upper_open_precentage > 0.75) {
398 // Looks like it is a disc going down from above since we are near
399 // the upper edge.
400 disc_direction = -1;
401 printf("Disc edge going down\n");
402 } else if (relative_lower_open_precentage > 0.75) {
403 // Looks like it is a disc going up from below since we are near
404 // the lower edge.
405 disc_direction = 1;
406 printf("Disc edge going up\n");
407 } else {
408 LOG(ERROR,
409 "Got an edge in the middle of what should be an open region.\n");
410 LOG(ERROR, "Open width: %f upper precentage %f %%\n",
411 open_width, relative_upper_open_precentage);
412 }
413 }
414
415 if (disc_direction > 0) {
Austin Schuh825bde92013-03-06 00:16:46 -0800416 // Moving up at a reasonable clip.
Austin Schuh1b864a12013-03-07 00:46:50 -0800417 // Find the highest disc that is below the top disc sensor.
418 // While we are at it, count the number above and log an error if there
419 // are too many.
420 if (frisbees_.size() == 0) {
421 Frisbee new_frisbee;
422 new_frisbee.has_been_indexed_ = true;
423 new_frisbee.index_start_position_ = index_position -
424 ConvertDiscPositionToIndex(kTopDiscDetectStart -
425 kIndexStartPosition);
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800426 ++hopper_disc_count_;
427 ++total_disc_count_;
Austin Schuh1b864a12013-03-07 00:46:50 -0800428 frisbees_.push_front(new_frisbee);
429 LOG(WARNING, "Added a disc to the hopper at the top sensor\n");
430 }
431
432 int above_disc_count = 0;
433 double highest_position = 0;
434 Frisbee *highest_frisbee_below_sensor = NULL;
435 for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
436 frisbee != rend; ++frisbee) {
437 const double disc_position = frisbee->absolute_position(
438 index_position);
439 // It is save to use the top position for the cuttoff, since the
440 // sensor being low will result in discs being pushed off of it.
441 if (disc_position >= kTopDiscDetectStop) {
442 ++above_disc_count;
443 } else if (!highest_frisbee_below_sensor ||
444 disc_position > highest_position) {
445 highest_frisbee_below_sensor = &*frisbee;
446 highest_position = disc_position;
447 }
448 }
449 if (above_disc_count > 1) {
450 LOG(ERROR, "We have 2 discs above the top sensor.\n");
451 }
452
453 // We now have the disc. Shift all the ones below the sensor up by the
454 // computed delta.
455 const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
456 highest_position - kTopDiscDetectStart);
457 for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
458 frisbee != rend; ++frisbee) {
459 const double disc_position = frisbee->absolute_position(
460 index_position);
461 if (disc_position < kTopDiscDetectStop) {
462 frisbee->OffsetDisc(disc_delta);
463 }
464 }
465 printf("Currently have %d discs, saw posedge moving up. "
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800466 "Moving down by %f to %f\n", frisbees_.size(),
Austin Schuh1b864a12013-03-07 00:46:50 -0800467 ConvertIndexToDiscPosition(disc_delta),
468 highest_frisbee_below_sensor->absolute_position(
469 wrist_loop_->X_hat(0, 0)));
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800470 } else if (disc_direction < 0) {
Austin Schuh825bde92013-03-06 00:16:46 -0800471 // Moving down at a reasonable clip.
Austin Schuh1b864a12013-03-07 00:46:50 -0800472 // There can only be 1 disc up top that would give us a posedge.
473 // Find it and place it at the one spot that it can be.
Brian Silverman94195052013-03-09 13:45:05 -0800474 double min_disc_position = 0;
Austin Schuh1b864a12013-03-07 00:46:50 -0800475 Frisbee *min_frisbee = NULL;
476 MinDiscPosition(&min_disc_position, &min_frisbee);
477 if (!min_frisbee) {
478 // Uh, oh, we see a disc but there isn't one...
479 LOG(ERROR, "Saw a disc up top but there isn't one in the hopper\n");
480 } else {
481 const double disc_position = min_frisbee->absolute_position(
482 index_position);
483
484 const double disc_delta_meters = disc_position - kTopDiscDetectStop;
485 const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
486 disc_delta_meters);
487 printf("Posedge going down. Moving top disc down by %f\n",
488 disc_delta_meters);
489 for (auto frisbee = frisbees_.begin(), end = frisbees_.end();
490 frisbee != end; ++frisbee) {
491 frisbee->OffsetDisc(disc_delta);
492 }
493 }
Austin Schuh825bde92013-03-06 00:16:46 -0800494 } else {
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800495 LOG(ERROR, "Not sure how to handle the upper posedge, doing nothing\n");
Austin Schuh825bde92013-03-06 00:16:46 -0800496 }
497 }
498 }
Austin Schuhf8c52252013-03-03 02:25:49 -0800499
Austin Schuhf8c52252013-03-03 02:25:49 -0800500 // Bool to track if it is safe for the goal to change yet.
Austin Schuhd78ab542013-03-01 22:22:19 -0800501 bool safe_to_change_state_ = true;
502 switch (safe_goal_) {
Austin Schuhf8c52252013-03-03 02:25:49 -0800503 case Goal::HOLD:
Austin Schuhd78ab542013-03-01 22:22:19 -0800504 // The goal should already be good, so sit tight with everything the same
505 // as it was.
Austin Schuhd78ab542013-03-01 22:22:19 -0800506 break;
Austin Schuhf8c52252013-03-03 02:25:49 -0800507 case Goal::READY_LOWER:
508 case Goal::INTAKE:
Austin Schuhd78ab542013-03-01 22:22:19 -0800509 {
510 Time now = Time::Now();
Austin Schuhd78ab542013-03-01 22:22:19 -0800511 if (position) {
Austin Schuh6328daf2013-03-05 00:53:15 -0800512 // Posedge of the disc entering the beam break.
513 if (position->bottom_disc_posedge_count !=
514 last_bottom_disc_posedge_count_) {
Austin Schuhd78ab542013-03-01 22:22:19 -0800515 transfer_frisbee_.Reset();
516 transfer_frisbee_.bottom_posedge_time_ = now;
517 printf("Posedge of bottom disc %f\n",
518 transfer_frisbee_.bottom_posedge_time_.ToSeconds());
519 ++hopper_disc_count_;
Austin Schuhf8c52252013-03-03 02:25:49 -0800520 ++total_disc_count_;
Austin Schuhd78ab542013-03-01 22:22:19 -0800521 }
522
523 // Disc exited the beam break now.
Austin Schuh6328daf2013-03-05 00:53:15 -0800524 if (position->bottom_disc_negedge_count !=
525 last_bottom_disc_negedge_count_) {
Austin Schuhd78ab542013-03-01 22:22:19 -0800526 transfer_frisbee_.bottom_negedge_time_ = now;
527 printf("Negedge of bottom disc %f\n",
528 transfer_frisbee_.bottom_negedge_time_.ToSeconds());
529 frisbees_.push_front(transfer_frisbee_);
530 }
531
532 if (position->bottom_disc_detect) {
Austin Schuhb6d898b2013-03-03 15:34:35 -0800533 intake_voltage = transfer_voltage = 12.0;
Austin Schuhd78ab542013-03-01 22:22:19 -0800534 // Must wait until the disc gets out before we can change state.
535 safe_to_change_state_ = false;
536
Austin Schuhf8c52252013-03-03 02:25:49 -0800537 // TODO(aschuh): A disc on the way through needs to start moving
538 // the indexer if it isn't already moving. Maybe?
Austin Schuhd78ab542013-03-01 22:22:19 -0800539
540 Time elapsed_posedge_time = now -
541 transfer_frisbee_.bottom_posedge_time_;
542 if (elapsed_posedge_time >= Time::InSeconds(0.3)) {
543 // It has been too long. The disc must be jammed.
544 LOG(ERROR, "Been way too long. Jammed disc?\n");
545 printf("Been way too long. Jammed disc?\n");
546 }
547 }
548
Austin Schuhf8c52252013-03-03 02:25:49 -0800549 // Check all non-indexed discs and see if they should be indexed.
Austin Schuhb6d898b2013-03-03 15:34:35 -0800550 for (auto frisbee = frisbees_.begin();
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800551 frisbee != frisbees_.end(); ++frisbee) {
Austin Schuhb6d898b2013-03-03 15:34:35 -0800552 if (!frisbee->has_been_indexed_) {
553 intake_voltage = transfer_voltage = 12.0;
Austin Schuhd78ab542013-03-01 22:22:19 -0800554
Austin Schuh6328daf2013-03-05 00:53:15 -0800555 if (last_bottom_disc_negedge_wait_count_ !=
556 position->bottom_disc_negedge_wait_count) {
557 // We have an index difference.
558 // Save the indexer position, and the time.
559 if (last_bottom_disc_negedge_wait_count_ + 1 !=
560 position->bottom_disc_negedge_wait_count) {
561 LOG(ERROR, "Funny, we got 2 edges since we last checked.\n");
562 }
563
564 // Save the captured position as the position at which the disc
565 // touched the indexer.
Austin Schuhd78ab542013-03-01 22:22:19 -0800566 LOG(INFO, "Grabbed on the index now at %f\n", index_position);
567 printf("Grabbed on the index now at %f\n", index_position);
Austin Schuhb6d898b2013-03-03 15:34:35 -0800568 frisbee->has_been_indexed_ = true;
Austin Schuh6328daf2013-03-05 00:53:15 -0800569 frisbee->index_start_position_ =
570 position->bottom_disc_negedge_wait_position;
Austin Schuhd78ab542013-03-01 22:22:19 -0800571 }
572 }
Austin Schuhb6d898b2013-03-03 15:34:35 -0800573 if (!frisbee->has_been_indexed_) {
Austin Schuhf8c52252013-03-03 02:25:49 -0800574 // All discs must be indexed before it is safe to stop indexing.
Austin Schuhd78ab542013-03-01 22:22:19 -0800575 safe_to_change_state_ = false;
576 }
577 }
578
Austin Schuhf8c52252013-03-03 02:25:49 -0800579 // Figure out where the indexer should be to move the discs down to
580 // the right position.
Brian Silverman94195052013-03-09 13:45:05 -0800581 double max_disc_position = 0;
Austin Schuh1b864a12013-03-07 00:46:50 -0800582 if (MaxDiscPosition(&max_disc_position, NULL)) {
Austin Schuhf8c52252013-03-03 02:25:49 -0800583 printf("There is a disc down here!\n");
584 // TODO(aschuh): Figure out what to do if grabbing the next one
585 // would cause things to jam into the loader.
586 // Say we aren't ready any more. Undefined behavior will result if
587 // that isn't observed.
588 double bottom_disc_position =
589 max_disc_position + ConvertDiscAngleToIndex(M_PI);
590 wrist_loop_->R << bottom_disc_position, 0.0;
Austin Schuhd78ab542013-03-01 22:22:19 -0800591
Austin Schuhf8c52252013-03-03 02:25:49 -0800592 // Verify that we are close enough to the goal so that we should be
593 // fine accepting the next disc.
594 double disc_error_meters = ConvertIndexToDiscPosition(
595 wrist_loop_->X_hat(0, 0) - bottom_disc_position);
596 // We are ready for the next disc if the first one is in the first
597 // half circle of the indexer. It will take time for the disc to
598 // come into the indexer, so we will be able to move it out of the
599 // way in time.
600 // This choice also makes sure that we don't claim that we aren't
601 // ready between full speed intaking.
602 if (-ConvertDiscAngleToIndex(M_PI) < disc_error_meters &&
603 disc_error_meters < 0.04) {
604 // We are only ready if we aren't being asked to change state or
605 // are full.
606 status->ready_to_intake =
607 (safe_goal_ == goal_enum) && hopper_disc_count_ < 4;
608 } else {
609 status->ready_to_intake = false;
Austin Schuhd78ab542013-03-01 22:22:19 -0800610 }
Austin Schuhf8c52252013-03-03 02:25:49 -0800611 } else {
612 // No discs! We are always ready for more if we aren't being
613 // asked to change state.
614 status->ready_to_intake = (safe_goal_ == goal_enum);
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800615 printf("Ready to intake, zero discs. %d %d %d\n",
616 status->ready_to_intake, hopper_disc_count_, safe_goal_);
Austin Schuhd78ab542013-03-01 22:22:19 -0800617 }
Austin Schuhf8c52252013-03-03 02:25:49 -0800618
619 // Turn on the transfer roller if we are ready.
620 if (status->ready_to_intake && hopper_disc_count_ < 4 &&
621 safe_goal_ == Goal::INTAKE) {
Austin Schuhb6d898b2013-03-03 15:34:35 -0800622 intake_voltage = transfer_voltage = 12.0;
Austin Schuhf8c52252013-03-03 02:25:49 -0800623 }
Austin Schuhd78ab542013-03-01 22:22:19 -0800624 }
Austin Schuhf8c52252013-03-03 02:25:49 -0800625 printf("INTAKE\n");
Austin Schuhd78ab542013-03-01 22:22:19 -0800626 }
627 break;
Austin Schuhf8c52252013-03-03 02:25:49 -0800628 case Goal::READY_SHOOTER:
629 case Goal::SHOOT:
630 // Check if we have any discs to shoot or load and handle them.
Brian Silverman94195052013-03-09 13:45:05 -0800631 double min_disc_position = 0;
Austin Schuh1b864a12013-03-07 00:46:50 -0800632 if (MinDiscPosition(&min_disc_position, NULL)) {
633 const double ready_disc_position = min_disc_position +
634 ConvertDiscPositionToIndex(kReadyToPreload - kIndexStartPosition);
Austin Schuhf8c52252013-03-03 02:25:49 -0800635
636 const double grabbed_disc_position =
637 min_disc_position +
638 ConvertDiscPositionToIndex(kReadyToLiftPosition -
639 kIndexStartPosition + 0.03);
640
641 // Check the state of the loader FSM.
642 // If it is ready to load discs, position the disc so that it is ready
643 // to be grabbed.
644 // If it isn't ready, there is a disc in there. It needs to finish it's
645 // cycle first.
646 if (loader_state_ != LoaderState::READY) {
647 // We already have a disc in the loader.
648 // Stage the discs back a bit.
649 wrist_loop_->R << ready_disc_position, 0.0;
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800650 printf("Loader not ready but asked to shoot\n");
Austin Schuhf8c52252013-03-03 02:25:49 -0800651
Austin Schuhbcdb90c2013-03-03 23:24:58 -0800652 // Shoot if we are grabbed and being asked to shoot.
653 if (loader_state_ == LoaderState::GRABBED &&
654 safe_goal_ == Goal::SHOOT) {
655 loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
656 }
657
Austin Schuhf8c52252013-03-03 02:25:49 -0800658 // Must wait until it has been grabbed to continue.
659 if (loader_state_ == LoaderState::GRABBING) {
660 safe_to_change_state_ = false;
661 }
662 } else {
663 // No disc up top right now.
664 wrist_loop_->R << grabbed_disc_position, 0.0;
665
666 // See if the disc has gotten pretty far up yet.
667 if (wrist_loop_->X_hat(0, 0) > ready_disc_position) {
668 // Point of no return. We are committing to grabbing it now.
669 safe_to_change_state_ = false;
670 const double robust_grabbed_disc_position =
671 (grabbed_disc_position -
672 ConvertDiscPositionToIndex(kGrabberLength));
673
674 // If close, start grabbing and/or shooting.
675 if (wrist_loop_->X_hat(0, 0) > robust_grabbed_disc_position) {
676 // Start the state machine.
677 if (safe_goal_ == Goal::SHOOT) {
678 loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
679 } else {
680 loader_goal_ = LoaderGoal::GRAB;
681 }
682 // This frisbee is now gone. Take it out of the queue.
683 frisbees_.pop_back();
Austin Schuhf8c52252013-03-03 02:25:49 -0800684 }
685 }
686 }
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800687 } else {
688 if (loader_state_ != LoaderState::READY) {
689 // Shoot if we are grabbed and being asked to shoot.
690 if (loader_state_ == LoaderState::GRABBED &&
691 safe_goal_ == Goal::SHOOT) {
692 loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
693 }
694 } else {
695 // Ok, no discs in sight. Spin the hopper up by 150% of it's full
696 // range and verify that we don't see anything.
697 printf("Moving the indexer to verify that it is clear\n");
698 const double hopper_clear_verification_position =
Austin Schuh723770b2013-03-10 13:26:20 -0700699 ::std::min(upper_open_region_.lower_bound(),
700 lower_open_region_.lower_bound()) +
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800701 ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
702
703 wrist_loop_->R << hopper_clear_verification_position, 0.0;
704 if (::std::abs(wrist_loop_->X_hat(0, 0) -
705 hopper_clear_verification_position) <
706 ConvertDiscPositionToIndex(0.05)) {
707 printf("Should be empty\n");
708 // We are at the end of the range. There are no more discs here.
709 while (frisbees_.size() > 0) {
710 LOG(ERROR, "Dropping an extra disc since it can't exist\n");
711 frisbees_.pop_back();
712 --hopper_disc_count_;
713 --total_disc_count_;
714 }
715 if (hopper_disc_count_ != 0) {
716 LOG(ERROR,
717 "Emptied the hopper out but there are still discs there\n");
718 }
719 }
720 }
721 }
722
723 {
724 const double hopper_clear_verification_position =
Austin Schuh723770b2013-03-10 13:26:20 -0700725 ::std::min(upper_open_region_.lower_bound(),
726 lower_open_region_.lower_bound()) +
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800727 ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
728
729 if (wrist_loop_->X_hat(0, 0) >
730 hopper_clear_verification_position +
731 ConvertDiscPositionToIndex(0.05)) {
732 // We are at the end of the range. There are no more discs here.
733 while (frisbees_.size() > 0) {
734 LOG(ERROR, "Dropping an extra disc since it can't exist\n");
735 frisbees_.pop_back();
736 --hopper_disc_count_;
737 --total_disc_count_;
738 }
739 if (hopper_disc_count_ != 0) {
740 LOG(ERROR,
741 "Emptied the hopper out but there are still discs there\n");
742 }
743 }
Austin Schuhf8c52252013-03-03 02:25:49 -0800744 }
745
746 printf("READY_SHOOTER or SHOOT\n");
Austin Schuhd78ab542013-03-01 22:22:19 -0800747 break;
Austin Schuhf8c52252013-03-03 02:25:49 -0800748 }
749
750 // The only way out of the loader is to shoot the disc. The FSM can only go
751 // forwards.
752 switch (loader_state_) {
753 case LoaderState::READY:
754 printf("Loader READY\n");
755 // Open and down, ready to accept a disc.
756 loader_up_ = false;
757 disc_clamped_ = false;
758 disc_ejected_ = false;
759 if (loader_goal_ == LoaderGoal::GRAB ||
760 loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
761 if (loader_goal_ == LoaderGoal::GRAB) {
762 printf("Told to GRAB, moving on\n");
763 } else {
764 printf("Told to SHOOT_AND_RESET, moving on\n");
765 }
766 loader_state_ = LoaderState::GRABBING;
767 loader_countdown_ = kGrabbingDelay;
768 } else {
769 break;
770 }
771 case LoaderState::GRABBING:
772 printf("Loader GRABBING %d\n", loader_countdown_);
773 // Closing the grabber.
774 loader_up_ = false;
775 disc_clamped_ = true;
776 disc_ejected_ = false;
777 if (loader_countdown_ > 0) {
778 --loader_countdown_;
779 break;
780 } else {
781 loader_state_ = LoaderState::GRABBED;
782 }
783 case LoaderState::GRABBED:
784 printf("Loader GRABBED\n");
785 // Grabber closed.
786 loader_up_ = false;
787 disc_clamped_ = true;
788 disc_ejected_ = false;
789 if (loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
790 // TODO(aschuh): Only shoot if the shooter is up to speed.
791 // Seems like that would have us shooting a bit later than we could be,
792 // but it also probably spins back up real fast.
793 loader_state_ = LoaderState::LIFTING;
794 loader_countdown_ = kLiftingDelay;
795 printf("Told to SHOOT_AND_RESET, moving on\n");
796 } else if (loader_goal_ == LoaderGoal::READY) {
797 LOG(ERROR, "Can't go to ready when we have something grabbed.\n");
798 printf("Can't go to ready when we have something grabbed.\n");
799 break;
800 } else {
801 break;
802 }
803 case LoaderState::LIFTING:
804 printf("Loader LIFTING %d\n", loader_countdown_);
805 // Lifting the disc.
806 loader_up_ = true;
807 disc_clamped_ = true;
808 disc_ejected_ = false;
809 if (loader_countdown_ > 0) {
810 --loader_countdown_;
811 break;
812 } else {
813 loader_state_ = LoaderState::LIFTED;
814 }
815 case LoaderState::LIFTED:
816 printf("Loader LIFTED\n");
817 // Disc lifted. Time to eject it out.
818 loader_up_ = true;
819 disc_clamped_ = true;
820 disc_ejected_ = false;
821 loader_state_ = LoaderState::SHOOTING;
822 loader_countdown_ = kShootingDelay;
823 case LoaderState::SHOOTING:
824 printf("Loader SHOOTING %d\n", loader_countdown_);
825 // Ejecting the disc into the shooter.
826 loader_up_ = true;
827 disc_clamped_ = false;
828 disc_ejected_ = true;
829 if (loader_countdown_ > 0) {
830 --loader_countdown_;
831 break;
832 } else {
833 loader_state_ = LoaderState::SHOOT;
834 }
835 case LoaderState::SHOOT:
836 printf("Loader SHOOT\n");
837 // The disc has been shot.
838 loader_up_ = true;
839 disc_clamped_ = false;
840 disc_ejected_ = true;
841 loader_state_ = LoaderState::LOWERING;
842 loader_countdown_ = kLoweringDelay;
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800843 --hopper_disc_count_;
Austin Schuh70be1ba2013-03-10 13:37:17 -0700844 ++shot_disc_count_;
Austin Schuhf8c52252013-03-03 02:25:49 -0800845 case LoaderState::LOWERING:
846 printf("Loader LOWERING %d\n", loader_countdown_);
847 // Lowering the loader back down.
848 loader_up_ = false;
849 disc_clamped_ = false;
850 disc_ejected_ = true;
851 if (loader_countdown_ > 0) {
852 --loader_countdown_;
853 break;
854 } else {
855 loader_state_ = LoaderState::LOWERED;
856 }
857 case LoaderState::LOWERED:
858 printf("Loader LOWERED\n");
859 // The indexer is lowered.
860 loader_up_ = false;
861 disc_clamped_ = false;
862 disc_ejected_ = false;
863 loader_state_ = LoaderState::READY;
864 // Once we have shot, we need to hang out in READY until otherwise
865 // notified.
866 loader_goal_ = LoaderGoal::READY;
Austin Schuhd78ab542013-03-01 22:22:19 -0800867 break;
868 }
869
870 // Update the observer.
871 wrist_loop_->Update(position != NULL, output == NULL);
872
873 if (position) {
Austin Schuhf8c52252013-03-03 02:25:49 -0800874 LOG(DEBUG, "pos=%f\n", position->index_position);
Austin Schuhd78ab542013-03-01 22:22:19 -0800875 last_bottom_disc_detect_ = position->bottom_disc_detect;
Austin Schuh825bde92013-03-06 00:16:46 -0800876 last_top_disc_detect_ = position->top_disc_detect;
Austin Schuh6328daf2013-03-05 00:53:15 -0800877 last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
878 last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
879 last_bottom_disc_negedge_wait_count_ =
880 position->bottom_disc_negedge_wait_count;
Austin Schuh825bde92013-03-06 00:16:46 -0800881 last_top_disc_posedge_count_ = position->top_disc_posedge_count;
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800882 last_top_disc_negedge_count_ = position->top_disc_negedge_count;
Austin Schuhd78ab542013-03-01 22:22:19 -0800883 }
884
885 status->hopper_disc_count = hopper_disc_count_;
886 status->total_disc_count = total_disc_count_;
Austin Schuh70be1ba2013-03-10 13:37:17 -0700887 status->shot_disc_count = shot_disc_count_;
Austin Schuhf8c52252013-03-03 02:25:49 -0800888 status->preloaded = (loader_state_ != LoaderState::READY);
Austin Schuhd78ab542013-03-01 22:22:19 -0800889
890 if (output) {
Austin Schuhb6d898b2013-03-03 15:34:35 -0800891 output->intake_voltage = intake_voltage;
Austin Schuhf8c52252013-03-03 02:25:49 -0800892 output->transfer_voltage = transfer_voltage;
Austin Schuhd78ab542013-03-01 22:22:19 -0800893 output->index_voltage = wrist_loop_->U(0, 0);
Austin Schuhf8c52252013-03-03 02:25:49 -0800894 output->loader_up = loader_up_;
895 output->disc_clamped = disc_clamped_;
896 output->disc_ejected = disc_ejected_;
Austin Schuhd78ab542013-03-01 22:22:19 -0800897 }
898
899 if (safe_to_change_state_) {
900 safe_goal_ = goal_enum;
901 }
Austin Schuh7c0e2aa2013-03-09 02:01:16 -0800902 if (hopper_disc_count_ < 0) {
903 LOG(ERROR, "NEGATIVE DISCS. VERY VERY BAD\n");
904 }
Austin Schuhd78ab542013-03-01 22:22:19 -0800905}
906
907} // namespace control_loops
908} // namespace frc971