Merge "Print message bandwidth in log_stats"
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 46a7542..077e87e 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -53,6 +53,8 @@
});
button_poll_ = event_loop->AddTimer([this]() {
+ const aos::monotonic_clock::time_point now =
+ this->event_loop()->context().monotonic_event_time;
if (robot_state_fetcher_.Fetch()) {
if (robot_state_fetcher_->user_button()) {
user_indicated_safe_to_reset_ = true;
@@ -63,14 +65,24 @@
if (joystick_state_fetcher_->has_alliance() &&
(joystick_state_fetcher_->alliance() != alliance_)) {
alliance_ = joystick_state_fetcher_->alliance();
- Replan();
+ is_planned_ = false;
+ // Only kick the planning out by 2 seconds. If we end up enabled in that
+ // second, then we will kick it out further based on the code below.
+ replan_timer_->Setup(now + std::chrono::seconds(2));
+ }
+ if (joystick_state_fetcher_->enabled()) {
+ if (!is_planned_) {
+ // Only replan once we've been disabled for 5 seconds.
+ replan_timer_->Setup(now + std::chrono::seconds(5));
+ }
}
}
});
}
void AutonomousActor::MaybeSendStartingPosition() {
- if (user_indicated_safe_to_reset_ && !sent_starting_position_) {
+ if (is_planned_ && user_indicated_safe_to_reset_ &&
+ !sent_starting_position_) {
CHECK(starting_position_);
SendStartingPosition(starting_position_.value());
}
@@ -107,6 +119,9 @@
} else {
starting_position_ = Eigen::Vector3d::Zero();
}
+
+ is_planned_ = true;
+
MaybeSendStartingPosition();
}
@@ -128,10 +143,11 @@
AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
SendStartingPosition(starting_position_.value());
}
-
- // Queue up a replan to occur as soon as this action completes.
- // TODO(james): Modify this so we don't replan during teleop.
- replan_timer_->Setup(monotonic_now());
+ // Clear this so that we don't accidentally resend things as soon as we replan
+ // later.
+ user_indicated_safe_to_reset_ = false;
+ is_planned_ = false;
+ starting_position_.reset();
AOS_LOG(INFO, "Params are %d\n", params->mode());
if (alliance_ == aos::Alliance::kInvalid) {
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 7cbabb0..7b60ca4 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -91,6 +91,8 @@
bool user_indicated_safe_to_reset_ = false;
bool sent_starting_position_ = false;
+ bool is_planned_ = false;
+
std::optional<Eigen::Vector3d> starting_position_;
};
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 17a2959..f582696 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -101,6 +101,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
+ "//y2020:constants",
":polydrivetrain_plants",
"//frc971:shifter_hall_effect",
"//frc971/control_loops/drivetrain:drivetrain_config",
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.cc b/y2020/control_loops/drivetrain/drivetrain_base.cc
index ff47197..9cd7494 100644
--- a/y2020/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_base.cc
@@ -2,8 +2,10 @@
#include <chrono>
+#include "aos/network/team_number.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2020/constants.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2020/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
#include "y2020/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
@@ -25,9 +27,8 @@
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
- // TODO(james): CHECK IF THIS IS IMU_X or IMU_FLIPPED_X.
- ::frc971::control_loops::drivetrain::IMUType::IMU_X,
+ ::frc971::control_loops::drivetrain::GyroType::IMU_X_GYRO,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_Z,
drivetrain::MakeDrivetrainLoop,
drivetrain::MakeVelocityDrivetrainLoop,
@@ -54,12 +55,24 @@
1.2 /* quickturn_wheel_multiplier */,
1.2 /* wheel_multiplier */,
true /*pistol_grip_shift_enables_line_follow*/,
- // TODO(james): Check X/Y axis transformations.
- (Eigen::Matrix<double, 3, 3>() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
- 1.0)
+ (Eigen::Matrix<double, 3, 3>() << 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0,
+ 0.0)
.finished() /*imu_transform*/,
};
+ if (::aos::network::GetTeamNumber() == constants::Values::kCompTeamNumber) {
+ // TODO(james): Check X/Y axis
+ // transformations.
+ kDrivetrainConfig.imu_transform = (Eigen::Matrix<double, 3, 3>() << 1.0,
+ 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)
+ .finished();
+ kDrivetrainConfig.gyro_type =
+ ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO;
+ // TODO(james): CHECK IF THIS IS IMU_X or IMU_FLIPPED_X.
+ kDrivetrainConfig.imu_type =
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X;
+ }
+
return kDrivetrainConfig;
};
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index ab600fa..a01f47d 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -1,5 +1,6 @@
#include "y2020/control_loops/superstructure/turret/aiming.h"
+#include "aos/network/team_number.h"
#include "frc971/control_loops/pose.h"
#include "gtest/gtest.h"
#include "y2020/constants.h"
@@ -15,6 +16,8 @@
class AimerTest : public ::testing::Test {
public:
+ AimerTest() { aos::network::OverrideTeamNumber(971); }
+
typedef Aimer::Goal Goal;
typedef Aimer::Status Status;
struct StatusData {