Remove global queuegroup claw_queue
Change-Id: Ie642d5da51f6c0934ee3206d1a50969d6f2e6d33
diff --git a/y2014/actors/autonomous_actor.cc b/y2014/actors/autonomous_actor.cc
index 1a874c2..bb38269 100644
--- a/y2014/actors/autonomous_actor.cc
+++ b/y2014/actors/autonomous_actor.cc
@@ -35,27 +35,40 @@
".y2014.sensors.auto_mode")),
hot_goal_fetcher_(
event_loop->MakeFetcher<::y2014::HotGoal>(".y2014.hot_goal")),
+ claw_goal_sender_(
+ event_loop->MakeSender<::y2014::control_loops::ClawQueue::Goal>(
+ ".y2014.control_loops.claw_queue.goal")),
+ claw_goal_fetcher_(
+ event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Goal>(
+ ".y2014.control_loops.claw_queue.goal")),
+ claw_status_fetcher_(
+ event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Status>(
+ ".y2014.control_loops.claw_queue.status")),
+ shooter_goal_sender_(
+ event_loop->MakeSender<::y2014::control_loops::ShooterQueue::Goal>(
+ ".y2014.control_loops.shooter_queue.goal")),
shoot_action_factory_(actors::ShootActor::MakeFactory(event_loop)) {}
void AutonomousActor::PositionClawVertically(double intake_power,
double centering_power) {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.0)
- .separation_angle(0.0)
- .intake(intake_power)
- .centering(centering_power)
- .Send()) {
+ auto goal_message = claw_goal_sender_.MakeMessage();
+ goal_message->bottom_angle = 0.0;
+ goal_message->separation_angle = 0.0;
+ goal_message->intake = intake_power;
+ goal_message->centering = centering_power;
+
+ if (!goal_message.Send()) {
LOG(WARNING, "sending claw goal failed\n");
}
}
void AutonomousActor::PositionClawBackIntake() {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(-2.273474)
- .separation_angle(0.0)
- .intake(12.0)
- .centering(12.0)
- .Send()) {
+ auto goal_message = claw_goal_sender_.MakeMessage();
+ goal_message->bottom_angle = -2.273474;
+ goal_message->separation_angle = 0.0;
+ goal_message->intake = 12.0;
+ goal_message->centering = 12.0;
+ if (!goal_message.Send()) {
LOG(WARNING, "sending claw goal failed\n");
}
}
@@ -63,35 +76,35 @@
void AutonomousActor::PositionClawUpClosed() {
// Move the claw to where we're going to shoot from but keep it closed until
// it gets there.
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.86)
- .separation_angle(0.0)
- .intake(4.0)
- .centering(1.0)
- .Send()) {
+ auto goal_message = claw_goal_sender_.MakeMessage();
+ goal_message->bottom_angle = 0.86;
+ goal_message->separation_angle = 0.0;
+ goal_message->intake = 4.0;
+ goal_message->centering = 1.0;
+ if (!goal_message.Send()) {
LOG(WARNING, "sending claw goal failed\n");
}
}
void AutonomousActor::PositionClawForShot() {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.86)
- .separation_angle(0.10)
- .intake(4.0)
- .centering(1.0)
- .Send()) {
+ auto goal_message = claw_goal_sender_.MakeMessage();
+ goal_message->bottom_angle = 0.86;
+ goal_message->separation_angle = 0.10;
+ goal_message->intake = 4.0;
+ goal_message->centering = 1.0;
+ if (!goal_message.Send()) {
LOG(WARNING, "sending claw goal failed\n");
}
}
void AutonomousActor::SetShotPower(double power) {
LOG(INFO, "Setting shot power to %f\n", power);
- if (!control_loops::shooter_queue.goal.MakeWithBuilder()
- .shot_power(power)
- .shot_requested(false)
- .unload_requested(false)
- .load_requested(false)
- .Send()) {
+ auto goal_message = shooter_goal_sender_.MakeMessage();
+ goal_message->shot_power = power;
+ goal_message->shot_requested = false;
+ goal_message->unload_requested = false;
+ goal_message->load_requested = false;
+ if (!goal_message.Send()) {
LOG(WARNING, "sending shooter goal failed\n");
}
}
@@ -116,22 +129,21 @@
::std::chrono::milliseconds(10) / 2);
// Poll the running bit and auto done bits.
phased_loop.SleepUntilNext();
- control_loops::claw_queue.status.FetchLatest();
- control_loops::claw_queue.goal.FetchLatest();
+ claw_status_fetcher_.Fetch();
+ claw_goal_fetcher_.Fetch();
if (ShouldCancel()) {
return false;
}
- if (control_loops::claw_queue.status.get() == nullptr ||
- control_loops::claw_queue.goal.get() == nullptr) {
+ if (claw_status_fetcher_.get() == nullptr ||
+ claw_goal_fetcher_.get() == nullptr) {
continue;
}
- bool ans =
- control_loops::claw_queue.status->zeroed &&
- (::std::abs(control_loops::claw_queue.status->bottom_velocity) < 1.0) &&
- (::std::abs(control_loops::claw_queue.status->bottom -
- control_loops::claw_queue.goal->bottom_angle) < 0.10) &&
- (::std::abs(control_loops::claw_queue.status->separation -
- control_loops::claw_queue.goal->separation_angle) < 0.4);
+ bool ans = claw_status_fetcher_->zeroed &&
+ (::std::abs(claw_status_fetcher_->bottom_velocity) < 1.0) &&
+ (::std::abs(claw_status_fetcher_->bottom -
+ claw_goal_fetcher_->bottom_angle) < 0.10) &&
+ (::std::abs(claw_status_fetcher_->separation -
+ claw_goal_fetcher_->separation_angle) < 0.4);
if (ans) {
return true;
}
diff --git a/y2014/actors/autonomous_actor.h b/y2014/actors/autonomous_actor.h
index 5529d36..e832980 100644
--- a/y2014/actors/autonomous_actor.h
+++ b/y2014/actors/autonomous_actor.h
@@ -10,6 +10,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "y2014/actors/shoot_actor.h"
+#include "y2014/control_loops/shooter/shooter.q.h"
#include "y2014/queues/auto_mode.q.h"
#include "y2014/queues/hot_goal.q.h"
@@ -40,6 +41,12 @@
::aos::Fetcher<::y2014::sensors::AutoMode> auto_mode_fetcher_;
::aos::Fetcher<::y2014::HotGoal> hot_goal_fetcher_;
+ ::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
+ ::aos::Fetcher<::y2014::control_loops::ClawQueue::Goal> claw_goal_fetcher_;
+ ::aos::Fetcher<::y2014::control_loops::ClawQueue::Status>
+ claw_status_fetcher_;
+ ::aos::Sender<::y2014::control_loops::ShooterQueue::Goal>
+ shooter_goal_sender_;
actors::ShootActor::Factory shoot_action_factory_;
};
diff --git a/y2014/actors/shoot_actor.cc b/y2014/actors/shoot_actor.cc
index 5520c49..4a57943 100644
--- a/y2014/actors/shoot_actor.cc
+++ b/y2014/actors/shoot_actor.cc
@@ -4,37 +4,13 @@
#include "aos/logging/logging.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/shooter/shooter.q.h"
namespace y2014 {
namespace actors {
-namespace {
-
-bool IntakeOff() {
- control_loops::claw_queue.goal.FetchLatest();
- if (!control_loops::claw_queue.goal.get()) {
- LOG(WARNING, "no claw goal\n");
- // If it doesn't have a goal, then the intake isn't on so we don't have to
- // turn it off.
- return true;
- } else {
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(control_loops::claw_queue.goal->bottom_angle)
- .separation_angle(control_loops::claw_queue.goal->separation_angle)
- .intake(0.0)
- .centering(0.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return false;
- }
- }
- return true;
-}
-
-} // namespace
constexpr double ShootActor::kOffsetRadians;
constexpr double ShootActor::kClawShootingSeparation;
@@ -42,14 +18,46 @@
ShootActor::ShootActor(::aos::EventLoop *event_loop)
: ::aos::common::actions::ActorBase<actors::ShootActionQueueGroup>(
- event_loop, ".y2014.actors.shoot_action") {}
+ event_loop, ".y2014.actors.shoot_action"),
+ claw_goal_fetcher_(
+ event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Goal>(
+ ".y2014.control_loops.claw_queue.goal")),
+ claw_status_fetcher_(
+ event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Status>(
+ ".y2014.control_loops.claw_queue.status")),
+ claw_goal_sender_(
+ event_loop->MakeSender<::y2014::control_loops::ClawQueue::Goal>(
+ ".y2014.control_loops.claw_queue.goal")) {}
double ShootActor::SpeedToAngleOffset(double speed) {
- const constants::Values& values = constants::GetValues();
+ const constants::Values &values = constants::GetValues();
// scale speed to a [0.0-1.0] on something close to the max
return (speed / values.drivetrain_max_speed) * kOffsetRadians;
}
+bool ShootActor::IntakeOff() {
+ claw_goal_fetcher_.Fetch();
+ if (!claw_goal_fetcher_.get()) {
+ LOG(WARNING, "no claw goal\n");
+ // If it doesn't have a goal, then the intake isn't on so we don't have to
+ // turn it off.
+ return true;
+ } else {
+ auto goal_message = claw_goal_sender_.MakeMessage();
+
+ goal_message->bottom_angle = claw_goal_fetcher_->bottom_angle;
+ goal_message->separation_angle = claw_goal_fetcher_->separation_angle;
+ goal_message->intake = 0.0;
+ goal_message->centering = 0.0;
+
+ if (!goal_message.Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return false;
+ }
+ }
+ return true;
+}
+
bool ShootActor::RunAction(const double&) {
InnerRunAction();
@@ -105,23 +113,22 @@
if (!IntakeOff()) return;
}
-bool ClawIsReady() {
- control_loops::claw_queue.goal.FetchLatest();
+bool ShootActor::ClawIsReady() {
+ claw_goal_fetcher_.Fetch();
- bool ans =
- control_loops::claw_queue.status->zeroed &&
- (::std::abs(control_loops::claw_queue.status->bottom_velocity) < 0.5) &&
- (::std::abs(control_loops::claw_queue.status->bottom -
- control_loops::claw_queue.goal->bottom_angle) < 0.10) &&
- (::std::abs(control_loops::claw_queue.status->separation -
- control_loops::claw_queue.goal->separation_angle) < 0.4);
+ bool ans = claw_status_fetcher_->zeroed &&
+ (::std::abs(claw_status_fetcher_->bottom_velocity) < 0.5) &&
+ (::std::abs(claw_status_fetcher_->bottom -
+ claw_goal_fetcher_->bottom_angle) < 0.10) &&
+ (::std::abs(claw_status_fetcher_->separation -
+ claw_goal_fetcher_->separation_angle) < 0.4);
LOG(DEBUG, "Claw is %sready zeroed %d bottom_velocity %f bottom %f sep %f\n",
- ans ? "" : "not ", control_loops::claw_queue.status->zeroed,
- ::std::abs(control_loops::claw_queue.status->bottom_velocity),
- ::std::abs(control_loops::claw_queue.status->bottom -
- control_loops::claw_queue.goal->bottom_angle),
- ::std::abs(control_loops::claw_queue.status->separation -
- control_loops::claw_queue.goal->separation_angle));
+ ans ? "" : "not ", claw_status_fetcher_->zeroed,
+ ::std::abs(claw_status_fetcher_->bottom_velocity),
+ ::std::abs(claw_status_fetcher_->bottom -
+ claw_goal_fetcher_->bottom_angle),
+ ::std::abs(claw_status_fetcher_->separation -
+ claw_goal_fetcher_->separation_angle));
return ans;
}
@@ -141,7 +148,7 @@
bool ShootActor::DoneSetupShot() {
control_loops::shooter_queue.status.FetchAnother();
- control_loops::claw_queue.status.FetchAnother();
+ claw_status_fetcher_.Fetch();
// Make sure that both the shooter and claw have reached the necessary
// states.
if (ShooterIsReady() && ClawIsReady()) {
@@ -153,8 +160,8 @@
}
bool ShootActor::DonePreShotOpen() {
- control_loops::claw_queue.status.FetchAnother();
- if (control_loops::claw_queue.status->separation > kClawShootingSeparation) {
+ claw_status_fetcher_.Fetch();
+ if (claw_status_fetcher_->separation > kClawShootingSeparation) {
LOG(INFO, "Opened up enough to shoot.\n");
return true;
}
diff --git a/y2014/actors/shoot_actor.h b/y2014/actors/shoot_actor.h
index 9b3bcf1..33a284c 100644
--- a/y2014/actors/shoot_actor.h
+++ b/y2014/actors/shoot_actor.h
@@ -3,10 +3,10 @@
#include <memory>
-#include "aos/actions/actor.h"
#include "aos/actions/actions.h"
-
+#include "aos/actions/actor.h"
#include "y2014/actors/shoot_action.q.h"
+#include "y2014/control_loops/claw/claw.q.h"
namespace y2014 {
namespace actors {
@@ -44,6 +44,15 @@
// in the right place
bool DoneSetupShot();
+ bool IntakeOff();
+ bool ClawIsReady();
+
+ private:
+ ::aos::Fetcher<::y2014::control_loops::ClawQueue::Goal> claw_goal_fetcher_;
+ ::aos::Fetcher<::y2014::control_loops::ClawQueue::Status>
+ claw_status_fetcher_;
+ ::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
+
// to track when shot is complete
int previous_shots_;
};
diff --git a/y2014/control_loops/claw/BUILD b/y2014/control_loops/claw/BUILD
index 0764895..6c162e6 100644
--- a/y2014/control_loops/claw/BUILD
+++ b/y2014/control_loops/claw/BUILD
@@ -2,18 +2,6 @@
load("//aos/build:queues.bzl", "queue_library")
-cc_binary(
- name = "replay_claw",
- srcs = [
- "replay_claw.cc",
- ],
- deps = [
- ":claw_queue",
- "//aos:init",
- "//aos/controls:replay_control_loop",
- ],
-)
-
queue_library(
name = "claw_queue",
srcs = [
@@ -81,19 +69,6 @@
)
cc_binary(
- name = "claw_calibration",
- srcs = [
- "claw_calibration.cc",
- ],
- deps = [
- ":claw_queue",
- "//aos:init",
- "//aos/controls:control_loop",
- "//y2014:constants",
- ],
-)
-
-cc_binary(
name = "claw",
srcs = [
"claw_main.cc",
diff --git a/y2014/control_loops/claw/claw.q b/y2014/control_loops/claw/claw.q
index efe445d..ff4c8b8 100644
--- a/y2014/control_loops/claw/claw.q
+++ b/y2014/control_loops/claw/claw.q
@@ -16,6 +16,7 @@
};
// All angles here are 0 vertical, positive "up" (aka backwards).
+// Published on ".y2014.control_loops.claw_queue"
queue_group ClawQueue {
implements aos.control_loops.ControlLoop;
@@ -67,8 +68,6 @@
queue Status status;
};
-queue_group ClawQueue claw_queue;
-
struct ClawPositionToLog {
double top;
double bottom;
diff --git a/y2014/control_loops/claw/claw_calibration.cc b/y2014/control_loops/claw/claw_calibration.cc
deleted file mode 100644
index 149ed3c..0000000
--- a/y2014/control_loops/claw/claw_calibration.cc
+++ /dev/null
@@ -1,317 +0,0 @@
-#include "y2014/control_loops/claw/claw.q.h"
-#include "frc971/control_loops/control_loops.q.h"
-
-#include "aos/init.h"
-#include "y2014/constants.h"
-
-namespace y2014 {
-
-typedef constants::Values::Claws Claws;
-using ::frc971::HallEffectStruct;
-
-class Sensor {
- public:
- Sensor(const double start_position,
- const HallEffectStruct &initial_hall_effect)
- : start_position_(start_position),
- last_hall_effect_(initial_hall_effect),
- last_posedge_count_(initial_hall_effect.posedge_count),
- last_negedge_count_(initial_hall_effect.negedge_count) {
- last_on_min_position_ = start_position;
- last_on_max_position_ = start_position;
- last_off_min_position_ = start_position;
- last_off_max_position_ = start_position;
- }
-
- bool DoGetPositionOfEdge(
- const ::y2014::control_loops::HalfClawPosition &claw_position,
- const HallEffectStruct &hall_effect, Claws::AnglePair *limits) {
- bool print = false;
-
- if (hall_effect.posedge_count != last_posedge_count_) {
- const double avg_off_position =
- (last_off_min_position_ + last_off_max_position_) / 2.0;
- if (hall_effect.posedge_value < avg_off_position) {
- printf("Posedge upper current %f posedge %f avg_off %f [%f, %f]\n",
- claw_position.position, hall_effect.posedge_value,
- avg_off_position, last_off_min_position_,
- last_off_max_position_);
- limits->upper_decreasing_angle =
- hall_effect.posedge_value - start_position_;
- } else {
- printf("Posedge lower current %f posedge %f avg_off %f [%f, %f]\n",
- claw_position.position, hall_effect.posedge_value,
- avg_off_position, last_off_min_position_,
- last_off_max_position_);
- limits->lower_angle =
- hall_effect.posedge_value - start_position_;
- }
- print = true;
- }
- if (hall_effect.negedge_count != last_negedge_count_) {
- const double avg_on_position =
- (last_on_min_position_ + last_on_max_position_) / 2.0;
- if (hall_effect.negedge_value > avg_on_position) {
- printf("Negedge upper current %f negedge %f last_on %f [%f, %f]\n",
- claw_position.position, hall_effect.negedge_value,
- avg_on_position, last_on_min_position_,
- last_on_max_position_);
- limits->upper_angle =
- hall_effect.negedge_value - start_position_;
- } else {
- printf("Negedge lower current %f negedge %f last_on %f [%f, %f]\n",
- claw_position.position, hall_effect.negedge_value,
- avg_on_position, last_on_min_position_,
- last_on_max_position_);
- limits->lower_decreasing_angle =
- hall_effect.negedge_value - start_position_;
- }
- print = true;
- }
-
- if (hall_effect.current) {
- if (!last_hall_effect_.current) {
- last_on_min_position_ = last_on_max_position_ = claw_position.position;
- } else {
- last_on_min_position_ =
- ::std::min(claw_position.position, last_on_min_position_);
- last_on_max_position_ =
- ::std::max(claw_position.position, last_on_max_position_);
- }
- } else {
- if (last_hall_effect_.current) {
- last_off_min_position_ = last_off_max_position_ =
- claw_position.position;
- } else {
- last_off_min_position_ =
- ::std::min(claw_position.position, last_off_min_position_);
- last_off_max_position_ =
- ::std::max(claw_position.position, last_off_max_position_);
- }
- }
-
- last_hall_effect_ = hall_effect;
- last_posedge_count_ = hall_effect.posedge_count;
- last_negedge_count_ = hall_effect.negedge_count;
-
- return print;
- }
-
- private:
- const double start_position_;
- HallEffectStruct last_hall_effect_;
- int32_t last_posedge_count_;
- int32_t last_negedge_count_;
- double last_on_min_position_;
- double last_off_min_position_;
- double last_on_max_position_;
- double last_off_max_position_;
-};
-
-class ClawSensors {
- public:
- ClawSensors(
- const double start_position,
- const ::y2014::control_loops::HalfClawPosition &initial_claw_position)
- : start_position_(start_position),
- front_(start_position, initial_claw_position.front),
- calibration_(start_position, initial_claw_position.calibration),
- back_(start_position, initial_claw_position.back) {}
-
- bool GetPositionOfEdge(
- const ::y2014::control_loops::HalfClawPosition &claw_position,
- Claws::Claw *claw) {
- bool print = false;
- if (front_.DoGetPositionOfEdge(claw_position,
- claw_position.front, &claw->front)) {
- print = true;
- } else if (calibration_.DoGetPositionOfEdge(claw_position,
- claw_position.calibration,
- &claw->calibration)) {
- print = true;
- } else if (back_.DoGetPositionOfEdge(claw_position,
- claw_position.back, &claw->back)) {
- print = true;
- }
-
- double position = claw_position.position - start_position_;
-
- if (position > claw->upper_limit) {
- claw->upper_hard_limit = claw->upper_limit = position;
- print = true;
- }
- if (position < claw->lower_limit) {
- claw->lower_hard_limit = claw->lower_limit = position;
- print = true;
- }
- return print;
- }
-
- private:
- const double start_position_;
- Sensor front_;
- Sensor calibration_;
- Sensor back_;
-};
-
-int Main() {
- ::y2014::control_loops::claw_queue.position.FetchNextBlocking();
-
- const double top_start_position =
- ::y2014::control_loops::claw_queue.position->top.position;
- const double bottom_start_position =
- ::y2014::control_loops::claw_queue.position->bottom.position;
-
- ClawSensors top(top_start_position,
- ::y2014::control_loops::claw_queue.position->top);
- ClawSensors bottom(bottom_start_position,
- ::y2014::control_loops::claw_queue.position->bottom);
-
- Claws limits;
-
- limits.claw_zeroing_off_speed = 0.5;
- limits.claw_zeroing_speed = 0.1;
- limits.claw_zeroing_separation = 0.1;
-
- // claw separation that would be considered a collision
- limits.claw_min_separation = 0.0;
- limits.claw_max_separation = 0.0;
-
- // We should never get closer/farther than these.
- limits.soft_min_separation = 0.0;
- limits.soft_max_separation = 0.0;
-
- limits.upper_claw.lower_hard_limit = 0.0;
- limits.upper_claw.upper_hard_limit = 0.0;
- limits.upper_claw.lower_limit = 0.0;
- limits.upper_claw.upper_limit = 0.0;
- limits.upper_claw.front.lower_angle = 0.0;
- limits.upper_claw.front.upper_angle = 0.0;
- limits.upper_claw.front.lower_decreasing_angle = 0.0;
- limits.upper_claw.front.upper_decreasing_angle = 0.0;
- limits.upper_claw.calibration.lower_angle = 0.0;
- limits.upper_claw.calibration.upper_angle = 0.0;
- limits.upper_claw.calibration.lower_decreasing_angle = 0.0;
- limits.upper_claw.calibration.upper_decreasing_angle = 0.0;
- limits.upper_claw.back.lower_angle = 0.0;
- limits.upper_claw.back.upper_angle = 0.0;
- limits.upper_claw.back.lower_decreasing_angle = 0.0;
- limits.upper_claw.back.upper_decreasing_angle = 0.0;
-
- limits.lower_claw.lower_hard_limit = 0.0;
- limits.lower_claw.upper_hard_limit = 0.0;
- limits.lower_claw.lower_limit = 0.0;
- limits.lower_claw.upper_limit = 0.0;
- limits.lower_claw.front.lower_angle = 0.0;
- limits.lower_claw.front.upper_angle = 0.0;
- limits.lower_claw.front.lower_decreasing_angle = 0.0;
- limits.lower_claw.front.upper_decreasing_angle = 0.0;
- limits.lower_claw.calibration.lower_angle = 0.0;
- limits.lower_claw.calibration.upper_angle = 0.0;
- limits.lower_claw.calibration.lower_decreasing_angle = 0.0;
- limits.lower_claw.calibration.upper_decreasing_angle = 0.0;
- limits.lower_claw.back.lower_angle = 0.0;
- limits.lower_claw.back.upper_angle = 0.0;
- limits.lower_claw.back.lower_decreasing_angle = 0.0;
- limits.lower_claw.back.upper_decreasing_angle = 0.0;
-
- limits.claw_unimportant_epsilon = 0.01;
- limits.start_fine_tune_pos = -0.2;
- limits.max_zeroing_voltage = 4.0;
-
- ::y2014::control_loops::ClawQueue::Position last_position =
- *::y2014::control_loops::claw_queue.position;
-
- while (true) {
- ::y2014::control_loops::claw_queue.position.FetchNextBlocking();
- bool print = false;
- if (top.GetPositionOfEdge(::y2014::control_loops::claw_queue.position->top,
- &limits.upper_claw)) {
- print = true;
- printf("Got an edge on the upper claw\n");
- }
- if (bottom.GetPositionOfEdge(
- ::y2014::control_loops::claw_queue.position->bottom,
- &limits.lower_claw)) {
- print = true;
- printf("Got an edge on the lower claw\n");
- }
- const double top_position =
- ::y2014::control_loops::claw_queue.position->top.position -
- top_start_position;
- const double bottom_position =
- ::y2014::control_loops::claw_queue.position->bottom.position -
- bottom_start_position;
- const double separation = top_position - bottom_position;
- if (separation > limits.claw_max_separation) {
- limits.soft_max_separation = limits.claw_max_separation = separation;
- print = true;
- }
- if (separation < limits.claw_min_separation) {
- limits.soft_min_separation = limits.claw_min_separation = separation;
- print = true;
- }
-
- if (print) {
- printf("{%f,\n", limits.claw_zeroing_off_speed);
- printf("%f,\n", limits.claw_zeroing_speed);
- printf("%f,\n", limits.claw_zeroing_separation);
- printf("%f,\n", limits.claw_min_separation);
- printf("%f,\n", limits.claw_max_separation);
- printf("%f,\n", limits.soft_min_separation);
- printf("%f,\n", limits.soft_max_separation);
- printf(
- "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
- "%f}},\n",
- limits.upper_claw.lower_hard_limit,
- limits.upper_claw.upper_hard_limit, limits.upper_claw.lower_limit,
- limits.upper_claw.upper_limit, limits.upper_claw.front.lower_angle,
- limits.upper_claw.front.upper_angle,
- limits.upper_claw.front.lower_decreasing_angle,
- limits.upper_claw.front.upper_decreasing_angle,
- limits.upper_claw.calibration.lower_angle,
- limits.upper_claw.calibration.upper_angle,
- limits.upper_claw.calibration.lower_decreasing_angle,
- limits.upper_claw.calibration.upper_decreasing_angle,
- limits.upper_claw.back.lower_angle,
- limits.upper_claw.back.upper_angle,
- limits.upper_claw.back.lower_decreasing_angle,
- limits.upper_claw.back.upper_decreasing_angle);
-
- printf(
- "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
- "%f}},\n",
- limits.lower_claw.lower_hard_limit,
- limits.lower_claw.upper_hard_limit, limits.lower_claw.lower_limit,
- limits.lower_claw.upper_limit, limits.lower_claw.front.lower_angle,
- limits.lower_claw.front.upper_angle,
- limits.lower_claw.front.lower_decreasing_angle,
- limits.lower_claw.front.upper_decreasing_angle,
- limits.lower_claw.calibration.lower_angle,
- limits.lower_claw.calibration.upper_angle,
- limits.lower_claw.calibration.lower_decreasing_angle,
- limits.lower_claw.calibration.upper_decreasing_angle,
- limits.lower_claw.back.lower_angle,
- limits.lower_claw.back.upper_angle,
- limits.lower_claw.back.lower_decreasing_angle,
- limits.lower_claw.back.upper_decreasing_angle);
- printf("%f, // claw_unimportant_epsilon\n",
- limits.claw_unimportant_epsilon);
- printf("%f, // start_fine_tune_pos\n", limits.start_fine_tune_pos);
- printf("%f,\n", limits.max_zeroing_voltage);
- printf("}\n");
- }
-
- last_position = *::y2014::control_loops::claw_queue.position;
- }
- return 0;
-}
-
-} // namespace y2014
-
-int main() {
- ::aos::Init();
- int returnvalue = ::y2014::Main();
- ::aos::Cleanup();
- return returnvalue;
-}
diff --git a/y2014/control_loops/claw/replay_claw.cc b/y2014/control_loops/claw/replay_claw.cc
deleted file mode 100644
index b943328..0000000
--- a/y2014/control_loops/claw/replay_claw.cc
+++ /dev/null
@@ -1,24 +0,0 @@
-#include "aos/controls/replay_control_loop.h"
-#include "aos/init.h"
-
-#include "y2014/control_loops/claw/claw.q.h"
-
-// Reads one or more log files and sends out all the queue messages (in the
-// correct order and at the correct time) to feed a "live" claw process.
-
-int main(int argc, char **argv) {
- if (argc <= 1) {
- fprintf(stderr, "Need at least one file to replay!\n");
- return EXIT_FAILURE;
- }
-
- ::aos::InitNRT();
-
- ::aos::controls::ControlLoopReplayer<::y2014::control_loops::ClawQueue>
- replayer(&::y2014::control_loops::claw_queue, "claw");
- for (int i = 1; i < argc; ++i) {
- replayer.ProcessFile(argv[i]);
- }
-
- ::aos::Cleanup();
-}
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index a8f0e7f..590c74a 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -159,6 +159,12 @@
: ::aos::input::ActionJoystickInput(
event_loop, control_loops::GetDrivetrainConfig(),
::aos::input::DrivetrainInputReader::InputType::kSteeringWheel, {}),
+ claw_status_fetcher_(
+ event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Status>(
+ ".y2014.control_loops.claw_queue.status")),
+ claw_goal_sender_(
+ event_loop->MakeSender<::y2014::control_loops::ClawQueue::Goal>(
+ ".y2014.control_loops.claw_queue.goal")),
shot_power_(80.0),
goal_angle_(0.0),
separation_angle_(kGrabSeparation),
@@ -349,10 +355,9 @@
}
if (moving_for_shot_) {
- auto &claw_status = control_loops::claw_queue.status;
- claw_status.FetchLatest();
- if (claw_status.get()) {
- if (::std::abs(claw_status->bottom - goal_angle) < 0.2) {
+ claw_status_fetcher_.Fetch();
+ if (claw_status_fetcher_.get()) {
+ if (::std::abs(claw_status_fetcher_->bottom - goal_angle) < 0.2) {
moving_for_shot_ = false;
separation_angle_ = shot_separation_angle_;
}
@@ -370,15 +375,18 @@
bool intaking =
data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition) ||
data.IsPressed(kIntakeOpenPosition) || data.IsPressed(kCatch);
- if (!control_loops::claw_queue.goal.MakeWithBuilder()
- .bottom_angle(goal_angle)
- .separation_angle(separation_angle)
- .intake(intaking ? 12.0
- : (data.IsPressed(kRollersOut) ? -12.0
- : intake_power_))
- .centering(intaking ? 12.0 : 0.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
+ {
+ auto goal_message = claw_goal_sender_.MakeMessage();
+ goal_message->bottom_angle = goal_angle;
+ goal_message->separation_angle = separation_angle;
+ goal_message->intake =
+ intaking ? 12.0
+ : (data.IsPressed(kRollersOut) ? -12.0 : intake_power_);
+ goal_message->centering = intaking ? 12.0 : 0.0;
+
+ if (!goal_message.Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
}
if (!control_loops::shooter_queue.goal.MakeWithBuilder()
@@ -400,6 +408,10 @@
}
private:
+ ::aos::Fetcher<::y2014::control_loops::ClawQueue::Status>
+ claw_status_fetcher_;
+ ::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
+
double shot_power_;
double goal_angle_;
double separation_angle_, shot_separation_angle_;