Merge "Test 2022 superstructure starting off collided"
diff --git a/aos/starter/BUILD b/aos/starter/BUILD
index 2186421..280a1c2 100644
--- a/aos/starter/BUILD
+++ b/aos/starter/BUILD
@@ -33,6 +33,7 @@
"//aos/events:event_loop",
"//aos/events:shm_event_loop",
"//aos/util:scoped_pipe",
+ "@boringssl//:crypto",
"@com_github_google_glog//:glog",
],
)
diff --git a/aos/starter/starter.fbs b/aos/starter/starter.fbs
index 4b66833..1fd990e 100644
--- a/aos/starter/starter.fbs
+++ b/aos/starter/starter.fbs
@@ -45,7 +45,10 @@
EXECV_ERR,
// Failed to change to the requested group
- SET_GRP_ERR
+ SET_GRP_ERR,
+
+ // Failed to either find the binary or open it for reading.
+ RESOLVE_ERR,
}
table Status {
@@ -73,6 +76,12 @@
// Indicates the reason the application is not running. Only valid if
// application is STOPPED.
last_stop_reason: LastStopReason (id: 6);
+
+ binary_sha256: string (id: 7);
+
+ // Resolved path to the binary executed. May be absolute or relative to the
+ // working directory of starter.
+ full_path: string (id: 8);
}
root_type Status;
diff --git a/aos/starter/subprocess.cc b/aos/starter/subprocess.cc
index c1eb618..76a22f2 100644
--- a/aos/starter/subprocess.cc
+++ b/aos/starter/subprocess.cc
@@ -6,7 +6,9 @@
#include <sys/types.h>
#include <sys/wait.h>
+#include "absl/strings/str_split.h"
#include "glog/logging.h"
+#include "openssl/sha.h"
namespace aos::starter {
@@ -92,6 +94,12 @@
start_timer_->Disable();
restart_timer_->Disable();
+ if (!UpdatePathAndChecksum()) {
+ stop_reason_ = aos::starter::LastStopReason::RESOLVE_ERR;
+ MaybeQueueRestart();
+ return;
+ }
+
status_pipes_ = util::ScopedPipe::MakePipe();
if (capture_stdout_) {
@@ -198,15 +206,15 @@
}
// argv[0] should be the program name
- args_.insert(args_.begin(), path_);
+ args_.insert(args_.begin(), full_path_);
std::vector<char *> cargs = CArgs();
- execvp(path_.c_str(), cargs.data());
+ execv(full_path_.c_str(), cargs.data());
// If we got here, something went wrong
status_pipes_.write->Write(
static_cast<uint32_t>(aos::starter::LastStopReason::EXECV_ERR));
- PLOG(WARNING) << "Could not execute " << name_ << " (" << path_ << ')';
+ PLOG(WARNING) << "Could not execute " << name_ << " (" << full_path_ << ')';
_exit(EXIT_FAILURE);
}
@@ -296,6 +304,15 @@
on_change_();
}
+void Application::MaybeQueueRestart() {
+ if (autorestart()) {
+ QueueStart();
+ } else {
+ status_ = aos::starter::State::STOPPED;
+ on_change_();
+ }
+}
+
std::vector<char *> Application::CArgs() {
std::vector<char *> cargs;
std::transform(args_.begin(), args_.end(), std::back_inserter(cargs),
@@ -345,10 +362,70 @@
}
}
+bool Application::UpdatePathAndChecksum() {
+ int fin = -1;
+ std::string test_path;
+ if (path_.find('/') != std::string::npos) {
+ test_path = path_;
+ fin = open(path_.c_str(), O_RDONLY);
+ } else {
+ char *path = secure_getenv("PATH");
+ for (std::string_view path_cmp : absl::StrSplit(path, ':')) {
+ test_path = absl::StrCat(path_cmp, "/", path_);
+ fin = open(test_path.c_str(), O_RDONLY);
+ if (fin != -1) break;
+ }
+ }
+ if (fin == -1) {
+ PLOG(WARNING) << "Failed to open binary '" << path_ << "' as file";
+ return false;
+ }
+
+ full_path_ = std::move(test_path);
+
+ // Hash iteratively to avoid reading the entire binary into memory
+ constexpr std::size_t kReadSize = 1024 * 16;
+
+ SHA256_CTX ctx;
+ CHECK_EQ(SHA256_Init(&ctx), 1);
+
+ std::array<uint8_t, kReadSize> buf;
+
+ while (true) {
+ const ssize_t result = read(fin, buf.data(), kReadSize);
+ PCHECK(result != -1);
+ if (result == 0) {
+ break;
+ } else {
+ CHECK_EQ(SHA256_Update(&ctx, buf.data(), result), 1);
+ }
+ }
+ PCHECK(close(fin) == 0);
+
+ std::array<uint8_t, SHA256_DIGEST_LENGTH> hash_buf;
+ CHECK_EQ(SHA256_Final(hash_buf.data(), &ctx), 1);
+
+ static constexpr std::string_view kHexTable = "0123456789abcdef";
+
+ static_assert(hash_buf.size() * 2 == kSha256HexStrSize);
+ for (std::size_t i = 0; i < hash_buf.size(); ++i) {
+ checksum_[i * 2] = kHexTable[(hash_buf[i] & 0xF0U) >> 4U];
+ checksum_[i * 2 + 1] = kHexTable[hash_buf[i] & 0x0FU];
+ }
+
+ return true;
+}
+
flatbuffers::Offset<aos::starter::ApplicationStatus>
Application::PopulateStatus(flatbuffers::FlatBufferBuilder *builder) {
CHECK_NOTNULL(builder);
auto name_fbs = builder->CreateString(name_);
+ auto full_path_fbs = builder->CreateString(full_path_);
+ flatbuffers::Offset<flatbuffers::String> binary_sha256_fbs;
+ if (pid_ != -1) {
+ binary_sha256_fbs =
+ builder->CreateString(checksum_.data(), checksum_.size());
+ }
aos::starter::ApplicationStatus::Builder status_builder(*builder);
status_builder.add_name(name_fbs);
@@ -360,6 +437,8 @@
if (pid_ != -1) {
status_builder.add_pid(pid_);
status_builder.add_id(id_);
+ status_builder.add_binary_sha256(binary_sha256_fbs);
+ status_builder.add_full_path(full_path_fbs);
}
status_builder.add_last_start_time(start_time_.time_since_epoch().count());
return status_builder.Finish();
@@ -441,12 +520,7 @@
LOG(WARNING) << "Failed to start '" << name_ << "' on pid " << pid_
<< " : Exited with status " << exit_code_.value();
}
- if (autorestart()) {
- QueueStart();
- } else {
- status_ = aos::starter::State::STOPPED;
- on_change_();
- }
+ MaybeQueueRestart();
break;
}
case aos::starter::State::RUNNING: {
@@ -458,12 +532,7 @@
<< " exited unexpectedly with status "
<< exit_code_.value();
}
- if (autorestart()) {
- QueueStart();
- } else {
- status_ = aos::starter::State::STOPPED;
- on_change_();
- }
+ MaybeQueueRestart();
break;
}
case aos::starter::State::STOPPING: {
diff --git a/aos/starter/subprocess.h b/aos/starter/subprocess.h
index 9ee9e31..ed4d0dd 100644
--- a/aos/starter/subprocess.h
+++ b/aos/starter/subprocess.h
@@ -75,6 +75,8 @@
bool autorestart() const { return autorestart_; }
+ std::string_view full_path() const { return full_path_; }
+
const std::string &GetStdout();
const std::string &GetStderr();
std::optional<int> exit_code() const { return exit_code_; }
@@ -91,6 +93,9 @@
void QueueStart();
+ // Queues start if autorestart set, otherwise moves state to stopped.
+ void MaybeQueueRestart();
+
// Copy flatbuffer vector of strings to vector of std::string.
static std::vector<std::string> FbsVectorToVector(
const flatbuffers::Vector<flatbuffers::Offset<flatbuffers::String>> &v);
@@ -106,11 +111,21 @@
// call).
std::vector<char *> CArgs();
+ // Resolves the path to the binary from the PATH environment variable. On
+ // success, updates full_path_ to the absolute path to the binary and the
+ // checksum_ to the hex-encoded sha256 hash of the file; returns true. On
+ // failure, returns false.
+ bool UpdatePathAndChecksum();
+
// Next unique id for all applications
static inline uint64_t next_id_ = 0;
+ static constexpr size_t kSha256HexStrSize = 256 / CHAR_BIT * 2;
+
std::string name_;
std::string path_;
+ std::string full_path_;
+ std::array<char, kSha256HexStrSize> checksum_{"DEADBEEF"};
std::vector<std::string> args_;
std::string user_name_;
std::optional<uid_t> user_;
diff --git a/aos/starter/subprocess_test.cc b/aos/starter/subprocess_test.cc
index 93fbf6a..ede39f8 100644
--- a/aos/starter/subprocess_test.cc
+++ b/aos/starter/subprocess_test.cc
@@ -49,6 +49,9 @@
event_loop.Run();
+ EXPECT_TRUE(echo_stdout.full_path() == "/bin/echo" ||
+ echo_stdout.full_path() == "/usr/bin/echo")
+ << echo_stdout.full_path();
ASSERT_EQ("abcdef\n", echo_stdout.GetStdout());
ASSERT_TRUE(echo_stdout.GetStderr().empty());
EXPECT_TRUE(observed_stopped);
diff --git a/frc971/input/joystick_state.fbs b/frc971/input/joystick_state.fbs
index 2f48bdf..49d2690 100644
--- a/frc971/input/joystick_state.fbs
+++ b/frc971/input/joystick_state.fbs
@@ -1,5 +1,7 @@
namespace aos;
+enum MatchType : byte { kNone, kPractice, kQualification, kElimination }
+
table Joystick {
// A bitmask of the butotn state.
buttons:ushort (id: 0);
@@ -44,6 +46,14 @@
// String corresponding to the game data string
game_data:string (id: 10);
+
+ // Driver station location.
+ location:ubyte (id: 11);
+
+ match_number:int (id: 12);
+ replay_number:int (id: 13);
+ match_type:MatchType (id: 14);
+ event_name:string (id: 15);
}
root_type JoystickState;
diff --git a/frc971/wpilib/ahal/DriverStation.cc b/frc971/wpilib/ahal/DriverStation.cc
index 2ed0a97..8b31af7 100644
--- a/frc971/wpilib/ahal/DriverStation.cc
+++ b/frc971/wpilib/ahal/DriverStation.cc
@@ -272,6 +272,54 @@
}
/**
+ * Returns the game specific message provided by the FMS.
+ *
+ * @return A string containing the game specific message.
+ */
+std::string_view DriverStation::GetGameSpecificMessage() const {
+ return std::string_view(
+ reinterpret_cast<const char *>(info_.gameSpecificMessage),
+ info_.gameSpecificMessageSize);
+}
+
+/**
+ * Returns the name of the competition event provided by the FMS.
+ *
+ * @return A string containing the event name
+ */
+std::string_view DriverStation::GetEventName() const {
+ return info_.eventName;
+}
+
+/**
+ * Returns the match number provided by the FMS.
+ *
+ * @return The number of the match
+ */
+DriverStation::MatchType DriverStation::GetMatchType() const {
+ return static_cast<DriverStation::MatchType>(info_.matchType);
+}
+
+/**
+ * Returns the match number provided by the FMS.
+ *
+ * @return The number of the match
+ */
+int DriverStation::GetMatchNumber() const {
+ return info_.matchNumber;
+}
+
+/**
+ * Returns the number of times the current match has been replayed from the
+ * FMS.
+ *
+ * @return The number of replays
+ */
+int DriverStation::GetReplayNumber() const {
+ return info_.replayNumber;
+}
+
+/**
* Return the alliance that the driver station says it is on.
*
* This could return kRed or kBlue.
@@ -374,10 +422,14 @@
HAL_ControlWord control_word;
HAL_GetControlWord(&control_word);
- is_enabled_ = control_word.enabled;
+ is_enabled_ = control_word.enabled && control_word.dsAttached;
is_autonomous_ = control_word.autonomous;
is_test_mode_ = control_word.test;
is_fms_attached_ = control_word.fmsAttached;
+ is_ds_attached_ = control_word.dsAttached;
+ is_teleop_ = !(control_word.autonomous || control_word.test);
+
+ HAL_GetMatchInfo(&info_);
}
/**
diff --git a/frc971/wpilib/ahal/DriverStation.h b/frc971/wpilib/ahal/DriverStation.h
index 5150360..8bcdcf1 100644
--- a/frc971/wpilib/ahal/DriverStation.h
+++ b/frc971/wpilib/ahal/DriverStation.h
@@ -27,6 +27,7 @@
class DriverStation {
public:
enum Alliance { kRed, kBlue, kInvalid };
+ enum MatchType { kNone, kPractice, kQualification, kElimination };
virtual ~DriverStation();
static DriverStation &GetInstance();
@@ -55,10 +56,18 @@
bool IsTestMode() const { return is_test_mode_; }
bool IsFmsAttached() const { return is_fms_attached_; }
bool IsAutonomous() const { return is_autonomous_; }
+ bool IsTeleop() const { return is_teleop_; }
+ bool IsDSAttached() const { return is_ds_attached_; }
bool IsSysActive() const;
bool IsBrownedOut() const;
+ std::string_view GetGameSpecificMessage() const;
+
+ std::string_view GetEventName() const;
+ MatchType GetMatchType() const;
+ int GetMatchNumber() const;
+ int GetReplayNumber() const;
Alliance GetAlliance() const;
int GetLocation() const;
double GetMatchTime() const;
@@ -88,6 +97,11 @@
bool is_test_mode_ = false;
bool is_autonomous_ = false;
bool is_fms_attached_ = false;
+ bool is_teleop_ = false;
+ bool is_ds_attached_ = false;
+
+ // statically allocated match info so we can return string_views into it.
+ HAL_MatchInfo info_;
};
} // namespace frc
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index fab3d05..9137a99 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -30,9 +30,6 @@
ds->RunIteration([&]() {
auto builder = joystick_state_sender_.MakeBuilder();
- HAL_MatchInfo match_info;
- auto status = HAL_GetMatchInfo(&match_info);
-
std::array<flatbuffers::Offset<Joystick>,
frc971::input::driver_station::JoystickFeature::kJoysticks>
joysticks;
@@ -67,32 +64,26 @@
joysticks_offset = builder.fbb()->CreateVector(joysticks.begin(),
joysticks.size());
- flatbuffers::Offset<flatbuffers::String> game_data_offset;
- if (status == 0) {
- static_assert(sizeof(match_info.gameSpecificMessage) == 64,
- "Check that the match info game specific message size "
- "hasn't changed and is still sane.");
- CHECK_LE(match_info.gameSpecificMessageSize,
- sizeof(match_info.gameSpecificMessage));
- game_data_offset = builder.fbb()->CreateString(
- reinterpret_cast<const char *>(match_info.gameSpecificMessage),
- match_info.gameSpecificMessageSize);
- }
+ flatbuffers::Offset<flatbuffers::String> game_data_offset =
+ builder.fbb()->CreateString(ds->GetGameSpecificMessage());
+
+ flatbuffers::Offset<flatbuffers::String> event_name_offset =
+ builder.fbb()->CreateString(ds->GetEventName());
aos::JoystickState::Builder joystick_state_builder =
builder.MakeBuilder<aos::JoystickState>();
joystick_state_builder.add_joysticks(joysticks_offset);
- if (status == 0) {
+ if (ds->GetGameSpecificMessage().size() >= 2u) {
joystick_state_builder.add_switch_left(
- match_info.gameSpecificMessage[0] == 'L' ||
- match_info.gameSpecificMessage[0] == 'l');
+ ds->GetGameSpecificMessage()[0] == 'L' ||
+ ds->GetGameSpecificMessage()[0] == 'l');
joystick_state_builder.add_scale_left(
- match_info.gameSpecificMessage[1] == 'L' ||
- match_info.gameSpecificMessage[1] == 'l');
- joystick_state_builder.add_game_data(game_data_offset);
+ ds->GetGameSpecificMessage()[1] == 'L' ||
+ ds->GetGameSpecificMessage()[1] == 'l');
}
+ joystick_state_builder.add_game_data(game_data_offset);
joystick_state_builder.add_test_mode(ds->IsTestMode());
joystick_state_builder.add_fms_attached(ds->IsFmsAttached());
@@ -109,7 +100,28 @@
joystick_state_builder.add_alliance(aos::Alliance::kInvalid);
break;
}
+ joystick_state_builder.add_location(ds->GetLocation());
+
joystick_state_builder.add_team_id(team_id_);
+ joystick_state_builder.add_match_number(ds->GetMatchNumber());
+ joystick_state_builder.add_replay_number(ds->GetReplayNumber());
+
+ switch (ds->GetMatchType()) {
+ case frc::DriverStation::kNone:
+ joystick_state_builder.add_match_type(aos::MatchType::kNone);
+ break;
+ case frc::DriverStation::kPractice:
+ joystick_state_builder.add_match_type(aos::MatchType::kPractice);
+ break;
+ case frc::DriverStation::kQualification:
+ joystick_state_builder.add_match_type(
+ aos::MatchType::kQualification);
+ break;
+ case frc::DriverStation::kElimination:
+ joystick_state_builder.add_match_type(aos::MatchType::kElimination);
+ break;
+ }
+ joystick_state_builder.add_event_name(event_name_offset);
if (builder.Send(joystick_state_builder.Finish()) !=
aos::RawSender::Error::kOk) {
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 0ed59e2..3ad292d 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -5,8 +5,8 @@
namespace y2020 {
namespace actors {
-constexpr double kFieldLength = 15.983;
-constexpr double kFieldWidth = 8.212;
+constexpr double kFieldLength = 16.4592;
+constexpr double kFieldWidth = 8.2296;
void MaybeFlipSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
diff --git a/y2022/BUILD b/y2022/BUILD
index 1623ec9..488026e 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -14,6 +14,7 @@
"@ctre_phoenix_cci_athena//:shared_libraries",
],
dirs = [
+ "//y2022/actors:splines",
"//y2022/www:www_files",
],
start_binaries = [
diff --git a/y2022/actors/BUILD b/y2022/actors/BUILD
index 57b1222..3c8c6b8 100644
--- a/y2022/actors/BUILD
+++ b/y2022/actors/BUILD
@@ -1,3 +1,5 @@
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
filegroup(
name = "binaries.stripped",
srcs = [
@@ -14,6 +16,24 @@
visibility = ["//visibility:public"],
)
+filegroup(
+ name = "spline_jsons",
+ srcs = glob([
+ "splines/*.json",
+ ]),
+ visibility = ["//visibility:public"],
+)
+
+aos_downloader_dir(
+ name = "splines",
+ srcs = [
+ ":spline_jsons",
+ ],
+ dir = "splines",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
cc_library(
name = "autonomous_action_lib",
srcs = [
@@ -33,6 +53,7 @@
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
"//frc971/control_loops/drivetrain:localizer_fbs",
+ "//y2022:constants",
"//y2022/control_loops/drivetrain:drivetrain_base",
"//y2022/control_loops/superstructure:superstructure_goal_fbs",
"//y2022/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2022/actors/auto_splines.cc b/y2022/actors/auto_splines.cc
index ce206c8..0c98fed 100644
--- a/y2022/actors/auto_splines.cc
+++ b/y2022/actors/auto_splines.cc
@@ -19,84 +19,35 @@
}
}
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
- flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
- flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
- flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+flatbuffers::Offset<frc971::MultiSpline> FixSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ flatbuffers::Offset<frc971::MultiSpline> spline_offset,
+ aos::Alliance alliance) {
+ frc971::MultiSpline *spline =
+ GetMutableTemporaryPointer(*builder->fbb(), spline_offset);
+ flatbuffers::Vector<float> *spline_x = spline->mutable_spline_x();
+ flatbuffers::Vector<float> *spline_y = spline->mutable_spline_y();
- {
- frc971::Constraint::Builder longitudinal_constraint_builder =
- builder->MakeBuilder<frc971::Constraint>();
- longitudinal_constraint_builder.add_constraint_type(
- frc971::ConstraintType::LONGITUDINAL_ACCELERATION);
- longitudinal_constraint_builder.add_value(1.0);
- longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ if (alliance == aos::Alliance::kBlue) {
+ for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+ spline_x->Mutate(ii, -spline_x->Get(ii));
+ }
+ for (size_t ii = 0; ii < spline_y->size(); ++ii) {
+ spline_y->Mutate(ii, -spline_y->Get(ii));
+ }
}
-
- {
- frc971::Constraint::Builder lateral_constraint_builder =
- builder->MakeBuilder<frc971::Constraint>();
- lateral_constraint_builder.add_constraint_type(
- frc971::ConstraintType::LATERAL_ACCELERATION);
- lateral_constraint_builder.add_value(1.0);
- lateral_constraint_offset = lateral_constraint_builder.Finish();
- }
-
- {
- frc971::Constraint::Builder voltage_constraint_builder =
- builder->MakeBuilder<frc971::Constraint>();
- voltage_constraint_builder.add_constraint_type(
- frc971::ConstraintType::VOLTAGE);
- voltage_constraint_builder.add_value(6.0);
- voltage_constraint_offset = voltage_constraint_builder.Finish();
- }
-
- flatbuffers::Offset<
- flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
- constraints_offset =
- builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
- {longitudinal_constraint_offset, lateral_constraint_offset,
- voltage_constraint_offset});
-
- const float startx = 0.4;
- const float starty = 3.4;
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
- builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
- 0.6f + startx, 0.4f + startx,
- 0.4f + startx, 1.0f + startx});
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
- builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
- starty - 0.3f, starty - 0.7f,
- starty - 1.0f, starty - 1.0f});
-
- frc971::MultiSpline::Builder multispline_builder =
- builder->MakeBuilder<frc971::MultiSpline>();
-
- multispline_builder.add_spline_count(1);
- multispline_builder.add_constraints(constraints_offset);
- multispline_builder.add_spline_x(spline_x_offset);
- multispline_builder.add_spline_y(spline_y_offset);
-
- return multispline_builder.Finish();
+ return spline_offset;
}
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
- builder->fbb()->CreateVector<float>(
- {-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
- builder->fbb()->CreateVector<float>({1.25, 1.25, 1.25, 1.25, 1.25, 1.25});
-
- frc971::MultiSpline::Builder multispline_builder =
- builder->MakeBuilder<frc971::MultiSpline>();
-
- multispline_builder.add_spline_count(1);
- multispline_builder.add_spline_x(spline_x_offset);
- multispline_builder.add_spline_y(spline_y_offset);
-
- return multispline_builder.Finish();
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_, builder->fbb()),
+ alliance);
}
} // namespace actors
diff --git a/y2022/actors/auto_splines.h b/y2022/actors/auto_splines.h
index 70c3569..9da6bc6 100644
--- a/y2022/actors/auto_splines.h
+++ b/y2022/actors/auto_splines.h
@@ -2,8 +2,10 @@
#define Y2022_ACTORS_AUTO_SPLINES_H_
#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/input/joystick_state_generated.h"
/*
The cooridinate system for the autonomous splines is the same as the spline
@@ -16,10 +18,22 @@
class AutonomousSplines {
public:
+ AutonomousSplines()
+ : test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/test_spline.json")) {}
+
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+
+ flatbuffers::Offset<frc971::MultiSpline> TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
+ private:
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
};
} // namespace actors
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index d96dd03..ed1a9ee 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -5,33 +5,253 @@
#include <cmath>
#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
+#include "aos/util/math.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2022/actors/auto_splines.h"
+#include "y2022/constants.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
+
namespace y2022 {
namespace actors {
+namespace {
+constexpr double kExtendIntakeGoal = 0.0;
+constexpr double kRetractIntakeGoal = 1.47;
+constexpr double kRollerVoltage = 12.0;
+constexpr double kCatapultReturnPosition = -0.908;
+} // namespace
using ::aos::monotonic_clock;
+using frc971::CreateProfileParameters;
using ::frc971::ProfileParametersT;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::control_loops::drivetrain::LocalizerControl;
+
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
: frc971::autonomous::BaseAutonomousActor(
- event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+ localizer_control_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::drivetrain::LocalizerControl>(
+ "/drivetrain")),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<control_loops::superstructure::Goal>(
+ "/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<control_loops::superstructure::Status>(
+ "/superstructure")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
+ auto_splines_() {
+ set_max_drivetrain_voltage(12.0);
+ replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
+ event_loop->OnRun([this, event_loop]() {
+ replan_timer_->Setup(event_loop->monotonic_now());
+ button_poll_->Setup(event_loop->monotonic_now(), chrono::milliseconds(50));
+ });
+
+ 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;
+ MaybeSendStartingPosition();
+ }
+ }
+ if (joystick_state_fetcher_.Fetch()) {
+ if (joystick_state_fetcher_->has_alliance() &&
+ (joystick_state_fetcher_->alliance() != alliance_)) {
+ alliance_ = joystick_state_fetcher_->alliance();
+ 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::Replan() {
+ LOG(INFO) << "Alliance " << static_cast<int>(alliance_);
+ if (alliance_ == aos::Alliance::kInvalid) {
+ return;
+ }
+ sent_starting_position_ = false;
+ if (FLAGS_spline_auto) {
+ test_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward);
+
+ starting_position_ = test_spline_->starting_position();
+ }
+
+ is_planned_ = true;
+
+ MaybeSendStartingPosition();
+}
+
+void AutonomousActor::MaybeSendStartingPosition() {
+ if (is_planned_ && user_indicated_safe_to_reset_ &&
+ !sent_starting_position_) {
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+}
void AutonomousActor::Reset() {
InitializeEncoders();
ResetDrivetrain();
+ RetractFrontIntake();
+ RetractBackIntake();
+
+ joystick_state_fetcher_.Fetch();
+ CHECK(joystick_state_fetcher_.get() != nullptr)
+ << "Expect at least one JoystickState message before running auto...";
+ alliance_ = joystick_state_fetcher_->alliance();
}
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams *params) {
Reset();
+ if (!user_indicated_safe_to_reset_) {
+ AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+ // 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) {
+ AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
+ return false;
+ }
+ if (FLAGS_spline_auto) {
+ SplineAuto();
+ }
+
return true;
}
+void AutonomousActor::SendStartingPosition(const Eigen::Vector3d &start) {
+ // Set up the starting position for the blue alliance.
+
+ // TODO(james): Resetting the localizer breaks the left/right statespace
+ // controller. That is a bug, but we can fix that later by not resetting.
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(start(0));
+ localizer_control_builder.add_y(start(1));
+ localizer_control_builder.add_theta(start(2));
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ LOG(INFO) << "User button pressed, x: " << start(0) << " y: " << start(1)
+ << " theta: " << start(2);
+ if (builder.Send(localizer_control_builder.Finish()) !=
+ aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
+ }
+}
+
+void AutonomousActor::SplineAuto() {
+ CHECK(test_spline_);
+
+ if (!test_spline_->WaitForPlan()) return;
+ test_spline_->Start();
+
+ if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::SendSuperstructureGoal() {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_front_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), intake_front_goal_,
+ CreateProfileParameters(*builder.fbb(), 20.0, 60.0));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_back_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), intake_back_goal_,
+ CreateProfileParameters(*builder.fbb(), 20.0, 60.0));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ catapult_return_position_offset =
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), kCatapultReturnPosition,
+ CreateProfileParameters(*builder.fbb(), 9.0, 50.0));
+
+ superstructure::CatapultGoal::Builder catapult_goal_builder(*builder.fbb());
+ catapult_goal_builder.add_shot_position(0.03);
+ catapult_goal_builder.add_shot_velocity(18.0);
+ catapult_goal_builder.add_return_position(catapult_return_position_offset);
+ flatbuffers::Offset<superstructure::CatapultGoal> catapult_goal_offset =
+ catapult_goal_builder.Finish();
+
+ superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_builder.add_intake_front(intake_front_offset);
+ superstructure_builder.add_intake_back(intake_back_offset);
+ superstructure_builder.add_roller_speed_compensation(1.5);
+ superstructure_builder.add_roller_speed_front(roller_front_voltage_);
+ superstructure_builder.add_roller_speed_back(roller_back_voltage_);
+ superstructure_builder.add_transfer_roller_speed(transfer_roller_voltage_);
+ superstructure_builder.add_catapult(catapult_goal_offset);
+ superstructure_builder.add_fire(fire_);
+ superstructure_builder.add_auto_aim(true);
+
+ if (builder.Send(superstructure_builder.Finish()) !=
+ aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+}
+
+void AutonomousActor::ExtendFrontIntake() {
+ set_intake_front_goal(kExtendIntakeGoal);
+ set_roller_front_voltage(kRollerVoltage);
+ set_transfer_roller_voltage(kRollerVoltage);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::RetractFrontIntake() {
+ set_intake_front_goal(kRetractIntakeGoal);
+ set_roller_front_voltage(kRollerVoltage);
+ set_transfer_roller_voltage(0.0);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::ExtendBackIntake() {
+ set_intake_back_goal(kExtendIntakeGoal);
+ set_roller_back_voltage(kRollerVoltage);
+ set_transfer_roller_voltage(-kRollerVoltage);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::RetractBackIntake() {
+ set_intake_back_goal(kRetractIntakeGoal);
+ set_roller_back_voltage(kRollerVoltage);
+ set_transfer_roller_voltage(0.0);
+ SendSuperstructureGoal();
+}
+
} // namespace actors
} // namespace y2022
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index ca13d38..f01da02 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -6,10 +6,18 @@
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2022/actors/auto_splines.h"
+#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
namespace y2022 {
namespace actors {
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+namespace superstructure = y2022::control_loops::superstructure;
+
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
explicit AutonomousActor(::aos::EventLoop *event_loop);
@@ -19,6 +27,66 @@
private:
void Reset();
+
+ void set_intake_front_goal(double intake_front_goal) {
+ intake_front_goal_ = intake_front_goal;
+ }
+ void set_intake_back_goal(double intake_back_goal) {
+ intake_back_goal_ = intake_back_goal;
+ }
+ void set_roller_front_voltage(double roller_front_voltage) {
+ roller_front_voltage_ = roller_front_voltage;
+ }
+ void set_roller_back_voltage(double roller_back_voltage) {
+ roller_back_voltage_ = roller_back_voltage;
+ }
+ void set_transfer_roller_voltage(double voltage) {
+ transfer_roller_voltage_ = voltage;
+ }
+
+ void set_fire_at_will(bool fire) { fire_ = fire; }
+
+ void SendSuperstructureGoal();
+ void ExtendFrontIntake();
+ void RetractFrontIntake();
+ void ExtendBackIntake();
+ void RetractBackIntake();
+ void SendStartingPosition(const Eigen::Vector3d &start);
+ void MaybeSendStartingPosition();
+
+ void SplineAuto();
+
+ void Replan();
+
+ double intake_front_goal_ = 0.0;
+ double intake_back_goal_ = 0.0;
+ double roller_front_voltage_ = 0.0;
+ double roller_back_voltage_ = 0.0;
+ double transfer_roller_voltage_ = 0.0;
+ bool fire_ = false;
+
+ aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
+ localizer_control_sender_;
+ aos::Sender<y2022::control_loops::superstructure::Goal>
+ superstructure_goal_sender_;
+ aos::Fetcher<y2022::control_loops::superstructure::Status>
+ superstructure_status_fetcher_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ aos::Fetcher<aos::RobotState> robot_state_fetcher_;
+
+ aos::TimerHandler *replan_timer_;
+ aos::TimerHandler *button_poll_;
+
+ std::optional<SplineHandle> test_spline_;
+
+ aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ AutonomousSplines auto_splines_;
+ bool user_indicated_safe_to_reset_ = false;
+ bool sent_starting_position_ = false;
+
+ bool is_planned_ = false;
+
+ std::optional<Eigen::Vector3d> starting_position_;
};
} // namespace actors
diff --git a/y2022/actors/splines/README.md b/y2022/actors/splines/README.md
new file mode 100644
index 0000000..c655416
--- /dev/null
+++ b/y2022/actors/splines/README.md
@@ -0,0 +1,3 @@
+# Spline Descriptions
+This folder contains reference material for what each spline does
+
diff --git a/y2022/actors/splines/test_spline.json b/y2022/actors/splines/test_spline.json
new file mode 100644
index 0000000..733d516
--- /dev/null
+++ b/y2022/actors/splines/test_spline.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index 2d5ae63..7bbd1f3 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -13,7 +13,7 @@
DEFINE_uint64(red_delta, 100,
"Required difference between green pixels vs. red");
-DEFINE_uint64(blue_delta, 50,
+DEFINE_uint64(blue_delta, 30,
"Required difference between green pixels vs. blue");
DEFINE_bool(use_outdoors, false,