Rounded score set down corner.
Change-Id: If1750d4b9611b4dfa11dc411d81ffc6bc62385a9
diff --git a/frc971/actors/score_action.q b/frc971/actors/score_action.q
index c2a5dba..5b14210 100644
--- a/frc971/actors/score_action.q
+++ b/frc971/actors/score_action.q
@@ -13,7 +13,9 @@
double upper_move_height;
double begin_horizontal_move_height;
double horizontal_move_target;
+ double horizontal_start_lowering;
double place_height;
+ double home_lift_horizontal_start_position;
double home_return_height;
};
diff --git a/frc971/actors/score_actor.cc b/frc971/actors/score_actor.cc
index faa2b07..27e2fe3 100644
--- a/frc971/actors/score_actor.cc
+++ b/frc971/actors/score_actor.cc
@@ -15,15 +15,15 @@
namespace actors {
namespace {
-const double kSlowMaxXVelocity = 0.60;
-const double kSlowMaxYVelocity = 0.25;
+const double kSlowMaxXVelocity = 0.80;
+const double kSlowMaxYVelocity = 0.30;
const double kFastMaxXVelocity = 0.80;
const double kFastMaxYVelocity = 0.30;
-const double kReallyFastMaxXVelocity = 1;
-const double kReallyFastMaxYVelocity = 0.30;
+const double kReallyFastMaxXVelocity = 1.0;
+const double kReallyFastMaxYVelocity = 0.6;
const double kMaxXAcceleration = 0.5;
-const double kMaxYAcceleration = 0.5;
+const double kMaxYAcceleration = 0.7;
const double kFastMaxXAcceleration = 1.2;
const double kFastMaxYAcceleration = 1.2;
const double kSlowMaxXAcceleration = 0.3;
@@ -79,20 +79,33 @@
// Move the fridge out.
if (!SendGoal(params.horizontal_move_target,
params.begin_horizontal_move_height, true, kSlowMaxXVelocity,
- kSlowMaxYVelocity, kSlowMaxXAcceleration,
+ kFastMaxYVelocity, kSlowMaxXAcceleration,
kSlowMaxYAcceleration)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
+ bool started_lowering = false;
+
while (true) {
::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
if (ShouldCancel()) {
return true;
}
+ // Round the moving out corner and start setting down.
+ if (params.place_the_stack && !started_lowering) {
+ if (CurrentGoalX() < params.horizontal_start_lowering) {
+ if (!SendGoal(params.horizontal_move_target, params.place_height, true,
+ kSlowMaxXVelocity, kFastMaxYVelocity,
+ kSlowMaxXAcceleration, kMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+ started_lowering = true;
+ }
+ }
- if (NearGoal(params.horizontal_move_target,
- params.begin_horizontal_move_height)) {
+ if (NearHorizontalGoal(params.horizontal_move_target)) {
LOG(INFO, "reached goal\n");
break;
}
@@ -143,19 +156,31 @@
if (ShouldCancel()) return true;
// Go back to the home position.
- if (!SendGoal(0.0, params.home_return_height, false, kReallyFastMaxXVelocity,
+ if (!SendGoal(0.0, params.place_height, false, kReallyFastMaxXVelocity,
kReallyFastMaxYVelocity, kFastMaxXAcceleration,
kFastMaxYAcceleration)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
+ bool has_lifted = false;
while (true) {
::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
if (ShouldCancel()) {
return true;
}
+ if (!has_lifted &&
+ CurrentGoalX() > params.home_lift_horizontal_start_position) {
+ if (!SendGoal(0.0, params.home_return_height, false,
+ kReallyFastMaxXVelocity, kReallyFastMaxYVelocity,
+ kFastMaxXAcceleration, kFastMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+ has_lifted = true;
+ }
+
if (NearGoal(0.0, params.home_return_height)) {
break;
}
@@ -200,11 +225,17 @@
return false;
}
- ::aos::util::ElevatorArmKinematics::KinematicResult results;
- kinematics_.ForwardKinematic(fridge_queue.status->height,
- fridge_queue.status->angle, 0.0, 0.0, &results);
+ return fridge_queue.status->x;
+}
- return results.fridge_x;
+double ScoreActor::CurrentGoalX() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ return fridge_queue.status->goal_x;
}
bool ScoreActor::SendGoal(double x, double y, bool grabbers_enabled,
@@ -230,7 +261,7 @@
return new_fridge_goal.Send();
}
-bool ScoreActor::NearGoal(double x, double y) {
+bool ScoreActor::NearHorizontalGoal(double x) {
fridge_queue.status.FetchLatest();
if (!fridge_queue.status.get()) {
LOG(ERROR, "Reading from fridge status queue failed.\n");
@@ -246,9 +277,21 @@
&goal_results);
return (::std::abs(results.fridge_x - x) < 0.020 &&
- ::std::abs(results.fridge_h - y) < 0.020 &&
- ::std::abs(goal_results.fridge_x - x) < 0.0001 &&
- ::std::abs(goal_results.fridge_h - y) < 0.0001);
+ ::std::abs(goal_results.fridge_x - x) < 0.0001);
+}
+
+bool ScoreActor::NearGoal(double x, double y) {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return false;
+ }
+
+ const auto &status = *fridge_queue.status;
+ return (::std::abs(status.x - x) < 0.020 &&
+ ::std::abs(status.y - y) < 0.020 &&
+ ::std::abs(status.goal_x - x) < 0.0001 &&
+ ::std::abs(status.goal_y - y) < 0.0001);
}
::std::unique_ptr<ScoreAction> MakeScoreAction(const ScoreParams& params) {
diff --git a/frc971/actors/score_actor.h b/frc971/actors/score_actor.h
index c2d7868..a524d36 100644
--- a/frc971/actors/score_actor.h
+++ b/frc971/actors/score_actor.h
@@ -20,6 +20,7 @@
::aos::util::ElevatorArmKinematics kinematics_;
bool NearGoal(double x, double y);
+ bool NearHorizontalGoal(double x);
bool PlaceTheStack(const ScoreParams ¶ms);
bool MoveStackIntoPosition(const ScoreParams ¶ms);
bool SendGoal(double x, double y, bool grabbers_enabled,
@@ -28,6 +29,7 @@
double CurrentHeight();
double CurrentGoalHeight();
double CurrentX();
+ double CurrentGoalX();
};
typedef aos::common::actions::TypedAction<ScoreActionQueueGroup> ScoreAction;
diff --git a/frc971/actors/score_actor_test.cc b/frc971/actors/score_actor_test.cc
index e462c54..450939f 100644
--- a/frc971/actors/score_actor_test.cc
+++ b/frc971/actors/score_actor_test.cc
@@ -59,7 +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.
- const ScoreParams params = {true, true, 0.14, 0.13, -0.7, -0.10, 0.1};
+ const ScoreParams params = {true, true, 0.14, 0.13, -0.7, -0.7, -0.10, -0.5, 0.1};
score.RunAction(params);
SUCCEED();
@@ -87,7 +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.
- const ScoreParams params = {false, true, 0.14, 0.13, -0.7, -0.10, 0.1};
+ const ScoreParams params = {false, true, 0.14, 0.13, -0.7, -0.7, -0.10, -0.5, 0.1};
score.RunAction(params);
SUCCEED();
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 474e2fd..26298b0 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -123,8 +123,10 @@
r.upper_move_height = 0.14;
r.begin_horizontal_move_height = 0.13;
r.horizontal_move_target = -0.7;
+ r.horizontal_start_lowering = -0.65;
+ r.home_lift_horizontal_start_position = -0.60;
r.place_height = -0.10;
- r.home_return_height = 0.1;
+ r.home_return_height = 0.05;
return r;
}
@@ -135,6 +137,8 @@
r.upper_move_height = 0.52;
r.begin_horizontal_move_height = 0.5;
r.horizontal_move_target = -0.48;
+ r.horizontal_start_lowering = r.horizontal_move_target;
+ r.home_lift_horizontal_start_position = -0.3;
r.place_height = 0.39;
r.home_return_height = 0.1;
return r;
@@ -147,6 +151,8 @@
r.upper_move_height = 0.17;
r.begin_horizontal_move_height = 0.16;
r.horizontal_move_target = -0.7;
+ r.horizontal_start_lowering = r.horizontal_move_target;
+ r.home_lift_horizontal_start_position = -0.3;
r.place_height = 0.0;
r.home_return_height = 0.1;
return r;