added a lockout so it won't stop the shooter too soon
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 4ec77ba..869409f 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -54,6 +54,7 @@
loader_up_(false),
disc_clamped_(false),
disc_ejected_(false),
+ is_shooting_(false),
last_bottom_disc_detect_(false),
last_top_disc_detect_(false),
no_prior_position_(true),
@@ -690,6 +691,7 @@
if (loader_state_ == LoaderState::GRABBED &&
safe_goal_ == Goal::SHOOT) {
loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ is_shooting_ = true;
}
// Must wait until it has been grabbed to continue.
@@ -983,6 +985,7 @@
// Once we have shot, we need to hang out in READY until otherwise
// notified.
loader_goal_ = LoaderGoal::READY;
+ is_shooting_ = false;
break;
}
@@ -1025,6 +1028,7 @@
status->total_disc_count = total_disc_count_;
status->shot_disc_count = shot_disc_count_;
status->preloaded = (loader_state_ != LoaderState::READY);
+ status->is_shooting = is_shooting_;
if (output) {
output->intake_voltage = intake_voltage;
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 7c68a7f..28cad07 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -323,6 +323,8 @@
bool disc_clamped_;
bool disc_ejected_;
+ bool is_shooting_;
+
// The frisbee that is flying through the transfer rollers.
Frisbee transfer_frisbee_;
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index f8fe449..b12e31c 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -77,6 +77,9 @@
bool preloaded;
// Indexer ready to accept more discs.
bool ready_to_intake;
+ // True from when we're committed to shooting util after the disk is clear
+ // of the robot.
+ bool is_shooting;
};
queue Goal goal;
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 981df5d..035880e 100644
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -4,6 +4,7 @@
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include "frc971/control_loops/index/index_motor.q.h"
namespace frc971 {
namespace control_loops {
@@ -13,7 +14,8 @@
loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
history_position_(0),
position_goal_(0.0),
- last_position_(0.0) {
+ last_position_(0.0),
+ last_velocity_goal_(0) {
memset(history_, 0, sizeof(history_));
}
@@ -26,11 +28,23 @@
const control_loops::ShooterLoop::Position *position,
::aos::control_loops::Output *output,
control_loops::ShooterLoop::Status *status) {
- const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ double velocity_goal = std::min(goal->velocity, kMaxSpeed);
const double current_position =
(position == NULL ? loop_->X_hat(0, 0) : position->position);
double output_voltage = 0.0;
+ if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ if (index_loop.status->is_shooting) {
+ if (velocity_goal != last_velocity_goal_ &&
+ velocity_goal < 130) {
+ velocity_goal = last_velocity_goal_;
+ }
+ }
+ } else {
+ LOG(WARNING, "assuming index isn't shooting\n");
+ }
+ last_velocity_goal_ = velocity_goal;
+
// Track the current position if the velocity goal is small.
if (velocity_goal <= 1.0) {
position_goal_ = current_position;
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index b1371a4..41c33c1 100644
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -29,6 +29,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:common',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
],
'export_dependent_settings': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 77c605b..7947f7a 100644
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -45,6 +45,9 @@
double position_goal_;
double last_position_;
+ // For making sure it keeps spinning if we're shooting.
+ double last_velocity_goal_;
+
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};