wait a bit when scoring to let the grabbers retract
Change-Id: I03cae5dcb1aa3f7f07b219caa7cfdfb4ab526983
diff --git a/aos/common/actions/actions.gyp b/aos/common/actions/actions.gyp
index 68c52b2..623569b 100644
--- a/aos/common/actions/actions.gyp
+++ b/aos/common/actions/actions.gyp
@@ -11,13 +11,17 @@
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:queues',
'<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(AOS)/common/common.gyp:time'
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:phased_loop',
],
'export_dependent_settings': [
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:queues',
'<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(AOS)/common/common.gyp:time'
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:phased_loop',
],
},
{
diff --git a/aos/common/actions/actor.h b/aos/common/actions/actor.h
index a3287db..32c22b1 100644
--- a/aos/common/actions/actor.h
+++ b/aos/common/actions/actor.h
@@ -9,6 +9,8 @@
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
#include "aos/common/time.h"
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/phased_loop.h"
namespace aos {
namespace common {
@@ -48,8 +50,8 @@
void WaitForStop(uint32_t running_id);
// Will run until the done condition is met or times out.
- // Will return false if successful and true if the action was canceled or
- // failed or end_time was reached before it succeeded.
+ // Will return false if successful or end_time is reached and true if the
+ // action was canceled or failed.
// Done condition are defined as functions that return true when done and have
// some sort of blocking statement (such as FetchNextBlocking) to throttle
// spin rate.
@@ -58,6 +60,18 @@
bool WaitUntil(::std::function<bool(void)> done_condition,
const ::aos::time::Time& end_time = ::aos::time::Time(0, 0));
+ // Waits for a certain amount of time from when this method is called.
+ // Returns false if the action was canceled or failed, and true if the wait
+ // succeeded.
+ bool WaitOrCancel(const ::aos::time::Time& duration) {
+ return !WaitUntil([]() {
+ ::aos::time::PhasedLoopXMS(
+ ::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ return false;
+ },
+ ::aos::time::Time::Now() + duration);
+ }
+
// Returns true if the action should be canceled.
bool ShouldCancel();
@@ -201,8 +215,8 @@
}
if (end_time != ::aos::time::Time(0, 0) &&
::aos::time::Time::Now() >= end_time) {
- LOG(INFO, "WaitUntil timed out\n");
- return true;
+ LOG(DEBUG, "WaitUntil timed out\n");
+ return false;
}
}
if (ShouldCancel() || abort_) {
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index da884d2..d1ede7a 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -76,15 +76,13 @@
'fridge_profile_lib.cc',
],
'dependencies' : [
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/build/aos.gyp:logging_interface',
'<(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)/build/aos.gyp:logging_interface',
'<(AOS)/common/actions/actions.gyp:action_lib',
- '<(AOS)/common/controls/controls.gyp:control_loop',
'<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
],
},
diff --git a/frc971/actors/fridge_profile_lib.h b/frc971/actors/fridge_profile_lib.h
index 36ea16b..0ba4bcf 100644
--- a/frc971/actors/fridge_profile_lib.h
+++ b/frc971/actors/fridge_profile_lib.h
@@ -3,7 +3,6 @@
#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"
@@ -146,16 +145,6 @@
}
}
}
-
- 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/score_actor.cc b/frc971/actors/score_actor.cc
index 6294f4c..faa2b07 100644
--- a/frc971/actors/score_actor.cc
+++ b/frc971/actors/score_actor.cc
@@ -123,7 +123,14 @@
}
}
- if (ShouldCancel()) return true;
+ // Release and give the grabers a chance to get out of the way.
+ if (!SendGoal(params.horizontal_move_target, params.place_height, false,
+ kFastMaxXVelocity, kFastMaxYVelocity, kMaxXAcceleration,
+ kMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+ if (!WaitOrCancel(::aos::time::Time::InSeconds(0.1))) return true;
// Release the grabbers.
if (!SendGoal(params.horizontal_move_target, params.place_height, false,