Merge "Retaining checkbox data and sending to flatbuffer"
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7d297e0..15d84ea 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -30,8 +30,8 @@
namespace {
// Maximum variation to allow in the gyro when zeroing.
-constexpr double kMaxYawGyroZeroingRange = 0.05;
-}
+constexpr double kMaxYawGyroZeroingRange = 0.08;
+} // namespace
DrivetrainFilters::DrivetrainFilters(const DrivetrainConfig<double> &dt_config,
::aos::EventLoop *event_loop,
@@ -219,8 +219,10 @@
case GyroType::FLIPPED_SPARTAN_GYRO:
if (!yaw_gyro_zero_.has_value()) {
yaw_gyro_zeroer_.AddData(last_gyro_rate_);
- if (yaw_gyro_zeroer_.GetRange() < kMaxYawGyroZeroingRange) {
+ if (yaw_gyro_zeroer_.full() && yaw_gyro_zeroer_.GetRange() < kMaxYawGyroZeroingRange) {
yaw_gyro_zero_ = yaw_gyro_zeroer_.GetAverage()(0);
+ VLOG(1) << "Zeroed to " << *yaw_gyro_zero_ << " Range "
+ << yaw_gyro_zeroer_.GetRange();
}
}
ready_ = yaw_gyro_zero_.has_value();
diff --git a/third_party/y2022/field/balls.jpeg b/third_party/y2022/field/balls.jpeg
index 9c7cc7f..8e0fba3 100644
--- a/third_party/y2022/field/balls.jpeg
+++ b/third_party/y2022/field/balls.jpeg
Binary files differ
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 5e344ac..8bfb2d9 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -212,7 +212,7 @@
set_turret_goal(constants::Values::kTurretFrontIntakePos());
set_fire_at_will(true);
SendSuperstructureGoal();
- if (!WaitForBallsShot(1)) return;
+ if (!WaitForBallsShot()) return;
set_fire_at_will(false);
SendSuperstructureGoal();
@@ -227,7 +227,7 @@
RetractBackIntake();
set_fire_at_will(true);
SendSuperstructureGoal();
- if (!WaitForBallsShot(2)) return;
+ if (!WaitForBallsShot()) return;
set_fire_at_will(false);
SendSuperstructureGoal();
@@ -250,7 +250,7 @@
// Fire the two balls once we stopped
set_fire_at_will(true);
SendSuperstructureGoal();
- if (!WaitForBallsShot(2)) return;
+ if (!WaitForBallsShot()) return;
set_fire_at_will(false);
SendSuperstructureGoal();
@@ -383,7 +383,27 @@
SendSuperstructureGoal();
}
-[[nodiscard]] bool AutonomousActor::WaitForBallsShot(int num_wanted) {
+[[nodiscard]] bool AutonomousActor::WaitForBallsShot() {
+ CHECK(superstructure_status_fetcher_.Fetch());
+
+ // Don't do anything if we aren't loaded
+ if (superstructure_status_fetcher_->state() !=
+ control_loops::superstructure::SuperstructureState::LOADED &&
+ superstructure_status_fetcher_->state() !=
+ control_loops::superstructure::SuperstructureState::SHOOTING) {
+ LOG(WARNING) << "No balls to shoot";
+ return true;
+ }
+
+ // Since we're loaded, there will atleast be 1 ball to shoot
+ int num_wanted = 1;
+
+ // If we have another ball, we will shoot 2
+ if (superstructure_status_fetcher_->front_intake_has_ball() ||
+ superstructure_status_fetcher_->back_intake_has_ball()) {
+ num_wanted++;
+ }
+
::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
ActorBase::kLoopOffset);
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index 461cfc1..0168630 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -50,9 +50,7 @@
void set_requested_intake(std::optional<RequestedIntake> requested_intake) {
requested_intake_ = requested_intake;
}
- void set_turret_goal(double turret_goal) {
- turret_goal_ = turret_goal;
- }
+ void set_turret_goal(double turret_goal) { turret_goal_ = turret_goal; }
void set_fire_at_will(bool fire) { fire_ = fire; }
void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
@@ -68,8 +66,8 @@
// Tells the superstructure the ball was preloaded and waits until it updates
// the state
[[nodiscard]] bool WaitForPreloaded();
- // Waits for a certain number of balls to be shot
- [[nodiscard]] bool WaitForBallsShot(int num_shot);
+ // Waits for the intaked balls to be shot
+ [[nodiscard]] bool WaitForBallsShot();
void SplineAuto();
void RapidReact();
diff --git a/y2022/control_loops/drivetrain/localizer.cc b/y2022/control_loops/drivetrain/localizer.cc
index 0735534..65df654 100644
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ b/y2022/control_loops/drivetrain/localizer.cc
@@ -14,6 +14,8 @@
localizer_output_fetcher_(
event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
"/localizer")),
+ joystick_state_fetcher_(
+ event_loop_->MakeFetcher<aos::JoystickState>("/aos")),
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")) {
@@ -41,6 +43,13 @@
double gyro_rate, const Eigen::Vector3d &accel) {
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate,
U.cast<float>(), accel.cast<float>(), now);
+ joystick_state_fetcher_.Fetch();
+ if (joystick_state_fetcher_.get() != nullptr &&
+ joystick_state_fetcher_->autonomous()) {
+ // TODO(james): This is an inelegant way to avoid having the localizer mess
+ // up splines. Do better.
+ return;
+ }
if (localizer_output_fetcher_.Fetch()) {
clock_offset_fetcher_.Fetch();
bool message_bridge_connected = true;
diff --git a/y2022/control_loops/drivetrain/localizer.h b/y2022/control_loops/drivetrain/localizer.h
index 4505e9b..0d2673b 100644
--- a/y2022/control_loops/drivetrain/localizer.h
+++ b/y2022/control_loops/drivetrain/localizer.h
@@ -8,6 +8,7 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2022/localizer/localizer_output_generated.h"
#include "aos/network/message_bridge_server_generated.h"
+#include "frc971/input/joystick_state_generated.h"
namespace y2022 {
namespace control_loops {
@@ -67,6 +68,7 @@
HybridEkf ekf_;
aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
// Target selector to allow us to satisfy the LocalizerInterface requirements.
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index 669e691..601be56 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -172,6 +172,7 @@
":superstructure_output_fbs",
":superstructure_status_fbs",
"//aos/events:event_loop",
+ "//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//frc971/control_loops:control_loop",
"//frc971/control_loops:control_loops_fbs",
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index e662f30..2093174 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -301,21 +301,21 @@
}
double u;
- size_t solution_horizon = 0;
+ size_t solution_number = 0;
if (current_controller_ == 0u) {
- while (solution_horizon < problems_[current_controller_]->horizon() &&
- problems_[current_controller_]->U(solution_horizon) < 0.01) {
- ++solution_horizon;
+ while (solution_number < problems_[current_controller_]->horizon() &&
+ problems_[current_controller_]->U(solution_number) < 0.01) {
+ u = problems_[current_controller_]->U(solution_number);
+ ++solution_number;
}
- } else {
- u = problems_[current_controller_]->U(0);
}
+ u = problems_[current_controller_]->U(solution_number);
- if (current_controller_ + 1u + solution_horizon < problems_.size()) {
- problems_[current_controller_ + solution_horizon + 1]->WarmStart(
+ if (current_controller_ + 1u + solution_number < problems_.size()) {
+ problems_[current_controller_ + solution_number + 1]->WarmStart(
*problems_[current_controller_]);
}
- ++current_controller_;
+ current_controller_ += 1u + solution_number;
return u;
}
diff --git a/y2022/control_loops/superstructure/led_indicator.cc b/y2022/control_loops/superstructure/led_indicator.cc
index 4ec934d..54b0b00 100644
--- a/y2022/control_loops/superstructure/led_indicator.cc
+++ b/y2022/control_loops/superstructure/led_indicator.cc
@@ -12,6 +12,9 @@
event_loop->MakeFetcher<Status>("/superstructure")),
server_statistics_fetcher_(
event_loop->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/roborio/aos")),
+ client_statistics_fetcher_(
+ event_loop->MakeFetcher<aos::message_bridge::ClientStatistics>(
"/roborio/aos")) {
led::CANdleConfiguration config;
config.statusLedOffWhenActive = true;
@@ -30,9 +33,20 @@
}
namespace {
-bool DisconnectedPi(const aos::message_bridge::ServerStatistics &server_stats) {
- for (const auto *pi_status : *server_stats.connections()) {
- if (pi_status->state() == aos::message_bridge::State::DISCONNECTED) {
+bool DisconnectedPiServer(
+ const aos::message_bridge::ServerStatistics &server_stats) {
+ for (const auto *pi_server_status : *server_stats.connections()) {
+ if (pi_server_status->state() == aos::message_bridge::State::DISCONNECTED) {
+ return true;
+ }
+ }
+ return false;
+}
+
+bool DisconnectedPiClient(
+ const aos::message_bridge::ClientStatistics &client_stats) {
+ for (const auto *pi_client_status : *client_stats.connections()) {
+ if (pi_client_status->state() == aos::message_bridge::State::DISCONNECTED) {
return true;
}
}
@@ -50,6 +64,7 @@
superstructure_status_fetcher_.Fetch();
server_statistics_fetcher_.Fetch();
drivetrain_output_fetcher_.Fetch();
+ client_statistics_fetcher_.Fetch();
// Estopped
if (superstructure_status_fetcher_.get() &&
@@ -66,12 +81,14 @@
}
// Pi disconnected
- if (server_statistics_fetcher_.get() &&
- DisconnectedPi(*server_statistics_fetcher_)) {
+ if ((server_statistics_fetcher_.get() &&
+ DisconnectedPiServer(*server_statistics_fetcher_)) ||
+ (client_statistics_fetcher_.get() &&
+ DisconnectedPiClient(*client_statistics_fetcher_))) {
if (disconnected_flash_) {
DisplayLed(255, 0, 0);
} else {
- DisplayLed(0, 0, 255);
+ DisplayLed(0, 255, 0);
}
if (disconnected_counter_ % kFlashIterations == 0) {
diff --git a/y2022/control_loops/superstructure/led_indicator.h b/y2022/control_loops/superstructure/led_indicator.h
index bdcee6c..680b875 100644
--- a/y2022/control_loops/superstructure/led_indicator.h
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -2,6 +2,7 @@
#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_client_generated.h"
#include "aos/network/message_bridge_server_generated.h"
#include "ctre/phoenix/led/CANdle.h"
#include "frc971/control_loops/control_loop.h"
@@ -21,7 +22,7 @@
//
// Red: estopped
// Yellow: not zeroed
- // Flash blue/red: pi disconnected
+ // Flash red/green: pi disconnected
// Purple: driving fast
//
// Statemachine:
@@ -52,6 +53,8 @@
aos::Fetcher<Status> superstructure_status_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics>
server_statistics_fetcher_;
+ aos::Fetcher<aos::message_bridge::ClientStatistics>
+ client_statistics_fetcher_;
size_t disconnected_counter_ = 0;
bool disconnected_flash_ = false;
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index ff3a181..24df19b 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -885,7 +885,7 @@
zeroer_(zeroing::ImuZeroer::FaultBehavior::kTemporary),
left_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()),
right_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()) {
- event_loop->SetRuntimeRealtimePriority(40);
+ event_loop->SetRuntimeRealtimePriority(10);
event_loop_->MakeWatcher(
"/drivetrain",
[this](
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index e342094..6e14bae 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -4,7 +4,17 @@
"name": "/roborio/aos",
"type": "aos.JoystickState",
"source_node": "roborio",
- "frequency": 75
+ "frequency": 75,
+ "logger_nodes" : [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 50000000
+ }
+ ]
},
{
"name": "/roborio/aos",
