Merge "5 ball auto for SFR"
diff --git a/BUILD b/BUILD
index 0d79e19..e2d5d63 100644
--- a/BUILD
+++ b/BUILD
@@ -15,10 +15,14 @@
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/error_response //scouting/webserver/requests/messages:error_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting //scouting/webserver/requests/messages:submit_data_scouting_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting_response //scouting/webserver/requests/messages:submit_data_scouting_response_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_notes //scouting/webserver/requests/messages:submit_notes_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_notes_response //scouting/webserver/requests/messages:submit_notes_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting_response //scouting/webserver/requests/messages:request_data_scouting_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting //scouting/webserver/requests/messages:request_data_scouting_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team_response //scouting/webserver/requests/messages:request_matches_for_team_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team //scouting/webserver/requests/messages:request_matches_for_team_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team_response //scouting/webserver/requests/messages:request_notes_for_team_response_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team //scouting/webserver/requests/messages:request_notes_for_team_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_matches_response //scouting/webserver/requests/messages:request_all_matches_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_matches //scouting/webserver/requests/messages:request_all_matches_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/refresh_match_list //scouting/webserver/requests/messages:refresh_match_list_go_fbs
diff --git a/scouting/webserver/requests/BUILD b/scouting/webserver/requests/BUILD
index df487f2..744772c 100644
--- a/scouting/webserver/requests/BUILD
+++ b/scouting/webserver/requests/BUILD
@@ -18,8 +18,12 @@
"//scouting/webserver/requests/messages:request_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:request_matches_for_team_go_fbs",
"//scouting/webserver/requests/messages:request_matches_for_team_response_go_fbs",
+ "//scouting/webserver/requests/messages:request_notes_for_team_go_fbs",
+ "//scouting/webserver/requests/messages:request_notes_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_response_go_fbs",
+ "//scouting/webserver/requests/messages:submit_notes_go_fbs",
+ "//scouting/webserver/requests/messages:submit_notes_response_go_fbs",
"//scouting/webserver/server",
"@com_github_google_flatbuffers//go:go_default_library",
],
@@ -43,8 +47,10 @@
"//scouting/webserver/requests/messages:request_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:request_matches_for_team_go_fbs",
"//scouting/webserver/requests/messages:request_matches_for_team_response_go_fbs",
+ "//scouting/webserver/requests/messages:request_notes_for_team_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_response_go_fbs",
+ "//scouting/webserver/requests/messages:submit_notes_go_fbs",
"//scouting/webserver/server",
"@com_github_google_flatbuffers//go:go_default_library",
],
diff --git a/scouting/webserver/requests/debug/BUILD b/scouting/webserver/requests/debug/BUILD
index 402503f..e5f5234 100644
--- a/scouting/webserver/requests/debug/BUILD
+++ b/scouting/webserver/requests/debug/BUILD
@@ -12,6 +12,8 @@
"//scouting/webserver/requests/messages:request_all_matches_response_go_fbs",
"//scouting/webserver/requests/messages:request_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:request_matches_for_team_response_go_fbs",
+ "//scouting/webserver/requests/messages:request_notes_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_response_go_fbs",
+ "//scouting/webserver/requests/messages:submit_notes_response_go_fbs",
],
)
diff --git a/scouting/webserver/requests/debug/debug.go b/scouting/webserver/requests/debug/debug.go
index 81be3d1..f005f0d 100644
--- a/scouting/webserver/requests/debug/debug.go
+++ b/scouting/webserver/requests/debug/debug.go
@@ -13,7 +13,9 @@
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_matches_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team_response"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting_response"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_notes_response"
)
// Use aliases to make the rest of the code more readable.
@@ -141,3 +143,23 @@
response := refresh_match_list_response.GetRootAsRefreshMatchListResponse(responseBytes, 0)
return response.UnPack(), nil
}
+
+func SubmitNotes(server string, requestBytes []byte) (*submit_notes_response.SubmitNotesResponseT, error) {
+ responseBytes, err := performPost(server+"/requests/submit/submit_notes", requestBytes)
+ if err != nil {
+ return nil, err
+ }
+
+ response := submit_notes_response.GetRootAsSubmitNotesResponse(responseBytes, 0)
+ return response.UnPack(), nil
+}
+
+func RequestNotes(server string, requestBytes []byte) (*request_notes_for_team_response.RequestNotesForTeamResponseT, error) {
+ responseBytes, err := performPost(server+"/requests/request/notes_for_team", requestBytes)
+ if err != nil {
+ return nil, err
+ }
+
+ response := request_notes_for_team_response.GetRootAsRequestNotesForTeamResponse(responseBytes, 0)
+ return response.UnPack(), nil
+}
diff --git a/scouting/webserver/requests/messages/BUILD b/scouting/webserver/requests/messages/BUILD
index c27f730..f7e194b 100644
--- a/scouting/webserver/requests/messages/BUILD
+++ b/scouting/webserver/requests/messages/BUILD
@@ -12,6 +12,10 @@
"request_data_scouting_response",
"refresh_match_list",
"refresh_match_list_response",
+ "submit_notes",
+ "submit_notes_response",
+ "request_notes_for_team",
+ "request_notes_for_team_response",
)
filegroup(
diff --git a/scouting/webserver/requests/messages/request_notes_for_team.fbs b/scouting/webserver/requests/messages/request_notes_for_team.fbs
new file mode 100644
index 0000000..9bda6d3
--- /dev/null
+++ b/scouting/webserver/requests/messages/request_notes_for_team.fbs
@@ -0,0 +1,8 @@
+namespace scouting.webserver.requests;
+
+table RequestNotesForTeam {
+ team:int (id: 0);
+}
+
+root_type RequestNotesForTeam;
+
diff --git a/scouting/webserver/requests/messages/request_notes_for_team_response.fbs b/scouting/webserver/requests/messages/request_notes_for_team_response.fbs
new file mode 100644
index 0000000..5c73098
--- /dev/null
+++ b/scouting/webserver/requests/messages/request_notes_for_team_response.fbs
@@ -0,0 +1,13 @@
+namespace scouting.webserver.requests;
+
+// A repeated table is better when we expect each index to have various data points
+table Note {
+ data:string (id: 0);
+}
+
+table RequestNotesForTeamResponse {
+ notes:[Note] (id: 0);
+}
+
+root_type RequestNotesForTeamResponse;
+
diff --git a/scouting/webserver/requests/messages/submit_notes.fbs b/scouting/webserver/requests/messages/submit_notes.fbs
new file mode 100644
index 0000000..cf111b3
--- /dev/null
+++ b/scouting/webserver/requests/messages/submit_notes.fbs
@@ -0,0 +1,8 @@
+namespace scouting.webserver.requests;
+
+table SubmitNotes {
+ team:int (id: 0);
+ notes:string (id: 1);
+}
+
+root_type SubmitNotes;
diff --git a/scouting/webserver/requests/messages/submit_notes_response.fbs b/scouting/webserver/requests/messages/submit_notes_response.fbs
new file mode 100644
index 0000000..2a5bea2
--- /dev/null
+++ b/scouting/webserver/requests/messages/submit_notes_response.fbs
@@ -0,0 +1,8 @@
+namespace scouting.webserver.requests;
+
+table SubmitNotesResponse {
+ // empty response
+}
+
+root_type SubmitNotesResponse;
+
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 6c4bdd1..07b7d8a 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -19,8 +19,12 @@
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team_response"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting_response"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_notes"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_notes_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/server"
flatbuffers "github.com/google/flatbuffers/go"
)
@@ -35,6 +39,10 @@
type RequestDataScoutingResponseT = request_data_scouting_response.RequestDataScoutingResponseT
type RefreshMatchList = refresh_match_list.RefreshMatchList
type RefreshMatchListResponseT = refresh_match_list_response.RefreshMatchListResponseT
+type SubmitNotes = submit_notes.SubmitNotes
+type SubmitNotesResponseT = submit_notes_response.SubmitNotesResponseT
+type RequestNotesForTeam = request_notes_for_team.RequestNotesForTeam
+type RequestNotesForTeamResponseT = request_notes_for_team_response.RequestNotesForTeamResponseT
// The interface we expect the database abstraction to conform to.
// We use an interface here because it makes unit testing easier.
@@ -45,6 +53,8 @@
ReturnStats() ([]db.Stats, error)
QueryMatches(int32) ([]db.Match, error)
QueryStats(int) ([]db.Stats, error)
+ QueryNotes(int32) (db.NotesData, error)
+ AddNotes(db.NotesData) error
}
type ScrapeMatchList func(int32, string) ([]scraping.Match, error)
@@ -392,6 +402,93 @@
w.Write(builder.FinishedBytes())
}
+func parseSubmitNotes(w http.ResponseWriter, buf []byte) (*SubmitNotes, bool) {
+ success := true
+ defer func() {
+ if r := recover(); r != nil {
+ respondWithError(w, http.StatusBadRequest, fmt.Sprintf("Failed to parse RefreshMatchList: %v", r))
+ success = false
+ }
+ }()
+ result := submit_notes.GetRootAsSubmitNotes(buf, 0)
+ return result, success
+}
+
+type submitNoteScoutingHandler struct {
+ db Database
+}
+
+func (handler submitNoteScoutingHandler) ServeHTTP(w http.ResponseWriter, req *http.Request) {
+ requestBytes, err := io.ReadAll(req.Body)
+ if err != nil {
+ respondWithError(w, http.StatusBadRequest, fmt.Sprint("Failed to read request bytes:", err))
+ return
+ }
+
+ request, success := parseSubmitNotes(w, requestBytes)
+ if !success {
+ return
+ }
+
+ err = handler.db.AddNotes(db.NotesData{
+ TeamNumber: request.Team(),
+ Notes: []string{string(request.Notes())},
+ })
+ if err != nil {
+ respondWithError(w, http.StatusInternalServerError, fmt.Sprintf("Failed to insert notes: %v", err))
+ return
+ }
+
+ var response SubmitNotesResponseT
+ builder := flatbuffers.NewBuilder(10)
+ builder.Finish((&response).Pack(builder))
+ w.Write(builder.FinishedBytes())
+}
+
+func parseRequestNotesForTeam(w http.ResponseWriter, buf []byte) (*RequestNotesForTeam, bool) {
+ success := true
+ defer func() {
+ if r := recover(); r != nil {
+ respondWithError(w, http.StatusBadRequest, fmt.Sprintf("Failed to parse RefreshMatchList: %v", r))
+ success = false
+ }
+ }()
+ result := request_notes_for_team.GetRootAsRequestNotesForTeam(buf, 0)
+ return result, success
+}
+
+type requestNotesForTeamHandler struct {
+ db Database
+}
+
+func (handler requestNotesForTeamHandler) ServeHTTP(w http.ResponseWriter, req *http.Request) {
+ requestBytes, err := io.ReadAll(req.Body)
+ if err != nil {
+ respondWithError(w, http.StatusBadRequest, fmt.Sprint("Failed to read request bytes:", err))
+ return
+ }
+
+ request, success := parseRequestNotesForTeam(w, requestBytes)
+ if !success {
+ return
+ }
+
+ notesData, err := handler.db.QueryNotes(request.Team())
+ if err != nil {
+ respondWithError(w, http.StatusInternalServerError, fmt.Sprintf("Failed to query notes: %v", err))
+ return
+ }
+
+ var response RequestNotesForTeamResponseT
+ for _, data := range notesData.Notes {
+ response.Notes = append(response.Notes, &request_notes_for_team_response.NoteT{data})
+ }
+
+ builder := flatbuffers.NewBuilder(1024)
+ builder.Finish((&response).Pack(builder))
+ w.Write(builder.FinishedBytes())
+}
+
func HandleRequests(db Database, scrape ScrapeMatchList, scoutingServer server.ScoutingServer) {
scoutingServer.HandleFunc("/requests", unknown)
scoutingServer.Handle("/requests/submit/data_scouting", submitDataScoutingHandler{db})
@@ -399,4 +496,6 @@
scoutingServer.Handle("/requests/request/matches_for_team", requestMatchesForTeamHandler{db})
scoutingServer.Handle("/requests/request/data_scouting", requestDataScoutingHandler{db})
scoutingServer.Handle("/requests/refresh_match_list", refreshMatchListHandler{db, scrape})
+ scoutingServer.Handle("/requests/submit/submit_notes", submitNoteScoutingHandler{db})
+ scoutingServer.Handle("/requests/request/notes_for_team", requestNotesForTeamHandler{db})
}
diff --git a/scouting/webserver/requests/requests_test.go b/scouting/webserver/requests/requests_test.go
index 60bee0e..e4169c0 100644
--- a/scouting/webserver/requests/requests_test.go
+++ b/scouting/webserver/requests/requests_test.go
@@ -19,8 +19,10 @@
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team_response"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting_response"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_notes"
"github.com/frc971/971-Robot-Code/scouting/webserver/server"
flatbuffers "github.com/google/flatbuffers/go"
)
@@ -274,6 +276,59 @@
}
}
+func TestSubmitNotes(t *testing.T) {
+ database := MockDatabase{}
+ scoutingServer := server.NewScoutingServer()
+ HandleRequests(&database, scrapeEmtpyMatchList, scoutingServer)
+ scoutingServer.Start(8080)
+ defer scoutingServer.Stop()
+
+ builder := flatbuffers.NewBuilder(1024)
+ builder.Finish((&submit_notes.SubmitNotesT{
+ Team: 971,
+ Notes: "Notes",
+ }).Pack(builder))
+
+ _, err := debug.SubmitNotes("http://localhost:8080", builder.FinishedBytes())
+ if err != nil {
+ t.Fatal("Failed to submit notes: ", err)
+ }
+
+ expected := []db.NotesData{
+ {TeamNumber: 971, Notes: []string{"Notes"}},
+ }
+
+ if !reflect.DeepEqual(database.notes, expected) {
+ t.Fatal("Submitted notes did not match", expected, database.notes)
+ }
+}
+
+func TestRequestNotes(t *testing.T) {
+ database := MockDatabase{
+ notes: []db.NotesData{{
+ TeamNumber: 971,
+ Notes: []string{"Notes"},
+ }},
+ }
+ scoutingServer := server.NewScoutingServer()
+ HandleRequests(&database, scrapeEmtpyMatchList, scoutingServer)
+ scoutingServer.Start(8080)
+ defer scoutingServer.Stop()
+
+ builder := flatbuffers.NewBuilder(1024)
+ builder.Finish((&request_notes_for_team.RequestNotesForTeamT{
+ Team: 971,
+ }).Pack(builder))
+ response, err := debug.RequestNotes("http://localhost:8080", builder.FinishedBytes())
+ if err != nil {
+ t.Fatal("Failed to submit notes: ", err)
+ }
+
+ if response.Notes[0].Data != "Notes" {
+ t.Fatal("requested notes did not match", response)
+ }
+}
+
// Validates that we can download the schedule from The Blue Alliance.
func TestRefreshMatchList(t *testing.T) {
scrapeMockSchedule := func(int32, string) ([]scraping.Match, error) {
@@ -353,6 +408,7 @@
type MockDatabase struct {
matches []db.Match
stats []db.Stats
+ notes []db.NotesData
}
func (database *MockDatabase) AddToMatch(match db.Match) error {
@@ -390,6 +446,21 @@
return []db.Stats{}, nil
}
+func (database *MockDatabase) QueryNotes(requestedTeam int32) (db.NotesData, error) {
+ var results []string
+ for _, data := range database.notes {
+ if data.TeamNumber == requestedTeam {
+ results = append(results, data.Notes[0])
+ }
+ }
+ return db.NotesData{TeamNumber: requestedTeam, Notes: results}, nil
+}
+
+func (database *MockDatabase) AddNotes(data db.NotesData) error {
+ database.notes = append(database.notes, data)
+ return nil
+}
+
// Returns an empty match list from the fake The Blue Alliance scraping.
func scrapeEmtpyMatchList(int32, string) ([]scraping.Match, error) {
return nil, nil
diff --git a/y2022/BUILD b/y2022/BUILD
index f2bdb8b..83438ec 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -251,6 +251,7 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:wpilib",
+ "//y2022/control_loops/superstructure:led_indicator_lib",
"//y2022/control_loops/superstructure:superstructure_can_position_fbs",
"//y2022/control_loops/superstructure:superstructure_output_fbs",
"//y2022/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index 4739cd1..669e691 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -159,6 +159,29 @@
],
)
+cc_library(
+ name = "led_indicator_lib",
+ srcs = ["led_indicator.cc"],
+ hdrs = ["led_indicator.h"],
+ data = [
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":superstructure_output_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//third_party:phoenix",
+ "//third_party:wpilib",
+ ],
+)
+
ts_library(
name = "superstructure_plotter",
srcs = ["superstructure_plotter.ts"],
diff --git a/y2022/control_loops/superstructure/led_indicator.cc b/y2022/control_loops/superstructure/led_indicator.cc
new file mode 100644
index 0000000..4ec934d
--- /dev/null
+++ b/y2022/control_loops/superstructure/led_indicator.cc
@@ -0,0 +1,125 @@
+#include "y2022/control_loops/superstructure/led_indicator.h"
+
+namespace led = ctre::phoenix::led;
+
+namespace y2022::control_loops::superstructure {
+
+LedIndicator::LedIndicator(aos::EventLoop *event_loop)
+ : drivetrain_output_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<Status>("/superstructure")),
+ server_statistics_fetcher_(
+ event_loop->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/roborio/aos")) {
+ led::CANdleConfiguration config;
+ config.statusLedOffWhenActive = true;
+ config.disableWhenLOS = false;
+ config.brightnessScalar = 1.0;
+ candle_.ConfigAllSettings(config, 0);
+
+ event_loop->AddPhasedLoop([&](int) { DecideColor(); },
+ std::chrono::milliseconds(20));
+}
+
+// This method will be called once per scheduler run
+void LedIndicator::DisplayLed(uint8_t r, uint8_t g, uint8_t b) {
+ candle_.SetLEDs(static_cast<int>(r), static_cast<int>(g),
+ static_cast<int>(b));
+}
+
+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) {
+ return true;
+ }
+ }
+ return false;
+}
+
+bool DrivingFast(
+ const frc971::control_loops::drivetrain::Output &drivetrain_out) {
+ return (drivetrain_out.left_voltage() >= 11.5 ||
+ drivetrain_out.right_voltage() >= 11.5);
+}
+} // namespace
+
+void LedIndicator::DecideColor() {
+ superstructure_status_fetcher_.Fetch();
+ server_statistics_fetcher_.Fetch();
+ drivetrain_output_fetcher_.Fetch();
+
+ // Estopped
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->estopped()) {
+ DisplayLed(255, 0, 0);
+ return;
+ }
+
+ // Not zeroed
+ if (superstructure_status_fetcher_.get() &&
+ !superstructure_status_fetcher_->zeroed()) {
+ DisplayLed(255, 255, 0);
+ return;
+ }
+
+ // Pi disconnected
+ if (server_statistics_fetcher_.get() &&
+ DisconnectedPi(*server_statistics_fetcher_)) {
+ if (disconnected_flash_) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(0, 0, 255);
+ }
+
+ if (disconnected_counter_ % kFlashIterations == 0) {
+ disconnected_flash_ = !disconnected_flash_;
+ }
+ disconnected_counter_++;
+ return;
+ }
+
+ // Driving fast
+ if (drivetrain_output_fetcher_.get() &&
+ DrivingFast(*drivetrain_output_fetcher_)) {
+ DisplayLed(138, 43, 226);
+ return;
+ }
+
+ // Statemachine
+ if (superstructure_status_fetcher_.get()) {
+ switch (superstructure_status_fetcher_->state()) {
+ case (SuperstructureState::IDLE):
+ DisplayLed(0, 0, 0);
+ break;
+ case (SuperstructureState::TRANSFERRING):
+ DisplayLed(0, 0, 255);
+ break;
+ case (SuperstructureState::LOADING):
+ DisplayLed(255, 255, 255);
+ break;
+ case (SuperstructureState::LOADED):
+ if (!superstructure_status_fetcher_->ready_to_fire()) {
+ DisplayLed(255, 140, 0);
+ } else if (superstructure_status_fetcher_->front_intake_has_ball() ||
+ superstructure_status_fetcher_->back_intake_has_ball()) {
+ DisplayLed(165, 42, 42);
+ } else {
+ DisplayLed(0, 255, 0);
+ }
+ break;
+ case (SuperstructureState::SHOOTING):
+ if (!superstructure_status_fetcher_->flippers_open()) {
+ DisplayLed(255, 105, 180);
+ } else {
+ DisplayLed(0, 255, 255);
+ }
+ break;
+ }
+ return;
+ }
+}
+
+} // namespace y2022::control_loops::superstructure
diff --git a/y2022/control_loops/superstructure/led_indicator.h b/y2022/control_loops/superstructure/led_indicator.h
new file mode 100644
index 0000000..bdcee6c
--- /dev/null
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -0,0 +1,62 @@
+#ifndef Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_server_generated.h"
+#include "ctre/phoenix/led/CANdle.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2022::control_loops::superstructure {
+
+class LedIndicator {
+ public:
+ LedIndicator(aos::EventLoop *event_loop);
+
+ // Colors in order of priority:
+ //
+ // Red: estopped
+ // Yellow: not zeroed
+ // Flash blue/red: pi disconnected
+ // Purple: driving fast
+ //
+ // Statemachine:
+ // IDLE:
+ // Off
+ // TRANSFERRING:
+ // Blue
+ // LOADING:
+ // White
+ // LOADED:
+ // Green: ready to fire
+ // Brown: intaked another ball
+ // Orange: loaded
+ // SHOOTING:
+ // Pink: flippers opening
+ // Cyan: superstructure shooting
+ void DecideColor();
+
+ private:
+ static constexpr size_t kFlashIterations = 5;
+
+ void DisplayLed(uint8_t r, uint8_t g, uint8_t b);
+
+ ctre::phoenix::led::CANdle candle_{0, ""};
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Output>
+ drivetrain_output_fetcher_;
+ aos::Fetcher<Status> superstructure_status_fetcher_;
+ aos::Fetcher<aos::message_bridge::ServerStatistics>
+ server_statistics_fetcher_;
+
+ size_t disconnected_counter_ = 0;
+ bool disconnected_flash_ = false;
+};
+
+} // namespace y2022::control_loops::superstructure
+
+#endif // Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 63fa5e2..63f3d60 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -234,6 +234,16 @@
frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*turret_loading_goal_buffer.fbb(), turret_loading_position));
+ const bool turret_near_goal =
+ turret_goal != nullptr &&
+ std::abs(turret_goal->unsafe_goal() - turret_.position()) <
+ kTurretGoalThreshold;
+ const bool collided = collision_avoidance_.IsCollided(
+ {.intake_front_position = intake_front_.estimated_position(),
+ .intake_back_position = intake_back_.estimated_position(),
+ .turret_position = turret_.estimated_position(),
+ .shooting = true});
+
switch (state_) {
case SuperstructureState::IDLE: {
// Only change the turret's goal loading position when idle, to prevent us
@@ -335,16 +345,6 @@
break;
}
case SuperstructureState::SHOOTING: {
- const bool turret_near_goal =
- turret_goal != nullptr &&
- std::abs(turret_goal->unsafe_goal() - turret_.position()) <
- kTurretGoalThreshold;
- const bool collided = collision_avoidance_.IsCollided(
- {.intake_front_position = intake_front_.estimated_position(),
- .intake_back_position = intake_back_.estimated_position(),
- .turret_position = turret_.estimated_position(),
- .shooting = true});
-
// Don't open the flippers until the turret's ready: give them as little
// time to get bumped as possible.
if (!turret_near_goal || collided) {
@@ -509,6 +509,8 @@
status_builder.add_flippers_open(flippers_open_);
status_builder.add_reseating_in_catapult(reseating_in_catapult_);
status_builder.add_fire(fire_);
+ status_builder.add_ready_to_fire(state_ == SuperstructureState::LOADED &&
+ turret_near_goal && !collided);
status_builder.add_state(state_);
if (!front_intake_has_ball_ && !back_intake_has_ball_) {
status_builder.add_intake_state(IntakeState::NO_BALL);
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index a1817a7..06fddd0 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -55,6 +55,8 @@
flippers_open:bool (id: 12);
// Whether the flippers failed to open and we are retrying
reseating_in_catapult:bool (id: 13);
+ // Whether the turret is ready for firing
+ ready_to_fire:bool (id: 20);
// Whether the catapult was told to fire,
// meaning that the turret and flippers are ready for firing
// and we were asked to fire. Different from fire flag in goal.
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index b11dc3a..2e33db3 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -50,6 +50,7 @@
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2022/constants.h"
+#include "y2022/control_loops/superstructure/led_indicator.h"
#include "y2022/control_loops/superstructure/superstructure_can_position_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
@@ -702,6 +703,12 @@
AddLoop(&output_event_loop);
+ // Thread 5.
+ ::aos::ShmEventLoop led_indicator_event_loop(&config.message());
+ control_loops::superstructure::LedIndicator led_indicator(
+ &led_indicator_event_loop);
+ AddLoop(&led_indicator_event_loop);
+
RunLoops();
}
};