implement coop placement
This means splitting out the parameters to score_action.
Change-Id: I201a3c038426433d1815401e01f0806f8e55ba20
diff --git a/frc971/actors/score_action.q b/frc971/actors/score_action.q
index f8401d2..79b8be3 100644
--- a/frc971/actors/score_action.q
+++ b/frc971/actors/score_action.q
@@ -4,10 +4,15 @@
// Parameters to send with start.
struct ScoreParams {
- // Height for the upper fridge pivot to be when we're scoring.
- double height;
// If false, then extend. Otherwise, place the stack and retract.
bool place_the_stack;
+
+ // TODO(Brian): Comments (by somebody who knows what these all mean).
+ double upper_move_height;
+ double begin_horizontal_move_height;
+ double horizontal_move_target;
+ double place_height;
+ double home_return_height;
};
queue_group ScoreActionQueueGroup {
diff --git a/frc971/actors/score_actor.cc b/frc971/actors/score_actor.cc
index 37b3463..d866f6a 100644
--- a/frc971/actors/score_actor.cc
+++ b/frc971/actors/score_actor.cc
@@ -12,20 +12,13 @@
namespace frc971 {
namespace actors {
-
namespace {
-const double kUpperMoveHeight = 0.14;
-const double kBeginHorizontalMoveHeight = 0.13;
-
-const double kHorizontalMoveTarget = -0.7;
-
-const double kPlaceHeight = -0.10;
-const double kHomeReturnHeight = 0.1;
const double kMaxXVelocity = 0.45;
const double kMaxYVelocity = 0.20;
const double kMaxXAcceleration = 0.5;
const double kMaxYAcceleration = 0.5;
+
} // namespace
ScoreActor::ScoreActor(ScoreActionQueueGroup* queues)
@@ -44,9 +37,9 @@
}
}
-bool ScoreActor::MoveStackIntoPosition(const ScoreParams& /*params*/) {
+bool ScoreActor::MoveStackIntoPosition(const ScoreParams& params) {
// Move the fridge up a little bit.
- if (!SendGoal(0.0, kUpperMoveHeight, true)) {
+ if (!SendGoal(0.0, params.upper_move_height, true)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -58,13 +51,14 @@
}
// Move on when it is clear of the tote knobs.
- if (CurrentGoalHeight() > kBeginHorizontalMoveHeight) {
+ if (CurrentGoalHeight() > params.begin_horizontal_move_height) {
break;
}
}
// Move the fridge out.
- if (!SendGoal(kHorizontalMoveTarget, kBeginHorizontalMoveHeight, true)) {
+ if (!SendGoal(params.horizontal_move_target,
+ params.begin_horizontal_move_height, true)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -75,7 +69,8 @@
return true;
}
- if (NearGoal(kHorizontalMoveTarget, kBeginHorizontalMoveHeight)) {
+ if (NearGoal(params.horizontal_move_target,
+ params.begin_horizontal_move_height)) {
LOG(INFO, "reached goal\n");
break;
}
@@ -86,9 +81,9 @@
return true;
}
-bool ScoreActor::PlaceTheStack(const ScoreParams& /*params*/) {
+bool ScoreActor::PlaceTheStack(const ScoreParams& params) {
// Once the fridge is way out, put it on the ground.
- if (!SendGoal(kHorizontalMoveTarget, kPlaceHeight, true)) {
+ if (!SendGoal(params.horizontal_move_target, params.place_height, true)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -99,7 +94,7 @@
return true;
}
- if (NearGoal(kHorizontalMoveTarget, kPlaceHeight)) {
+ if (NearGoal(params.horizontal_move_target, params.place_height)) {
break;
}
}
@@ -107,7 +102,7 @@
if (ShouldCancel()) return true;
// Release the grabbers.
- if (!SendGoal(kHorizontalMoveTarget, kPlaceHeight, false)) {
+ if (!SendGoal(params.horizontal_move_target, params.place_height, false)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -115,7 +110,7 @@
if (ShouldCancel()) return true;
// Go back to the home position.
- if (!SendGoal(0.0, kHomeReturnHeight, false)) {
+ if (!SendGoal(0.0, params.home_return_height, false)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -126,7 +121,7 @@
return true;
}
- if (NearGoal(0.0, kHomeReturnHeight)) {
+ if (NearGoal(0.0, params.home_return_height)) {
break;
}
}
diff --git a/frc971/actors/score_actor_test.cc b/frc971/actors/score_actor_test.cc
index 3d0c6c3..25f3aa1 100644
--- a/frc971/actors/score_actor_test.cc
+++ b/frc971/actors/score_actor_test.cc
@@ -59,8 +59,7 @@
frc971::actors::score_action.goal.MakeWithBuilder().run(false).Send();
// let the action start running, if we return from this call it has worked.
- bool place_the_stack = true;
- const ScoreParams params = {0.75, place_the_stack};
+ const ScoreParams params = {true, 0.14, 0.13, -0.7, -0.10, 0.1};
score.RunAction(params);
SUCCEED();
@@ -88,8 +87,7 @@
frc971::actors::score_action.goal.MakeWithBuilder().run(false).Send();
// let the action start running, if we return from this call it has worked.
- bool place_the_stack = false;
- const ScoreParams params = {0.75, place_the_stack};
+ const ScoreParams params = {false, 0.14, 0.13, -0.7, -0.10, 0.1};
score.RunAction(params);
SUCCEED();
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 462498f..8ef2966 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -93,10 +93,14 @@
// Move the fridge out with the stack in preparation for scoring.
const ButtonLocation kScore(4, 8);
-
// Release the stack and retract back in.
const ButtonLocation kRetractFromScore(4, 12);
+const ButtonLocation kCoopTop(3, 8);
+const ButtonLocation kCoopTopRetract(3, 7);
+const ButtonLocation kCoopBottom(3, 6);
+const ButtonLocation kCoopBottomRetract(3, 9);
+
const POVLocation kFridgeToggle(4, 270);
const ButtonLocation kSpit(4, 3);
@@ -110,6 +114,39 @@
public:
Reader() : was_running_(false) {}
+ static actors::ScoreParams MakeScoreParams(bool place_the_stack) {
+ actors::ScoreParams r;
+ r.place_the_stack = place_the_stack;
+ r.upper_move_height = 0.14;
+ r.begin_horizontal_move_height = 0.13;
+ r.horizontal_move_target = -0.7;
+ r.place_height = -0.10;
+ r.home_return_height = 0.1;
+ return r;
+ }
+
+ static actors::ScoreParams MakeCoopTopParams(bool place_the_stack) {
+ actors::ScoreParams r;
+ r.place_the_stack = place_the_stack;
+ r.upper_move_height = 0.52;
+ r.begin_horizontal_move_height = 0.5;
+ r.horizontal_move_target = -0.48;
+ r.place_height = 0.39;
+ r.home_return_height = 0.1;
+ return r;
+ }
+
+ static actors::ScoreParams MakeCoopBottomParams(bool place_the_stack) {
+ actors::ScoreParams r;
+ r.place_the_stack = place_the_stack;
+ r.upper_move_height = 0.17;
+ r.begin_horizontal_move_height = 0.16;
+ r.horizontal_move_target = -0.7;
+ r.place_height = 0.0;
+ r.home_return_height = 0.1;
+ return r;
+ }
+
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
bool last_auto_running = auto_running_;
auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
@@ -311,15 +348,32 @@
}
if (data.PosEdge(kScore)) {
- actors::ScoreParams params;
- params.place_the_stack = false;
- action_queue_.EnqueueAction(actors::MakeScoreAction(params));
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeScoreParams(false)));
+ }
+ if (data.PosEdge(kRetractFromScore)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeScoreParams(true)));
+ fridge_closed_ = false;
}
- if (data.PosEdge(kRetractFromScore)) {
- actors::ScoreParams params;
- params.place_the_stack = true;
- action_queue_.EnqueueAction(actors::MakeScoreAction(params));
+ if (data.PosEdge(kCoopTop)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopTopParams(false)));
+ }
+ if (data.PosEdge(kCoopTopRetract)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopTopParams(true)));
+ fridge_closed_ = false;
+ }
+
+ if (data.PosEdge(kCoopBottom)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopBottomParams(false)));
+ }
+ if (data.PosEdge(kCoopBottomRetract)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopBottomParams(true)));
fridge_closed_ = false;
}