Ignore the turret and finisher and hood on the practice robot.
It won't have one, and won't take the shot unless they are ready.
Ignore them when the team is 9971
Change-Id: I4eaa7c8e80cdc020cb8af2e040bb0b452b5edc84
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index e6fa3cf..99854fe 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -23,7 +23,7 @@
accelerator_right_(accelerator::MakeIntegralAcceleratorLoop(),
accelerator::kBemf, accelerator::kResistance) {}
-bool Shooter::UpToSpeed(const ShooterGoal *goal) {
+void Shooter::UpToSpeed(const ShooterGoal *goal) {
finisher_ready_ =
(std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
kVelocityToleranceFinisher &&
@@ -42,7 +42,6 @@
std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
kVelocityToleranceAccelerator &&
goal->velocity_accelerator() > kVelocityToleranceAccelerator);
- return (finisher_ready_ && accelerator_ready_);
}
flatbuffers::Offset<ShooterStatus> Shooter::RunIteration(
@@ -76,7 +75,7 @@
accelerator_right_.Update(output == nullptr);
if (goal) {
- ready_ = UpToSpeed(goal);
+ UpToSpeed(goal);
}
flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index 2520df6..ccbb326 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -27,7 +27,7 @@
flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
const aos::monotonic_clock::time_point position_timestamp);
- bool ready() const { return ready_; }
+ bool ready() const { return finisher_ready() && accelerator_ready(); }
bool finisher_ready() const { return finisher_ready_; }
bool accelerator_ready() const { return accelerator_ready_; }
@@ -44,10 +44,9 @@
FlywheelController finisher_, accelerator_left_, accelerator_right_;
- bool UpToSpeed(const ShooterGoal *goal);
+ void UpToSpeed(const ShooterGoal *goal);
bool finisher_ready_ = false;
bool accelerator_ready_ = false;
- bool ready_ = false;
int balls_shot_ = 0;
bool finisher_goal_changed_ = false;
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index d3c2464..ae79e76 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -2,6 +2,7 @@
#include "aos/containers/sized_array.h"
#include "aos/events/event_loop.h"
+#include "aos/network/team_number.h"
namespace y2020 {
namespace control_loops {
@@ -22,7 +23,8 @@
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
joystick_state_fetcher_(
- event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ has_turret_(::aos::network::GetTeamNumber() != 9971) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -199,7 +201,7 @@
flatbuffers::Offset<flatbuffers::Vector<Subsystem>>
subsystems_not_ready_offset;
const bool turret_ready =
- (std::abs(turret_.goal(0) - turret_.position()) < 0.025);
+ (std::abs(turret_.goal(0) - turret_.position()) < 0.025) || !has_turret_;
if (unsafe_goal && unsafe_goal->shooting() &&
(!shooter_.ready() || !turret_ready)) {
aos::SizedArray<Subsystem, 3> subsystems_not_ready;
@@ -270,7 +272,9 @@
}
if (unsafe_goal->shooting()) {
- if (shooter_.ready() && turret_ready) {
+ if ((shooter_.ready() ||
+ (!has_turret_ && shooter_.accelerator_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.h b/y2020/control_loops/superstructure/superstructure.h
index 07b7575..7f565cf 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -78,6 +78,8 @@
aos::monotonic_clock::time_point preloading_backpower_timeout_ =
aos::monotonic_clock::min_time;
+ bool has_turret_ = true;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};