Fridge Presets:
- Added calls to profiles to implement presets.
Final cleanup by Austin and Brian.
Change-Id: Id706acd5dc3d382a68e3c609a760c2be89b60924
diff --git a/aos/common/actions/action_test.cc b/aos/common/actions/action_test.cc
index 0db4b29..423769e 100644
--- a/aos/common/actions/action_test.cc
+++ b/aos/common/actions/action_test.cc
@@ -17,31 +17,51 @@
namespace actions {
namespace testing {
+class TestActorIndex
+ : public aos::common::actions::ActorBase<actions::TestActionQueueGroup> {
+ public:
+ explicit TestActorIndex(actions::TestActionQueueGroup *s)
+ : aos::common::actions::ActorBase<actions::TestActionQueueGroup>(s) {}
+
+ bool RunAction(const uint32_t &new_index) override {
+ index = new_index;
+ return true;
+ }
+
+ uint32_t index = 0;
+};
+
+::std::unique_ptr<
+ aos::common::actions::TypedAction<actions::TestActionQueueGroup>>
+MakeTestActionIndex(uint32_t index) {
+ return ::std::unique_ptr<
+ aos::common::actions::TypedAction<actions::TestActionQueueGroup>>(
+ new aos::common::actions::TypedAction<actions::TestActionQueueGroup>(
+ &actions::test_action, index));
+}
+
class TestActorNOP
: public aos::common::actions::ActorBase<actions::TestActionQueueGroup> {
public:
- explicit TestActorNOP(actions::TestActionQueueGroup* s)
+ explicit TestActorNOP(actions::TestActionQueueGroup *s)
: actions::ActorBase<actions::TestActionQueueGroup>(s) {}
- bool RunAction() { return true; }
+ bool RunAction(const uint32_t &) override { return true; }
};
::std::unique_ptr<
aos::common::actions::TypedAction<actions::TestActionQueueGroup>>
MakeTestActionNOP() {
- return ::std::unique_ptr<
- aos::common::actions::TypedAction<actions::TestActionQueueGroup>>(
- new aos::common::actions::TypedAction<actions::TestActionQueueGroup>(
- &actions::test_action));
+ return MakeTestActionIndex(0);
}
class TestActorShouldCancel
: public aos::common::actions::ActorBase<actions::TestActionQueueGroup> {
public:
- explicit TestActorShouldCancel(actions::TestActionQueueGroup* s)
+ explicit TestActorShouldCancel(actions::TestActionQueueGroup *s)
: aos::common::actions::ActorBase<actions::TestActionQueueGroup>(s) {}
- bool RunAction() {
+ bool RunAction(const uint32_t &) override {
while (!ShouldCancel()) {
LOG(FATAL, "NOT CANCELED!!\n");
}
@@ -52,10 +72,25 @@
::std::unique_ptr<
aos::common::actions::TypedAction<actions::TestActionQueueGroup>>
MakeTestActionShouldCancel() {
+ return MakeTestActionIndex(0);
+}
+
+class TestActor2Nop
+ : public aos::common::actions::ActorBase<actions::TestAction2QueueGroup> {
+ public:
+ explicit TestActor2Nop(actions::TestAction2QueueGroup *s)
+ : actions::ActorBase<actions::TestAction2QueueGroup>(s) {}
+
+ bool RunAction(const actions::MyParams &) { return true; }
+};
+
+::std::unique_ptr<
+ aos::common::actions::TypedAction<actions::TestAction2QueueGroup>>
+MakeTestAction2NOP(const actions::MyParams ¶ms) {
return ::std::unique_ptr<
- aos::common::actions::TypedAction<actions::TestActionQueueGroup>>(
- new aos::common::actions::TypedAction<actions::TestActionQueueGroup>(
- &actions::test_action));
+ aos::common::actions::TypedAction<actions::TestAction2QueueGroup>>(
+ new aos::common::actions::TypedAction<actions::TestAction2QueueGroup>(
+ &actions::test_action2, params));
}
class ActionTest : public ::testing::Test {
@@ -65,11 +100,15 @@
// test.
actions::test_action.goal.Clear();
actions::test_action.status.Clear();
+ actions::test_action2.goal.Clear();
+ actions::test_action2.status.Clear();
}
virtual ~ActionTest() {
actions::test_action.goal.Clear();
actions::test_action.status.Clear();
+ actions::test_action2.goal.Clear();
+ actions::test_action2.status.Clear();
}
// Bring up and down Core.
@@ -88,8 +127,8 @@
// Tests that the queues are properly configured for testing. Tests that queues
// work exactly as used in the tests.
TEST_F(ActionTest, QueueCheck) {
- actions::TestActionQueueGroup* send_side = &actions::test_action;
- actions::TestActionQueueGroup* recv_side = &actions::test_action;
+ actions::TestActionQueueGroup *send_side = &actions::test_action;
+ actions::TestActionQueueGroup *recv_side = &actions::test_action;
send_side->goal.MakeMessage();
send_side->goal.MakeWithBuilder().run(1).Send();
@@ -280,6 +319,76 @@
EXPECT_FALSE(action_queue_.Running());
}
+// Tests that we do get an index with our goal
+TEST_F(ActionTest, ActionIndex) {
+ TestActorIndex idx_act(&actions::test_action);
+
+ // Tick an empty queue and make sure it was not running.
+ action_queue_.Tick();
+ EXPECT_FALSE(action_queue_.Running());
+
+ // Enqueue action to post index.
+ action_queue_.EnqueueAction(MakeTestActionIndex(5));
+ EXPECT_TRUE(actions::test_action.goal.FetchLatest());
+ EXPECT_EQ(5u, actions::test_action.goal->params);
+ EXPECT_EQ(0u, idx_act.index);
+
+ idx_act.WaitForActionRequest();
+ action_queue_.Tick();
+
+ // Check the new action is the right one.
+ uint32_t test_id = 0;
+ EXPECT_TRUE(action_queue_.GetCurrentActionState(nullptr, nullptr, nullptr,
+ nullptr, &test_id, nullptr));
+
+ // Run the next action so it can accomplish signal completion.
+ idx_act.RunIteration();
+ action_queue_.Tick();
+ idx_act.WaitForStop(test_id);
+ EXPECT_EQ(5u, idx_act.index);
+
+ // Enqueue action to post index.
+ action_queue_.EnqueueAction(MakeTestActionIndex(3));
+ EXPECT_TRUE(actions::test_action.goal.FetchLatest());
+ EXPECT_EQ(3u, actions::test_action.goal->params);
+
+ // Run the next action so it can accomplish signal completion.
+ idx_act.RunIteration();
+ action_queue_.Tick();
+ idx_act.WaitForStop(test_id);
+ EXPECT_EQ(3u, idx_act.index);
+}
+
+// Tests that an action with a structure params works.
+TEST_F(ActionTest, StructParamType) {
+ TestActor2Nop nop_act(&actions::test_action2);
+
+ // Tick an empty queue and make sure it was not running.
+ action_queue_.Tick();
+ EXPECT_FALSE(action_queue_.Running());
+
+ actions::MyParams p;
+ p.param1 = 5.0;
+ p.param2 = 7;
+
+ action_queue_.EnqueueAction(MakeTestAction2NOP(p));
+ nop_act.WaitForActionRequest();
+
+ // We started an action and it should be running.
+ EXPECT_TRUE(action_queue_.Running());
+
+ // Tick it and make sure it is still running.
+ action_queue_.Tick();
+ EXPECT_TRUE(action_queue_.Running());
+
+ // Run the action so it can signal completion.
+ nop_act.RunIteration();
+ action_queue_.Tick();
+
+ // Make sure it stopped.
+ EXPECT_FALSE(action_queue_.Running());
+}
+
} // namespace testing.
} // namespace actions.
} // namespace common.
diff --git a/aos/common/actions/actions.h b/aos/common/actions/actions.h
index 08d94f7..15054d2 100644
--- a/aos/common/actions/actions.h
+++ b/aos/common/actions/actions.h
@@ -44,13 +44,13 @@
bool Running();
// Retrieves the internal state of the current action for testing.
- // See comments on the private members of TypedAction<T> for details.
+ // See comments on the private members of TypedAction<T, S> for details.
bool GetCurrentActionState(bool* has_started, bool* sent_started,
bool* sent_cancel, bool* interrupted,
uint32_t* run_value, uint32_t* old_run_value);
// Retrieves the internal state of the next action for testing.
- // See comments on the private members of TypedAction<T> for details.
+ // See comments on the private members of TypedAction<T, S> for details.
bool GetNextActionState(bool* has_started, bool* sent_started,
bool* sent_cancel, bool* interrupted,
uint32_t* run_value, uint32_t* old_run_value);
@@ -76,7 +76,7 @@
void WaitUntilDone() { DoWaitUntilDone(); }
// Retrieves the internal state of the action for testing.
- // See comments on the private members of TypedAction<T> for details.
+ // See comments on the private members of TypedAction<T, S> for details.
void GetState(bool* has_started, bool* sent_started, bool* sent_cancel,
bool* interrupted, uint32_t* run_value,
uint32_t* old_run_value) {
@@ -95,7 +95,7 @@
// Blocks until complete.
virtual void DoWaitUntilDone() = 0;
// For testing we will need to get the internal state.
- // See comments on the private members of TypedAction<T> for details.
+ // See comments on the private members of TypedAction<T, S> for details.
virtual void DoGetState(bool* has_started, bool* sent_started,
bool* sent_cancel, bool* interrupted,
uint32_t* run_value, uint32_t* old_run_value) = 0;
@@ -107,16 +107,19 @@
public:
// A convenient way to refer to the type of our goals.
typedef typename std::remove_reference<decltype(
- *(static_cast<T*>(NULL)->goal.MakeMessage().get()))>::type GoalType;
+ *(static_cast<T*>(nullptr)->goal.MakeMessage().get()))>::type GoalType;
+ typedef typename std::remove_reference<
+ decltype(static_cast<GoalType*>(nullptr)->params)>::type ParamType;
- TypedAction(T* queue_group)
+ TypedAction(T* queue_group, const ParamType ¶ms)
: queue_group_(queue_group),
goal_(queue_group_->goal.MakeMessage()),
// This adds 1 to the counter (atomically because it's potentially
// shared across threads) and then bitwise-ORs the bottom of the PID to
// differentiate it from other processes's values (ie a unique id).
run_value_(run_counter_.fetch_add(1, ::std::memory_order_relaxed) |
- ((getpid() & 0xFFFF) << 16)) {
+ ((getpid() & 0xFFFF) << 16)),
+ params_(params) {
LOG(INFO, "Action %" PRIx32 " created on queue %s\n", run_value_,
queue_group_->goal.name());
// Clear out any old status messages from before now.
@@ -173,6 +176,9 @@
// The value we're going to use for goal.run etc.
const uint32_t run_value_;
+ // flag passed to action in order to have differing types
+ const ParamType params_;
+
// The old value for running that we may have seen. If we see any value other
// than this or run_value_, somebody else got in the way and we're done. 0 if
// there was nothing there to start with. Only valid after sent_started_
@@ -282,6 +288,7 @@
if (goal_) {
LOG(INFO, "Starting action %" PRIx32 "\n", run_value_);
goal_->run = run_value_;
+ goal_->params = params_;
sent_started_ = true;
if (!goal_.Send()) {
LOG(ERROR, "sending goal for action %" PRIx32 " failed\n", run_value_);
diff --git a/aos/common/actions/actions.q b/aos/common/actions/actions.q
index 8950dd1..8b75ee7 100644
--- a/aos/common/actions/actions.q
+++ b/aos/common/actions/actions.q
@@ -24,6 +24,9 @@
// The unique value to put into status.running while running this instance or
// 0 to cancel.
uint32_t run;
+ // Default parameter. The more useful thing to do would be to define your own
+ // goal type to change param to a useful structure.
+ double param;
};
interface ActionQueueGroup {
diff --git a/aos/common/actions/actor.h b/aos/common/actions/actor.h
index b64051a..2193ea1 100644
--- a/aos/common/actions/actor.h
+++ b/aos/common/actions/actor.h
@@ -17,12 +17,17 @@
template <class T>
class ActorBase {
public:
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<T*>(nullptr)->goal.MakeMessage().get()))>::type GoalType;
+ typedef typename std::remove_reference<
+ decltype(static_cast<GoalType*>(nullptr)->params)>::type ParamType;
+
ActorBase(T* acq) : action_q_(acq) {}
// Will return true if finished or asked to cancel.
// Will return false if it failed accomplish its goal
// due to a problem with the system.
- virtual bool RunAction() = 0;
+ virtual bool RunAction(const ParamType& params) = 0;
// Runs action while enabled.
void Run();
@@ -110,7 +115,7 @@
.Send()) {
LOG(ERROR, "Failed to send the status.\n");
}
- abort_ = !RunAction();
+ abort_ = !RunAction(action_q_->goal->params);
LOG(INFO, "Done with action %" PRIx32 "\n", running_id);
// If we have a new one to run, we shouldn't say we're stopped in between.
diff --git a/aos/common/actions/test_action.q b/aos/common/actions/test_action.q
index c46e182..3451c04 100644
--- a/aos/common/actions/test_action.q
+++ b/aos/common/actions/test_action.q
@@ -7,7 +7,24 @@
message Goal {
uint32_t run;
- double test_value;
+ uint32_t params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+struct MyParams {
+ double param1;
+ int32_t param2;
+};
+
+queue_group TestAction2QueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ MyParams params;
};
queue Goal goal;
@@ -15,3 +32,4 @@
};
queue_group TestActionQueueGroup test_action;
+queue_group TestAction2QueueGroup test_action2;
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 6593d80..239ba73 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -51,6 +51,7 @@
'drivetrain_action_lib',
],
},
+
{
'target_name': 'fridge_profile_action_queue',
'type': 'static_library',
diff --git a/frc971/actors/drivetrain_action.q b/frc971/actors/drivetrain_action.q
index 826900a..6da2280 100644
--- a/frc971/actors/drivetrain_action.q
+++ b/frc971/actors/drivetrain_action.q
@@ -2,17 +2,22 @@
import "aos/common/actions/actions.q";
+// Parameters to send with start.
+struct DrivetrainActionParams {
+ double left_initial_position;
+ double right_initial_position;
+ double y_offset;
+ double theta_offset;
+ double maximum_velocity;
+ double maximum_acceleration;
+};
+
queue_group DrivetrainActionQueueGroup {
implements aos.common.actions.ActionQueueGroup;
message Goal {
uint32_t run;
- double left_initial_position;
- double right_initial_position;
- double y_offset;
- double theta_offset;
- double maximum_velocity;
- double maximum_acceleration;
+ DrivetrainActionParams params;
};
queue Goal goal;
diff --git a/frc971/actors/drivetrain_actor.cc b/frc971/actors/drivetrain_actor.cc
index 165e6cb..78724f6 100644
--- a/frc971/actors/drivetrain_actor.cc
+++ b/frc971/actors/drivetrain_actor.cc
@@ -21,12 +21,12 @@
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
-bool DrivetrainActor::RunAction() {
+bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
static const auto K = constants::GetValues().make_drivetrain_loop().K();
- const double yoffset = action_q_->goal->y_offset;
+ const double yoffset = params.y_offset;
const double turn_offset =
- action_q_->goal->theta_offset * constants::GetValues().turn_width / 2.0;
+ params.theta_offset * constants::GetValues().turn_width / 2.0;
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
@@ -37,8 +37,8 @@
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
- profile.set_maximum_acceleration(action_q_->goal->maximum_acceleration);
- profile.set_maximum_velocity(action_q_->goal->maximum_velocity);
+ profile.set_maximum_acceleration(params.maximum_acceleration);
+ profile.set_maximum_velocity(params.maximum_velocity);
turn_profile.set_maximum_acceleration(
10.0 * constants::GetValues().turn_width / 2.0);
turn_profile.set_maximum_velocity(3.0 * constants::GetValues().turn_width /
@@ -112,13 +112,13 @@
if (ShouldCancel()) return true;
LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_goal_state(0, 0) + action_q_->goal->left_initial_position,
- right_goal_state(0, 0) + action_q_->goal->right_initial_position);
+ left_goal_state(0, 0) + params.left_initial_position,
+ right_goal_state(0, 0) + params.right_initial_position);
control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
//.highgear(false)
- .left_goal(left_goal_state(0, 0) + action_q_->goal->left_initial_position)
- .right_goal(right_goal_state(0, 0) + action_q_->goal->right_initial_position)
+ .left_goal(left_goal_state(0, 0) + params.left_initial_position)
+ .right_goal(right_goal_state(0, 0) + params.right_initial_position)
.left_velocity_goal(left_goal_state(1, 0))
.right_velocity_goal(right_goal_state(1, 0))
.Send();
@@ -137,10 +137,10 @@
const double left_error = ::std::abs(
control_loops::drivetrain_queue.status->filtered_left_position -
- (left_goal_state(0, 0) + action_q_->goal->left_initial_position));
+ (left_goal_state(0, 0) + params.left_initial_position));
const double right_error = ::std::abs(
control_loops::drivetrain_queue.status->filtered_right_position -
- (right_goal_state(0, 0) + action_q_->goal->right_initial_position));
+ (right_goal_state(0, 0) + params.right_initial_position));
const double velocity_error =
::std::abs(control_loops::drivetrain_queue.status->robot_speed);
if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
@@ -156,14 +156,10 @@
return true;
}
-::std::unique_ptr<aos::common::actions::TypedAction<
- ::frc971::actors::DrivetrainActionQueueGroup>>
-MakeDrivetrainAction() {
- return ::std::unique_ptr<aos::common::actions::TypedAction<
- ::frc971::actors::DrivetrainActionQueueGroup>>(
- new aos::common::actions::TypedAction<
- ::frc971::actors::DrivetrainActionQueueGroup>(
- &::frc971::actors::drivetrain_action));
+::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
+ const ::frc971::actors::DrivetrainActionParams& params) {
+ return ::std::unique_ptr<DrivetrainAction>(
+ new DrivetrainAction(&::frc971::actors::drivetrain_action, params));
}
} // namespace actors
diff --git a/frc971/actors/drivetrain_actor.h b/frc971/actors/drivetrain_actor.h
index 2ded4ec..c035002 100644
--- a/frc971/actors/drivetrain_actor.h
+++ b/frc971/actors/drivetrain_actor.h
@@ -15,12 +15,15 @@
public:
explicit DrivetrainActor(DrivetrainActionQueueGroup* s);
- bool RunAction() override;
+ bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
};
+typedef aos::common::actions::TypedAction<DrivetrainActionQueueGroup>
+ DrivetrainAction;
+
// Makes a new DrivetrainActor action.
-::std::unique_ptr<aos::common::actions::TypedAction<DrivetrainActionQueueGroup>>
- MakeDrivetrainAction();
+::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
+ const ::frc971::actors::DrivetrainActionParams& params);
} // namespace actors
} // namespace frc971
diff --git a/frc971/actors/fridge_profile_action.q b/frc971/actors/fridge_profile_action.q
index 7e93a5e..231a499 100644
--- a/frc971/actors/fridge_profile_action.q
+++ b/frc971/actors/fridge_profile_action.q
@@ -2,21 +2,26 @@
import "aos/common/actions/actions.q";
+// Parameters to send with start.
+struct FridgeProfileParams {
+ double arm_angle;
+ double arm_max_velocity;
+ double arm_max_acceleration;
+ double elevator_height;
+ double elevator_max_velocity;
+ double elevator_max_acceleration;
+ bool top_front_grabber;
+ bool top_back_grabber;
+ bool bottom_front_grabber;
+ bool bottom_back_grabber;
+};
+
queue_group FridgeProfileActionQueueGroup {
implements aos.common.actions.ActionQueueGroup;
message Goal {
uint32_t run;
- double arm_angle;
- double arm_max_velocity;
- double arm_max_acceleration;
- double elevator_height;
- double elevator_max_velocity;
- double elevator_max_acceleration;
- bool top_front_grabber;
- bool top_back_grabber;
- bool bottom_front_grabber;
- bool bottom_back_grabber;
+ FridgeProfileParams params;
};
queue Goal goal;
diff --git a/frc971/actors/fridge_profile_actor.cc b/frc971/actors/fridge_profile_actor.cc
index 794fb7f..c84e765 100644
--- a/frc971/actors/fridge_profile_actor.cc
+++ b/frc971/actors/fridge_profile_actor.cc
@@ -18,53 +18,76 @@
namespace actors {
FridgeProfileActor::FridgeProfileActor(actors::FridgeProfileActionQueueGroup* s)
- : aos::common::actions::ActorBase<actors::FridgeProfileActionQueueGroup>(s) {}
+ : aos::common::actions::ActorBase<actors::FridgeProfileActionQueueGroup>(
+ s) {}
bool FridgeProfileActor::InitializeProfile(double angle_max_vel,
double angle_max_accel,
double height_max_vel,
double height_max_accel) {
- if (arm_profile_ != nullptr || elevator_profile_ != nullptr) {
- return false;
+ // if they have no vel/accel they will not move
+ if (angle_max_vel != 0.0 && angle_max_accel != 0.0) {
+ // Initialize arm profile.
+ arm_profile_.reset(
+ new ::aos::util::TrapezoidProfile(::aos::time::Time::InMS(5)));
+ arm_profile_->set_maximum_velocity(angle_max_vel);
+ arm_profile_->set_maximum_acceleration(angle_max_accel);
+ } else {
+ arm_profile_.reset();
}
- // Initialize arm profile.
- arm_profile_.reset(
- new ::aos::util::TrapezoidProfile(::aos::time::Time::InMS(5)));
- arm_profile_->set_maximum_velocity(angle_max_vel);
- arm_profile_->set_maximum_acceleration(angle_max_accel);
- // Initialize elevator profile.
- elevator_profile_.reset(
- new ::aos::util::TrapezoidProfile(::aos::time::Time::InMS(5)));
- elevator_profile_->set_maximum_velocity(height_max_vel);
- elevator_profile_->set_maximum_acceleration(height_max_accel);
+ // if they have no vel/accel they will not move
+ if (height_max_vel != 0.0 && height_max_accel != 0.0) {
+ // Initialize elevator profile.
+ elevator_profile_.reset(
+ new ::aos::util::TrapezoidProfile(::aos::time::Time::InMS(5)));
+ elevator_profile_->set_maximum_velocity(height_max_vel);
+ elevator_profile_->set_maximum_acceleration(height_max_accel);
+ } else {
+ elevator_profile_.reset();
+ }
+
return true;
}
bool FridgeProfileActor::IterateProfile(double goal_angle, double goal_height,
- double* next_angle,
- double* next_height,
+ double* next_angle, double* next_height,
double* next_angle_velocity,
double* next_height_velocity) {
+ CHECK_NOTNULL(next_angle);
+ CHECK_NOTNULL(next_height);
+ CHECK_NOTNULL(next_angle_velocity);
+ CHECK_NOTNULL(next_height_velocity);
::Eigen::Matrix<double, 2, 1> goal_state;
- goal_state = arm_profile_->Update(goal_angle, 0.0);
- *next_angle = goal_state(0, 0);
- *next_angle_velocity = goal_state(1, 0);
- goal_state = elevator_profile_->Update(goal_height, 0.0);
- *next_height = goal_state(0, 0);
- *next_height_velocity = goal_state(1, 0);
+ if (!arm_profile_) {
+ *next_angle = 0.0;
+ *next_angle_velocity = 0.0;
+ } else {
+ goal_state = arm_profile_->Update(goal_angle, 0.0);
+ *next_angle = goal_state(0, 0);
+ *next_angle_velocity = goal_state(1, 0);
+ }
+
+ if (!elevator_profile_) {
+ *next_height = 0.0;
+ *next_height_velocity = 0.0;
+ } else {
+ goal_state = elevator_profile_->Update(goal_height, 0.0);
+ *next_height = goal_state(0, 0);
+ *next_height_velocity = goal_state(1, 0);
+ }
return true;
}
-bool FridgeProfileActor::RunAction() {
- double goal_angle = action_q_->goal->arm_angle;
- double goal_height = action_q_->goal->elevator_height;
- bool top_front = action_q_->goal->top_front_grabber;
- bool top_back = action_q_->goal->top_back_grabber;
- bool bottom_front = action_q_->goal->bottom_front_grabber;
- bool bottom_back = action_q_->goal->bottom_back_grabber;
+bool FridgeProfileActor::RunAction(const FridgeProfileParams ¶ms) {
+ double goal_angle = params.arm_angle;
+ double goal_height = params.elevator_height;
+ bool top_front = params.top_front_grabber;
+ bool top_back = params.top_back_grabber;
+ bool bottom_front = params.bottom_front_grabber;
+ bool bottom_back = params.bottom_back_grabber;
LOG(INFO,
"Fridge profile goal: arm (%f) elev (%f) with grabbers(%d,%d,%d,%d).\n",
goal_angle, goal_height, top_front, top_back, bottom_front, bottom_back);
@@ -73,10 +96,9 @@
const double angle_epsilon = 0.01, height_epsilon = 0.01;
// Initialize arm profile.
- if(!InitializeProfile(action_q_->goal->arm_max_velocity,
- action_q_->goal->arm_max_acceleration,
- action_q_->goal->elevator_max_velocity,
- action_q_->goal->elevator_max_acceleration)) {
+ if (!InitializeProfile(params.arm_max_velocity, params.arm_max_acceleration,
+ params.elevator_max_velocity,
+ params.elevator_max_acceleration)) {
return false;
}
@@ -100,7 +122,7 @@
double delta_angle, delta_height;
double angle_vel, height_vel;
if (!IterateProfile(goal_angle, goal_height, &delta_angle, &delta_height,
- &angle_vel, &height_vel)) {
+ &angle_vel, &height_vel)) {
return false;
}
@@ -124,19 +146,23 @@
if (!control_loops::fridge_queue.status.get()) {
return false;
}
- const double current_height = control_loops::fridge_queue.status->height;
- const double current_angle = control_loops::fridge_queue.status->angle;
- LOG_STRUCT(DEBUG, "Got fridge status",
- *control_loops::fridge_queue.status);
+ double current_angle = control_loops::fridge_queue.status->angle;
+ double current_height = control_loops::fridge_queue.status->height;
+ LOG_STRUCT(DEBUG, "Got fridge status", *control_loops::fridge_queue.status);
- if (::std::abs(arm_start_angle_ + delta_angle - goal_angle) <
- angle_epsilon &&
- ::std::abs(arm_start_angle_ + delta_angle - current_angle) <
- angle_epsilon &&
- ::std::abs(elev_start_height_ + delta_height - goal_height) <
- height_epsilon &&
- ::std::abs(elev_start_height_ + delta_height - current_height) <
- height_epsilon) {
+ if (testing_) {
+ current_angle = goal_angle;
+ current_height = goal_height;
+ }
+
+ if ((!arm_profile_ || (::std::abs(arm_start_angle_ + delta_angle -
+ goal_angle) < angle_epsilon &&
+ ::std::abs(arm_start_angle_ + delta_angle -
+ current_angle) < angle_epsilon)) &&
+ (!elevator_profile_ || (::std::abs(elev_start_height_ + delta_height -
+ goal_height) < height_epsilon &&
+ ::std::abs(elev_start_height_ + delta_height -
+ current_height) < height_epsilon))) {
break;
}
}
@@ -150,14 +176,10 @@
return true;
}
-::std::unique_ptr<aos::common::actions::TypedAction<
- ::frc971::actors::FridgeProfileActionQueueGroup>>
-MakeFridgeProfileAction() {
- return ::std::unique_ptr<aos::common::actions::TypedAction<
- ::frc971::actors::FridgeProfileActionQueueGroup>>(
- new aos::common::actions::TypedAction<
- ::frc971::actors::FridgeProfileActionQueueGroup>(
- &::frc971::actors::fridge_profile_action));
+::std::unique_ptr<FridgeAction> MakeFridgeProfileAction(
+ const FridgeProfileParams& p) {
+ return ::std::unique_ptr<FridgeAction>(
+ new FridgeAction(&::frc971::actors::fridge_profile_action, p));
}
} // namespace actors
diff --git a/frc971/actors/fridge_profile_actor.h b/frc971/actors/fridge_profile_actor.h
index 7da3cbd..3b3d818 100644
--- a/frc971/actors/fridge_profile_actor.h
+++ b/frc971/actors/fridge_profile_actor.h
@@ -14,7 +14,7 @@
class FridgeProfileActor
: public aos::common::actions::ActorBase<FridgeProfileActionQueueGroup> {
public:
- explicit FridgeProfileActor(FridgeProfileActionQueueGroup* s);
+ explicit FridgeProfileActor(FridgeProfileActionQueueGroup *s);
// sets up profiles. Returns false if things have already been setup
bool InitializeProfile(double angle_max_vel, double angle_max_accel,
@@ -22,23 +22,29 @@
// Takes a goal and computes the next step toward that goal. Returns false if
// things are broken.
- bool IterateProfile(double goal_angle, double goal_height, double* next_angle,
- double* next_height, double* next_angle_velocity,
- double* next_angle_accel);
+ bool IterateProfile(double goal_angle, double goal_height, double *next_angle,
+ double *next_height, double *next_angle_velocity,
+ double *next_angle_accel);
- bool RunAction() override;
+ bool RunAction(const FridgeProfileParams ¶ms) override;
+
+ // only for unit test
+ void SetTesting() { testing_ = true; }
private:
- ::std::unique_ptr<::aos::util::TrapezoidProfile> arm_profile_;
- ::std::unique_ptr<::aos::util::TrapezoidProfile> elevator_profile_;
+ ::std::unique_ptr<aos::util::TrapezoidProfile> arm_profile_;
+ ::std::unique_ptr<aos::util::TrapezoidProfile> elevator_profile_;
double arm_start_angle_ = 0.0;
double elev_start_height_ = 0.0;
+ bool testing_ = false;
};
-// Makes a new FridgeProfileActor action.
-::std::unique_ptr<aos::common::actions::TypedAction<FridgeProfileActionQueueGroup>>
- MakeFridgeProfileAction();
+typedef aos::common::actions::TypedAction<FridgeProfileActionQueueGroup>
+ FridgeAction;
+// Makes a new FridgeProfileActor action.
+::std::unique_ptr<FridgeAction> MakeFridgeProfileAction(
+ const FridgeProfileParams &fridge_params);
} // namespace actors
} // namespace frc971
diff --git a/frc971/actors/fridge_profile_actor_test.cc b/frc971/actors/fridge_profile_actor_test.cc
index cbebec2..d2d7dd2 100644
--- a/frc971/actors/fridge_profile_actor_test.cc
+++ b/frc971/actors/fridge_profile_actor_test.cc
@@ -53,11 +53,12 @@
double last_angle[2] = {0.0, 0.0};
double last_height[2] = {0.0, 0.0};
double angle_vel = 0, angle_accel = 0, height_vel = 0, height_accel = 0;
- double next_angle = 0, next_height = 0, next_angle_vel = 0.0, next_height_vel = 0.0;
+ double next_angle = 0, next_height = 0, next_angle_vel = 0.0,
+ next_height_vel = 0.0;
// Angle (0.250000, 0.250000, 0.25) Height (0.250000, 0.250000, 0.25)
- EXPECT_TRUE(
- fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height, &next_angle_vel, &next_height_vel));
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_EQ(0.25, next_angle);
@@ -70,8 +71,8 @@
EXPECT_EQ(0.25, height_accel);
// Angle (1.000000, 0.750000, 0.50) Height (1.000000, 0.750000, 0.50)
- EXPECT_TRUE(
- fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height, &next_angle_vel, &next_height_vel));
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_EQ(1.0, next_angle);
@@ -84,8 +85,8 @@
EXPECT_EQ(0.50, height_accel);
// Angle (2.000000, 1.000000, 0.25) Height (2.000000, 1.000000, 0.25)
- EXPECT_TRUE(
- fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height, &next_angle_vel, &next_height_vel));
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_EQ(2.0, next_angle);
@@ -98,8 +99,8 @@
EXPECT_EQ(0.25, height_accel);
// Angle (3.000000, 1.000000, 0.00) Height (3.000000, 1.000000, 0.00)
- EXPECT_TRUE(
- fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height, &next_angle_vel, &next_height_vel));
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_EQ(3.0, next_angle);
@@ -112,8 +113,8 @@
EXPECT_EQ(0.0, height_accel);
// Angle (4.000000, 1.000000, 0.00) Height (4.000000, 1.000000, 0.00)
- EXPECT_TRUE(
- fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height, &next_angle_vel, &next_height_vel));
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_EQ(4.0, next_angle);
@@ -126,8 +127,8 @@
EXPECT_EQ(0.0, height_accel);
// Angle (4.750000, 0.750000, -0.25) Height (4.750000, 0.750000, -0.25)
- EXPECT_TRUE(
- fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height, &next_angle_vel, &next_height_vel));
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_EQ(4.75, next_angle);
@@ -140,8 +141,8 @@
EXPECT_EQ(-0.25, height_accel);
// Angle (5.000000, 0.250000, -0.50) Height (5.000000, 0.250000, -0.50)
- EXPECT_TRUE(
- fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height, &next_angle_vel, &next_height_vel));
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_EQ(5.0, next_angle);
@@ -154,8 +155,8 @@
EXPECT_EQ(-0.50, height_accel);
// Angle (5.000000, 0.000000, -0.25) Height (5.000000, 0.000000, -0.25)
- EXPECT_TRUE(
- fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height, &next_angle_vel, &next_height_vel));
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_EQ(5.0, next_angle);
@@ -190,13 +191,14 @@
double last_angle[2] = {0.0, 0.0};
double last_height[2] = {0.0, 0.0};
double angle_vel = 0, angle_accel = 0, height_vel = 0, height_accel = 0;
- double next_angle = 0, next_height = 0, next_angle_vel = 0.0, next_height_vel = 0.0;
+ double next_angle = 0, next_height = 0, next_angle_vel = 0.0,
+ next_height_vel = 0.0;
- for (int i=0; i < 7; i++) {
+ for (int i = 0; i < 7; i++) {
EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle,
&next_height, &next_angle_vel,
&next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
+ GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_GE(1.0, angle_vel);
EXPECT_GE(0.5, angle_accel);
@@ -206,11 +208,11 @@
EXPECT_EQ(5.0, next_angle);
- for (int i=0; i < 7; i++) {
+ for (int i = 0; i < 7; i++) {
EXPECT_TRUE(fridge_profile.IterateProfile(10.0, 10.0, &next_angle,
&next_height, &next_angle_vel,
&next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
+ GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
GetVelAccel(next_height, last_height, &height_vel, &height_accel);
EXPECT_GE(1.0, angle_vel);
EXPECT_GE(0.5, angle_accel);
@@ -225,18 +227,21 @@
TEST_F(FridgeProfileTest, ProfileQueueValid) {
FridgeProfileActor fridge_profile(&frc971::actors::fridge_profile_action);
+ FridgeProfileParams params;
+ params.arm_angle = 5.0;
+ params.arm_max_velocity = 200.0;
+ params.arm_max_acceleration = 20000.0;
+ params.elevator_height = 5.0;
+ params.elevator_max_velocity = 200.0;
+ params.elevator_max_acceleration = 20000.0;
+ params.top_front_grabber = true;
+ params.top_back_grabber = false;
+ params.bottom_front_grabber = true;
+ params.bottom_back_grabber = false;
+
frc971::actors::fridge_profile_action.goal.MakeWithBuilder()
.run(true)
- .arm_angle(5.0)
- .arm_max_velocity(200.0)
- .arm_max_acceleration(20000.0)
- .elevator_height(5.0)
- .elevator_max_velocity(200.0)
- .elevator_max_acceleration(20000.0)
- .top_front_grabber(true)
- .top_back_grabber(false)
- .bottom_front_grabber(true)
- .bottom_back_grabber(false)
+ .params(params)
.Send();
// tell it the fridge is zeroed
@@ -246,9 +251,11 @@
.height(0.0)
.Send();
+ fridge_profile.SetTesting();
+
// do the action and it will post to the goal queue
fridge_profile.WaitForActionRequest();
- fridge_profile.RunAction();
+ fridge_profile.RunAction(params);
// a= 0.250000, e= 0.250000, av= 100.000000, ev= 100.000000
EXPECT_TRUE(control_loops::fridge_queue.goal.FetchNext());
@@ -310,6 +317,45 @@
EXPECT_FALSE(control_loops::fridge_queue.goal.FetchNext());
}
+// Make sure that giving 0 velocity+acceleration makes it not move at all.
+TEST_F(FridgeProfileTest, ProfileNoVel) {
+ FridgeProfileActor fridge_profile(&frc971::actors::fridge_profile_action);
+
+ FridgeProfileParams params;
+ params.arm_angle = 5.0;
+ params.arm_max_velocity = 200.0;
+ params.arm_max_acceleration = 20000.0;
+ params.elevator_height = 5.0;
+ params.elevator_max_velocity = 0.0;
+ params.elevator_max_acceleration = 0.0;
+ params.top_front_grabber = true;
+ params.top_back_grabber = false;
+ params.bottom_front_grabber = true;
+ params.bottom_back_grabber = false;
+
+ frc971::actors::fridge_profile_action.goal.MakeWithBuilder()
+ .run(true)
+ .params(params)
+ .Send();
+
+ // tell it the fridge is zeroed
+ control_loops::fridge_queue.status.MakeWithBuilder()
+ .zeroed(true)
+ .angle(0.0)
+ .height(0.0)
+ .Send();
+
+ fridge_profile.SetTesting();
+
+ // do the action and it will post to the goal queue
+ fridge_profile.WaitForActionRequest();
+ fridge_profile.RunAction(params);
+
+ while (control_loops::fridge_queue.goal.FetchNext()) {
+ EXPECT_EQ(0.0, control_loops::fridge_queue.goal->height);
+ }
+}
+
} // namespace testing.
} // namespace actors.
} // namespace frc971.
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index d2353fe..0dd7ef4 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -60,7 +60,7 @@
void DriveSpin(double radians) {
LOG(INFO, "going to spin %f\n", radians);
- //TODO(sensors): update this time thing maybe?
+ // TODO(sensors): update this time thing maybe?
::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
::Eigen::Matrix<double, 2, 1> driveTrainState;
const double goal_velocity = 0.0;
@@ -107,19 +107,20 @@
}
}
-::std::unique_ptr<aos::common::actions::TypedAction<
- ::frc971::actors::DrivetrainActionQueueGroup>>
-SetDriveGoal(double distance, bool slow_acceleration,
- double maximum_velocity = 1.7, double theta = 0) {
+::std::unique_ptr<::frc971::actors::DrivetrainAction> SetDriveGoal(
+ double distance, bool slow_acceleration, double maximum_velocity = 1.7,
+ double theta = 0) {
LOG(INFO, "Driving to %f\n", distance);
- auto drivetrain_action = actors::MakeDrivetrainAction();
- drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
- drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
- drivetrain_action->GetGoal()->y_offset = distance;
- drivetrain_action->GetGoal()->theta_offset = theta;
- drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
- drivetrain_action->GetGoal()->maximum_acceleration =
- slow_acceleration ? 2.5 : 3.0;
+
+ ::frc971::actors::DrivetrainActionParams params;
+ params.left_initial_position = left_initial_position;
+ params.right_initial_position = right_initial_position;
+ params.y_offset = distance;
+ params.theta_offset = theta;
+ params.maximum_velocity = maximum_velocity;
+ params.maximum_acceleration = slow_acceleration ? 2.5 : 3.0;
+ auto drivetrain_action = actors::MakeDrivetrainAction(params);
+
drivetrain_action->Start();
left_initial_position +=
distance - theta * constants::GetValues().turn_width / 2.0;
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index 8f7d8a5..a566e06 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -29,6 +29,7 @@
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/util/util.gyp:log_interval',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
'<(DEPTH)/frc971/queues/queues.gyp:gyro',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
@@ -36,7 +37,8 @@
'<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
- '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/frc971/actors/actors.gyp:fridge_profile_action_queue',
+ '<(DEPTH)/frc971/actors/actors.gyp:fridge_profile_action_lib',
],
},
],
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 09d815c..ba25e4b 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -17,6 +17,8 @@
#include "frc971/constants.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
+#include "frc971/actors/fridge_profile_action.q.h"
+#include "frc971/actors/fridge_profile_actor.h"
using ::frc971::control_loops::claw_queue;
using ::frc971::control_loops::drivetrain_queue;
@@ -31,6 +33,12 @@
namespace input {
namespace joysticks {
+// preset motion limits
+static const double kArmDebugVelocity = 0.17;
+static const double kArmDebugAcceleration = 0.8;
+static const double kElevatorDebugVelocity = 0.2;
+static const double kElevatorDebugAcceleration = 2.2;
+
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
@@ -43,6 +51,12 @@
const ButtonLocation kClawUp(3, 7);
const ButtonLocation kClawDown(3, 6);
+// TODO(ben): Real buttons for all of these.
+const ButtonLocation kArmPresetOne(99, 99);
+const ButtonLocation kArmPresetTwo(99, 99);
+const ButtonLocation kElevatorPresetOne(99, 99);
+const ButtonLocation kElevatorPresetTwo(99, 99);
+
// Testing mode.
const double kElevatorVelocity = 0.5;
const double kArmVelocity = 0.5;
@@ -160,16 +174,60 @@
claw_goal_ -= kClawVelocity * kJoystickDt;
}
- if (!fridge_queue.goal.MakeWithBuilder()
- .height(elevator_goal_)
- .angle(arm_goal_)
- .Send()) {
- LOG(ERROR, "Sending fridge goal failed.\n");
+ if (!action_queue_.Running()) {
+ if (!fridge_queue.goal.MakeWithBuilder()
+ .height(elevator_goal_)
+ .angle(arm_goal_)
+ .Send()) {
+ LOG(ERROR, "Sending fridge goal failed.\n");
+ }
+ if (!claw_queue.goal.MakeWithBuilder().angle(claw_goal_).Send()) {
+ LOG(ERROR, "Sending claw goal failed.\n");
+ }
}
- if (!claw_queue.goal.MakeWithBuilder()
- .angle(claw_goal_)
- .Send()) {
- LOG(ERROR, "Sending claw goal failed.\n");
+ if (data.IsPressed(kArmPresetOne) || data.IsPressed(kArmPresetTwo)) {
+ actors::FridgeProfileParams fridge_params;
+ fridge_params.arm_max_velocity = kArmDebugVelocity;
+ fridge_params.arm_max_acceleration = kArmDebugAcceleration;
+ if (data.IsPressed(kArmPresetOne)) {
+ LOG(INFO, "Preset asked for test arm position one position.\n");
+ fridge_params.arm_angle = M_PI / 4.0;
+ fridge_params.top_front_grabber = false;
+ fridge_params.top_back_grabber = false;
+ fridge_params.bottom_front_grabber = false;
+ fridge_params.bottom_back_grabber = false;
+ action_queue_.EnqueueAction(MakeFridgeProfileAction(fridge_params));
+ } else if (data.IsPressed(kArmPresetTwo)) {
+ LOG(INFO, "Preset asked for test arm position two position.\n");
+ fridge_params.arm_angle = -M_PI / 4.0;
+ fridge_params.top_front_grabber = true;
+ fridge_params.top_back_grabber = true;
+ fridge_params.bottom_front_grabber = true;
+ fridge_params.bottom_back_grabber = true;
+ action_queue_.EnqueueAction(MakeFridgeProfileAction(fridge_params));
+ }
+ } else if (data.IsPressed(kElevatorPresetOne) ||
+ data.IsPressed(kElevatorPresetOne)) {
+ actors::FridgeProfileParams fridge_params;
+ fridge_params.elevator_max_velocity = kElevatorDebugVelocity;
+ fridge_params.elevator_max_acceleration = kElevatorDebugAcceleration;
+ if (data.IsPressed(kElevatorPresetOne)) {
+ LOG(INFO, "Preset asked for test elevator position one position.\n");
+ fridge_params.elevator_height = 0.5;
+ fridge_params.top_front_grabber = false;
+ fridge_params.top_back_grabber = false;
+ fridge_params.bottom_front_grabber = false;
+ fridge_params.bottom_back_grabber = false;
+ action_queue_.EnqueueAction(MakeFridgeProfileAction(fridge_params));
+ } else if (data.IsPressed(kElevatorPresetTwo)) {
+ LOG(INFO, "Preset asked for test elevator position two position.\n");
+ fridge_params.elevator_height = 1.2;
+ fridge_params.top_front_grabber = true;
+ fridge_params.top_back_grabber = true;
+ fridge_params.bottom_front_grabber = true;
+ fridge_params.bottom_back_grabber = true;
+ action_queue_.EnqueueAction(MakeFridgeProfileAction(fridge_params));
+ }
}
}
}