Sped up vertical can pickup
Change-Id: Ifec45b19f8000ae246fbd793350efca737249bc0
diff --git a/frc971/actors/can_pickup_action.q b/frc971/actors/can_pickup_action.q
index 95e0b3f..4846722 100644
--- a/frc971/actors/can_pickup_action.q
+++ b/frc971/actors/can_pickup_action.q
@@ -7,13 +7,19 @@
// It starts by lifting up, then moving the arm out, then lifting the can out of
// the claw, and then ends with the can inside the bot.
struct CanPickupParams {
- // Angle to move the claw to when picking up.
- double pickup_angle;
- // Height to move the elevator to when picking up.
- double pickup_height;
+ // X position for the fridge when picking up.
+ double pickup_x;
+ // Y position for the fridge when picking up.
+ double pickup_y;
// Height to move the elevator to to lift the can out of the claw.
double lift_height;
+ // Height to use when lifting before moving it back.
+ double pickup_goal_before_move_height;
+ // The height at which to start pulling back.
+ double before_place_height;
+ // X at which to start lowering the can.
+ double start_lowering_x;
// End position with the can.
double end_height;
double end_angle;
diff --git a/frc971/actors/can_pickup_actor.cc b/frc971/actors/can_pickup_actor.cc
index ee1d21e..ca493c3 100644
--- a/frc971/actors/can_pickup_actor.cc
+++ b/frc971/actors/can_pickup_actor.cc
@@ -7,16 +7,41 @@
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw.q.h"
+using ::frc971::control_loops::fridge_queue;
+
namespace frc971 {
namespace actors {
namespace {
-constexpr ProfileParams kArmMove{1.0, 1.6};
-constexpr ProfileParams kElevatorMove{0.6, 2.2};
+constexpr ProfileParams kHorizontalMove{1.1, 2.5};
+constexpr ProfileParams kVerticalMove{0.3, 2.0};
+constexpr ProfileParams kFastHorizontalMove{1.25, 5.0};
+constexpr ProfileParams kFastVerticalMove{0.40, 2.0};
+constexpr ProfileParams kPureVerticalMove{1.20, 5.0};
} // namespace
CanPickupActor::CanPickupActor(CanPickupActionQueueGroup *queues)
: FridgeActorBase<CanPickupActionQueueGroup>(queues) {}
+double CanPickupActor::CurrentGoalX() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ return fridge_queue.status->goal_x;
+}
+
+double CanPickupActor::CurrentGoalHeight() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ return fridge_queue.status->goal_y;
+}
+
bool CanPickupActor::RunAction(const CanPickupParams ¶ms) {
// Make sure the claw is down.
{
@@ -33,23 +58,43 @@
}
// Go around the can.
- DoFridgeProfile(params.pickup_height, params.pickup_angle, kElevatorMove,
- kArmMove, false);
+ DoFridgeXYProfile(params.pickup_x, params.pickup_y, kFastHorizontalMove,
+ kFastVerticalMove, true, false, false);
if (ShouldCancel()) return true;
- // Lift and grab.
- DoFridgeProfile(params.lift_height, params.pickup_angle, kElevatorMove,
- kArmMove, true);
+ if (!StartFridgeXYProfile(
+ params.pickup_x, params.pickup_goal_before_move_height,
+ kHorizontalMove, kPureVerticalMove, true, true, true)) {
+ return false;
+ }
+
+ bool above_claw = false;
+ while (true) {
+ if (CurrentGoalHeight() > params.lift_height && !above_claw) {
+ if (!StartFridgeXYProfile(0.0, params.before_place_height,
+ kHorizontalMove, kVerticalMove, true, true,
+ true)) {
+ return false;
+ }
+ above_claw = true;
+ }
+ if (CurrentGoalX() < params.start_lowering_x) {
+ // Getting close, start lowering.
+ LOG(DEBUG, "Starting to lower the can onto the tray.\n");
+ break;
+ }
+ ProfileStatus status =
+ IterateXYProfile(0.0, params.before_place_height, kHorizontalMove,
+ kVerticalMove, true, true, true);
+ if (status == DONE || status == CANCELED) {
+ break;
+ }
+ }
if (ShouldCancel()) return true;
- // Pull it back in.
- DoFridgeProfile(params.lift_height, params.end_angle, kElevatorMove, kArmMove,
- true);
- if (ShouldCancel()) return true;
-
- // Pull it back in.
- DoFridgeProfile(params.end_height, params.end_angle, kElevatorMove, kArmMove,
- true);
+ // Lower it.
+ DoFridgeXYProfile(0.0, params.end_height, kHorizontalMove, kPureVerticalMove,
+ true);
if (ShouldCancel()) return true;
return true;
diff --git a/frc971/actors/can_pickup_actor.h b/frc971/actors/can_pickup_actor.h
index f343214..3e109d1 100644
--- a/frc971/actors/can_pickup_actor.h
+++ b/frc971/actors/can_pickup_actor.h
@@ -18,6 +18,10 @@
explicit CanPickupActor(CanPickupActionQueueGroup *queues);
bool RunAction(const CanPickupParams ¶ms) override;
+
+ private:
+ double CurrentGoalHeight();
+ double CurrentGoalX();
};
typedef aos::common::actions::TypedAction<CanPickupActionQueueGroup>
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index f318aa7..474e2fd 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -235,10 +235,13 @@
// Vertical can pickup.
if (data.PosEdge(kCanPickup)) {
actors::CanPickupParams params;
- params.pickup_angle = -0.93;
- params.pickup_height = 0.265;
- params.lift_height = 0.65;
+ params.pickup_x = 0.6;
+ params.pickup_y = 0.1;
+ params.lift_height = 0.2;
+ params.pickup_goal_before_move_height = 0.3;
+ params.start_lowering_x = 0.1;
// End low so the can is supported.
+ params.before_place_height = 0.4;
params.end_height = 0.3;
params.end_angle = 0.0;
action_queue_.EnqueueAction(actors::MakeCanPickupAction(params));