Added horizontal can pickup action.
Change-Id: Ic58099d2399b5e6370e9a6a68d81d22f217ff817
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 2c2f212..5ecdd46 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -284,6 +284,55 @@
],
},
{
+ 'target_name': 'horizontal_can_pickup_action_queue',
+ 'type': 'static_library',
+ 'sources': ['horizontal_can_pickup_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'horizontal_can_pickup_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'horizontal_can_pickup_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_action_lib',
+ 'horizontal_can_pickup_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',
+ 'horizontal_can_pickup_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'horizontal_can_pickup_action',
+ 'type': 'executable',
+ 'sources': [
+ 'horizontal_can_pickup_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'horizontal_can_pickup_action_queue',
+ 'horizontal_can_pickup_action_lib',
+ ],
+ },
+ {
'target_name': 'stack_action_queue',
'type': 'static_library',
'sources': ['stack_action.q'],
diff --git a/frc971/actors/horizontal_can_pickup_action.q b/frc971/actors/horizontal_can_pickup_action.q
new file mode 100644
index 0000000..87fbc6d
--- /dev/null
+++ b/frc971/actors/horizontal_can_pickup_action.q
@@ -0,0 +1,40 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+// Parameters to send with start.
+// This action picks a horizontal can from the claw.
+struct HorizontalCanPickupParams {
+ // Elevator catch height.
+ double elevator_height;
+ // Angle to move the claw to when placing the base of the can on the robot.
+ double pickup_angle;
+
+ // Time and power to pull the can in when lifted.
+ double suck_time;
+ double suck_power;
+
+ // Time to push down and suck in to slide the claw down on the can.
+ double claw_settle_time;
+ double claw_settle_power;
+
+ // Angle to lift the claw to to lift the can.
+ double claw_full_lift_angle;
+
+ // Angle to move the claw back down to.
+ double claw_end_angle;
+};
+
+queue_group HorizontalCanPickupActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ HorizontalCanPickupParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group HorizontalCanPickupActionQueueGroup horizontal_can_pickup_action;
diff --git a/frc971/actors/horizontal_can_pickup_actor.cc b/frc971/actors/horizontal_can_pickup_actor.cc
new file mode 100644
index 0000000..dd8fc79
--- /dev/null
+++ b/frc971/actors/horizontal_can_pickup_actor.cc
@@ -0,0 +1,162 @@
+#include <math.h>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+
+#include "frc971/actors/horizontal_can_pickup_actor.h"
+#include "frc971/actors/fridge_profile_actor.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr double kClawPickupVelocity = 3.00;
+constexpr double kClawPickupAcceleration = 4.0;
+constexpr double kClawMoveVelocity = 3.00;
+constexpr double kClawMoveAcceleration = 8.0;
+
+constexpr double kArmVelocity = 1.00;
+constexpr double kArmAcceleration = 1.6;
+constexpr double kElevatorVelocity = 0.6;
+constexpr double kElevatorAcceleration = 2.2;
+
+constexpr double kAngleEpsilon = 0.10;
+
+} // namespace
+
+HorizontalCanPickupActor::HorizontalCanPickupActor(
+ HorizontalCanPickupActionQueueGroup *queues)
+ : aos::common::actions::ActorBase<HorizontalCanPickupActionQueueGroup>(
+ queues) {}
+
+void HorizontalCanPickupActor::DoProfile(double height, double angle,
+ bool grabbers) {
+ DoProfile(height, angle, grabbers, grabbers, grabbers);
+}
+
+void HorizontalCanPickupActor::DoProfile(double height, double angle,
+ bool top_grabbers, bool front_grabbers,
+ bool back_grabbers) {
+ FridgeProfileParams params;
+
+ params.elevator_height = height;
+ params.elevator_max_velocity = kElevatorVelocity;
+ params.elevator_max_acceleration = kElevatorAcceleration;
+
+ params.arm_angle = angle;
+ params.arm_max_velocity = kArmVelocity;
+ params.arm_max_acceleration = kArmAcceleration;
+
+ params.top_front_grabber = top_grabbers;
+ params.top_back_grabber = top_grabbers;
+ params.bottom_front_grabber = front_grabbers;
+ params.bottom_back_grabber = back_grabbers;
+
+ ::std::unique_ptr<FridgeAction> profile = MakeFridgeProfileAction(params);
+ profile->Start();
+ while (!profile->CheckIteration()) {
+ // wait until next Xms tick
+ ::aos::time::PhasedLoopXMS(5, 2500);
+ if (ShouldCancel()) {
+ profile->Cancel();
+ return;
+ }
+ }
+}
+
+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();
+ if (ShouldCancel()) return false;
+ const double current_angle = control_loops::claw_queue.status->angle;
+ LOG_STRUCT(DEBUG, "Got claw status", *control_loops::claw_queue.status);
+
+ if (::std::abs(current_angle - angle) < kAngleEpsilon) {
+ return true;
+ }
+ }
+}
+
+void HorizontalCanPickupActor::MoveArm(double angle, double intake_power) {
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = angle;
+ message->max_velocity = kClawPickupVelocity;
+ message->max_acceleration = kClawPickupAcceleration;
+ message->angular_velocity = 0.0;
+ message->intake = intake_power;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+}
+
+bool HorizontalCanPickupActor::RunAction(
+ const HorizontalCanPickupParams ¶ms) {
+ // Go around the can.
+ DoProfile(params.elevator_height, 0.0, false, false, true);
+ if (ShouldCancel()) return true;
+
+ MoveArm(params.pickup_angle, 0.0);
+
+ if (!WaitUntilNear(params.pickup_angle)) {
+ return true;
+ }
+
+ MoveArm(params.pickup_angle, params.suck_power);
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.suck_time))) {
+ return true;
+ }
+
+ MoveArm(0.0, 0.0);
+
+ if (!WaitUntilNear(0.0)) {
+ return true;
+ }
+
+ MoveArm(0.0, params.claw_settle_power);
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.claw_settle_time))) {
+ return true;
+ }
+
+ MoveArm(params.claw_full_lift_angle, 0.0);
+
+ if (!WaitUntilNear(params.claw_full_lift_angle)) {
+ return true;
+ }
+
+ DoProfile(params.elevator_height, 0.0, false, true, true);
+
+ MoveArm(params.claw_end_angle, 7.0);
+
+ if (!WaitUntilNear(params.claw_end_angle)) {
+ return true;
+ }
+ MoveArm(params.claw_end_angle, 0.0);
+
+ return true;
+}
+
+::std::unique_ptr<HorizontalCanPickupAction> MakeHorizontalCanPickupAction(
+ const HorizontalCanPickupParams ¶ms) {
+ return ::std::unique_ptr<HorizontalCanPickupAction>(
+ new HorizontalCanPickupAction(
+ &::frc971::actors::horizontal_can_pickup_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/frc971/actors/horizontal_can_pickup_actor.h b/frc971/actors/horizontal_can_pickup_actor.h
new file mode 100644
index 0000000..288127c
--- /dev/null
+++ b/frc971/actors/horizontal_can_pickup_actor.h
@@ -0,0 +1,49 @@
+#ifndef FRC971_ACTORS_HORIZONTAL_CAN_PICKUP_ACTOR_H_
+#define FRC971_ACTORS_HORIZONTAL_CAN_PICKUP_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/actors/horizontal_can_pickup_action.q.h"
+
+namespace frc971 {
+namespace actors {
+
+class HorizontalCanPickupActor : public aos::common::actions::ActorBase<
+ HorizontalCanPickupActionQueueGroup> {
+ public:
+ explicit HorizontalCanPickupActor(
+ HorizontalCanPickupActionQueueGroup *queues);
+
+ bool RunAction(const HorizontalCanPickupParams ¶ms) override;
+
+ private:
+ void DoProfile(double height, double angle, bool grabbers);
+ void DoProfile(double height, double angle, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers);
+
+ // 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);
+
+ void MoveArm(double angle, double intake_power);
+};
+
+typedef aos::common::actions::TypedAction<HorizontalCanPickupActionQueueGroup>
+ HorizontalCanPickupAction;
+
+// Makes a new HorizontalCanPickupActor action.
+::std::unique_ptr<HorizontalCanPickupAction> MakeHorizontalCanPickupAction(
+ const HorizontalCanPickupParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // FRC971_ACTORS_HORIZONTAL_CAN_PICKUP_ACTOR_H_
diff --git a/frc971/actors/horizontal_can_pickup_actor_main.cc b/frc971/actors/horizontal_can_pickup_actor_main.cc
new file mode 100644
index 0000000..c8e1d3c
--- /dev/null
+++ b/frc971/actors/horizontal_can_pickup_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actors/horizontal_can_pickup_action.q.h"
+#include "frc971/actors/horizontal_can_pickup_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::HorizontalCanPickupActor horizontal_can_pickup(
+ &::frc971::actors::horizontal_can_pickup_action);
+ horizontal_can_pickup.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index e8e42b0..8164b7d 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -43,6 +43,7 @@
'<(DEPTH)/frc971/actors/actors.gyp:pickup_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:lift_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:can_pickup_action_lib',
+ '<(DEPTH)/frc971/actors/actors.gyp:horizontal_can_pickup_action_lib',
],
},
],
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 5d3077c..ea07798 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -22,6 +22,7 @@
#include "frc971/actors/stack_actor.h"
#include "frc971/actors/lift_actor.h"
#include "frc971/actors/can_pickup_actor.h"
+#include "frc971/actors/horizontal_can_pickup_actor.h"
using ::frc971::control_loops::claw_queue;
using ::frc971::control_loops::drivetrain_queue;
@@ -48,6 +49,7 @@
// TODO(danielp): Real buttons for all of these.
const ButtonLocation kElevatorUp(3, 10);
+const ButtonLocation kElevatorCanUp(1, 1);
const ButtonLocation kElevatorDown(3, 3);
const ButtonLocation kArmUp(3, 8);
const ButtonLocation kArmHighUp(1, 4);
@@ -57,9 +59,10 @@
const ButtonLocation kClawDown(3, 6);
const ButtonLocation kClawOpen(3, 11);
const ButtonLocation kClawClosed(3, 5);
-const ButtonLocation kFridgeOpen(3, 1);
-const ButtonLocation kFridgeClosed(2, 11);
+const ButtonLocation kFridgeClosed(3, 1);
+const ButtonLocation kFridgeOpen(2, 11);
const ButtonLocation kRollersIn(3, 4);
+const ButtonLocation kRollersOut(1, 9);
const ButtonLocation kClawMiddle(3, 2);
const ButtonLocation kPickup(2, 10);
const ButtonLocation kZero(2, 7);
@@ -170,6 +173,22 @@
params.intake_voltage = 9.0;
action_queue_.EnqueueAction(actors::MakePickupAction(params));
}
+
+ if (data.PosEdge(kElevatorCanUp)) {
+ actors::HorizontalCanPickupParams params;
+ params.elevator_height = 0.3;
+ params.pickup_angle = 0.54;
+ params.suck_time = 0.05;
+ params.suck_power = 8.0;
+
+ params.claw_settle_time = 0.05;
+ params.claw_settle_power = 5.0;
+ params.claw_full_lift_angle = 1.35;
+ params.claw_end_angle = 0.5;
+ action_queue_.EnqueueAction(
+ actors::MakeHorizontalCanPickupAction(params));
+ }
+
if (data.PosEdge(kElevatorDown)) {
claw_goal_ = 0.0;
@@ -194,7 +213,7 @@
}
if (data.PosEdge(kArmHighUp)) {
- claw_goal_ = 1.6;
+ claw_goal_ = 1.3;
}
if (data.PosEdge(kZero)) {
@@ -229,20 +248,20 @@
waiting_for_zero_ = true;
}
- if (waiting_for_zero_) {
- claw_queue.status.FetchLatest();
- fridge_queue.status.FetchLatest();
- if (!claw_queue.status.get()) {
- LOG(ERROR, "Got no claw status packet.\n");
- // Not safe to continue.
- return;
- }
- if (!fridge_queue.status.get()) {
- LOG(ERROR, "Got no fridge status packet.\n");
- return;
- }
+ claw_queue.status.FetchLatest();
+ fridge_queue.status.FetchLatest();
+ if (!claw_queue.status.get()) {
+ LOG(ERROR, "Got no claw status packet.\n");
+ // Not safe to continue.
+ return;
+ }
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Got no fridge status packet.\n");
+ return;
+ }
- if (claw_queue.status->zeroed && fridge_queue.status->zeroed) {
+ if (claw_queue.status->zeroed && fridge_queue.status->zeroed) {
+ if (waiting_for_zero_) {
LOG(INFO, "Zeroed! Starting teleop mode.\n");
waiting_for_zero_ = false;
@@ -250,10 +269,13 @@
elevator_goal_ = fridge_queue.status->goal_height;
arm_goal_ = fridge_queue.status->goal_angle;
claw_goal_ = claw_queue.status->angle;
- } else {
- return;
}
} else {
+ waiting_for_zero_ = true;
+ return;
+ }
+
+ if (!waiting_for_zero_) {
if (!action_queue_.Running()) {
auto new_fridge_goal = fridge_queue.goal.MakeMessage();
new_fridge_goal->height = elevator_goal_;
@@ -274,7 +296,9 @@
if (!claw_queue.goal.MakeWithBuilder()
.angle(claw_goal_)
.rollers_closed(claw_rollers_closed_)
- .intake(data.IsPressed(kRollersIn) ? 12.0 : 0.0)
+ .intake(data.IsPressed(kRollersIn)
+ ? 12.0
+ : (data.IsPressed(kRollersOut) ? -12.0 : 0.0))
.Send()) {
LOG(ERROR, "Sending claw goal failed.\n");
}
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index be8c7d3..1b19d7a 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -28,6 +28,7 @@
'../actors/actors.gyp:pickup_action',
'../actors/actors.gyp:stack_action',
'../actors/actors.gyp:can_pickup_action',
+ '../actors/actors.gyp:horizontal_can_pickup_action',
'../actors/actors.gyp:lift_action',
'../actors/actors.gyp:intake_action',
'../actors/actors.gyp:intake_action_test',