Added shooter spinup wait state machine.
Change-Id: Ie4ee7dff2706c780b72d10951d63756addc37aa7
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 0e8f3a5..2f1ae4a 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -11,6 +11,8 @@
namespace control_loops {
namespace shooter {
+using ::aos::time::Time;
+
// TODO(austin): Pseudo current limit?
ShooterSide::ShooterSide()
@@ -67,7 +69,8 @@
}
Shooter::Shooter(ShooterQueue *my_shooter)
- : aos::controls::ControlLoop<ShooterQueue>(my_shooter) {}
+ : aos::controls::ControlLoop<ShooterQueue>(my_shooter),
+ last_pre_shot_timeout_(0, 0) {}
void Shooter::RunIteration(const ShooterQueue::Goal *goal,
const ShooterQueue::Position *position,
@@ -94,8 +97,53 @@
output->voltage_right = right_.voltage();
if (goal) {
+ bool shoot = false;
+ switch (state_) {
+ case ShooterLatchState::PASS_THROUGH:
+ if (goal->push_to_shooter) {
+ if (::std::abs(goal->angular_velocity) > 10) {
+ if (status->ready) {
+ state_ = ShooterLatchState::WAITING_FOR_SPINDOWN;
+ shoot = true;
+ }
+ } else {
+ shoot = true;
+ }
+ }
+ last_pre_shot_timeout_ = Time::Now() + Time::InSeconds(1.0);
+ break;
+ case ShooterLatchState::WAITING_FOR_SPINDOWN:
+ shoot = true;
+ if (left_.velocity() < goal->angular_velocity * 0.9 ||
+ right_.velocity() < goal->angular_velocity * 0.9) {
+ state_ = ShooterLatchState::WAITING_FOR_SPINUP;
+ }
+ if (::std::abs(goal->angular_velocity) < 10 ||
+ last_pre_shot_timeout_ < Time::Now()) {
+ state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+ }
+ break;
+ case ShooterLatchState::WAITING_FOR_SPINUP:
+ shoot = true;
+ if (left_.velocity() > goal->angular_velocity * 0.95 &&
+ right_.velocity() > goal->angular_velocity * 0.95) {
+ state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+ }
+ if (::std::abs(goal->angular_velocity) < 10 ||
+ last_pre_shot_timeout_ < Time::Now()) {
+ state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+ }
+ break;
+ case ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE:
+ shoot = true;
+ if (!goal->push_to_shooter) {
+ state_ = ShooterLatchState::PASS_THROUGH;
+ }
+ break;
+ }
+
output->clamp_open = goal->clamp_open;
- output->push_to_shooter = goal->push_to_shooter;
+ output->push_to_shooter = shoot;
}
}
}
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index 0354bb5..d3a2f86 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -4,6 +4,7 @@
#include <memory>
#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2016/control_loops/shooter/shooter_integral_plant.h"
@@ -32,6 +33,9 @@
// Returns the control loop calculated voltage.
double voltage() const;
+ // Returns the instantaneous velocity.
+ double velocity() const { return loop_->X_hat(1, 0); }
+
// Executes the control loop for a cycle.
void Update(bool disabled);
@@ -44,7 +48,7 @@
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 10;
::std::array<double, kHistoryLength> history_;
- ptrdiff_t history_position_;
+ ptrdiff_t history_position_ = 0;
DISALLOW_COPY_AND_ASSIGN(ShooterSide);
};
@@ -54,6 +58,17 @@
explicit Shooter(
ShooterQueue *shooter_queue = &control_loops::shooter::shooter_queue);
+ enum class ShooterLatchState {
+ // Any shoot commands will be passed through without modification.
+ PASS_THROUGH = 0,
+ // We are latched shooting waiting for the wheel to loose RPM.
+ WAITING_FOR_SPINDOWN = 1,
+ // We are latched shooting waiting for the wheel to spin back up.
+ WAITING_FOR_SPINUP = 2,
+ // Wait until the button is released.
+ WAITING_FOR_SHOT_NEGEDGE = 3
+ };
+
protected:
void RunIteration(const ShooterQueue::Goal *goal,
const ShooterQueue::Position *position,
@@ -63,6 +78,10 @@
private:
ShooterSide left_, right_;
+ // Current state.
+ ShooterLatchState state_ = ShooterLatchState::PASS_THROUGH;
+ ::aos::time::Time last_pre_shot_timeout_;
+
DISALLOW_COPY_AND_ASSIGN(Shooter);
};
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
index e0b4531..5559bbb 100644
--- a/y2016/control_loops/shooter/shooter.q
+++ b/y2016/control_loops/shooter/shooter.q
@@ -23,7 +23,11 @@
double angular_velocity;
bool clamp_open; // True to release our clamp on the ball.
- bool push_to_shooter; // True to push the ball into the shooter.
+ // True to push the ball into the shooter.
+ // If we are in the act of shooting with a goal velocity != 0, wait until it
+ // is up to speed, push the ball into the shooter, and then wait until it
+ // spins up and down before letting the piston be released.
+ bool push_to_shooter;
};
message Position {