Added self catch action. moved gyro to the othersensors queue. add sonar and travis hall effect to that queue. fixed a few bugs in shooter action.
diff --git a/frc971/actions/selfcatch_action.cc b/frc971/actions/selfcatch_action.cc
new file mode 100644
index 0000000..fc9a06c
--- /dev/null
+++ b/frc971/actions/selfcatch_action.cc
@@ -0,0 +1,223 @@
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/constants.h"
+
+#include "frc971/actions/selfcatch_action.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/queues/othersensors.q.h"
+
+namespace frc971 {
+namespace actions {
+
+SelfCatchAction::SelfCatchAction(actions::SelfCatchActionGroup* s)
+ : actions::ActionBase<actions::SelfCatchActionGroup>(s) {}
+
+double SelfCatchAction::SpeedToAngleOffset(double speed) {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ // scale speed to a [0.0-1.0] on something close to the max
+ return (speed / values.drivetrain_max_speed) *
+ SelfCatchAction::kSpeedOffsetRadians;
+}
+
+void SelfCatchAction::RunAction() {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+
+ // Set shot power to established constant
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
+ kShotPower)
+ .shot_requested(false).unload_requested(false).load_requested(false)
+ .Send()) {
+ LOG(ERROR, "Failed to send the shoot action\n");
+ return;
+ }
+
+ // Set claw angle to account for velocity
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ 0.0 +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneSetupShot, this))) return;
+
+ // Open up the claw in preparation for shooting.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ 0.0 +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(values.shooter_action.claw_separation_goal)
+ .intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for the claw to open up a little before we shoot
+ if (WaitUntil(::std::bind(&SelfCatchAction::DonePreShotOpen, this))) return;
+
+ // Make sure that we have the latest shooter status.
+ control_loops::shooter_queue_group.status.FetchLatest();
+ // Get the number of shots fired up to this point. This should not be updated
+ // again for another few cycles.
+ previous_shots_ = control_loops::shooter_queue_group.status->shots;
+ // Shoot!
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
+ kShotPower)
+ .shot_requested(true).unload_requested(false).load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+
+ // wait for record of shot having been fired
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneShot, this))) return;
+
+ // Set claw angle to account for velocity note this is negative
+ // since we will be catching from behind
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ 0.0 -
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(kCatchSeperation).intake(kCatchIntake)
+ .centering(kCatchCentering).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for the sonar to trigger
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneFoundSonar, this))) return;
+
+ // close the claw
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ kFinishAngle)
+ .separation_angle(0.0).intake(kCatchIntake).centering(kCatchCentering)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // claw now closed
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneClawWithBall, this))) return;
+ // ball is fully in
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneBallIn, this))) return;
+
+ // head to a finshed pose
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ kFinishAngle)
+ .separation_angle(kFinishAngle).intake(0.0).centering(0.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // thats it
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneClawWithBall, this))) return;
+
+ // done with action
+ return;
+}
+
+
+bool SelfCatchAction::DoneBallIn() {
+ if (!queues::othersensors.FetchLatest()) {
+ queues::othersensors.FetchNextBlocking();
+ }
+ if (queues::othersensors->travis_hall_effect_distance > 0.005)
+ LOG(INFO, "Ball in at %.2f.\n",
+ queues::othersensors->travis_hall_effect_distance);
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneClawWithBall() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ // Make sure that both the shooter and claw have reached the necessary
+ // states.
+ if (control_loops::claw_queue_group.status->done_with_ball) {
+ LOG(INFO, "Claw at goal.\n");
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneFoundSonar() {
+ if (!queues::othersensors.FetchLatest()) {
+ queues::othersensors.FetchNextBlocking();
+ }
+ if (queues::othersensors->sonar_distance > 0.3 &&
+ queues::othersensors->sonar_distance < kSonarTriggerDist) {
+ LOG(INFO, "Hit Sonar at %.2f.\n", queues::othersensors->sonar_distance);
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneSetupShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ // Make sure that both the shooter and claw have reached the necessary
+ // states.
+ if (control_loops::shooter_queue_group.status->ready &&
+ control_loops::claw_queue_group.status->done_with_ball) {
+ LOG(INFO, "Claw and Shooter ready for shooting.\n");
+ // TODO(james): Get realer numbers for shooter_action.
+ return true;
+ }
+
+ // update the claw position to track velocity
+ // TODO(ben): the claw may never reach the goal if the velocity is
+ // continually changing, we will need testing to see
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ shoot_action.goal->shot_angle +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ abort_ = true;
+ return true;
+ } else {
+ LOG(INFO, "Updating claw angle for velocity offset(%.4f).\n",
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed));
+ }
+ return false;
+}
+
+bool SelfCatchAction::DonePreShotOpen() {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ if (control_loops::claw_queue_group.status->separation >
+ values.shooter_action.claw_shooting_separation) {
+ LOG(INFO, "Opened up enough to shoot.\n");
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (control_loops::shooter_queue_group.status->shots > previous_shots_) {
+ LOG(INFO, "Shot succeeded!\n");
+ return true;
+ }
+ return false;
+}
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/selfcatch_action.h b/frc971/actions/selfcatch_action.h
new file mode 100644
index 0000000..0b34e8c
--- /dev/null
+++ b/frc971/actions/selfcatch_action.h
@@ -0,0 +1,48 @@
+#include "frc971/actions/selfcatch_action.q.h"
+#include "frc971/actions/action.h"
+#include "aos/common/time.h"
+
+namespace frc971 {
+namespace actions {
+
+class SelfCatchAction : public ActionBase<actions::SelfCatchActionGroup> {
+ public:
+
+ explicit SelfCatchAction(actions::SelfCatchActionGroup* s);
+
+ // Actually execute the action of moving the claw and shooter into position
+ // and actually firing them.
+ void RunAction();
+
+ // calc an offset to our requested shot based on robot speed
+ double SpeedToAngleOffset(double speed);
+
+ static constexpr double kSpeedOffsetRadians = 0.2;
+ static constexpr double kShotPower = 100.0;
+ static constexpr double kCatchSeperation = 1.0;
+ static constexpr double kCatchIntake = 12.0;
+ static constexpr double kCatchCentering = 12.0;
+ static constexpr double kFinishAngle = 0.2;
+
+ protected:
+ // completed shot
+ bool DoneShot();
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupShot();
+ // sonar is in valid range to close
+ bool DoneFoundSonar();
+ // Claw reports it is done
+ bool DoneClawWithBall();
+ // hall effect reports the ball is in
+ bool DoneBallIn();
+
+ // to track when shot is complete
+ int previous_shots_;
+ aos::Time::Time action_step_start_;
+};
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/selfcatch_action.q b/frc971/actions/selfcatch_action.q
new file mode 100644
index 0000000..b0254f7
--- /dev/null
+++ b/frc971/actions/selfcatch_action.q
@@ -0,0 +1,18 @@
+package frc971.actions;
+
+queue_group SelfCatchActionGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run; // Shot power in joules.
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group SelfCatchActionGroup selfcatch_action;
diff --git a/frc971/actions/selfcatch_action_main.cc b/frc971/actions/selfcatch_action_main.cc
new file mode 100644
index 0000000..b56ed50
--- /dev/null
+++ b/frc971/actions/selfcatch_action_main.cc
@@ -0,0 +1,22 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actions/selfcatch_action.q.h"
+#include "frc971/actions/self_catch_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actions::SelfCatchAction selfcatch(
+ &::frc971::actions::selfcatch_action);
+ selfcatch.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
index baa5d10..473f1af 100644
--- a/frc971/actions/shoot_action.cc
+++ b/frc971/actions/shoot_action.cc
@@ -48,7 +48,8 @@
// Open up the claw in preparation for shooting.
if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action.goal->shot_angle)
+ shoot_action.goal->shot_angle +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
.separation_angle(values.shooter_action.claw_separation_goal)
.intake(2.0).centering(1.0).Send()) {
LOG(WARNING, "sending claw goal failed\n");
@@ -74,14 +75,18 @@
// wait for record of shot having been fired
if (WaitUntil(::std::bind(&ShootAction::DoneShot, this))) return;
-
+
// done with action
return;
}
bool ShootAction::DoneSetupShot() {
- control_loops::shooter_queue_group.status.FetchNextBlocking();
- control_loops::claw_queue_group.status.FetchNextBlocking();
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
// Make sure that both the shooter and claw have reached the necessary
// states.
if (control_loops::shooter_queue_group.status->ready &&
@@ -111,7 +116,9 @@
bool ShootAction::DonePreShotOpen() {
const frc971::constants::Values& values = frc971::constants::GetValues();
- control_loops::claw_queue_group.status.FetchNextBlocking();
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
if (control_loops::claw_queue_group.status->separation >
values.shooter_action.claw_shooting_separation) {
LOG(INFO, "Opened up enough to shoot.\n");
@@ -121,7 +128,9 @@
}
bool ShootAction::DoneShot() {
- control_loops::shooter_queue_group.status.FetchNextBlocking();
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
if (control_loops::shooter_queue_group.status->shots > previous_shots_) {
LOG(INFO, "Shot succeeded!\n");
return true;
@@ -131,3 +140,4 @@
} // namespace actions
} // namespace frc971
+
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 85751d9..c891c8b 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -36,7 +36,8 @@
// correct side of the zero and go zero.
// Valid region plan.
-// Difference between the arms has a range, and the values of each arm has a range.
+// Difference between the arms has a range, and the values of each arm has a
+// range.
// If a claw runs up against a static limit, don't let the goal change outside
// the limit.
// If a claw runs up against a movable limit, move both claws outwards to get
@@ -196,13 +197,13 @@
if (posedge_filter_ == nullptr && this_sensor.posedge_count_changed() &&
!sensorA.posedge_count_changed() && !sensorB.posedge_count_changed() &&
this_sensor.value() && !this_sensor.last_value()) {
- posedge_filter_ = &this_sensor;
+ posedge_filter_ = &this_sensor;
} else if (posedge_filter_ == &this_sensor &&
!this_sensor.posedge_count_changed() &&
!sensorA.posedge_count_changed() &&
!sensorB.posedge_count_changed() && this_sensor.value()) {
posedge_filter_ = nullptr;
- return true;
+ return true;
} else if (posedge_filter_ == &this_sensor) {
posedge_filter_ = nullptr;
}
@@ -221,7 +222,7 @@
!sensorA.negedge_count_changed() &&
!sensorB.negedge_count_changed() && !this_sensor.value()) {
negedge_filter_ = nullptr;
- return true;
+ return true;
} else if (negedge_filter_ == &this_sensor) {
negedge_filter_ = nullptr;
}
@@ -237,7 +238,7 @@
if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
- LOG(INFO, "%s: Uncertain which side, rejecting posedge\n", name_);
+ LOG(INFO, "%s: Uncertain which side, rejecting posedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
@@ -259,7 +260,7 @@
if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
- LOG(INFO, "%s: Uncertain which side, rejecting negedge\n", name_);
+ LOG(INFO, "%s: Uncertain which side, rejecting negedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
@@ -285,16 +286,16 @@
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
const constants::Values::Claws::Claw &claw_values, double *edge_encoder,
double *edge_angle) {
- if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle,
- front_, calibration_, back_, "front")) {
+ if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle, front_,
+ calibration_, back_, "front")) {
return true;
}
if (DoGetPositionOfEdge(claw_values.calibration, edge_encoder, edge_angle,
calibration_, front_, back_, "calibration")) {
return true;
}
- if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle,
- back_, calibration_, front_, "back")) {
+ if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle, back_,
+ calibration_, front_, "back")) {
return true;
}
return false;
@@ -471,9 +472,9 @@
mode_ = READY;
} else if (top_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
bottom_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
// Time to fine tune the zero.
// Limit the goals here.
if (!enabled) {
@@ -515,10 +516,9 @@
mode_ = PREP_FINE_TUNE_BOTTOM;
}
- if (position &&
- bottom_claw_.SawFilteredPosedge(
- bottom_claw_.calibration(),
- bottom_claw_.front(), bottom_claw_.back())) {
+ if (position && bottom_claw_.SawFilteredPosedge(
+ bottom_claw_.calibration(), bottom_claw_.front(),
+ bottom_claw_.back())) {
// do calibration
bottom_claw_.SetCalibration(
position->bottom.posedge_value,
@@ -588,7 +588,7 @@
top_claw_goal_ = values.claw.start_fine_tune_pos;
top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
mode_ = PREP_FINE_TUNE_TOP;
- }
+ }
}
// now set the bottom claw to track
bottom_claw_goal_ = top_claw_goal_ - values.claw.claw_zeroing_separation;
@@ -691,8 +691,7 @@
case UNKNOWN_LOCATION: {
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
double dx = (claw_.uncapped_average_voltage() -
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ values.claw.max_zeroing_voltage) / claw_.K(0, 0);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
capped_goal_ = true;
@@ -704,8 +703,7 @@
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
double dx = (claw_.uncapped_average_voltage() +
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ values.claw.max_zeroing_voltage) / claw_.K(0, 0);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
capped_goal_ = true;
@@ -717,14 +715,17 @@
if (output) {
if (goal) {
//setup the intake
- output->intake_voltage = (goal->intake > 12.0) ? 12 :
- (goal->intake < -12.0) ? -12.0 : goal->intake;
+ output->intake_voltage =
+ (goal->intake > 12.0) ? 12 : (goal->intake < -12.0) ? -12.0
+ : goal->intake;
output->tusk_voltage = goal->centering;
- output->tusk_voltage = (goal->centering > 12.0) ? 12 :
- (goal->centering < -12.0) ? -12.0 : goal->centering;
+ output->tusk_voltage =
+ (goal->centering > 12.0) ? 12 : (goal->centering < -12.0)
+ ? -12.0
+ : goal->centering;
}
output->top_claw_voltage = claw_.U(1, 0) + claw_.U(0, 0);
- output->bottom_claw_voltage = claw_.U(0, 0);
+ output->bottom_claw_voltage = claw_.U(0, 0);
if (output->top_claw_voltage > kMaxVoltage) {
output->top_claw_voltage = kMaxVoltage;
@@ -743,9 +744,13 @@
::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.005;
bool separation_done =
::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) <
- 0.005;
+ goal->separation_angle) < 0.005;
+ bool separation_done_with_ball =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.06;
status->done = is_ready() && separation_done && bottom_done;
+ status->done_with_ball =
+ is_ready() && separation_done_with_ball && bottom_done;
status->bottom = bottom_absolute_position();
status->separation = top_absolute_position() - bottom_absolute_position();
@@ -757,3 +762,4 @@
} // namespace control_loops
} // namespace frc971
+
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 5d6633b..7f200a4 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -54,6 +54,9 @@
message Status {
// True if zeroed and within tolerance for separation and bottom angle.
bool done;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ // seperation allowance much wider as a ball may be included
+ bool done_with_ball;
// Dump the values of the state matrix.
double bottom;
double bottom_velocity;
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 284f722..bfdd924 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -15,10 +15,10 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/othersensors.q.h"
#include "frc971/constants.h"
-using frc971::sensors::gyro;
+using frc971::sensors::othersensors;
namespace frc971 {
namespace control_loops {
@@ -632,9 +632,9 @@
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
- if (gyro.FetchLatest()) {
- LOG_STRUCT(DEBUG, "using", *gyro);
- dt_closedloop.SetPosition(left_encoder, right_encoder, gyro->angle,
+ if (othersensors.FetchLatest()) {
+ LOG(DEBUG, "using %.3f", othersensors->gyro_angle);
+ dt_closedloop.SetPosition(left_encoder, right_encoder, othersensors->gyro_angle,
control_loop_driving);
} else {
dt_closedloop.SetRawPosition(left_encoder, right_encoder);
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index eb1ede2..275d33f 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -12,7 +12,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/othersensors.q.h"
using ::aos::time::Time;
@@ -133,7 +133,7 @@
.reader_pid(254)
.cape_resets(5)
.Send();
- ::frc971::sensors::gyro.Clear();
+ ::frc971::sensors::othersensors.Clear();
SendDSPacket(true);
}
@@ -157,7 +157,7 @@
virtual ~DrivetrainTest() {
::aos::robot_state.Clear();
- ::frc971::sensors::gyro.Clear();
+ ::frc971::sensors::othersensors.Clear();
::bbb::sensor_generation.Clear();
}
};
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 02343f2..9e57e6f 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -9,14 +9,14 @@
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/othersensors.q.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
#include "frc971/actions/shoot_action.q.h"
using ::frc971::control_loops::drivetrain;
-using ::frc971::sensors::gyro;
+using ::frc971::sensors::othersensors;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -224,12 +224,12 @@
static double filtered_goal_distance = 0.0;
if (data.PosEdge(kDriveControlLoopEnable1) ||
data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
+ if (drivetrain.position.FetchLatest() && othersensors.FetchLatest()) {
distance = (drivetrain.position->left_encoder +
drivetrain.position->right_encoder) /
2.0 -
throttle * kThrottleGain / 2.0;
- angle = gyro->angle;
+ angle = othersensors->gyro_angle;
filtered_goal_distance = distance;
}
}
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index f375f20..47927c4 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -9,7 +9,7 @@
#include "bbb/sensor_reader.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/othersensors.q.h"
#include "frc971/constants.h"
#include "frc971/queues/to_log.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
@@ -20,7 +20,7 @@
#endif
using ::frc971::control_loops::drivetrain;
-using ::frc971::sensors::gyro;
+using ::frc971::sensors::othersensors;
using ::aos::util::WrappingCounter;
namespace frc971 {
@@ -129,7 +129,7 @@
} else if (data->bad_gyro) {
LOG(ERROR, "bad gyro\n");
bad_gyro = true;
- gyro.MakeWithBuilder().angle(0).Send();
+ othersensors.MakeWithBuilder().gyro_angle(0).Send();
} else if (data->old_gyro_reading) {
LOG(WARNING, "old/bad gyro reading\n");
bad_gyro = true;
@@ -138,8 +138,8 @@
}
if (!bad_gyro) {
- gyro.MakeWithBuilder()
- .angle(gyro_translate(data->gyro_angle))
+ othersensors.MakeWithBuilder()
+ .gyro_angle(gyro_translate(data->gyro_angle))
.Send();
}
diff --git a/frc971/queues/gyro_angle.q b/frc971/queues/gyro_angle.q
deleted file mode 100644
index bcf3ac4..0000000
--- a/frc971/queues/gyro_angle.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package frc971.sensors;
-
-message Gyro {
- double angle;
-};
-
-queue Gyro gyro;
diff --git a/frc971/queues/othersensors.q b/frc971/queues/othersensors.q
new file mode 100644
index 0000000..8541b94
--- /dev/null
+++ b/frc971/queues/othersensors.q
@@ -0,0 +1,9 @@
+package frc971.sensors;
+
+message OtherSensors {
+ double sonar_distance;
+ double gyro_angle;
+ double travis_hall_effect_distance;
+};
+
+queue OtherSensors othersensors;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index ac53a70..57d2e60 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -1,7 +1,7 @@
{
'variables': {
'queue_files': [
- 'gyro_angle.q',
+ 'othersensors.q',
'to_log.q',
]
},