List superstructure subsystems that aren't ready
Put this in the status for debugging why we aren't shooting yet
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I3ae07995a2b15b5db1c02863605e4079e1e2f571
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index a39f3df..803a65f 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -78,6 +78,7 @@
":superstructure_output_fbs",
":superstructure_position_fbs",
":superstructure_status_fbs",
+ "//aos/containers:sized_array",
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
"//frc971/control_loops:control_loops_fbs",
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index e454aed..9f4edb2 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -24,21 +24,23 @@
accelerator::kBemf, accelerator::kResistance) {}
bool Shooter::UpToSpeed(const ShooterGoal *goal) {
- return (
- std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
- kVelocityToleranceFinisher &&
- std::abs(goal->velocity_accelerator() -
- accelerator_left_.avg_angular_velocity()) <
- kVelocityToleranceAccelerator &&
- std::abs(goal->velocity_accelerator() -
- accelerator_right_.avg_angular_velocity()) <
- kVelocityToleranceAccelerator &&
- std::abs(goal->velocity_finisher() - finisher_.velocity()) <
- kVelocityToleranceFinisher &&
- std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
- kVelocityToleranceAccelerator &&
- std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
- kVelocityToleranceAccelerator);
+ finisher_ready_ =
+ (std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
+ kVelocityToleranceFinisher &&
+ std::abs(goal->velocity_finisher() - finisher_.velocity()) <
+ kVelocityToleranceFinisher);
+ accelerator_ready_ =
+ (std::abs(goal->velocity_accelerator() -
+ accelerator_left_.avg_angular_velocity()) <
+ kVelocityToleranceAccelerator &&
+ std::abs(goal->velocity_accelerator() -
+ accelerator_right_.avg_angular_velocity()) <
+ kVelocityToleranceAccelerator &&
+ std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
+ kVelocityToleranceAccelerator &&
+ std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
+ kVelocityToleranceAccelerator);
+ return (finisher_ready_ && accelerator_ready_);
}
flatbuffers::Offset<ShooterStatus> Shooter::RunIteration(
@@ -78,6 +80,8 @@
ready_ = true;
} else {
ready_ = false;
+ finisher_ready_ = false;
+ accelerator_ready_ = false;
}
}
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index 7a3393b..2520df6 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -27,7 +27,9 @@
flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
const aos::monotonic_clock::time_point position_timestamp);
- bool ready() { return ready_; }
+ bool ready() const { return ready_; }
+ bool finisher_ready() const { return finisher_ready_; }
+ bool accelerator_ready() const { return accelerator_ready_; }
float finisher_goal() const { return finisher_.goal(); }
float accelerator_goal() const { return accelerator_left_.goal(); }
@@ -43,6 +45,8 @@
FlywheelController finisher_, accelerator_left_, accelerator_right_;
bool UpToSpeed(const ShooterGoal *goal);
+ bool finisher_ready_ = false;
+ bool accelerator_ready_ = false;
bool ready_ = false;
int balls_shot_ = 0;
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 72fb1ac..1a61a29 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -1,5 +1,6 @@
#include "y2020/control_loops/superstructure/superstructure.h"
+#include "aos/containers/sized_array.h"
#include "aos/events/event_loop.h"
namespace y2020 {
@@ -195,6 +196,28 @@
turret_status->estopped();
}
+ flatbuffers::Offset<flatbuffers::Vector<Subsystem>>
+ subsystems_not_ready_offset;
+ const bool turret_ready =
+ (std::abs(turret_.goal(0) - turret_.position()) < 0.025);
+ if (unsafe_goal && unsafe_goal->shooting() &&
+ (!shooter_.ready() || !turret_ready)) {
+ aos::SizedArray<Subsystem, 3> subsystems_not_ready;
+ if (!shooter_.finisher_ready()) {
+ subsystems_not_ready.push_back(Subsystem::FINISHER);
+ }
+ if (!shooter_.accelerator_ready()) {
+ subsystems_not_ready.push_back(Subsystem::ACCELERATOR);
+ }
+ if (!turret_ready) {
+ subsystems_not_ready.push_back(Subsystem::TURRET);
+ }
+
+ subsystems_not_ready_offset =
+ status->fbb()->CreateVector(subsystems_not_ready.backing_array().data(),
+ subsystems_not_ready.size());
+ }
+
Status::Builder status_builder = status->MakeBuilder<Status>();
status_builder.add_zeroed(zeroed);
@@ -205,6 +228,7 @@
status_builder.add_turret(turret_status_offset);
status_builder.add_shooter(shooter_status_offset);
status_builder.add_aimer(aimer_status_offset);
+ status_builder.add_subsystems_not_ready(subsystems_not_ready_offset);
status->Send(status_builder.Finish());
@@ -232,9 +256,7 @@
}
if (unsafe_goal->shooting()) {
- if (shooter_.ready() && shooter_.finisher_goal() > 10.0 &&
- shooter_.accelerator_goal() > 10.0 &&
- std::abs(turret_.goal(0) - turret_.position()) < 0.025) {
+ if (shooter_.ready() && turret_ready) {
output_struct.feeder_voltage = 12.0;
}
output_struct.washing_machine_spinner_voltage = 5.0;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 0970c81..2b8b39a 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -536,6 +536,7 @@
->avg_angular_velocity(),
0.001);
}
+ EXPECT_FALSE(superstructure_status_fetcher_->has_subsystems_not_ready());
}
void CheckIfZeroed() {
@@ -855,10 +856,26 @@
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_shooting(true);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
+ // In the beginning, the finisher and accelerator should not be ready
+ test_event_loop_
+ ->AddTimer([&]() {
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_->has_subsystems_not_ready());
+ const auto subsystems_not_ready =
+ superstructure_status_fetcher_->subsystems_not_ready();
+ ASSERT_EQ(subsystems_not_ready->size(), 2);
+ EXPECT_TRUE((subsystems_not_ready->Get(0) == Subsystem::FINISHER) !=
+ (subsystems_not_ready->Get(1) == Subsystem::FINISHER));
+ EXPECT_TRUE((subsystems_not_ready->Get(0) == Subsystem::ACCELERATOR) !=
+ (subsystems_not_ready->Get(1) == Subsystem::ACCELERATOR));
+ })
+ ->Setup(test_event_loop_->monotonic_now() + chrono::milliseconds(1));
+
// Give it a lot of time to get there.
RunFor(chrono::seconds(8));
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index aab1341..16f7445 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -3,6 +3,12 @@
namespace y2020.control_loops.superstructure;
+enum Subsystem:byte {
+ FINISHER,
+ ACCELERATOR,
+ TURRET
+}
+
table FlywheelControllerStatus {
// The current average velocity in radians/second over the last kHistoryLength
// in shooter.h
@@ -76,6 +82,9 @@
// Status of the vision auto-tracking.
aimer:AimerStatus (id: 7);
+
+ // Vector of the subsystems that are not at goal and are preventing shooting.
+ subsystems_not_ready:[Subsystem] (id: 8);
}
root_type Status;