Tuned actions.
Change-Id: I7e21db5c9344ff85aca1bb08f29432cf20fad0a7
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 022a924..5a93ebb 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -384,6 +384,7 @@
'dependencies': [
'fridge_profile_lib',
'stack_and_hold_action_queue',
+ 'stack_action_lib',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:phased_loop',
'<(AOS)/common/actions/actions.gyp:action_lib',
diff --git a/frc971/actors/held_to_lift_action.q b/frc971/actors/held_to_lift_action.q
index 78d2e95..3825ac8 100644
--- a/frc971/actors/held_to_lift_action.q
+++ b/frc971/actors/held_to_lift_action.q
@@ -12,6 +12,8 @@
double arm_clearance;
// End height.
double bottom_height;
+ // Amount to wait for the elevator to settle before lifting.
+ double before_lift_settle_time;
// Amount to wait to clamp.
double clamp_pause_time;
diff --git a/frc971/actors/held_to_lift_actor.cc b/frc971/actors/held_to_lift_actor.cc
index fa23631..2bd4a88 100644
--- a/frc971/actors/held_to_lift_actor.cc
+++ b/frc971/actors/held_to_lift_actor.cc
@@ -49,22 +49,40 @@
message.Send();
}
}
- // Lift the box straight up.
- DoFridgeProfile(params.bottom_height, params.arm_clearance, kFastElevatorMove,
- kFastArmMove, false);
- if (ShouldCancel()) return true;
- // Move it back to the storage location.
- DoFridgeProfile(params.bottom_height, 0.0, kElevatorMove, kArmMove, false);
- if (ShouldCancel()) return true;
+ control_loops::fridge_queue.status.FetchLatest();
+ if (!control_loops::fridge_queue.status.get()) {
+ return false;
+ }
- // Clamp
- DoFridgeProfile(params.bottom_height, 0.0, kElevatorMove, kArmMove, true);
- if (ShouldCancel()) return true;
+ if (control_loops::fridge_queue.status->goal_height != params.bottom_height ||
+ control_loops::fridge_queue.status->goal_angle != 0.0) {
+ // Lower with the fridge clamps open and move it forwards slightly to clear.
+ DoFridgeProfile(control_loops::fridge_queue.status->goal_height,
+ params.arm_clearance, kFastElevatorMove, kFastArmMove,
+ false);
+ if (ShouldCancel()) return true;
+ DoFridgeProfile(params.bottom_height, params.arm_clearance,
+ kFastElevatorMove, kFastArmMove, false);
+ if (ShouldCancel()) return true;
- if (!WaitOrCancel(aos::time::Time::InSeconds(params.clamp_pause_time))) {
- return true;
+ // Move it back to the storage location.
+ DoFridgeProfile(params.bottom_height, 0.0, kElevatorMove, kArmMove, false);
+ if (ShouldCancel()) return true;
+
+ if (!WaitOrCancel(
+ aos::time::Time::InSeconds(params.before_lift_settle_time))) {
+ return true;
+ }
+
+ // Clamp
+ DoFridgeProfile(params.bottom_height, 0.0, kElevatorMove, kArmMove, true);
+ if (ShouldCancel()) return true;
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.clamp_pause_time))) {
+ return true;
+ }
}
{
diff --git a/frc971/actors/horizontal_can_pickup_actor.cc b/frc971/actors/horizontal_can_pickup_actor.cc
index 03fc781..cab89f3 100644
--- a/frc971/actors/horizontal_can_pickup_actor.cc
+++ b/frc971/actors/horizontal_can_pickup_actor.cc
@@ -13,11 +13,14 @@
namespace actors {
namespace {
constexpr double kClawPickupVelocity = 3.00;
-constexpr double kClawPickupAcceleration = 4.0;
+constexpr double kClawPickupAcceleration = 2.0;
constexpr ProfileParams kArmMove{1.0, 1.6};
constexpr ProfileParams kElevatorMove{0.6, 2.2};
+constexpr ProfileParams kFastArmMove{2.0, 3.0};
+constexpr ProfileParams kFastElevatorMove{1.0, 3.0};
+
constexpr double kAngleEpsilon = 0.10;
} // namespace
@@ -57,9 +60,10 @@
bool HorizontalCanPickupActor::RunAction(
const HorizontalCanPickupParams ¶ms) {
// Go around the can.
- DoFridgeProfile(params.elevator_height, 0.0, kElevatorMove, kArmMove, false,
- false, true);
- if (ShouldCancel()) return true;
+ if (!StartFridgeProfile(params.elevator_height, 0.0, kFastElevatorMove,
+ kFastArmMove, false, false, true)) {
+ return true;
+ }
MoveArm(params.pickup_angle, 0.0);
@@ -85,6 +89,17 @@
return true;
}
+ while (true) {
+ ProfileStatus status =
+ IterateProfile(params.elevator_height, 0.0, kFastElevatorMove,
+ kFastArmMove, false, false, true);
+ if (status == DONE) {
+ break;
+ } else if (status == CANCELED) {
+ return true;
+ }
+ }
+
MoveArm(params.claw_full_lift_angle, 0.0);
if (!WaitUntilNear(params.claw_full_lift_angle)) {
diff --git a/frc971/actors/stack_action_params.q b/frc971/actors/stack_action_params.q
index a1fb6f6..3958f7d 100644
--- a/frc971/actors/stack_action_params.q
+++ b/frc971/actors/stack_action_params.q
@@ -2,6 +2,12 @@
// Parameters to send with start.
struct StackParams {
+ // If true, don't grab the lower tote after lowering.
+ bool only_place;
+
+ // The angle to move the arm to while lowering it.
+ double arm_clearance;
+
double claw_out_angle;
// The height just above the box to move before lowering.
double over_box_before_place_height;
diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
index a820f77..3b80184 100644
--- a/frc971/actors/stack_actor.cc
+++ b/frc971/actors/stack_actor.cc
@@ -11,10 +11,9 @@
namespace frc971 {
namespace actors {
namespace {
-constexpr ProfileParams kReallySlowArmMove{0.75, 0.75};
+constexpr ProfileParams kArmWithStackMove{1.75, 0.60};
constexpr ProfileParams kSlowArmMove{1.3, 1.4};
constexpr ProfileParams kSlowElevatorMove{0.5, 3.0};
-constexpr ProfileParams kReallySlowElevatorMove{0.10, 1.0};
constexpr ProfileParams kFastArmMove{0.8, 4.0};
constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
@@ -26,16 +25,33 @@
bool StackActor::RunAction(const StackParams ¶ms) {
const auto &values = constants::GetValues();
+ control_loops::fridge_queue.status.FetchLatest();
+ if (!control_loops::fridge_queue.status.get()) {
+ LOG(ERROR, "Got no fridge status packet.\n");
+ return false;
+ }
+
+ // If we are really high, probably have a can. Move over before down.
+ if (control_loops::fridge_queue.status->goal_height >
+ params.over_box_before_place_height + 0.1) {
+ // Set the current stack down on top of the bottom box.
+ DoFridgeProfile(control_loops::fridge_queue.status->goal_height, 0.0,
+ kSlowElevatorMove, kArmWithStackMove, true);
+ if (ShouldCancel()) return true;
+ }
+
// Set the current stack down on top of the bottom box.
DoFridgeProfile(params.over_box_before_place_height, 0.0, kSlowElevatorMove,
- kReallySlowArmMove, true);
+ kArmWithStackMove, true);
if (ShouldCancel()) return true;
// Set down on the box.
DoFridgeProfile(params.bottom + values.tote_height, 0.0, kSlowElevatorMove,
kSlowArmMove, true);
if (ShouldCancel()) return true;
+
// Clamp.
- {
+ if (!params.only_place) {
+ // Move the claw out of the way only if we are supposed to pick up.
bool send_goal = true;
control_loops::claw_queue.status.FetchLatest();
if (control_loops::claw_queue.status.get()) {
@@ -57,7 +73,20 @@
message.Send();
}
}
- DoFridgeProfile(params.bottom, -0.05, kFastElevatorMove, kFastArmMove, false);
+
+ if (params.only_place) {
+ DoFridgeProfile(params.bottom + values.tote_height, 0.0, kFastElevatorMove,
+ kFastArmMove, false);
+ // Finish early if we aren't supposed to grab.
+ return true;
+ }
+
+ DoFridgeProfile(params.bottom + values.tote_height, params.arm_clearance,
+ kFastElevatorMove, kFastArmMove, false);
+
+ if (ShouldCancel()) return true;
+ DoFridgeProfile(params.bottom, params.arm_clearance, kFastElevatorMove,
+ kFastArmMove, false);
if (ShouldCancel()) return true;
DoFridgeProfile(params.bottom, 0.0, kFastElevatorMove, kFastArmMove, false);
if (ShouldCancel()) return true;
diff --git a/frc971/actors/stack_and_hold_action.q b/frc971/actors/stack_and_hold_action.q
index 6bf8d65..7042096 100644
--- a/frc971/actors/stack_and_hold_action.q
+++ b/frc971/actors/stack_and_hold_action.q
@@ -5,6 +5,9 @@
// Parameters to send with start.
struct StackAndHoldParams {
+ // If true, there is no tote on the tray, and we should place instead.
+ bool place_not_stack;
+
double claw_out_angle;
// The height just above the box to move before lowering.
double over_box_before_place_height;
@@ -12,6 +15,9 @@
// Bottom position.
double bottom;
+ // If we are placing, clamp the stack with the claw.
+ double claw_clamp_angle;
+
// Amount to wait to release.
double clamp_pause_time;
diff --git a/frc971/actors/stack_and_hold_actor.cc b/frc971/actors/stack_and_hold_actor.cc
index ae59831..2acb415 100644
--- a/frc971/actors/stack_and_hold_actor.cc
+++ b/frc971/actors/stack_and_hold_actor.cc
@@ -7,37 +7,82 @@
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/actors/stack_actor.h"
namespace frc971 {
namespace actors {
namespace {
-constexpr ProfileParams kSlowArmMove{0.8, 1.4};
-constexpr ProfileParams kSlowElevatorMove{0.5, 3.0};
+constexpr ProfileParams kReallySlowArmMove{0.1, 1.0};
constexpr ProfileParams kReallySlowElevatorMove{0.10, 1.0};
constexpr ProfileParams kFastArmMove{0.8, 4.0};
-constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
+constexpr ProfileParams kFastElevatorMove{1.2, 4.0};
} // namespace
StackAndHoldActor::StackAndHoldActor(StackAndHoldActionQueueGroup *queues)
: FridgeActorBase<StackAndHoldActionQueueGroup>(queues) {}
bool StackAndHoldActor::RunAction(const StackAndHoldParams ¶ms) {
- const auto &values = constants::GetValues();
+ if (params.place_not_stack) {
+ // Move the arm out of the way.
+ {
+ bool send_goal = true;
+ control_loops::claw_queue.status.FetchLatest();
+ if (control_loops::claw_queue.status.get()) {
+ if (control_loops::claw_queue.status->goal_angle <
+ params.claw_out_angle) {
+ send_goal = false;
+ }
+ }
+ if (send_goal) {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.claw_out_angle;
+ message->angular_velocity = 0.0;
+ message->intake = 0.0;
+ message->rollers_closed = true;
+ message->max_velocity = 6.0;
+ message->max_acceleration = 10.0;
- // Set the current stack down on top of the bottom box.
- DoFridgeProfile(params.over_box_before_place_height, 0.0, kSlowArmMove,
- kReallySlowElevatorMove, true);
- if (ShouldCancel()) return true;
- // Set down on the box.
- DoFridgeProfile(params.bottom + values.tote_height, 0.0, kSlowArmMove,
- kSlowElevatorMove, true);
- if (ShouldCancel()) return true;
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+ }
- // Release
- DoFridgeProfile(params.bottom + values.tote_height, 0.0, kFastArmMove,
- kFastElevatorMove, false);
- if (ShouldCancel()) return true;
+ // Get close, but keep the arm forwards
+ DoFridgeProfile(params.bottom + 0.04, -0.05, kFastArmMove,
+ kFastElevatorMove, true);
+
+ // Lower and pull back.
+ if (ShouldCancel()) return true;
+ DoFridgeProfile(params.bottom, 0.0, kReallySlowArmMove,
+ kReallySlowElevatorMove, true, true, false);
+
+ // Release.
+ if (ShouldCancel()) return true;
+ DoFridgeProfile(params.bottom, 0.0, kReallySlowArmMove,
+ kReallySlowElevatorMove, false);
+ } else {
+ StackParams stack_params;
+ stack_params.only_place = true;
+ stack_params.arm_clearance = params.arm_clearance;
+ stack_params.claw_out_angle = params.claw_out_angle;
+ stack_params.over_box_before_place_height =
+ params.over_box_before_place_height;
+ stack_params.bottom = params.bottom;
+ ::std::unique_ptr<StackAction> stack_action =
+ MakeStackAction(stack_params);
+ stack_action->Start();
+ while (stack_action->Running()) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ 2500);
+
+ if (ShouldCancel()) {
+ stack_action->Cancel();
+ LOG(WARNING, "Cancelling fridge and claw.\n");
+ return true;
+ }
+ }
+ }
if (!WaitOrCancel(aos::time::Time::InSeconds(params.clamp_pause_time))) {
return true;
@@ -46,7 +91,22 @@
// Go up.
DoFridgeProfile(params.hold_height, params.arm_clearance, kFastArmMove,
kFastElevatorMove, false);
+
if (ShouldCancel()) return true;
+
+ if (params.place_not_stack) {
+ // Clamp the stack with the claw.
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.claw_clamp_angle;
+ message->angular_velocity = 0.0;
+ message->intake = 0.0;
+ message->rollers_closed = true;
+ message->max_velocity = 6.0;
+ message->max_acceleration = 6.0;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
// Move back
DoFridgeProfile(params.hold_height, 0.0, kFastArmMove, kFastElevatorMove,
false);
diff --git a/frc971/actors/stack_and_lift_actor.cc b/frc971/actors/stack_and_lift_actor.cc
index 67340e2..ad2263f 100644
--- a/frc971/actors/stack_and_lift_actor.cc
+++ b/frc971/actors/stack_and_lift_actor.cc
@@ -19,8 +19,10 @@
bool StackAndLiftActor::RunAction(const StackAndLiftParams ¶ms) {
{
+ StackParams stack_params = params.stack_params;
+ stack_params.only_place = false;
::std::unique_ptr<StackAction> stack_action =
- MakeStackAction(params.stack_params);
+ MakeStackAction(stack_params);
stack_action->Start();
while (stack_action->Running()) {
::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),