Added a bunch of new actions.
Change-Id: I890b4cef168baa12e02e181de683243ffa02e7c0
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index bff37e8..546a779 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -62,9 +62,11 @@
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/actions/actions.gyp:action_lib',
'<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
],
'export_dependent_settings' : [
'<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
'<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
],
},
@@ -282,9 +284,175 @@
],
},
{
+ 'target_name': 'held_to_lift_action_queue',
+ 'type': 'static_library',
+ 'sources': ['held_to_lift_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'lift_action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'lift_action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'held_to_lift_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'held_to_lift_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'held_to_lift_action_queue',
+ 'lift_action_lib',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'held_to_lift_action_queue',
+ 'fridge_profile_lib',
+ ],
+ },
+ {
+ 'target_name': 'held_to_lift_action',
+ 'type': 'executable',
+ 'sources': [
+ 'held_to_lift_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'held_to_lift_action_queue',
+ 'held_to_lift_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'stack_and_hold_action_queue',
+ 'type': 'static_library',
+ 'sources': ['stack_and_hold_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_queue',
+ 'lift_action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_queue',
+ 'lift_action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'stack_and_hold_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'stack_and_hold_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'stack_and_hold_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_and_hold_action_queue',
+ 'fridge_profile_lib',
+ ],
+ },
+ {
+ 'target_name': 'stack_and_hold_action',
+ 'type': 'executable',
+ 'sources': [
+ 'stack_and_hold_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_and_hold_action_queue',
+ 'stack_and_hold_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'stack_and_lift_action_queue',
+ 'type': 'static_library',
+ 'sources': ['stack_and_lift_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_queue',
+ 'lift_action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_queue',
+ 'lift_action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'stack_and_lift_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'stack_and_lift_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'stack_and_lift_action_queue',
+ 'stack_action_lib',
+ 'lift_action_lib',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_and_lift_action_queue',
+ 'fridge_profile_lib',
+ ],
+ },
+ {
+ 'target_name': 'stack_and_lift_action',
+ 'type': 'executable',
+ 'sources': [
+ 'stack_and_lift_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_and_lift_action_queue',
+ 'stack_and_lift_action_lib',
+ ],
+ },
+ {
'target_name': 'stack_action_queue',
'type': 'static_library',
- 'sources': ['stack_action.q'],
+ 'sources': [
+ 'stack_action.q',
+ 'stack_action_params.q',
+ ],
'variables': {
'header_path': 'frc971/actors',
},
@@ -353,7 +521,10 @@
{
'target_name': 'lift_action_queue',
'type': 'static_library',
- 'sources': ['lift_action.q'],
+ 'sources': [
+ 'lift_action.q',
+ 'lift_action_params.q',
+ ],
'variables': {
'header_path': 'frc971/actors',
},
diff --git a/frc971/actors/fridge_profile_lib.h b/frc971/actors/fridge_profile_lib.h
index 7c26f9d..8f82ce2 100644
--- a/frc971/actors/fridge_profile_lib.h
+++ b/frc971/actors/fridge_profile_lib.h
@@ -3,7 +3,9 @@
#include <cmath>
+#include "aos/common/controls/control_loop.h"
#include "aos/common/actions/actor.h"
+#include "aos/common/util/phased_loop.h"
#include "frc971/control_loops/fridge/fridge.q.h"
namespace frc971 {
@@ -104,6 +106,16 @@
}
}
}
+
+ bool WaitOrCancel(::aos::time::Time duration) {
+ ::aos::time::Time end_time = ::aos::time::Time::Now() + duration;
+ while (::aos::time::Time::Now() <= end_time) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ 2500);
+ if (this->ShouldCancel()) return false;
+ }
+ return true;
+ }
};
} // namespace actors
diff --git a/frc971/actors/held_to_lift_action.q b/frc971/actors/held_to_lift_action.q
new file mode 100644
index 0000000..78d2e95
--- /dev/null
+++ b/frc971/actors/held_to_lift_action.q
@@ -0,0 +1,34 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+import "frc971/actors/lift_action_params.q";
+
+// Parameters to send with start.
+struct HeldToLiftParams {
+ // The maximum claw value to avoid collisions.
+ double claw_out_angle;
+
+ // The value to move the arm forwards to clear the stack when lowering.
+ double arm_clearance;
+ // End height.
+ double bottom_height;
+ // Amount to wait to clamp.
+ double clamp_pause_time;
+
+ // Lift parameters
+ LiftParams lift_params;
+};
+
+queue_group HeldToLiftActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ HeldToLiftParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group HeldToLiftActionQueueGroup held_to_lift_action;
diff --git a/frc971/actors/held_to_lift_actor.cc b/frc971/actors/held_to_lift_actor.cc
new file mode 100644
index 0000000..fa23631
--- /dev/null
+++ b/frc971/actors/held_to_lift_actor.cc
@@ -0,0 +1,96 @@
+#include "frc971/actors/held_to_lift_actor.h"
+
+#include <math.h>
+
+#include "aos/common/time.h"
+#include "frc971/constants.h"
+#include "frc971/actors/fridge_profile_lib.h"
+#include "frc971/actors/lift_actor.h"
+#include "frc971/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr ProfileParams kArmMove{0.6, 2.0};
+constexpr ProfileParams kFastArmMove{1.2, 4.0};
+constexpr ProfileParams kElevatorMove{0.9, 3.0};
+constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
+} // namespace
+
+HeldToLiftActor::HeldToLiftActor(HeldToLiftActionQueueGroup *queues)
+ : FridgeActorBase<HeldToLiftActionQueueGroup>(queues) {}
+
+bool HeldToLiftActor::RunAction(const HeldToLiftParams ¶ms) {
+ control_loops::fridge_queue.status.FetchLatest();
+ if (!control_loops::fridge_queue.status.get()) {
+ return false;
+ }
+
+ // Move claw out of the way.
+ {
+ bool send_goal = true;
+ double claw_goal = params.claw_out_angle;
+ control_loops::claw_queue.status.FetchLatest();
+ if (control_loops::claw_queue.status.get()) {
+ if (control_loops::claw_queue.status->goal_angle < claw_goal) {
+ 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->max_velocity = 6.0;
+ message->max_acceleration = 10.0;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ 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;
+
+ // 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;
+ }
+
+ {
+ ::std::unique_ptr<LiftAction> lift_action =
+ MakeLiftAction(params.lift_params);
+ lift_action->Start();
+ while (lift_action->Running()) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ 2500);
+
+ if (ShouldCancel()) {
+ lift_action->Cancel();
+ LOG(WARNING, "Cancelling fridge and claw.\n");
+ return true;
+ }
+ }
+ }
+
+ return true;
+}
+
+::std::unique_ptr<HeldToLiftAction> MakeHeldToLiftAction(
+ const HeldToLiftParams ¶ms) {
+ return ::std::unique_ptr<HeldToLiftAction>(
+ new HeldToLiftAction(&::frc971::actors::held_to_lift_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/frc971/actors/held_to_lift_actor.h b/frc971/actors/held_to_lift_actor.h
new file mode 100644
index 0000000..4b3c22a
--- /dev/null
+++ b/frc971/actors/held_to_lift_actor.h
@@ -0,0 +1,33 @@
+#ifndef FRC971_ACTORS_HELD_TO_LIFT_ACTOR_H_
+#define FRC971_ACTORS_HELD_TO_LIFT_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/actors/held_to_lift_action.q.h"
+#include "frc971/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class HeldToLiftActor : public FridgeActorBase<HeldToLiftActionQueueGroup> {
+ public:
+ explicit HeldToLiftActor(HeldToLiftActionQueueGroup *queues);
+
+ bool RunAction(const HeldToLiftParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<HeldToLiftActionQueueGroup>
+ HeldToLiftAction;
+
+// Makes a new HeldToLiftActor action.
+::std::unique_ptr<HeldToLiftAction> MakeHeldToLiftAction(
+ const HeldToLiftParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // FRC971_ACTORS_HELD_TO_LIFT_ACTOR_H_
diff --git a/frc971/actors/held_to_lift_actor_main.cc b/frc971/actors/held_to_lift_actor_main.cc
new file mode 100644
index 0000000..ad3e20c
--- /dev/null
+++ b/frc971/actors/held_to_lift_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actors/held_to_lift_action.q.h"
+#include "frc971/actors/held_to_lift_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::HeldToLiftActor lift(
+ &::frc971::actors::held_to_lift_action);
+ lift.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/actors/horizontal_can_pickup_action.q b/frc971/actors/horizontal_can_pickup_action.q
index 87fbc6d..f5ebcfd 100644
--- a/frc971/actors/horizontal_can_pickup_action.q
+++ b/frc971/actors/horizontal_can_pickup_action.q
@@ -23,6 +23,10 @@
// Angle to move the claw back down to.
double claw_end_angle;
+
+ // The end arm and elevator position once we are done lifting.
+ double elevator_end_height;
+ double arm_end_angle;
};
queue_group HorizontalCanPickupActionQueueGroup {
diff --git a/frc971/actors/horizontal_can_pickup_actor.cc b/frc971/actors/horizontal_can_pickup_actor.cc
index 1e87315..03fc781 100644
--- a/frc971/actors/horizontal_can_pickup_actor.cc
+++ b/frc971/actors/horizontal_can_pickup_actor.cc
@@ -26,15 +26,6 @@
HorizontalCanPickupActionQueueGroup *queues)
: FridgeActorBase<HorizontalCanPickupActionQueueGroup>(queues) {}
-bool HorizontalCanPickupActor::WaitOrCancel(::aos::time::Time duration) {
- ::aos::time::Time end_time = ::aos::time::Time::Now() + duration;
- while (::aos::time::Time::Now() <= end_time) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
- if (ShouldCancel()) return false;
- }
- return true;
-}
-
bool HorizontalCanPickupActor::WaitUntilNear(double angle) {
while (true) {
control_loops::claw_queue.status.FetchAnother();
@@ -111,6 +102,11 @@
}
MoveArm(params.claw_end_angle, 0.0);
+ if (ShouldCancel()) return true;
+
+ DoFridgeProfile(params.elevator_end_height, params.arm_end_angle,
+ kElevatorMove, kArmMove, false, true, true);
+
return true;
}
diff --git a/frc971/actors/horizontal_can_pickup_actor.h b/frc971/actors/horizontal_can_pickup_actor.h
index 88f2987..c69b5a1 100644
--- a/frc971/actors/horizontal_can_pickup_actor.h
+++ b/frc971/actors/horizontal_can_pickup_actor.h
@@ -22,10 +22,6 @@
bool RunAction(const HorizontalCanPickupParams ¶ms) override;
private:
- // Waits until the duration has elapsed, or we are asked to cancel.
- // Returns false if we should cancel.
- bool WaitOrCancel(::aos::time::Time duration);
-
// Waits until we are near the angle.
// Returns false if we should cancel.
bool WaitUntilNear(double angle);
diff --git a/frc971/actors/lift_action.q b/frc971/actors/lift_action.q
index 10e169c..bb5615a 100644
--- a/frc971/actors/lift_action.q
+++ b/frc971/actors/lift_action.q
@@ -1,14 +1,7 @@
package frc971.actors;
import "aos/common/actions/actions.q";
-
-// Parameters to send with start.
-struct LiftParams {
- // Lift height
- double lift_height;
- // Arm goal.
- double lift_arm;
-};
+import "frc971/actors/lift_action_params.q";
queue_group LiftActionQueueGroup {
implements aos.common.actions.ActionQueueGroup;
diff --git a/frc971/actors/lift_action_params.q b/frc971/actors/lift_action_params.q
new file mode 100644
index 0000000..a5a1403
--- /dev/null
+++ b/frc971/actors/lift_action_params.q
@@ -0,0 +1,9 @@
+package frc971.actors;
+
+// Parameters to send with start.
+struct LiftParams {
+ // Lift height
+ double lift_height;
+ // Arm goal.
+ double lift_arm;
+};
diff --git a/frc971/actors/lift_actor.cc b/frc971/actors/lift_actor.cc
index 25213ca..9809cc6 100644
--- a/frc971/actors/lift_actor.cc
+++ b/frc971/actors/lift_actor.cc
@@ -16,11 +16,23 @@
: FridgeActorBase<LiftActionQueueGroup>(queues) {}
bool LiftActor::RunAction(const LiftParams ¶ms) {
+ control_loops::fridge_queue.status.FetchLatest();
+ if (!control_loops::fridge_queue.status.get()) {
+ return false;
+ }
+
// Lift the box straight up.
- DoFridgeProfile(params.lift_height, 0.0, kElevatorMove, kArmMove, true);
+ DoFridgeProfile(params.lift_height, 0.0, kElevatorMove, kArmMove,
+ control_loops::fridge_queue.status->grabbers.top_front,
+ control_loops::fridge_queue.status->grabbers.bottom_front,
+ control_loops::fridge_queue.status->grabbers.bottom_back);
+ if (ShouldCancel()) return true;
+
// Move it back to the storage location.
DoFridgeProfile(params.lift_height, params.lift_arm, kElevatorMove, kArmMove,
- true);
+ control_loops::fridge_queue.status->grabbers.top_front,
+ control_loops::fridge_queue.status->grabbers.bottom_front,
+ control_loops::fridge_queue.status->grabbers.bottom_back);
return true;
}
diff --git a/frc971/actors/stack_action.q b/frc971/actors/stack_action.q
index caa8abf..122afc9 100644
--- a/frc971/actors/stack_action.q
+++ b/frc971/actors/stack_action.q
@@ -1,12 +1,7 @@
package frc971.actors;
import "aos/common/actions/actions.q";
-
-// Parameters to send with start.
-struct StackParams {
- // Angle to move the claw to when picking up.
- double claw_out_angle;
-};
+import "frc971/actors/stack_action_params.q";
queue_group StackActionQueueGroup {
implements aos.common.actions.ActionQueueGroup;
diff --git a/frc971/actors/stack_action_params.q b/frc971/actors/stack_action_params.q
new file mode 100644
index 0000000..a1fb6f6
--- /dev/null
+++ b/frc971/actors/stack_action_params.q
@@ -0,0 +1,11 @@
+package frc971.actors;
+
+// Parameters to send with start.
+struct StackParams {
+ double claw_out_angle;
+ // The height just above the box to move before lowering.
+ double over_box_before_place_height;
+
+ // Bottom position.
+ double bottom;
+};
diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
index 7267a4b..6f350d3 100644
--- a/frc971/actors/stack_actor.cc
+++ b/frc971/actors/stack_actor.cc
@@ -24,29 +24,41 @@
bool StackActor::RunAction(const StackParams ¶ms) {
const auto &values = constants::GetValues();
- const double bottom = 0.020;
// Set the current stack down on top of the bottom box.
- DoFridgeProfile(0.39, 0.0, kSlowArmMove, kReallySlowElevatorMove, true);
+ DoFridgeProfile(params.over_box_before_place_height, 0.0, kSlowArmMove,
+ kReallySlowElevatorMove, true);
if (ShouldCancel()) return true;
// Set down on the box.
- DoFridgeProfile(bottom + values.tote_height, 0.0, kSlowArmMove,
+ DoFridgeProfile(params.bottom + values.tote_height, 0.0, kSlowArmMove,
kSlowElevatorMove, true);
if (ShouldCancel()) return true;
// Clamp.
{
- 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;
+ 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;
- LOG_STRUCT(DEBUG, "Sending claw goal", *message);
- message.Send();
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
}
- DoFridgeProfile(bottom, -0.05, kFastArmMove, kFastElevatorMove, false);
+ DoFridgeProfile(params.bottom, -0.05, kFastArmMove, kFastElevatorMove, false);
if (ShouldCancel()) return true;
- DoFridgeProfile(bottom, 0.0, kFastArmMove, kFastElevatorMove, false);
+ DoFridgeProfile(params.bottom, 0.0, kFastArmMove, kFastElevatorMove, false);
if (ShouldCancel()) return true;
aos::time::SleepFor(aos::time::Time::InMS(100));
diff --git a/frc971/actors/stack_actor_test.cc b/frc971/actors/stack_actor_test.cc
index e8bc786..b14ac1e 100644
--- a/frc971/actors/stack_actor_test.cc
+++ b/frc971/actors/stack_actor_test.cc
@@ -61,7 +61,8 @@
frc971::actors::stack_action.goal.MakeWithBuilder().run(false).Send();
// let the action start running, if we return from this call it has worked.
- stack.RunAction(0);
+ StackParams s;
+ stack.RunAction(s);
SUCCEED();
}
diff --git a/frc971/actors/stack_and_hold_action.q b/frc971/actors/stack_and_hold_action.q
new file mode 100644
index 0000000..6bf8d65
--- /dev/null
+++ b/frc971/actors/stack_and_hold_action.q
@@ -0,0 +1,36 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+import "frc971/actors/stack_action_params.q";
+
+// Parameters to send with start.
+struct StackAndHoldParams {
+ double claw_out_angle;
+ // The height just above the box to move before lowering.
+ double over_box_before_place_height;
+
+ // Bottom position.
+ double bottom;
+
+ // Amount to wait to release.
+ double clamp_pause_time;
+
+ // The value to move the arm forwards to clear the stack when lifting.
+ double arm_clearance;
+ // Hold height
+ double hold_height;
+};
+
+queue_group StackAndHoldActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ StackAndHoldParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group StackAndHoldActionQueueGroup stack_and_hold_action;
diff --git a/frc971/actors/stack_and_hold_actor.cc b/frc971/actors/stack_and_hold_actor.cc
new file mode 100644
index 0000000..ae59831
--- /dev/null
+++ b/frc971/actors/stack_and_hold_actor.cc
@@ -0,0 +1,69 @@
+#include "frc971/actors/stack_and_hold_actor.h"
+
+#include <math.h>
+
+#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr ProfileParams kSlowArmMove{0.8, 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};
+} // namespace
+
+StackAndHoldActor::StackAndHoldActor(StackAndHoldActionQueueGroup *queues)
+ : FridgeActorBase<StackAndHoldActionQueueGroup>(queues) {}
+
+bool StackAndHoldActor::RunAction(const StackAndHoldParams ¶ms) {
+ const auto &values = constants::GetValues();
+
+ // 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;
+
+ // Release
+ DoFridgeProfile(params.bottom + values.tote_height, 0.0, kFastArmMove,
+ kFastElevatorMove, false);
+ if (ShouldCancel()) return true;
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.clamp_pause_time))) {
+ return true;
+ }
+
+ // Go up.
+ DoFridgeProfile(params.hold_height, params.arm_clearance, kFastArmMove,
+ kFastElevatorMove, false);
+ if (ShouldCancel()) return true;
+ // Move back
+ DoFridgeProfile(params.hold_height, 0.0, kFastArmMove, kFastElevatorMove,
+ false);
+ if (ShouldCancel()) return true;
+ // Grab
+ DoFridgeProfile(params.hold_height, 0.0, kFastArmMove, kFastElevatorMove,
+ true);
+ if (ShouldCancel()) return true;
+
+ return true;
+}
+
+::std::unique_ptr<StackAndHoldAction> MakeStackAndHoldAction(
+ const StackAndHoldParams ¶ms) {
+ return ::std::unique_ptr<StackAndHoldAction>(
+ new StackAndHoldAction(&::frc971::actors::stack_and_hold_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/frc971/actors/stack_and_hold_actor.h b/frc971/actors/stack_and_hold_actor.h
new file mode 100644
index 0000000..bfc877b
--- /dev/null
+++ b/frc971/actors/stack_and_hold_actor.h
@@ -0,0 +1,33 @@
+#ifndef FRC971_ACTORS_STACK_AND_HOLD_ACTOR_H_
+#define FRC971_ACTORS_STACK_AND_HOLD_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/actors/stack_and_hold_action.q.h"
+#include "frc971/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class StackAndHoldActor : public FridgeActorBase<StackAndHoldActionQueueGroup> {
+ public:
+ explicit StackAndHoldActor(StackAndHoldActionQueueGroup *queues);
+
+ bool RunAction(const StackAndHoldParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<StackAndHoldActionQueueGroup>
+ StackAndHoldAction;
+
+// Makes a new stackActor action.
+::std::unique_ptr<StackAndHoldAction> MakeStackAndHoldAction(
+ const StackAndHoldParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // FRC971_ACTORS_STACK_AND_HOLD_ACTOR_H_
diff --git a/frc971/actors/stack_and_hold_actor_main.cc b/frc971/actors/stack_and_hold_actor_main.cc
new file mode 100644
index 0000000..2bd63bb
--- /dev/null
+++ b/frc971/actors/stack_and_hold_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actors/stack_and_hold_action.q.h"
+#include "frc971/actors/stack_and_hold_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::StackAndHoldActor stack_and_hold(
+ &::frc971::actors::stack_and_hold_action);
+ stack_and_hold.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/actors/stack_and_lift_action.q b/frc971/actors/stack_and_lift_action.q
new file mode 100644
index 0000000..f61ac32
--- /dev/null
+++ b/frc971/actors/stack_and_lift_action.q
@@ -0,0 +1,34 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+import "frc971/actors/stack_action_params.q";
+import "frc971/actors/lift_action_params.q";
+
+// Parameters to send with start.
+struct StackAndLiftParams {
+ // Stack parameters
+ StackParams stack_params;
+ // True if the fridge should be clamped while lifting.
+ bool grab_after_stack;
+ // The time after clamping to pause.
+ double clamp_pause_time;
+ // Lift parameters
+ LiftParams lift_params;
+ // True if the fridge should be clamped when done.
+ bool grab_after_lift;
+};
+
+queue_group StackAndLiftActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ StackAndLiftParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group StackAndLiftActionQueueGroup stack_and_lift_action;
diff --git a/frc971/actors/stack_and_lift_actor.cc b/frc971/actors/stack_and_lift_actor.cc
new file mode 100644
index 0000000..67340e2
--- /dev/null
+++ b/frc971/actors/stack_and_lift_actor.cc
@@ -0,0 +1,100 @@
+#include "frc971/actors/stack_and_lift_actor.h"
+
+#include <math.h>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/actors/stack_actor.h"
+#include "frc971/actors/lift_actor.h"
+
+namespace frc971 {
+namespace actors {
+
+StackAndLiftActor::StackAndLiftActor(StackAndLiftActionQueueGroup *queues)
+ : FridgeActorBase<StackAndLiftActionQueueGroup>(queues) {}
+
+bool StackAndLiftActor::RunAction(const StackAndLiftParams ¶ms) {
+ {
+ ::std::unique_ptr<StackAction> stack_action =
+ MakeStackAction(params.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;
+ }
+ }
+ }
+
+ {
+ control_loops::fridge_queue.goal.FetchLatest();
+ if (!control_loops::fridge_queue.goal.get()) {
+ return false;
+ }
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ *new_fridge_goal = *control_loops::fridge_queue.goal;
+ new_fridge_goal->grabbers.top_front = params.grab_after_stack;
+ new_fridge_goal->grabbers.top_back = params.grab_after_stack;
+ new_fridge_goal->grabbers.bottom_front = params.grab_after_stack;
+ new_fridge_goal->grabbers.bottom_back = params.grab_after_stack;
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge close.\n");
+ return false;
+ }
+ }
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.clamp_pause_time))) {
+ return true;
+ }
+
+ {
+ ::std::unique_ptr<LiftAction> lift_action =
+ MakeLiftAction(params.lift_params);
+ lift_action->Start();
+ while (lift_action->Running()) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ 2500);
+
+ if (ShouldCancel()) {
+ lift_action->Cancel();
+ LOG(WARNING, "Cancelling fridge and claw.\n");
+ return true;
+ }
+ }
+ }
+
+ {
+ control_loops::fridge_queue.goal.FetchLatest();
+ if (!control_loops::fridge_queue.goal.get()) {
+ return false;
+ }
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ *new_fridge_goal = *control_loops::fridge_queue.goal;
+ new_fridge_goal->grabbers.top_front = params.grab_after_lift;
+ new_fridge_goal->grabbers.top_back = params.grab_after_lift;
+ new_fridge_goal->grabbers.bottom_front = params.grab_after_lift;
+ new_fridge_goal->grabbers.bottom_back = params.grab_after_lift;
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge close.\n");
+ return false;
+ }
+ }
+ return true;
+}
+
+::std::unique_ptr<StackAndLiftAction> MakeStackAndLiftAction(
+ const StackAndLiftParams ¶ms) {
+ return ::std::unique_ptr<StackAndLiftAction>(
+ new StackAndLiftAction(&::frc971::actors::stack_and_lift_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/frc971/actors/stack_and_lift_actor.h b/frc971/actors/stack_and_lift_actor.h
new file mode 100644
index 0000000..7dcaba5
--- /dev/null
+++ b/frc971/actors/stack_and_lift_actor.h
@@ -0,0 +1,33 @@
+#ifndef FRC971_ACTORS_STACK_AND_LIFT_ACTOR_H_
+#define FRC971_ACTORS_STACK_AND_LIFT_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/actors/stack_and_lift_action.q.h"
+#include "frc971/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class StackAndLiftActor : public FridgeActorBase<StackAndLiftActionQueueGroup> {
+ public:
+ explicit StackAndLiftActor(StackAndLiftActionQueueGroup *queues);
+
+ bool RunAction(const StackAndLiftParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<StackAndLiftActionQueueGroup>
+ StackAndLiftAction;
+
+// Makes a new stackActor action.
+::std::unique_ptr<StackAndLiftAction> MakeStackAndLiftAction(
+ const StackAndLiftParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // FRC971_ACTORS_STACK_AND_LIFT_ACTOR_H_
diff --git a/frc971/actors/stack_and_lift_actor_main.cc b/frc971/actors/stack_and_lift_actor_main.cc
new file mode 100644
index 0000000..882d9d7
--- /dev/null
+++ b/frc971/actors/stack_and_lift_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actors/stack_and_lift_action.q.h"
+#include "frc971/actors/stack_and_lift_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::StackAndLiftActor stack_and_lift(
+ &::frc971::actors::stack_and_lift_action);
+ stack_and_lift.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 58066c6..0b4fd6b 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -130,7 +130,7 @@
// 0 is the portion of the elevator carriage that Spencer removed
// lining up with the bolt.
// Positive is up.
- {-0.005, 0.679000, 0.010000, 0.650000},
+ {-0.005, 0.689000, 0.010000, 0.680000},
// Arm values, in radians.
// 0 is sticking straight out horizontally over the intake/front.
@@ -198,7 +198,7 @@
// 0 is the portion of the elevator carriage that Spencer removed
// lining up with the bolt.
// Positive is up.
- {-0.00500, 0.679000, 0.010000, 0.650000},
+ {-0.00500, 0.689000, 0.010000, 0.680000},
// Arm values, in radians.
// 0 is sticking straight out horizontally over the intake/front.
@@ -265,7 +265,7 @@
{// Elevator values, in meters.
// 0 is at the top of the elevator frame.
// Positive is down towards the drivebase.
- {-0.00500, 0.679000, 0.010000, 0.650000},
+ {-0.00500, 0.689000, 0.010000, 0.680000},
// Arm values, in radians.
// 0 is sticking straight out horizontally over the intake/front.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 64ba98e..ff378c8 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -258,12 +258,17 @@
if (output) {
output->voltage = claw_loop_->U(0, 0);
- if (unsafe_goal) {
- output->intake_voltage = unsafe_goal->intake;
- output->rollers_closed = unsafe_goal->rollers_closed;
- } else {
+ if (state_ != RUNNING) {
output->intake_voltage = 0.0;
output->rollers_closed = false;
+ } else {
+ if (unsafe_goal) {
+ output->intake_voltage = unsafe_goal->intake;
+ output->rollers_closed = unsafe_goal->rollers_closed;
+ } else {
+ output->intake_voltage = 0.0;
+ output->rollers_closed = false;
+ }
}
if (output->rollers_closed != last_rollers_closed_) {
last_piston_edge_ = Time::Now();
diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
index d1f5158..2337921 100644
--- a/frc971/control_loops/fridge/fridge.q
+++ b/frc971/control_loops/fridge/fridge.q
@@ -70,10 +70,10 @@
// state of the grabber pistons
GrabberPistons grabbers;
- // Goal angle of the arm.
+ // Goal angle and velocity of the arm.
double goal_angle;
double goal_angular_velocity;
- // Goal height of the elevator.
+ // Goal height and velocity of the elevator.
double goal_height;
double goal_velocity;
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index bf95418..4e4b257 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -38,8 +38,11 @@
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
'<(DEPTH)/frc971/actors/actors.gyp:stack_action_lib',
+ '<(DEPTH)/frc971/actors/actors.gyp:stack_and_lift_action_lib',
+ '<(DEPTH)/frc971/actors/actors.gyp:stack_and_hold_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:pickup_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:lift_action_lib',
+ '<(DEPTH)/frc971/actors/actors.gyp:held_to_lift_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:can_pickup_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:horizontal_can_pickup_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:fridge_profile_lib',
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index a0cf5c4..56aa509 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -29,6 +29,9 @@
'../actors/actors.gyp:score_action_test',
'../actors/actors.gyp:pickup_action',
'../actors/actors.gyp:stack_action',
+ '../actors/actors.gyp:stack_and_lift_action',
+ '../actors/actors.gyp:stack_and_hold_action',
+ '../actors/actors.gyp:held_to_lift_action',
'../actors/actors.gyp:can_pickup_action',
'../actors/actors.gyp:horizontal_can_pickup_action',
'../actors/actors.gyp:lift_action',