Added column and tests
The column works! We can also shut the intake down for hanging.
Change-Id: I4369d489d1a07a688f204fd9bb00ef7ad787f5a3
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 6ebba72..84803af 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -55,6 +55,7 @@
for (auto &estimator : estimators_) {
estimator.Reset();
}
+ should_reset_ = true;
}
// Returns the controller.
@@ -98,6 +99,9 @@
return loop_->X_hat();
}
double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+ double &mutable_X_hat(int row, int col) const {
+ return loop_->mutable_X_hat(row, col);
+ }
// Returns the current internal estimator state for logging.
typename ZeroingEstimator::State EstimatorState(int index) {
@@ -114,8 +118,6 @@
protected:
void set_zeroed(int index, bool val) { zeroed_[index] = val; }
- // TODO(austin): It's a bold assumption to assume that we will have the same
- // number of sensors as axes. So far, that's been fine.
::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
number_of_states, number_of_inputs, number_of_outputs>>
loop_;
@@ -125,6 +127,10 @@
bool initialized_ = false;
+ // If true, the subclass should reset in Update. It should then clear this
+ // flag.
+ bool should_reset_ = true;
+
::std::array<ZeroingEstimator, number_of_axes> estimators_;
private:
@@ -334,6 +340,15 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::Update(bool disable) {
+ // TODO(austin): What do we want to do with the profile on reset? Also, we
+ // should probably reset R, the offset, the profile, etc.
+ if (this->should_reset_) {
+ this->loop_->mutable_X_hat(0, 0) = Y_(0, 0);
+ this->loop_->mutable_X_hat(1, 0) = 0.0;
+ this->loop_->mutable_X_hat(2, 0) = 0.0;
+ this->should_reset_ = false;
+ }
+
if (!disable) {
::Eigen::Matrix<double, 2, 1> goal_state = profile_.Update(
this->unprofiled_goal_(0, 0), this->unprofiled_goal_(1, 0));
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index ce334bf..13a949a 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -214,6 +214,11 @@
}
void Arm::Update(bool disable) {
+ // TODO(austin): Reset the state vectors and internal state here.
+ if (should_reset_) {
+ should_reset_ = false;
+ }
+
if (!disable) {
// Compute next goal.
loop_->mutable_next_R().block<2, 1>(0, 0) = shoulder_profile_.Update(
diff --git a/y2017/BUILD b/y2017/BUILD
index a7e08ba..c226bb3 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -11,16 +11,15 @@
],
deps = [
'//aos/common/logging',
- '//aos/common:once',
'//aos/common/network:team_number',
'//aos/common:mutex',
+ '//aos/common:once',
'//frc971:constants',
'//y2017/control_loops/drivetrain:polydrivetrain_plants',
- '//y2017/control_loops/superstructure/shooter:shooter_plants',
- '//y2017/control_loops/superstructure/intake:intake_plants',
- '//y2017/control_loops/superstructure/turret:turret_plants',
- '//y2017/control_loops/superstructure/indexer:indexer_plants',
+ '//y2017/control_loops/superstructure/column:column_plants',
'//y2017/control_loops/superstructure/hood:hood_plants',
+ '//y2017/control_loops/superstructure/intake:intake_plants',
+ '//y2017/control_loops/superstructure/shooter:shooter_plants',
],
)
@@ -39,6 +38,7 @@
'//aos/linux_code:init',
'//frc971/autonomous:auto_queue',
'//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//y2017/actors:autonomous_action_lib',
'//y2017/control_loops/superstructure:superstructure_queue',
],
)
diff --git a/y2017/analysis/turret_test b/y2017/analysis/turret_test
index e471b93..2ae056c 100644
--- a/y2017/analysis/turret_test
+++ b/y2017/analysis/turret_test
@@ -1,7 +1,9 @@
superstructure_lib_test output voltage_turret
-superstructure_lib_test status turret estimator_state position
+superstructure_lib_test status turret position
+superstructure_lib_test status turret velocity
+superstructure_lib_test status turret goal_position
+superstructure_lib_test status turret goal_velocity
+superstructure_lib_test status turret unprofiled_goal_position
superstructure_lib_test status turret state
superstructure_lib_test goal turret angle
-superstructure_lib_test position turret pot
-superstructure_lib_test position turret encoder
diff --git a/y2017/constants.cc b/y2017/constants.cc
index b9361c7..bcfd66e 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -48,6 +48,7 @@
constexpr double Values::kTurretEncoderCountsPerRevolution,
Values::kTurretEncoderRatio, Values::kMaxTurretEncoderPulsesPerSecond;
+constexpr ::frc971::constants::Range Values::kTurretRange;
constexpr double Values::kIndexerEncoderCountsPerRevolution,
Values::kIndexerEncoderRatio, Values::kIndexerEncoderIndexDifference,
@@ -72,8 +73,13 @@
intake->zeroing.moving_buffer_size = 20;
intake->zeroing.allowable_encoder_error = 0.3;
+ column->turret_zeroed_distance = M_PI / 2.0;
column->indexer_zeroing.index_difference = 2.0 * M_PI;
+ column->indexer_zeroing.hall_trigger_zeroing_length = 2;
+ column->indexer_zeroing.zeroing_move_direction = true;
column->turret_zeroing.index_difference = 2.0 * M_PI;
+ column->turret_zeroing.hall_trigger_zeroing_length = 2;
+ column->turret_zeroing.zeroing_move_direction = false;
hood->zeroing.index_pulse_count = 2;
hood->zeroing.index_difference = Values::kHoodEncoderIndexDifference;
@@ -85,11 +91,13 @@
intake->pot_offset = 0;
intake->zeroing.measured_absolute_position = 0;
- column->indexer_zeroing.lower_hall_position = 2.0;
- column->indexer_zeroing.upper_hall_position = 2.1;
+ // TODO(austin): Swap the turret and indexer limits and make sure the
+ // tests still pass.
+ column->indexer_zeroing.lower_hall_position = 0.1;
+ column->indexer_zeroing.upper_hall_position = 0.2;
- column->turret_zeroing.lower_hall_position = 0;
- column->turret_zeroing.upper_hall_position = 0.1;
+ column->turret_zeroing.lower_hall_position = 2;
+ column->turret_zeroing.upper_hall_position = 2.1;
hood->pot_offset = 0.1;
hood->zeroing.measured_index_position = 0.05;
@@ -115,14 +123,14 @@
break;
case kPracticeTeamNumber:
- intake->pot_offset = 0.2921 + 0.00039 + 0.012236 - 0.023602;
- intake->zeroing.measured_absolute_position = 0.031437;
+ intake->pot_offset = 0.2921 + 0.00039 + 0.012236 - 0.023602 + 0.010722;
+ intake->zeroing.measured_absolute_position = 0.045893;
- column->indexer_zeroing.lower_hall_position = 2.0;
- column->indexer_zeroing.upper_hall_position = 2.1;
+ column->indexer_zeroing.lower_hall_position = 5.014364;
+ column->indexer_zeroing.upper_hall_position = 5.292234;
- column->turret_zeroing.lower_hall_position = 0;
- column->turret_zeroing.upper_hall_position = 0.1;
+ column->turret_zeroing.lower_hall_position = -4.838110;
+ column->turret_zeroing.upper_hall_position = -4.655730;
hood->zeroing.measured_index_position = 0.655432 - 0.460505;
diff --git a/y2017/constants.h b/y2017/constants.h
index e8faae1..8858996 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -7,11 +7,10 @@
#include "frc971/constants.h"
#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017/control_loops/superstructure/shooter/shooter_plant.h"
-#include "y2017/control_loops/superstructure/intake/intake_plant.h"
-#include "y2017/control_loops/superstructure/turret/turret_plant.h"
-#include "y2017/control_loops/superstructure/indexer/indexer_plant.h"
+#include "y2017/control_loops/superstructure/column/column_plant.h"
#include "y2017/control_loops/superstructure/hood/hood_plant.h"
+#include "y2017/control_loops/superstructure/intake/intake_plant.h"
+#include "y2017/control_loops/superstructure/shooter/shooter_plant.h"
namespace y2017 {
namespace constants {
@@ -40,6 +39,10 @@
struct Column {
::frc971::constants::HallEffectZeroingConstants indexer_zeroing;
::frc971::constants::HallEffectZeroingConstants turret_zeroing;
+ // The max absolute value of the turret angle that we need to get to to be
+ // classified as zeroed. Otherwise, we may be ambiguous on which wrap we
+ // are on.
+ double turret_zeroed_distance;
};
static const int kZeroingSampleSize = 200;
@@ -75,7 +78,7 @@
constants::Values::kIntakeEncoderRatio *
kIntakeEncoderCountsPerRevolution;
static constexpr ::frc971::constants::Range kIntakeRange{-0.01, 0.240, 0.01,
- 0.21};
+ 0.230};
static constexpr double kHoodEncoderCountsPerRevolution = 2048 * 4;
static constexpr double kHoodEncoderRatio = 20.0 / 345.0;
@@ -92,8 +95,8 @@
static constexpr double kTurretEncoderCountsPerRevolution = 256 * 4;
static constexpr double kTurretEncoderRatio = 11.0 / 94.0;
static constexpr double kMaxTurretEncoderPulsesPerSecond =
- control_loops::superstructure::turret::kFreeSpeed *
- control_loops::superstructure::turret::kOutputRatio /
+ control_loops::superstructure::column::kTurretFreeSpeed *
+ control_loops::superstructure::column::kTurretOutputRatio /
constants::Values::kTurretEncoderRatio *
kTurretEncoderCountsPerRevolution;
@@ -102,10 +105,13 @@
static constexpr double kIndexerEncoderIndexDifference =
2.0 * M_PI * kIndexerEncoderRatio;
static constexpr double kMaxIndexerEncoderPulsesPerSecond =
- control_loops::superstructure::indexer::kFreeSpeed *
- control_loops::superstructure::indexer::kOutputRatio /
+ control_loops::superstructure::column::kIndexerFreeSpeed *
+ control_loops::superstructure::column::kIndexerOutputRatio /
constants::Values::kIndexerEncoderRatio *
kIndexerEncoderCountsPerRevolution;
+ static constexpr ::frc971::constants::Range kTurretRange{
+ -460.0 / 2.0 * M_PI / 180.0, 460.0 / 2.0 * M_PI / 180.0,
+ -450.0 / 2.0 * M_PI / 180.0, 450.0 / 2.0 * M_PI / 180.0};
double drivetrain_max_speed;
diff --git a/y2017/control_loops/python/column.py b/y2017/control_loops/python/column.py
old mode 100644
new mode 100755
index ebb2f60..e94c227
--- a/y2017/control_loops/python/column.py
+++ b/y2017/control_loops/python/column.py
@@ -20,8 +20,6 @@
pass
-# TODO(austin): Shut down with no counts on the turret.
-
class ColumnController(control_loop.ControlLoop):
def __init__(self, name='Column'):
super(ColumnController, self).__init__(name)
@@ -39,18 +37,18 @@
self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
self.B_continuous = numpy.matrix(numpy.zeros((3, 2)))
- self.A_continuous[1 - 1, 1 - 1] = -(self.indexer.Kt / self.indexer.Kv / (self.indexer.J * self.indexer.resistance * self.indexer.G * self.indexer.G) +
+ self.A_continuous[0, 0] = -(self.indexer.Kt / self.indexer.Kv / (self.indexer.J * self.indexer.resistance * self.indexer.G * self.indexer.G) +
self.turret.Kt / self.turret.Kv / (self.indexer.J * self.turret.resistance * self.turret.G * self.turret.G))
- self.A_continuous[1 - 1, 3 - 1] = self.turret.Kt / self.turret.Kv / (self.indexer.J * self.turret.resistance * self.turret.G * self.turret.G)
- self.B_continuous[1 - 1, 0] = self.indexer.Kt / (self.indexer.J * self.indexer.resistance * self.indexer.G)
- self.B_continuous[1 - 1, 1] = -self.turret.Kt / (self.indexer.J * self.turret.resistance * self.turret.G)
+ self.A_continuous[0, 2] = self.turret.Kt / self.turret.Kv / (self.indexer.J * self.turret.resistance * self.turret.G * self.turret.G)
+ self.B_continuous[0, 0] = self.indexer.Kt / (self.indexer.J * self.indexer.resistance * self.indexer.G)
+ self.B_continuous[0, 1] = -self.turret.Kt / (self.indexer.J * self.turret.resistance * self.turret.G)
- self.A_continuous[2 - 1, 3 - 1] = 1
+ self.A_continuous[1, 2] = 1
- self.A_continuous[3 - 1, 1 - 1] = self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
- self.A_continuous[3 - 1, 3 - 1] = -self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
+ self.A_continuous[2, 0] = self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
+ self.A_continuous[2, 2] = -self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
- self.B_continuous[3 - 1, 1] = self.turret.Kt / (self.turret.J * self.turret.resistance * self.turret.G)
+ self.B_continuous[2, 1] = self.turret.Kt / (self.turret.J * self.turret.resistance * self.turret.G)
self.C = numpy.matrix([[1, 0, 0], [0, 1, 0]])
self.D = numpy.matrix([[0, 0], [0, 0]])
@@ -58,9 +56,10 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- q_pos = 0.015
- q_vel = 0.3
- self.Q = numpy.matrix([[(1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ q_indexer_vel = 13.0
+ q_pos = 0.04
+ q_vel = 0.8
+ self.Q = numpy.matrix([[(1.0 / (q_indexer_vel ** 2.0)), 0.0, 0.0],
[0.0, (1.0 / (q_pos ** 2.0)), 0.0],
[0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
@@ -68,6 +67,8 @@
[0.0, (1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ glog.debug('Controller poles are ' + repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
q_vel_indexer_ff = 0.000005
q_pos_ff = 0.0000005
q_vel_ff = 0.00008
@@ -84,7 +85,7 @@
class Column(ColumnController):
- def __init__(self, name='Column'):
+ def __init__(self, name='Column', disable_indexer=False):
super(Column, self).__init__(name)
A_continuous = numpy.matrix(numpy.zeros((4, 4)))
B_continuous = numpy.matrix(numpy.zeros((4, 2)))
@@ -99,8 +100,6 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- glog.debug('Eig is ' + repr(numpy.linalg.eig(self.A_continuous)))
-
self.C = numpy.matrix([[1, 0, 0, 0], [-1, 0, 1, 0]])
self.D = numpy.matrix([[0, 0], [0, 0]])
@@ -108,6 +107,12 @@
self.K = numpy.matrix(numpy.zeros((2, 4)))
self.K[:, 1:] = orig_K
+ glog.debug('K is ' + repr(self.K))
+ # TODO(austin): Do we want to damp velocity out or not when disabled?
+ #if disable_indexer:
+ # self.K[0, 1] = 0.0
+ # self.K[1, 1] = 0.0
+
orig_Kff = self.Kff
self.Kff = numpy.matrix(numpy.zeros((2, 4)))
self.Kff[:, 1:] = orig_Kff
@@ -131,7 +136,8 @@
class IntegralColumn(Column):
- def __init__(self, name='IntegralColumn', voltage_error_noise=None):
+ def __init__(self, name='IntegralColumn', voltage_error_noise=None,
+ disable_indexer=False):
super(IntegralColumn, self).__init__(name)
A_continuous = numpy.matrix(numpy.zeros((6, 6)))
@@ -143,25 +149,27 @@
self.A_continuous = A_continuous
self.B_continuous = B_continuous
- glog.debug('A_continuous: ' + repr(self.A_continuous))
- glog.debug('B_continuous: ' + repr(self.B_continuous))
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- glog.debug('Eig is ' + repr(numpy.linalg.eig(self.A_continuous)))
-
C = numpy.matrix(numpy.zeros((2, 6)))
C[0:2, 0:4] = self.C
self.C = C
- glog.debug('C is ' + repr(self.C))
self.D = numpy.matrix([[0, 0], [0, 0]])
orig_K = self.K
self.K = numpy.matrix(numpy.zeros((2, 6)))
self.K[:, 0:4] = orig_K
- self.K[0, 4] = 1
+
+ # TODO(austin): I'm not certain this is ideal. If someone spins the bottom
+ # at a constant rate, we'll learn a voltage offset. That should translate
+ # directly to a voltage on the turret to hold it steady. I'm also not
+ # convinced we care that much. If the indexer is off, it'll stop rather
+ # quickly anyways, so this is mostly a moot point.
+ if not disable_indexer:
+ self.K[0, 4] = 1
self.K[1, 5] = 1
orig_Kff = self.Kff
@@ -290,7 +298,7 @@
offset = 0.0
if i > 100:
offset = 1.0
- column.Update(U + numpy.matrix([[offset], [0.0]]))
+ column.Update(U + numpy.matrix([[0.0], [offset]]))
observer_column.PredictObserver(U)
@@ -306,31 +314,31 @@
numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
glog.debug('Time: %f', self.t[-1])
- glog.debug('goal_error %s', repr(end_goal - goal))
- glog.debug('error %s', repr(observer_column.X_hat - end_goal))
+ glog.debug('goal_error %s', repr((end_goal - goal).T))
+ glog.debug('error %s', repr((observer_column.X_hat - end_goal).T))
def Plot(self):
pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.xi, label='xi')
- pylab.plot(self.t, self.xt, label='xt')
+ pylab.plot(self.t, self.xi, label='x_indexer')
+ pylab.plot(self.t, self.xt, label='x_turret')
pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.plot(self.t, self.turret_error, label='turret_error')
+ pylab.plot(self.t, self.turret_error, label='turret_error * 100')
pylab.legend()
pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.ui, label='ui')
- pylab.plot(self.t, self.ui_fb, label='ui_fb')
- pylab.plot(self.t, self.ut, label='ut')
- pylab.plot(self.t, self.ut_fb, label='ut_fb')
- pylab.plot(self.t, self.offseti, label='voltage_offseti')
- pylab.plot(self.t, self.offsett, label='voltage_offsett')
+ pylab.plot(self.t, self.ui, label='u_indexer')
+ pylab.plot(self.t, self.ui_fb, label='u_indexer_fb')
+ pylab.plot(self.t, self.ut, label='u_turret')
+ pylab.plot(self.t, self.ut_fb, label='u_turret_fb')
+ pylab.plot(self.t, self.offseti, label='voltage_offset_indexer')
+ pylab.plot(self.t, self.offsett, label='voltage_offset_turret')
pylab.legend()
pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.ai, label='ai')
- pylab.plot(self.t, self.at, label='at')
- pylab.plot(self.t, self.vi, label='vi')
- pylab.plot(self.t, self.vt, label='vt')
+ pylab.plot(self.t, self.ai, label='a_indexer')
+ pylab.plot(self.t, self.at, label='a_turret')
+ pylab.plot(self.t, self.vi, label='v_indexer')
+ pylab.plot(self.t, self.vt, label='v_turret')
pylab.legend()
pylab.show()
@@ -346,7 +354,7 @@
initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
R = numpy.matrix([[0.0], [10.0], [5.0], [0.0], [0.0], [0.0]])
scenario_plotter.run_test(column, end_goal=R, controller_column=column_controller,
- observer_column=observer_column, iterations=600)
+ observer_column=observer_column, iterations=400)
if FLAGS.plot:
scenario_plotter.Plot()
@@ -358,11 +366,23 @@
column = Column('Column')
loop_writer = control_loop.ControlLoopWriter('Column', [column],
namespaces=namespaces)
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kIndexerFreeSpeed', '%f', column.indexer.free_speed))
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kIndexerOutputRatio', '%f', column.indexer.G))
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kTurretFreeSpeed', '%f', column.turret.free_speed))
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kTurretOutputRatio', '%f', column.turret.G))
loop_writer.Write(argv[1], argv[2])
+ # IntegralColumn controller 1 will disable the indexer.
integral_column = IntegralColumn('IntegralColumn')
+ disabled_integral_column = IntegralColumn('DisabledIntegralColumn',
+ disable_indexer=True)
integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralColumn', [integral_column], namespaces=namespaces)
+ 'IntegralColumn', [integral_column, disabled_integral_column],
+ namespaces=namespaces)
integral_loop_writer.Write(argv[3], argv[4])
stuck_integral_column = IntegralColumn('StuckIntegralColumn', voltage_error_noise=8.0)
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 1632850..63e06f1 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -25,11 +25,10 @@
deps = [
':superstructure_queue',
'//aos/common/controls:control_loop',
+ '//y2017/control_loops/superstructure/column',
'//y2017/control_loops/superstructure/hood',
- '//y2017/control_loops/superstructure/indexer',
'//y2017/control_loops/superstructure/intake',
'//y2017/control_loops/superstructure/shooter',
- '//y2017/control_loops/superstructure/turret',
'//y2017:constants',
],
)
@@ -49,11 +48,10 @@
'//aos/testing:googletest',
'//frc971/control_loops:position_sensor_sim',
'//frc971/control_loops:team_number_test_environment',
+ '//y2017/control_loops/superstructure/column:column_plants',
'//y2017/control_loops/superstructure/hood:hood_plants',
- '//y2017/control_loops/superstructure/indexer:indexer_plants',
'//y2017/control_loops/superstructure/intake:intake_plants',
'//y2017/control_loops/superstructure/shooter:shooter_plants',
- '//y2017/control_loops/superstructure/turret:turret_plants',
],
)
diff --git a/y2017/control_loops/superstructure/column/BUILD b/y2017/control_loops/superstructure/column/BUILD
index 5a79dbc..8704cf5 100644
--- a/y2017/control_loops/superstructure/column/BUILD
+++ b/y2017/control_loops/superstructure/column/BUILD
@@ -9,8 +9,8 @@
'column_plant.cc',
'column_integral_plant.h',
'column_integral_plant.cc',
- 'stuck_column_integral_plant.cc',
'stuck_column_integral_plant.h',
+ 'stuck_column_integral_plant.cc',
],
)
@@ -20,10 +20,12 @@
srcs = [
'column_plant.cc',
'column_integral_plant.cc',
+ 'stuck_column_integral_plant.cc',
],
hdrs = [
'column_plant.h',
'column_integral_plant.h',
+ 'stuck_column_integral_plant.h',
],
deps = [
'//frc971/control_loops:state_feedback_loop',
@@ -31,6 +33,27 @@
)
cc_library(
+ name = 'column',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'column.cc',
+ ],
+ hdrs = [
+ 'column.h',
+ ],
+ deps = [
+ ':column_plants',
+ ':column_zeroing',
+ '//aos/common/controls:control_loop',
+ '//aos/common:math',
+ '//frc971/control_loops:profiled_subsystem',
+ '//y2017/control_loops/superstructure/intake:intake',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017:constants',
+ ],
+)
+
+cc_library(
name = 'column_zeroing',
srcs = [
'column_zeroing.cc',
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
new file mode 100644
index 0000000..5e9a21c
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -0,0 +1,610 @@
+#include "y2017/control_loops/superstructure/column/column.h"
+
+#include <array>
+#include <chrono>
+#include <memory>
+#include <utility>
+
+#include "Eigen/Dense"
+
+#include "aos/common/commonmath.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/profiled_subsystem.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2017/control_loops/superstructure/column/column_integral_plant.h"
+#include "y2017/control_loops/superstructure/column/stuck_column_integral_plant.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace column {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+using ::frc971::zeroing::PulseIndexZeroingEstimator;
+
+namespace {
+constexpr double kTolerance = 10.0;
+constexpr chrono::milliseconds kForwardTimeout{500};
+constexpr chrono::milliseconds kReverseTimeout{500};
+constexpr chrono::milliseconds kReverseMinTimeout{100};
+} // namespace
+
+constexpr double Column::kZeroingVoltage;
+constexpr double Column::kOperatingVoltage;
+constexpr double Column::kIntakeZeroingMinDistance;
+constexpr double Column::kIntakeTolerance;
+constexpr double Column::kStuckZeroingTrackingError;
+
+ColumnProfiledSubsystem::ColumnProfiledSubsystem(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>>
+ loop,
+ const ::y2017::constants::Values::Column &zeroing_constants,
+ const ::frc971::constants::Range &range, double default_velocity,
+ double default_acceleration)
+ : ProfiledSubsystem<6, 1, ColumnZeroingEstimator, 2, 2>(
+ ::std::move(loop), {{zeroing_constants}}),
+ stuck_indexer_detector_(new StateFeedbackLoop<6, 2, 2>(
+ column::MakeStuckIntegralColumnLoop())),
+ profile_(::aos::controls::kLoopFrequency),
+ range_(range),
+ default_velocity_(default_velocity),
+ default_acceleration_(default_acceleration) {
+ Y_.setZero();
+ offset_.setZero();
+ X_hat_current_.setZero();
+ stuck_indexer_X_hat_current_.setZero();
+ indexer_history_.fill(0);
+ AdjustProfile(0.0, 0.0);
+}
+
+void ColumnProfiledSubsystem::AddOffset(double indexer_offset_delta,
+ double turret_offset_delta) {
+ UpdateOffset(offset_(0, 0) + indexer_offset_delta,
+ offset_(1, 0) + turret_offset_delta);
+}
+
+void ColumnProfiledSubsystem::UpdateOffset(double indexer_offset,
+ double turret_offset) {
+ const double indexer_doffset = indexer_offset - offset_(0, 0);
+ const double turret_doffset = turret_offset - offset_(1, 0);
+
+ LOG(INFO, "Adjusting indexer offset from %f to %f\n", offset_(0, 0),
+ indexer_offset);
+ LOG(INFO, "Adjusting turret offset from %f to %f\n", offset_(1, 0),
+ turret_offset);
+
+ loop_->mutable_X_hat()(0, 0) += indexer_doffset;
+ loop_->mutable_X_hat()(2, 0) += turret_doffset + indexer_doffset;
+
+ stuck_indexer_detector_->mutable_X_hat()(0, 0) += indexer_doffset;
+ stuck_indexer_detector_->mutable_X_hat()(2, 0) +=
+ turret_doffset + indexer_doffset;
+ Y_(0, 0) += indexer_doffset;
+ Y_(1, 0) += turret_doffset;
+ turret_last_position_ += turret_doffset + indexer_doffset;
+ loop_->mutable_R(0, 0) += indexer_doffset;
+ loop_->mutable_R(2, 0) += turret_doffset + indexer_doffset;
+
+ profile_.MoveGoal(turret_doffset + indexer_doffset);
+ offset_(0, 0) = indexer_offset;
+ offset_(1, 0) = turret_offset;
+
+ CapGoal("R", &loop_->mutable_R());
+}
+
+void ColumnProfiledSubsystem::Correct(const ColumnPosition &new_position) {
+ estimators_[0].UpdateEstimate(new_position);
+
+ if (estimators_[0].error()) {
+ LOG(ERROR, "zeroing error\n");
+ return;
+ }
+
+ if (!initialized_) {
+ if (estimators_[0].offset_ready()) {
+ UpdateOffset(estimators_[0].indexer_offset(),
+ estimators_[0].turret_offset());
+ initialized_ = true;
+ }
+ }
+
+ if (!zeroed(0) && estimators_[0].zeroed()) {
+ UpdateOffset(estimators_[0].indexer_offset(),
+ estimators_[0].turret_offset());
+ set_zeroed(0, true);
+ }
+
+ turret_last_position_ = turret_position();
+ Y_ << new_position.indexer.position, new_position.turret.position;
+ Y_ += offset_;
+ loop_->Correct(Y_);
+
+ indexer_history_[indexer_history_position_] = new_position.indexer.position;
+ indexer_history_position_ = (indexer_history_position_ + 1) % kHistoryLength;
+
+ indexer_dt_velocity_ =
+ (new_position.indexer.position - indexer_last_position_) /
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count();
+ indexer_last_position_ = new_position.indexer.position;
+
+ stuck_indexer_detector_->Correct(Y_);
+
+ // Compute the oldest point in the history.
+ const int indexer_oldest_history_position =
+ ((indexer_history_position_ == 0) ? kHistoryLength
+ : indexer_history_position_) -
+ 1;
+
+ // Compute the distance moved over that time period.
+ indexer_average_angular_velocity_ =
+ (indexer_history_[indexer_oldest_history_position] -
+ indexer_history_[indexer_history_position_]) /
+ (chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count() *
+ static_cast<double>(kHistoryLength - 1));
+
+ // Ready if average angular velocity is close to the goal.
+ indexer_error_ = indexer_average_angular_velocity_ - unprofiled_goal_(1, 0);
+
+ indexer_ready_ =
+ std::abs(indexer_error_) < kTolerance && unprofiled_goal_(1, 0) > 0.1;
+
+ // Pull state from the profiled subsystem.
+ X_hat_current_ = controller().X_hat();
+ stuck_indexer_X_hat_current_ = stuck_indexer_detector_->X_hat();
+ indexer_position_error_ = X_hat_current_(0, 0) - Y(0, 0);
+}
+
+void ColumnProfiledSubsystem::CapGoal(const char *name,
+ Eigen::Matrix<double, 6, 1> *goal) {
+ // Limit the goal to min/max allowable positions.
+ if (zeroed()) {
+ if ((*goal)(2, 0) > range_.upper) {
+ LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(2, 0),
+ range_.upper);
+ (*goal)(2, 0) = range_.upper;
+ }
+ if ((*goal)(2, 0) < range_.lower) {
+ LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(2, 0),
+ range_.lower);
+ (*goal)(2, 0) = range_.lower;
+ }
+ } else {
+ const double kMaxRange = range().upper_hard - range().lower_hard;
+
+ // Limit the goal to min/max allowable positions much less agressively.
+ // We don't know where the limits are, so we have to let the user move far
+ // enough to find them (and the index pulse which might be right next to
+ // one).
+ // Upper - lower hard may be a bit generous, but we are moving slow.
+
+ if ((*goal)(2, 0) > kMaxRange) {
+ LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(2, 0),
+ kMaxRange);
+ (*goal)(2, 0) = kMaxRange;
+ }
+ if ((*goal)(2, 0) < -kMaxRange) {
+ LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(2, 0),
+ -kMaxRange);
+ (*goal)(2, 0) = -kMaxRange;
+ }
+ }
+}
+
+void ColumnProfiledSubsystem::ForceGoal(double goal_velocity, double goal) {
+ set_unprofiled_goal(goal_velocity, goal);
+ loop_->mutable_R() = unprofiled_goal_;
+ loop_->mutable_next_R() = loop_->R();
+
+ const ::Eigen::Matrix<double, 6, 1> &R = loop_->R();
+ profile_.MoveCurrentState(R.block<2, 1>(2, 0));
+}
+
+void ColumnProfiledSubsystem::set_unprofiled_goal(double goal_velocity,
+ double unprofiled_goal) {
+ unprofiled_goal_(0, 0) = 0.0;
+ unprofiled_goal_(1, 0) = goal_velocity;
+ unprofiled_goal_(2, 0) = unprofiled_goal;
+ unprofiled_goal_(3, 0) = 0.0;
+ unprofiled_goal_(4, 0) = 0.0;
+ unprofiled_goal_(5, 0) = 0.0;
+ CapGoal("unprofiled R", &unprofiled_goal_);
+}
+
+void ColumnProfiledSubsystem::set_indexer_unprofiled_goal(
+ double goal_velocity) {
+ unprofiled_goal_(0, 0) = 0.0;
+ unprofiled_goal_(1, 0) = goal_velocity;
+ unprofiled_goal_(4, 0) = 0.0;
+ CapGoal("unprofiled R", &unprofiled_goal_);
+}
+
+void ColumnProfiledSubsystem::set_turret_unprofiled_goal(
+ double unprofiled_goal) {
+ unprofiled_goal_(2, 0) = unprofiled_goal;
+ unprofiled_goal_(3, 0) = 0.0;
+ unprofiled_goal_(5, 0) = 0.0;
+ CapGoal("unprofiled R", &unprofiled_goal_);
+}
+
+void ColumnProfiledSubsystem::Update(bool disable) {
+ // TODO(austin): If we really need to reset, reset the profiles, etc. That'll
+ // be covered by the layer above when disabled though, so we can get away with
+ // not doing it yet.
+ if (should_reset_) {
+ loop_->mutable_X_hat(0, 0) = Y_(0, 0);
+ loop_->mutable_X_hat(1, 0) = 0.0;
+ loop_->mutable_X_hat(2, 0) = Y_(0, 0) + Y_(1, 0);
+ loop_->mutable_X_hat(3, 0) = 0.0;
+ loop_->mutable_X_hat(4, 0) = 0.0;
+ loop_->mutable_X_hat(5, 0) = 0.0;
+
+ LOG(INFO, "Resetting\n");
+ stuck_indexer_detector_->mutable_X_hat() = loop_->X_hat();
+ should_reset_ = false;
+ saturated_ = false;
+ }
+
+ if (!disable) {
+ ::Eigen::Matrix<double, 2, 1> goal_state =
+ profile_.Update(unprofiled_goal_(2, 0), unprofiled_goal_(3, 0));
+
+ loop_->mutable_next_R(0, 0) = 0.0;
+ loop_->mutable_next_R(1, 0) = unprofiled_goal_(1, 0);
+ loop_->mutable_next_R(2, 0) = goal_state(0, 0);
+ loop_->mutable_next_R(3, 0) = goal_state(1, 0);
+ loop_->mutable_next_R(4, 0) = 0.0;
+ loop_->mutable_next_R(5, 0) = 0.0;
+ CapGoal("next R", &loop_->mutable_next_R());
+ }
+
+ // If the indexer goal velocity is low, switch to the indexer controller which
+ // won't fight to keep it moving at 0.
+ if (::std::abs(unprofiled_goal_(1, 0)) < 0.1) {
+ loop_->set_index(1);
+ } else {
+ loop_->set_index(0);
+ }
+ loop_->Update(disable);
+
+ if (!disable && loop_->U(1, 0) != loop_->U_uncapped(1, 0)) {
+ const ::Eigen::Matrix<double, 6, 1> &R = loop_->R();
+ profile_.MoveCurrentState(R.block<2, 1>(2, 0));
+ saturated_ = true;
+ } else {
+ saturated_ = false;
+ }
+
+ // Run the KF predict step.
+ stuck_indexer_detector_->UpdateObserver(loop_->U(),
+ ::aos::controls::kLoopFrequency);
+}
+
+bool ColumnProfiledSubsystem::CheckHardLimits() {
+ // Returns whether hard limits have been exceeded.
+
+ if (turret_position() > range_.upper_hard || turret_position() < range_.lower_hard) {
+ LOG(ERROR,
+ "ColumnProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
+ turret_position(), range_.lower_hard, range_.upper_hard);
+ return true;
+ }
+
+ return false;
+}
+
+void ColumnProfiledSubsystem::AdjustProfile(
+ const ::frc971::ProfileParameters &profile_parameters) {
+ AdjustProfile(profile_parameters.max_velocity,
+ profile_parameters.max_acceleration);
+}
+
+void ColumnProfiledSubsystem::AdjustProfile(double max_angular_velocity,
+ double max_angular_acceleration) {
+ profile_.set_maximum_velocity(
+ ::frc971::control_loops::internal::UseUnlessZero(max_angular_velocity,
+ default_velocity_));
+ profile_.set_maximum_acceleration(
+ ::frc971::control_loops::internal::UseUnlessZero(max_angular_acceleration,
+ default_acceleration_));
+}
+
+double ColumnProfiledSubsystem::IndexerStuckVoltage() const {
+ // Compute the voltage from the control loop, excluding the voltage error
+ // term.
+ const double uncapped_applied_voltage =
+ uncapped_indexer_voltage() + X_hat(4, 0);
+ if (uncapped_applied_voltage < 0) {
+ return +stuck_indexer_X_hat_current_(4, 0);
+ } else {
+ return -stuck_indexer_X_hat_current_(4, 0);
+ }
+}
+bool ColumnProfiledSubsystem::IsIndexerStuck() const {
+ return IndexerStuckVoltage() > 4.0;
+}
+
+void ColumnProfiledSubsystem::PartialIndexerReset() {
+ mutable_X_hat(4, 0) = 0.0;
+ stuck_indexer_detector_->mutable_X_hat(4, 0) = 0.0;
+}
+
+void ColumnProfiledSubsystem::PartialTurretReset() {
+ mutable_X_hat(5, 0) = 0.0;
+ stuck_indexer_detector_->mutable_X_hat(5, 0) = 0.0;
+}
+
+void ColumnProfiledSubsystem::PopulateIndexerStatus(IndexerStatus *status) {
+ status->avg_angular_velocity = indexer_average_angular_velocity_;
+
+ status->angular_velocity = X_hat_current_(1, 0);
+ status->ready = indexer_ready_;
+
+ status->voltage_error = X_hat_current_(4, 0);
+ status->stuck_voltage_error = stuck_indexer_X_hat_current_(4, 0);
+ status->position_error = indexer_position_error_;
+ status->instantaneous_velocity = indexer_dt_velocity_;
+
+ status->stuck = IsIndexerStuck();
+
+ status->stuck_voltage = IndexerStuckVoltage();
+}
+
+Column::Column()
+ : profiled_subsystem_(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 6, 2, 2>(MakeIntegralColumnLoop())),
+ constants::GetValues().column, constants::Values::kTurretRange, 7.0,
+ 50.0) {}
+
+void Column::Reset() {
+ state_ = State::UNINITIALIZED;
+ indexer_state_ = IndexerState::RUNNING;
+ profiled_subsystem_.Reset();
+ // intake will automatically clear the minimum position on reset, so we don't
+ // need to do it here.
+ freeze_ = false;
+}
+
+void Column::Iterate(const control_loops::IndexerGoal *unsafe_indexer_goal,
+ const control_loops::TurretGoal *unsafe_turret_goal,
+ const ColumnPosition *position, double *indexer_output,
+ double *turret_output, IndexerStatus *indexer_status,
+ TurretProfiledSubsystemStatus *turret_status,
+ intake::Intake *intake) {
+ bool disable = turret_output == nullptr || indexer_output == nullptr;
+ profiled_subsystem_.Correct(*position);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Wait in the uninitialized state until the turret is initialized.
+ // Set the goals to where we are now so when we start back up, we don't
+ // jump.
+ profiled_subsystem_.ForceGoal(0.0, profiled_subsystem_.turret_position());
+ state_ = State::ZEROING_UNINITIALIZED;
+
+ // Fall through so we can start the zeroing process immediately.
+
+ case State::ZEROING_UNINITIALIZED:
+ // Set up the profile to be the zeroing profile.
+ profiled_subsystem_.AdjustProfile(0.50, 3);
+
+ // Force the intake out.
+ intake->set_min_position(kIntakeZeroingMinDistance);
+
+ if (disable) {
+ // If we are disabled, we want to reset the turret to stay where it
+ // currently is.
+ profiled_subsystem_.ForceGoal(0.0,
+ profiled_subsystem_.turret_position());
+ } else if (profiled_subsystem_.initialized()) {
+ // If we are initialized, there's no value in continuing to move so stop
+ // and wait on the intake.
+ profiled_subsystem_.set_indexer_unprofiled_goal(0.0);
+ } else {
+ // Spin slowly backwards.
+ profiled_subsystem_.set_indexer_unprofiled_goal(2.0);
+ }
+
+ // See if we are zeroed or initialized and far enough out and execute the
+ // proper state transition.
+ if (profiled_subsystem_.zeroed()) {
+ intake->clear_min_position();
+ state_ = State::RUNNING;
+ } else if (profiled_subsystem_.initialized() &&
+ intake->position() >
+ kIntakeZeroingMinDistance - kIntakeTolerance) {
+ if (profiled_subsystem_.turret_position() > 0) {
+ // We need to move in the negative direction.
+ state_ = State::ZEROING_NEGATIVE;
+ } else {
+ // We need to move in the positive direction.
+ state_ = State::ZEROING_POSITIVE;
+ }
+ }
+ break;
+
+ case State::ZEROING_POSITIVE:
+ // We are now going to be moving in the positive direction towards 0. If
+ // we get close enough, we'll zero.
+ profiled_subsystem_.set_unprofiled_goal(0.0, 0.0);
+ intake->set_min_position(kIntakeZeroingMinDistance);
+
+ if (profiled_subsystem_.zeroed()) {
+ intake->clear_min_position();
+ state_ = State::RUNNING;
+ } else if (disable) {
+ // We are disabled, so pick back up from the current position.
+ profiled_subsystem_.ForceGoal(0.0,
+ profiled_subsystem_.turret_position());
+ } else if (profiled_subsystem_.turret_position() <
+ profiled_subsystem_.goal(2, 0) -
+ kStuckZeroingTrackingError ||
+ profiled_subsystem_.saturated()) {
+ LOG(INFO,
+ "Turret stuck going positive, switching directions. At %f, goal "
+ "%f\n",
+ profiled_subsystem_.turret_position(),
+ profiled_subsystem_.goal(2, 0));
+ // The turret got too far behind. Declare it stuck and reverse.
+ profiled_subsystem_.AddOffset(0.0, 2.0 * M_PI);
+ profiled_subsystem_.set_unprofiled_goal(0.0, 0.0);
+ profiled_subsystem_.ForceGoal(0.0,
+ profiled_subsystem_.turret_position());
+ profiled_subsystem_.PartialTurretReset();
+ profiled_subsystem_.PartialIndexerReset();
+ state_ = State::ZEROING_NEGATIVE;
+ }
+ break;
+
+ case State::ZEROING_NEGATIVE:
+ // We are now going to be moving in the negative direction towards 0. If
+ // we get close enough, we'll zero.
+ profiled_subsystem_.set_unprofiled_goal(0.0, 0.0);
+ intake->set_min_position(kIntakeZeroingMinDistance);
+
+ if (profiled_subsystem_.zeroed()) {
+ intake->clear_min_position();
+ state_ = State::RUNNING;
+ } else if (disable) {
+ // We are disabled, so pick back up from the current position.
+ profiled_subsystem_.ForceGoal(0.0,
+ profiled_subsystem_.turret_position());
+ } else if (profiled_subsystem_.turret_position() >
+ profiled_subsystem_.goal(2, 0) +
+ kStuckZeroingTrackingError ||
+ profiled_subsystem_.saturated()) {
+ // The turret got too far behind. Declare it stuck and reverse.
+ LOG(INFO,
+ "Turret stuck going negative, switching directions. At %f, goal "
+ "%f\n",
+ profiled_subsystem_.turret_position(),
+ profiled_subsystem_.goal(2, 0));
+ profiled_subsystem_.AddOffset(0.0, -2.0 * M_PI);
+ profiled_subsystem_.set_unprofiled_goal(0.0, 0.0);
+ profiled_subsystem_.ForceGoal(0.0,
+ profiled_subsystem_.turret_position());
+ profiled_subsystem_.PartialTurretReset();
+ profiled_subsystem_.PartialIndexerReset();
+ state_ = State::ZEROING_POSITIVE;
+ }
+ break;
+
+ case State::RUNNING: {
+ double starting_goal_angle = profiled_subsystem_.goal(2, 0);
+ if (disable) {
+ // Reset the profile to the current position so it starts from here when
+ // we get re-enabled.
+ profiled_subsystem_.ForceGoal(0.0,
+ profiled_subsystem_.turret_position());
+ }
+
+ if (unsafe_turret_goal && unsafe_indexer_goal) {
+ profiled_subsystem_.AdjustProfile(unsafe_turret_goal->profile_params);
+ profiled_subsystem_.set_unprofiled_goal(
+ unsafe_indexer_goal->angular_velocity, unsafe_turret_goal->angle);
+
+ if (freeze_) {
+ profiled_subsystem_.ForceGoal(unsafe_indexer_goal->angular_velocity,
+ starting_goal_angle);
+ }
+ }
+
+ // ESTOP if we hit the hard limits.
+ if (profiled_subsystem_.CheckHardLimits() ||
+ profiled_subsystem_.error()) {
+ state_ = State::ESTOP;
+ }
+ } break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ disable = true;
+ break;
+ }
+
+ // Start indexing at the suggested velocity.
+ // If a "stuck" event is detected, reverse. Stay reversed until either
+ // unstuck, or 0.5 seconds have elapsed.
+ // Then, start going forwards. Don't detect stuck for 0.5 seconds.
+
+ monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ switch (indexer_state_) {
+ case IndexerState::RUNNING:
+ // The velocity goal is already set above in this case, so leave it
+ // alone.
+
+ // If we are stuck and weren't just reversing, try reversing to unstick
+ // us. We don't want to chatter back and forth too fast if reversing
+ // isn't working.
+ if (profiled_subsystem_.IsIndexerStuck() &&
+ monotonic_now > kForwardTimeout + last_transition_time_) {
+ indexer_state_ = IndexerState::REVERSING;
+ last_transition_time_ = monotonic_now;
+ profiled_subsystem_.PartialIndexerReset();
+ LOG(INFO, "Partial indexer reset while going forwards\n");
+ LOG(INFO, "Indexer RUNNING -> REVERSING\n");
+ }
+ break;
+ case IndexerState::REVERSING:
+ // "Reverse" "slowly".
+ profiled_subsystem_.set_indexer_unprofiled_goal(
+ -5.0 * ::aos::sign(profiled_subsystem_.unprofiled_goal(1, 0)));
+
+ // If we've timed out or are no longer stuck, try running again.
+ if ((!profiled_subsystem_.IsIndexerStuck() &&
+ monotonic_now > last_transition_time_ + kReverseMinTimeout) ||
+ monotonic_now > kReverseTimeout + last_transition_time_) {
+ indexer_state_ = IndexerState::RUNNING;
+ LOG(INFO, "Indexer REVERSING -> RUNNING, stuck %d\n",
+ profiled_subsystem_.IsIndexerStuck());
+
+ // Only reset if we got stuck going this way too.
+ if (monotonic_now > kReverseTimeout + last_transition_time_) {
+ LOG(INFO, "Partial indexer reset while reversing\n");
+ profiled_subsystem_.PartialIndexerReset();
+ }
+ last_transition_time_ = monotonic_now;
+ }
+ break;
+ }
+
+ // Set the voltage limits.
+ const double max_voltage =
+ (state_ == State::RUNNING) ? kOperatingVoltage : kZeroingVoltage;
+
+ profiled_subsystem_.set_max_voltage({{max_voltage, max_voltage}});
+
+ // Calculate the loops for a cycle.
+ profiled_subsystem_.Update(disable);
+
+ // Write out all the voltages.
+ if (indexer_output) {
+ *indexer_output = profiled_subsystem_.indexer_voltage();
+ }
+ if (turret_output) {
+ *turret_output = profiled_subsystem_.turret_voltage();
+ }
+
+ // Save debug/internal state.
+ // TODO(austin): Save more.
+ turret_status->zeroed = profiled_subsystem_.zeroed();
+ profiled_subsystem_.PopulateTurretStatus(turret_status);
+ turret_status->estopped = (state_ == State::ESTOP);
+ turret_status->state = static_cast<int32_t>(state_);
+
+ profiled_subsystem_.PopulateIndexerStatus(indexer_status);
+ indexer_status->state = static_cast<int32_t>(indexer_state_);
+}
+
+} // namespace column
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
new file mode 100644
index 0000000..6bd7aa5
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -0,0 +1,229 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_COLUMN_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_COLUMN_H_
+
+#include <array>
+#include <chrono>
+#include <memory>
+#include <utility>
+
+#include "Eigen/Dense"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/profiled_subsystem.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/column/column_zeroing.h"
+#include "y2017/control_loops/superstructure/intake/intake.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace column {
+
+class ColumnProfiledSubsystem
+ : public ::frc971::control_loops::ProfiledSubsystem<
+ 6, 1, ColumnZeroingEstimator, 2, 2> {
+ public:
+ ColumnProfiledSubsystem(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>>
+ loop,
+ const ::y2017::constants::Values::Column &zeroing_constants,
+ const ::frc971::constants::Range &range, double default_angular_velocity,
+ double default_angular_acceleration);
+
+ // Updates our estimator with the latest position.
+ void Correct(const ColumnPosition &position);
+ // Runs the controller and profile generator for a cycle.
+ void Update(bool disabled);
+
+ // Fills out the ProfiledJointStatus structure with the current state.
+ template <class StatusType>
+ void PopulateTurretStatus(StatusType *status);
+
+ // Forces the current goal to the provided goal, bypassing the profiler.
+ void ForceGoal(double goal_velocity, double goal);
+ // Sets the unprofiled goal. The profiler will generate a profile to go to
+ // this goal.
+ void set_indexer_unprofiled_goal(double goal_velocity);
+ void set_turret_unprofiled_goal(double unprofiled_goal);
+ void set_unprofiled_goal(double goal_velocity, double unprofiled_goal);
+ // Limits our profiles to a max velocity and acceleration for proper motion.
+ void AdjustProfile(const ::frc971::ProfileParameters &profile_parameters);
+ void AdjustProfile(double max_angular_velocity,
+ double max_angular_acceleration);
+
+ // Returns true if we have exceeded any hard limits.
+ bool CheckHardLimits();
+
+ // Returns the requested voltage.
+ double indexer_voltage() const { return loop_->U(0, 0); }
+ double uncapped_indexer_voltage() const { return loop_->U_uncapped(0, 0); }
+ double turret_voltage() const { return loop_->U(1, 0); }
+
+ // Returns the current Y.
+ const ::Eigen::Matrix<double, 2, 1> Y() const { return Y_; }
+ double Y(int i, int j) const { return Y_(i, j); }
+
+ // Returns the current indexer position.
+ double indexer_position() const { return Y_(0, 0); }
+
+ bool saturated() const { return saturated_; }
+
+ // Returns the current turret position.
+ double turret_position() const { return Y_(1, 0) + Y_(0, 0); }
+
+ // For testing:
+ // Triggers an estimator error.
+ void TriggerEstimatorError() { estimators_[0].TriggerError(); }
+
+ const ::frc971::constants::Range &range() const { return range_; }
+
+ bool IsIndexerStuck() const;
+ double IndexerStuckVoltage() const;
+ void PartialIndexerReset();
+ void PartialTurretReset();
+ void PopulateIndexerStatus(IndexerStatus *status);
+
+ void AddOffset(double indexer_offset_delta, double turret_offset_delta);
+
+ protected:
+ // Limits the provided goal to the soft limits. Prints "name" when it fails
+ // to aid debugging.
+ virtual void CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal);
+
+ private:
+ void UpdateOffset(double indexer_offset, double turret_offset);
+
+ ::std::unique_ptr<StateFeedbackLoop<6, 2, 2>> stuck_indexer_detector_;
+
+ // History array for calculating a filtered angular velocity.
+ static constexpr int kHistoryLength = 5;
+ ::std::array<double, kHistoryLength> indexer_history_;
+ ptrdiff_t indexer_history_position_ = 0;
+
+ double indexer_error_ = 0.0;
+ double indexer_dt_velocity_ = 0.0;
+ double indexer_last_position_ = 0.0;
+ double indexer_average_angular_velocity_ = 0.0;
+ double indexer_position_error_ = 0.0;
+ bool indexer_ready_ = false;
+
+ bool saturated_ = false;
+
+ Eigen::Matrix<double, 6, 1> X_hat_current_;
+ Eigen::Matrix<double, 6, 1> stuck_indexer_X_hat_current_;
+
+ aos::util::TrapezoidProfile profile_;
+
+ // Current measurement.
+ Eigen::Matrix<double, 2, 1> Y_;
+ // Current offset. Y_ = offset_ + raw_sensor;
+ Eigen::Matrix<double, 2, 1> offset_;
+
+ const ::frc971::constants::Range range_;
+
+ const double default_velocity_;
+ const double default_acceleration_;
+
+ double turret_last_position_ = 0;
+};
+
+template <typename StatusType>
+void ColumnProfiledSubsystem::PopulateTurretStatus(StatusType *status) {
+ status->zeroed = zeroed();
+ status->state = -1;
+ // We don't know, so default to the bad case.
+ status->estopped = true;
+
+ status->position = X_hat(2, 0);
+ status->velocity = X_hat(3, 0);
+ status->goal_position = goal(2, 0);
+ status->goal_velocity = goal(3, 0);
+ status->unprofiled_goal_position = unprofiled_goal(2, 0);
+ status->unprofiled_goal_velocity = unprofiled_goal(3, 0);
+ status->voltage_error = X_hat(5, 0);
+ status->calculated_velocity =
+ (turret_position() - turret_last_position_) /
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count();
+
+ status->estimator_state = EstimatorState(0);
+
+ Eigen::Matrix<double, 6, 1> error = controller().error();
+ status->position_power = controller().controller().K(0, 0) * error(0, 0);
+ status->velocity_power = controller().controller().K(0, 1) * error(1, 0);
+}
+
+class Column {
+ public:
+ Column();
+ double goal(int row, int col) const {
+ return profiled_subsystem_.goal(row, col);
+ }
+
+ double turret_position() const {
+ return profiled_subsystem_.turret_position();
+ }
+
+ void set_freeze(bool freeze) { freeze_ = freeze; }
+
+ // The zeroing and operating voltages.
+ static constexpr double kZeroingVoltage = 5.0;
+ static constexpr double kOperatingVoltage = 12.0;
+ static constexpr double kIntakeZeroingMinDistance = 0.08;
+ static constexpr double kIntakeTolerance = 0.005;
+ static constexpr double kStuckZeroingTrackingError = 0.02;
+ static constexpr double kTurretNearZero = 0.1;
+
+ void Iterate(const control_loops::IndexerGoal *unsafe_indexer_goal,
+ const control_loops::TurretGoal *unsafe_turret_goal,
+ const ColumnPosition *position, double *indexer_output,
+ double *turret_output, IndexerStatus *indexer_status,
+ TurretProfiledSubsystemStatus *turret_status,
+ intake::Intake *intake);
+
+ void Reset();
+
+ enum class State : int32_t {
+ UNINITIALIZED = 0,
+ ZEROING_UNINITIALIZED = 1,
+ ZEROING_POSITIVE = 2,
+ ZEROING_NEGATIVE = 3,
+ RUNNING = 4,
+ ESTOP = 5,
+ };
+
+ enum class IndexerState : int32_t {
+ // The system is running correctly, no stuck condition detected.
+ RUNNING = 0,
+ // The system is reversing to unjam.
+ REVERSING = 1
+ };
+
+ State state() const { return state_; }
+ IndexerState indexer_state() const { return indexer_state_; }
+
+ private:
+ State state_ = State::UNINITIALIZED;
+
+ IndexerState indexer_state_ = IndexerState::RUNNING;
+
+ bool freeze_ = false;
+
+ // The last time that we transitioned from RUNNING to REVERSING or the
+ // reverse. Used to implement the timeouts.
+ ::aos::monotonic_clock::time_point last_transition_time_ =
+ ::aos::monotonic_clock::min_time;
+
+ ColumnProfiledSubsystem profiled_subsystem_;
+};
+
+} // namespace column
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_COLUMN_H_
diff --git a/y2017/control_loops/superstructure/indexer/BUILD b/y2017/control_loops/superstructure/indexer/BUILD
deleted file mode 100644
index 10b726f..0000000
--- a/y2017/control_loops/superstructure/indexer/BUILD
+++ /dev/null
@@ -1,50 +0,0 @@
-genrule(
- name = 'genrule_indexer',
- cmd = '$(location //y2017/control_loops/python:indexer) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:indexer',
- ],
- outs = [
- 'indexer_plant.h',
- 'indexer_plant.cc',
- 'indexer_integral_plant.h',
- 'indexer_integral_plant.cc',
- 'stuck_indexer_integral_plant.h',
- 'stuck_indexer_integral_plant.cc',
- ],
-)
-
-cc_library(
- name = 'indexer_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'indexer_plant.cc',
- 'indexer_integral_plant.cc',
- 'stuck_indexer_integral_plant.cc',
- ],
- hdrs = [
- 'indexer_plant.h',
- 'indexer_integral_plant.h',
- 'stuck_indexer_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
-)
-
-cc_library(
- name = 'indexer',
- visibility = ['//visibility:public'],
- srcs = [
- 'indexer.cc',
- ],
- hdrs = [
- 'indexer.h',
- ],
- deps = [
- ':indexer_plants',
- '//aos/common/controls:control_loop',
- '//aos/common:math',
- '//y2017/control_loops/superstructure:superstructure_queue',
- ],
-)
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
deleted file mode 100644
index 0560ec4..0000000
--- a/y2017/control_loops/superstructure/indexer/indexer.cc
+++ /dev/null
@@ -1,200 +0,0 @@
-#include "y2017/control_loops/superstructure/indexer/indexer.h"
-
-#include <chrono>
-
-#include "aos/common/commonmath.h"
-#include "aos/common/controls/control_loops.q.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/logging/queue_logging.h"
-#include "aos/common/time.h"
-#include "y2017/control_loops/superstructure/indexer/indexer_integral_plant.h"
-#include "y2017/control_loops/superstructure/indexer/stuck_indexer_integral_plant.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
-
-namespace y2017 {
-namespace control_loops {
-namespace superstructure {
-namespace indexer {
-
-namespace chrono = ::std::chrono;
-using ::aos::monotonic_clock;
-
-namespace {
-constexpr double kTolerance = 10.0;
-constexpr chrono::milliseconds kForwardTimeout{500};
-constexpr chrono::milliseconds kReverseTimeout{500};
-constexpr chrono::milliseconds kReverseMinTimeout{100};
-} // namespace
-
-// TODO(austin): Pseudo current limit?
-
-IndexerController::IndexerController()
- : loop_(new StateFeedbackLoop<3, 1, 1>(
- superstructure::indexer::MakeIntegralIndexerLoop())),
- stuck_indexer_detector_(new StateFeedbackLoop<3, 1, 1>(
- superstructure::indexer::MakeStuckIntegralIndexerLoop())) {
- history_.fill(0);
- Y_.setZero();
- X_hat_current_.setZero();
- stuck_indexer_X_hat_current_.setZero();
-}
-
-void IndexerController::set_goal(double angular_velocity_goal) {
- loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
-}
-
-void IndexerController::set_position(double current_position) {
- // Update position in the model.
- Y_ << current_position;
-
- // Add the position to the history.
- history_[history_position_] = current_position;
- history_position_ = (history_position_ + 1) % kHistoryLength;
-
- dt_velocity_ = (current_position - last_position_) /
- chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
- last_position_ = current_position;
-}
-
-double IndexerController::voltage() const { return loop_->U(0, 0); }
-
-double IndexerController::StuckVoltage() const {
- const double applied_voltage = voltage() + loop_->X_hat(2, 0);
- if (applied_voltage < 0) {
- return +stuck_indexer_X_hat_current_(2, 0) + applied_voltage;
- } else {
- return -stuck_indexer_X_hat_current_(2, 0) - applied_voltage;
- }
-}
-bool IndexerController::IsStuck() const { return StuckVoltage() > 1.5; }
-
-void IndexerController::Reset() { reset_ = true; }
-
-void IndexerController::PartialReset() { loop_->mutable_X_hat(2, 0) = 0.0; }
-
-void IndexerController::Update(bool disabled) {
- loop_->mutable_R() = loop_->next_R();
- if (::std::abs(loop_->R(1, 0)) < 0.1) {
- // Kill power at low angular velocities.
- disabled = true;
- }
-
- if (reset_) {
- loop_->mutable_X_hat(0, 0) = Y_(0, 0);
- loop_->mutable_X_hat(1, 0) = 0.0;
- loop_->mutable_X_hat(2, 0) = 0.0;
- stuck_indexer_detector_->mutable_X_hat(0, 0) = Y_(0, 0);
- stuck_indexer_detector_->mutable_X_hat(1, 0) = 0.0;
- stuck_indexer_detector_->mutable_X_hat(2, 0) = 0.0;
- reset_ = false;
- }
-
- loop_->Correct(Y_);
- stuck_indexer_detector_->Correct(Y_);
-
- // Compute the oldest point in the history.
- const int oldest_history_position =
- ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
-
- // Compute the distance moved over that time period.
- average_angular_velocity_ =
- (history_[oldest_history_position] - history_[history_position_]) /
- (chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count() *
- static_cast<double>(kHistoryLength - 1));
-
- // Ready if average angular velocity is close to the goal.
- error_ = average_angular_velocity_ - loop_->next_R(1, 0);
-
- ready_ = std::abs(error_) < kTolerance && loop_->next_R(1, 0) > 1.0;
-
- X_hat_current_ = loop_->X_hat();
- stuck_indexer_X_hat_current_ = stuck_indexer_detector_->X_hat();
- position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
-
- loop_->Update(disabled);
- stuck_indexer_detector_->UpdateObserver(loop_->U(),
- ::aos::controls::kLoopFrequency);
-}
-
-void IndexerController::SetStatus(IndexerStatus *status) {
- status->avg_angular_velocity = average_angular_velocity_;
-
- status->angular_velocity = X_hat_current_(1, 0);
- status->ready = ready_;
-
- status->voltage_error = X_hat_current_(2, 0);
- status->stuck_voltage_error = stuck_indexer_X_hat_current_(2, 0);
- status->position_error = position_error_;
- status->instantaneous_velocity = dt_velocity_;
-
- status->stuck = IsStuck();
-
- status->stuck_voltage = StuckVoltage();
-}
-
-void Indexer::Reset() { indexer_.Reset(); }
-
-void Indexer::Iterate(const control_loops::IndexerGoal *goal,
- const double *position, double *output,
- control_loops::IndexerStatus *status) {
- if (goal) {
- // Start indexing at the suggested velocity.
- // If a "stuck" event is detected, reverse. Stay reversed until either
- // unstuck, or 0.5 seconds have elapsed.
- // Then, start going forwards. Don't detect stuck for 0.5 seconds.
-
- monotonic_clock::time_point monotonic_now = monotonic_clock::now();
- switch (state_) {
- case State::RUNNING:
- // Pass the velocity goal through.
- indexer_.set_goal(goal->angular_velocity);
- // If we are stuck and weren't just reversing, try reversing to unstick
- // us. We don't want to chatter back and forth too fast if reversing
- // isn't working.
- if (indexer_.IsStuck() &&
- monotonic_now > kForwardTimeout + last_transition_time_) {
- state_ = State::REVERSING;
- last_transition_time_ = monotonic_now;
- indexer_.Reset();
- }
- break;
- case State::REVERSING:
- // "Reverse" "slowly".
- indexer_.set_goal(-5.0 * aos::sign(goal->angular_velocity));
-
- // If we've timed out or are no longer stuck, try running again.
- if ((!indexer_.IsStuck() &&
- monotonic_now > last_transition_time_ + kReverseMinTimeout) ||
- monotonic_now > kReverseTimeout + last_transition_time_) {
- state_ = State::RUNNING;
-
- // Only reset if we got stuck going this way too.
- if (monotonic_now > kReverseTimeout + last_transition_time_) {
- indexer_.Reset();
- }
- last_transition_time_ = monotonic_now;
- }
- break;
- }
- }
-
- indexer_.set_position(*position);
-
- indexer_.Update(output == nullptr);
-
- indexer_.SetStatus(status);
- status->state = static_cast<int32_t>(state_);
-
- if (output) {
- *output = indexer_.voltage();
- }
-}
-
-} // namespace indexer
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2017
diff --git a/y2017/control_loops/superstructure/indexer/indexer.h b/y2017/control_loops/superstructure/indexer/indexer.h
deleted file mode 100644
index 9d71eed..0000000
--- a/y2017/control_loops/superstructure/indexer/indexer.h
+++ /dev/null
@@ -1,121 +0,0 @@
-#ifndef Y2017_CONTROL_LOOPS_INDEXER_INDEXER_H_
-#define Y2017_CONTROL_LOOPS_INDEXER_INDEXER_H_
-
-#include <memory>
-
-#include "aos/common/controls/control_loop.h"
-#include "aos/common/time.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-
-#include "y2017/control_loops/superstructure/indexer/indexer_integral_plant.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
-
-namespace y2017 {
-namespace control_loops {
-namespace superstructure {
-namespace indexer {
-
-class IndexerController {
- public:
- IndexerController();
-
- // Sets the velocity goal in radians/sec
- void set_goal(double angular_velocity_goal);
- // Sets the current encoder position in radians
- void set_position(double current_position);
-
- // Populates the status structure.
- void SetStatus(control_loops::IndexerStatus *status);
-
- // Returns the control loop calculated voltage.
- double voltage() const;
-
- // Returns the instantaneous velocity.
- double velocity() const { return loop_->X_hat(1, 0); }
-
- double dt_velocity() const { return dt_velocity_; }
-
- double error() const { return error_; }
-
- // Returns true if the indexer is stuck.
- bool IsStuck() const;
- double StuckVoltage() const;
-
- // Executes the control loop for a cycle.
- void Update(bool disabled);
-
- // Resets the kalman filter and any other internal state.
- void Reset();
- // Resets the voltage error for when we reverse directions.
- void PartialReset();
-
- private:
- // The current sensor measurement.
- Eigen::Matrix<double, 1, 1> Y_;
- // The control loop.
- ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
-
- // The stuck indexer detector
- ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> stuck_indexer_detector_;
-
- // History array for calculating a filtered angular velocity.
- static constexpr int kHistoryLength = 5;
- ::std::array<double, kHistoryLength> history_;
- ptrdiff_t history_position_ = 0;
-
- double error_ = 0.0;
- double dt_velocity_ = 0.0;
- double last_position_ = 0.0;
- double average_angular_velocity_ = 0.0;
- double position_error_ = 0.0;
-
- Eigen::Matrix<double, 3, 1> X_hat_current_;
- Eigen::Matrix<double, 3, 1> stuck_indexer_X_hat_current_;
-
- bool ready_ = false;
- bool reset_ = false;
-
- DISALLOW_COPY_AND_ASSIGN(IndexerController);
-};
-
-class Indexer {
- public:
- Indexer() {}
-
- enum class State {
- // The system is running correctly, no stuck condition detected.
- RUNNING = 0,
- // The system is reversing to unjam.
- REVERSING = 1
- };
-
- // Iterates the indexer control loop one cycle. position and status must
- // never be nullptr. goal can be nullptr if no goal exists, and output should
- // be nullptr if disabled.
- void Iterate(const control_loops::IndexerGoal *goal, const double *position,
- double *output, control_loops::IndexerStatus *status);
-
- // Sets the indexer up to reset the kalman filter next time Iterate is called.
- void Reset();
-
- State state() const { return state_; }
-
- private:
- IndexerController indexer_;
-
- State state_ = State::RUNNING;
-
- // The last time that we transitioned from RUNNING to REVERSING or the
- // reverse. Used to implement the timeouts.
- ::aos::monotonic_clock::time_point last_transition_time_ =
- ::aos::monotonic_clock::min_time;
-
- DISALLOW_COPY_AND_ASSIGN(Indexer);
-};
-
-} // namespace indexer
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2017
-
-#endif // Y2017_CONTROL_LOOPS_INDEXER_INDEXER_H_
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
index 9d56542..3183634 100644
--- a/y2017/control_loops/superstructure/intake/intake.cc
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -18,11 +18,15 @@
new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
3, 1, 1>(MakeIntegralIntakeLoop())),
constants::GetValues().intake.zeroing,
- constants::Values::kIntakeRange, 0.3, 5.0) {}
+ constants::Values::kIntakeRange, 0.3, 5.0) {
+ clear_min_position();
+}
void Intake::Reset() {
state_ = State::UNINITIALIZED;
+ clear_min_position();
profiled_subsystem_.Reset();
+ disable_count_ = 0;
}
void Intake::Iterate(
@@ -82,6 +86,22 @@
if (unsafe_goal) {
profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
profiled_subsystem_.set_unprofiled_goal(unsafe_goal->distance);
+ if (unsafe_goal->disable_intake) {
+ disable = true;
+ ++disable_count_;
+ if (disable_count_ > 200) {
+ state_ = State::ESTOP;
+ }
+ } else {
+ disable_count_ = 0;
+ }
+ }
+
+ // Force the intake to be at least at min_position_ out.
+ if (profiled_subsystem_.unprofiled_goal(0, 0) < min_position_) {
+ LOG(INFO, "Limiting intake to %f from %f\n", min_position_,
+ profiled_subsystem_.unprofiled_goal(0, 0));
+ profiled_subsystem_.set_unprofiled_goal(min_position_);
}
// ESTOP if we hit the hard limits.
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index c6990bc..941b4ce 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -2,6 +2,7 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
#include "frc971/control_loops/profiled_subsystem.h"
+#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
namespace y2017 {
@@ -16,6 +17,17 @@
return profiled_subsystem_.goal(row, col);
}
+ // Sets the minimum position we'll allow the intake to be at. This forces the
+ // intake to stay out far enough to avoid collisions.
+ void set_min_position(double min_position) { min_position_ = min_position; }
+
+ // Moves min_position_ to a position which won't affect any other goal requests.
+ void clear_min_position() {
+ min_position_ = constants::Values::kIntakeRange.lower_hard;
+ }
+
+ double position() const { return profiled_subsystem_.position(); }
+
// The zeroing and operating voltages.
static constexpr double kZeroingVoltage = 2.5;
static constexpr double kOperatingVoltage = 12.0;
@@ -37,7 +49,11 @@
State state() const { return state_; }
private:
- State state_;
+ State state_ = State::UNINITIALIZED;
+ double min_position_;
+
+ // The number of times in a row we've been asked to disable.
+ int disable_count_ = 0;
::frc971::control_loops::SingleDOFProfiledSubsystem<
::frc971::zeroing::PotAndAbsEncoderZeroingEstimator> profiled_subsystem_;
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 14beff5..343d809 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -3,9 +3,10 @@
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/column/column.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
-#include "y2017/control_loops/superstructure/turret/turret.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
+#include "y2017/control_loops/superstructure/shooter/shooter.h"
namespace y2017 {
namespace control_loops {
@@ -30,32 +31,70 @@
if (WasReset()) {
LOG(ERROR, "WPILib reset, restarting\n");
hood_.Reset();
- turret_.Reset();
intake_.Reset();
shooter_.Reset();
- indexer_.Reset();
+ column_.Reset();
}
hood_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->hood) : nullptr,
&(position->hood),
output != nullptr ? &(output->voltage_hood) : nullptr,
&(status->hood));
- turret_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->turret) : nullptr,
- &(position->turret),
- output != nullptr ? &(output->voltage_turret) : nullptr,
- &(status->turret));
- intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake),
- output != nullptr ? &(output->voltage_intake) : nullptr,
- &(status->intake));
shooter_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->shooter) : nullptr,
&(position->theta_shooter), position->sent_time,
output != nullptr ? &(output->voltage_shooter) : nullptr,
&(status->shooter));
- indexer_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->indexer) : nullptr,
- &(position->theta_indexer),
- output != nullptr ? &(output->voltage_indexer) : nullptr,
- &(status->indexer));
+
+ // Implement collision avoidance by passing down a freeze or range restricting
+ // signal to the column and intake objects.
+
+ // Wait until the column is ready before doing collision avoidance.
+ if (unsafe_goal && column_.state() == column::Column::State::RUNNING) {
+ if (!ignore_collisions_) {
+ // The turret is in a position (or wants to be in a position) where we
+ // need the intake out. Push it out.
+ if (::std::abs(unsafe_goal->turret.angle) >
+ column::Column::kTurretNearZero ||
+ ::std::abs(column_.turret_position()) >
+ column::Column::kTurretNearZero) {
+ intake_.set_min_position(column::Column::kIntakeZeroingMinDistance);
+ } else {
+ intake_.clear_min_position();
+ }
+ // The intake is in a position where it could hit. Don't move the turret.
+ if (intake_.position() < column::Column::kIntakeZeroingMinDistance -
+ column::Column::kIntakeTolerance &&
+ ::std::abs(column_.turret_position()) >
+ column::Column::kTurretNearZero) {
+ column_.set_freeze(true);
+ } else {
+ column_.set_freeze(false);
+ }
+ } else {
+ // If we are ignoring collisions, unfreeze and un-limit the min.
+ column_.set_freeze(false);
+ intake_.clear_min_position();
+ }
+ } else {
+ column_.set_freeze(false);
+ }
+
+ // Make some noise if someone left this set...
+ if (ignore_collisions_) {
+ LOG(ERROR, "Collisions ignored\n");
+ }
+
+ intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
+ &(position->intake),
+ output != nullptr ? &(output->voltage_intake) : nullptr,
+ &(status->intake));
+
+ column_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->indexer) : nullptr,
+ unsafe_goal != nullptr ? &(unsafe_goal->turret) : nullptr,
+ &(position->column),
+ output != nullptr ? &(output->voltage_indexer) : nullptr,
+ output != nullptr ? &(output->voltage_turret) : nullptr,
+ &(status->indexer), &(status->turret), &intake_);
if (output && unsafe_goal) {
output->voltage_intake_rollers =
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index 575d1d3..a605db0 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -6,11 +6,10 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
-#include "y2017/control_loops/superstructure/indexer/indexer.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/shooter/shooter.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
-#include "y2017/control_loops/superstructure/turret/turret.h"
+#include "y2017/control_loops/superstructure/column/column.h"
namespace y2017 {
namespace control_loops {
@@ -24,10 +23,14 @@
&control_loops::superstructure_queue);
const hood::Hood &hood() const { return hood_; }
- const turret::Turret &turret() const { return turret_; }
const intake::Intake &intake() const { return intake_; }
const shooter::Shooter &shooter() const { return shooter_; }
- const indexer::Indexer &indexer() const { return indexer_; }
+ const column::Column &column() const { return column_; }
+
+ // Sets the ignore collisions bit. This should *not* be used on the robot.
+ void set_ignore_collisions(bool ignore_collisions) {
+ ignore_collisions_ = ignore_collisions;
+ }
protected:
virtual void RunIteration(
@@ -38,10 +41,12 @@
private:
hood::Hood hood_;
- turret::Turret turret_;
intake::Intake intake_;
shooter::Shooter shooter_;
- indexer::Indexer indexer_;
+ column::Column column_;
+
+ // If true, we ignore collisions.
+ bool ignore_collisions_ = false;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 99989b3..dd8a65a 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -18,6 +18,9 @@
// Voltage to send to the rollers. Positive is sucking in.
double voltage_rollers;
+
+ // If true, disable the intake so we can hang.
+ bool disable_intake;
};
struct IndexerGoal {
@@ -120,7 +123,7 @@
struct ColumnEstimatorState {
bool error;
bool zeroed;
- .frc971.HallEffectAndPositionEstimatorState intake;
+ .frc971.HallEffectAndPositionEstimatorState indexer;
.frc971.HallEffectAndPositionEstimatorState turret;
};
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 579fb1e..114b449 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -11,11 +11,10 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/column/column_plant.h"
#include "y2017/control_loops/superstructure/hood/hood_plant.h"
-#include "y2017/control_loops/superstructure/indexer/indexer_plant.h"
#include "y2017/control_loops/superstructure/intake/intake_plant.h"
#include "y2017/control_loops/superstructure/shooter/shooter_plant.h"
-#include "y2017/control_loops/superstructure/turret/turret_plant.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -49,25 +48,6 @@
double voltage_offset_ = 0.0;
};
-class IndexerPlant : public StateFeedbackPlant<2, 1, 1> {
- public:
- explicit IndexerPlant(StateFeedbackPlant<2, 1, 1> &&other)
- : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
-
- void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
- EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
- EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
- }
-
- double voltage_offset() const { return voltage_offset_; }
- void set_voltage_offset(double voltage_offset) {
- voltage_offset_ = voltage_offset;
- }
-
- private:
- double voltage_offset_ = 0.0;
-};
-
class HoodPlant : public StateFeedbackPlant<2, 1, 1> {
public:
explicit HoodPlant(StateFeedbackPlant<2, 1, 1> &&other)
@@ -87,23 +67,31 @@
double voltage_offset_ = 0.0;
};
-class TurretPlant : public StateFeedbackPlant<2, 1, 1> {
+class ColumnPlant : public StateFeedbackPlant<4, 2, 2> {
public:
- explicit TurretPlant(StateFeedbackPlant<2, 1, 1> &&other)
- : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+ explicit ColumnPlant(StateFeedbackPlant<4, 2, 2> &&other)
+ : StateFeedbackPlant<4, 2, 2>(::std::move(other)) {}
- void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
- EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
- EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+ void CheckU(const ::Eigen::Matrix<double, 2, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + indexer_voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + indexer_voltage_offset_);
+
+ EXPECT_LE(U(1, 0), U_max(1, 0) + 0.00001 + turret_voltage_offset_);
+ EXPECT_GE(U(1, 0), U_min(1, 0) - 0.00001 + turret_voltage_offset_);
}
- double voltage_offset() const { return voltage_offset_; }
- void set_voltage_offset(double voltage_offset) {
- voltage_offset_ = voltage_offset;
+ double indexer_voltage_offset() const { return indexer_voltage_offset_; }
+ void set_indexer_voltage_offset(double indexer_voltage_offset) {
+ indexer_voltage_offset_ = indexer_voltage_offset;
+ }
+ double turret_voltage_offset() const { return turret_voltage_offset_; }
+ void set_turret_voltage_offset(double turret_voltage_offset) {
+ turret_voltage_offset_ = turret_voltage_offset;
}
private:
- double voltage_offset_ = 0.0;
+ double indexer_voltage_offset_ = 0.0;
+ double turret_voltage_offset_ = 0.0;
};
class IntakePlant : public StateFeedbackPlant<2, 1, 1> {
@@ -134,10 +122,6 @@
::y2017::control_loops::superstructure::hood::MakeHoodPlant())),
hood_encoder_(constants::Values::kHoodEncoderIndexDifference),
- turret_plant_(new TurretPlant(
- ::y2017::control_loops::superstructure::turret::MakeTurretPlant())),
- turret_pot_encoder_(constants::Values::kTurretEncoderIndexDifference),
-
intake_plant_(new IntakePlant(
::y2017::control_loops::superstructure::intake::MakeIntakePlant())),
intake_pot_encoder_(constants::Values::kIntakeEncoderIndexDifference),
@@ -145,8 +129,10 @@
shooter_plant_(new ShooterPlant(::y2017::control_loops::superstructure::
shooter::MakeShooterPlant())),
- indexer_plant_(new IndexerPlant(::y2017::control_loops::superstructure::
- indexer::MakeIndexerPlant())),
+ indexer_encoder_(2.0 * M_PI),
+ turret_encoder_(2.0 * M_PI),
+ column_plant_(new ColumnPlant(
+ ::y2017::control_loops::superstructure::column::MakeColumnPlant())),
superstructure_queue_(".y2017.control_loops.superstructure", 0xdeadbeef,
".y2017.control_loops.superstructure.goal",
@@ -159,6 +145,9 @@
2.0);
// Start the turret out in the middle by default.
+ InitializeIndexerPosition(0.0);
+
+ // Start the turret out in the middle by default.
InitializeTurretPosition((constants::Values::kTurretRange.lower +
constants::Values::kTurretRange.upper) /
2.0);
@@ -178,13 +167,28 @@
constants::GetValues().hood.zeroing.measured_index_position);
}
- void InitializeTurretPosition(double start_pos) {
- turret_plant_->mutable_X(0, 0) = start_pos;
- turret_plant_->mutable_X(1, 0) = 0.0;
+ void InitializeIndexerPosition(double start_pos) {
+ column_plant_->mutable_X(0, 0) = start_pos;
+ column_plant_->mutable_X(1, 0) = 0.0;
- turret_pot_encoder_.Initialize(
- start_pos, kNoiseScalar, 0.0,
- constants::GetValues().turret.zeroing.measured_absolute_position);
+ indexer_encoder_.InitializeHallEffectAndPosition(
+ start_pos,
+ constants::GetValues().column.indexer_zeroing.lower_hall_position,
+ constants::GetValues().column.indexer_zeroing.upper_hall_position);
+
+ // Reset the turret encoder too (to the current position), since it has the
+ // potential to change.
+ InitializeTurretPosition(column_plant_->X(2, 0));
+ }
+
+ void InitializeTurretPosition(double start_pos) {
+ column_plant_->mutable_X(2, 0) = start_pos;
+ column_plant_->mutable_X(3, 0) = 0.0;
+
+ turret_encoder_.InitializeHallEffectAndPosition(
+ column_plant_->X(2, 0) - column_plant_->X(0, 0),
+ constants::GetValues().column.turret_zeroing.lower_hall_position,
+ constants::GetValues().column.turret_zeroing.upper_hall_position);
}
void InitializeIntakePosition(double start_pos) {
@@ -202,52 +206,58 @@
superstructure_queue_.position.MakeMessage();
hood_encoder_.GetSensorValues(&position->hood);
- turret_pot_encoder_.GetSensorValues(&position->turret);
intake_pot_encoder_.GetSensorValues(&position->intake);
position->theta_shooter = shooter_plant_->Y(0, 0);
- position->theta_indexer = indexer_plant_->Y(0, 0);
+
+ turret_encoder_.GetSensorValues(&position->column.turret);
+ indexer_encoder_.GetSensorValues(&position->column.indexer);
+
position.Send();
}
double hood_position() const { return hood_plant_->X(0, 0); }
double hood_angular_velocity() const { return hood_plant_->X(1, 0); }
- double turret_position() const { return turret_plant_->X(0, 0); }
- double turret_angular_velocity() const { return turret_plant_->X(1, 0); }
+ double turret_position() const { return column_plant_->X(2, 0); }
+ double turret_angular_velocity() const { return column_plant_->X(3, 0); }
double intake_position() const { return intake_plant_->X(0, 0); }
double intake_velocity() const { return intake_plant_->X(1, 0); }
double shooter_velocity() const { return shooter_plant_->X(1, 0); }
- double indexer_velocity() const { return indexer_plant_->X(1, 0); }
+ double indexer_velocity() const { return column_plant_->X(1, 0); }
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
- void set_hood_power_error(double power_error) {
- hood_plant_->set_voltage_offset(power_error);
+ void set_hood_voltage_offset(double voltage_offset) {
+ hood_plant_->set_voltage_offset(voltage_offset);
}
- void set_turret_power_error(double power_error) {
- turret_plant_->set_voltage_offset(power_error);
+ void set_intake_voltage_offset(double voltage_offset) {
+ intake_plant_->set_voltage_offset(voltage_offset);
}
- void set_intake_power_error(double power_error) {
- intake_plant_->set_voltage_offset(power_error);
+ void set_shooter_voltage_offset(double voltage_offset) {
+ shooter_plant_->set_voltage_offset(voltage_offset);
}
- void set_shooter_voltage_offset(double power_error) {
- shooter_plant_->set_voltage_offset(power_error);
+ void set_indexer_voltage_offset(double voltage_offset) {
+ column_plant_->set_indexer_voltage_offset(voltage_offset);
}
- void set_indexer_voltage_offset(double power_error) {
- indexer_plant_->set_voltage_offset(power_error);
+ void set_turret_voltage_offset(double voltage_offset) {
+ column_plant_->set_turret_voltage_offset(voltage_offset);
}
// Triggers the indexer to freeze in position.
void set_freeze_indexer(bool freeze_indexer) {
freeze_indexer_ = freeze_indexer;
}
+ // Triggers the turret to freeze relative to the indexer.
+ void set_freeze_turret(bool freeze_turret) {
+ freeze_turret_ = freeze_turret;
+ }
// Simulates the superstructure for a single timestep.
void Simulate() {
@@ -261,12 +271,19 @@
? superstructure::hood::Hood::kOperatingVoltage
: superstructure::hood::Hood::kZeroingVoltage;
- const double voltage_check_turret =
- (static_cast<turret::Turret::State>(
+ const double voltage_check_indexer =
+ (static_cast<column::Column::State>(
superstructure_queue_.status->turret.state) ==
- turret::Turret::State::RUNNING)
- ? superstructure::turret::Turret::kOperatingVoltage
- : superstructure::turret::Turret::kZeroingVoltage;
+ column::Column::State::RUNNING)
+ ? superstructure::column::Column::kOperatingVoltage
+ : superstructure::column::Column::kZeroingVoltage;
+
+ const double voltage_check_turret =
+ (static_cast<column::Column::State>(
+ superstructure_queue_.status->turret.state) ==
+ column::Column::State::RUNNING)
+ ? superstructure::column::Column::kOperatingVoltage
+ : superstructure::column::Column::kZeroingVoltage;
const double voltage_check_intake =
(static_cast<intake::Intake::State>(
@@ -278,20 +295,20 @@
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_hood),
voltage_check_hood);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_turret),
- voltage_check_turret);
-
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
voltage_check_intake);
+ EXPECT_LE(::std::abs(superstructure_queue_.output->voltage_indexer),
+ voltage_check_indexer)
+ << ": check voltage " << voltage_check_indexer;
+
+ CHECK_LE(::std::abs(superstructure_queue_.output->voltage_turret),
+ voltage_check_turret);
+
::Eigen::Matrix<double, 1, 1> hood_U;
hood_U << superstructure_queue_.output->voltage_hood +
hood_plant_->voltage_offset();
- ::Eigen::Matrix<double, 1, 1> turret_U;
- turret_U << superstructure_queue_.output->voltage_turret +
- turret_plant_->voltage_offset();
-
::Eigen::Matrix<double, 1, 1> intake_U;
intake_U << superstructure_queue_.output->voltage_intake +
intake_plant_->voltage_offset();
@@ -300,22 +317,43 @@
shooter_U << superstructure_queue_.output->voltage_shooter +
shooter_plant_->voltage_offset();
- ::Eigen::Matrix<double, 1, 1> indexer_U;
- indexer_U << superstructure_queue_.output->voltage_indexer +
- indexer_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 2, 1> column_U;
+ column_U << superstructure_queue_.output->voltage_indexer +
+ column_plant_->indexer_voltage_offset(),
+ superstructure_queue_.output->voltage_turret +
+ column_plant_->turret_voltage_offset();
hood_plant_->Update(hood_U);
- turret_plant_->Update(turret_U);
intake_plant_->Update(intake_U);
shooter_plant_->Update(shooter_U);
+
+ // Freeze the indexer by setting it's voltage to 0, and then freezing the
+ // position and setting the velocity to 0.
+ const double indexer_position = column_plant_->X(0, 0);
+ const double turret_encoder_position =
+ column_plant_->X(2, 0) - column_plant_->X(0, 0);
if (freeze_indexer_) {
- indexer_plant_->mutable_X(1, 0) = 0.0;
- } else {
- indexer_plant_->Update(indexer_U);
+ column_U(0, 0) = 0.0;
+ }
+ if (freeze_turret_) {
+ column_U(1, 0) = 0.0;
+ }
+
+ column_plant_->Update(column_U);
+
+ if (freeze_indexer_) {
+ column_plant_->mutable_X(0, 0) = indexer_position;
+ column_plant_->mutable_X(1, 0) = 0.0;
+ column_plant_->UpdateY(column_U);
+ }
+ if (freeze_turret_) {
+ column_plant_->mutable_X(2, 0) =
+ column_plant_->X(0, 0) + turret_encoder_position;
+ column_plant_->mutable_X(3, 0) = column_plant_->X(1, 0);
+ column_plant_->UpdateY(column_U);
}
double angle_hood = hood_plant_->Y(0, 0);
- const double angle_turret = turret_plant_->Y(0, 0);
const double position_intake = intake_plant_->Y(0, 0);
// The hood is special. We don't want to fault when we hit the hard stop.
@@ -324,22 +362,47 @@
LOG(INFO, "At the hood upper hard stop of %f\n",
constants::Values::kHoodRange.upper_hard);
angle_hood = constants::Values::kHoodRange.upper_hard;
- hood_plant_->mutable_Y(0, 0) = angle_hood;
hood_plant_->mutable_X(0, 0) = angle_hood;
hood_plant_->mutable_X(1, 0) = 0.0;
+ hood_plant_->UpdateY(hood_U);
} else if (angle_hood < constants::Values::kHoodRange.lower_hard) {
LOG(INFO, "At the hood lower hard stop of %f\n",
constants::Values::kHoodRange.lower_hard);
angle_hood = constants::Values::kHoodRange.lower_hard;
- hood_plant_->mutable_Y(0, 0) = angle_hood;
hood_plant_->mutable_X(0, 0) = angle_hood;
hood_plant_->mutable_X(1, 0) = 0.0;
+ hood_plant_->UpdateY(hood_U);
+ }
+
+ const double angle_indexer = column_plant_->Y(0, 0);
+ const double encoder_angle_turret = column_plant_->Y(1, 0);
+ double angle_turret = column_plant_->X(2, 0);
+
+ // The expected zeroing procedure involves yanking on the wires for the
+ // turret in some cases. So, implement the hard stop like the hood.
+ if (angle_turret > constants::Values::kTurretRange.upper_hard) {
+ LOG(INFO, "At the turret upper hard stop of %f\n",
+ constants::Values::kTurretRange.upper_hard);
+ angle_turret = constants::Values::kTurretRange.upper_hard;
+ column_plant_->mutable_X(2, 0) = angle_turret;
+ column_plant_->mutable_X(3, 0) = 0.0;
+
+ column_plant_->UpdateY(column_U);
+ } else if (angle_turret < constants::Values::kTurretRange.lower_hard) {
+ LOG(INFO, "At the turret lower hard stop of %f\n",
+ constants::Values::kTurretRange.lower_hard);
+ angle_turret = constants::Values::kTurretRange.lower_hard;
+ column_plant_->mutable_X(2, 0) = angle_turret;
+ column_plant_->mutable_X(3, 0) = 0.0;
+
+ column_plant_->UpdateY(column_U);
}
hood_encoder_.MoveTo(angle_hood);
- turret_pot_encoder_.MoveTo(angle_turret);
intake_pot_encoder_.MoveTo(position_intake);
+ indexer_encoder_.MoveTo(angle_indexer);
+ turret_encoder_.MoveTo(encoder_angle_turret);
EXPECT_GE(angle_turret, constants::Values::kTurretRange.lower_hard);
EXPECT_LE(angle_turret, constants::Values::kTurretRange.upper_hard);
@@ -351,16 +414,17 @@
::std::unique_ptr<HoodPlant> hood_plant_;
PositionSensorSimulator hood_encoder_;
- ::std::unique_ptr<TurretPlant> turret_plant_;
- PositionSensorSimulator turret_pot_encoder_;
-
::std::unique_ptr<IntakePlant> intake_plant_;
PositionSensorSimulator intake_pot_encoder_;
::std::unique_ptr<ShooterPlant> shooter_plant_;
- ::std::unique_ptr<IndexerPlant> indexer_plant_;
+ PositionSensorSimulator indexer_encoder_;
+ PositionSensorSimulator turret_encoder_;
+ ::std::unique_ptr<ColumnPlant> column_plant_;
+
bool freeze_indexer_ = false;
+ bool freeze_turret_ = false;
SuperstructureQueue superstructure_queue_;
};
@@ -557,6 +621,8 @@
// Makes sure that the voltage on a motor is properly pulled back after
// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
+//
+// We are going to disable collision detection to make this easier to implement.
TEST_F(SuperstructureTest, SaturationTest) {
// Zero it before we move.
{
@@ -569,6 +635,8 @@
RunForTime(chrono::seconds(8));
VerifyNearGoal();
+ superstructure_.set_ignore_collisions(true);
+
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
@@ -681,6 +749,37 @@
EXPECT_NEAR(constants::Values::kTurretRange.lower,
superstructure_queue_.status->turret.position, 0.001);
+ EXPECT_NEAR(column::Column::kIntakeZeroingMinDistance,
+ superstructure_queue_.status->intake.position, 0.001);
+
+ // Now, center the turret so we can try ridiculous things without having the
+ // intake pushed out.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->hood.angle = -100.0;
+ goal->hood.profile_params.max_velocity = 1;
+ goal->hood.profile_params.max_acceleration = 0.5;
+
+ goal->turret.angle = 0.0;
+ goal->turret.profile_params.max_velocity = 1;
+ goal->turret.profile_params.max_acceleration = 0.5;
+
+ goal->intake.distance = -100.0;
+ goal->intake.profile_params.max_velocity = 1;
+ goal->intake.profile_params.max_acceleration = 0.5;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(10));
+
+ // Check that we are near our soft limit.
+ superstructure_queue_.status.FetchLatest();
+ EXPECT_NEAR(constants::Values::kHoodRange.lower,
+ superstructure_queue_.status->hood.position, 0.001);
+
+ EXPECT_NEAR(0.0, superstructure_queue_.status->turret.position, 0.001);
+
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
superstructure_queue_.status->intake.position, 0.001);
}
@@ -697,7 +796,7 @@
goal->turret.profile_params.max_velocity = 1;
goal->turret.profile_params.max_acceleration = 0.5;
- goal->intake.distance = constants::Values::kIntakeRange.lower;
+ goal->intake.distance = constants::Values::kIntakeRange.upper;
goal->intake.profile_params.max_velocity = 1;
goal->intake.profile_params.max_acceleration = 0.5;
ASSERT_TRUE(goal.Send());
@@ -713,8 +812,8 @@
RunForTime(chrono::seconds(5));
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
- EXPECT_EQ(turret::Turret::State::RUNNING, superstructure_.turret().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
+ EXPECT_EQ(column::Column::State::RUNNING, superstructure_.column().state());
}
TEST_F(SuperstructureTest, LowerHardstopStartup) {
@@ -730,10 +829,10 @@
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower;
goal->turret.angle = constants::Values::kTurretRange.lower;
- goal->intake.distance = constants::Values::kIntakeRange.lower;
+ goal->intake.distance = constants::Values::kIntakeRange.upper;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(5));
+ RunForTime(chrono::seconds(10));
VerifyNearGoal();
}
@@ -755,7 +854,7 @@
goal->intake.distance = constants::Values::kIntakeRange.upper;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(5));
+ RunForTime(chrono::seconds(10));
VerifyNearGoal();
}
@@ -765,8 +864,7 @@
superstructure_plant_.InitializeHoodPosition(
constants::Values::kHoodRange.upper);
- superstructure_plant_.InitializeTurretPosition(
- constants::Values::kTurretRange.upper);
+ superstructure_plant_.InitializeTurretPosition(0.0);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
@@ -775,29 +873,30 @@
goal->hood.angle = constants::Values::kHoodRange.upper - 0.1;
goal->turret.angle = constants::Values::kTurretRange.upper - 0.1;
goal->intake.distance = constants::Values::kIntakeRange.upper - 0.1;
+ goal->indexer.angular_velocity = -5.0;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(10));
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
- EXPECT_EQ(turret::Turret::State::RUNNING, superstructure_.turret().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
+ EXPECT_EQ(column::Column::State::RUNNING, superstructure_.column().state());
VerifyNearGoal();
SimulateSensorReset();
RunForTime(chrono::milliseconds(100));
EXPECT_EQ(hood::Hood::State::ZEROING, superstructure_.hood().state());
- EXPECT_EQ(turret::Turret::State::UNINITIALIZED,
- superstructure_.turret().state());
EXPECT_EQ(intake::Intake::State::UNINITIALIZED,
superstructure_.intake().state());
+ EXPECT_EQ(column::Column::State::ZEROING_UNINITIALIZED,
+ superstructure_.column().state());
- RunForTime(chrono::milliseconds(5000));
+ RunForTime(chrono::seconds(10));
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
- EXPECT_EQ(turret::Turret::State::RUNNING, superstructure_.turret().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
+ EXPECT_EQ(column::Column::State::RUNNING, superstructure_.column().state());
VerifyNearGoal();
}
@@ -814,14 +913,14 @@
RunForTime(chrono::milliseconds(100), false);
EXPECT_EQ(0.0, superstructure_.hood().goal(0, 0));
- EXPECT_EQ(0.0, superstructure_.turret().goal(0, 0));
EXPECT_EQ(0.0, superstructure_.intake().goal(0, 0));
+ EXPECT_EQ(0.0, superstructure_.column().goal(2, 0));
// Now make sure they move correctly
RunForTime(chrono::seconds(4), true);
EXPECT_NE(0.0, superstructure_.hood().goal(0, 0));
- EXPECT_NE(0.0, superstructure_.turret().goal(0, 0));
EXPECT_NE(0.0, superstructure_.intake().goal(0, 0));
+ EXPECT_NE(0.0, superstructure_.column().goal(2, 0));
}
// Tests that zeroing while disabled works. Starts the superstructure near a
@@ -834,7 +933,7 @@
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower;
- goal->turret.angle = constants::Values::kTurretRange.lower;
+ goal->turret.angle = 0.0;
goal->intake.distance = constants::Values::kIntakeRange.lower;
ASSERT_TRUE(goal.Send());
}
@@ -843,18 +942,25 @@
RunForTime(chrono::seconds(2), false);
EXPECT_EQ(hood::Hood::State::DISABLED_INITIALIZED,
superstructure_.hood().state());
- EXPECT_EQ(turret::Turret::State::RUNNING,
- superstructure_.turret().state());
- EXPECT_EQ(intake::Intake::State::RUNNING,
- superstructure_.intake().state());
+ EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
+ EXPECT_EQ(column::Column::State::ZEROING_UNINITIALIZED,
+ superstructure_.column().state());
- superstructure_plant_.set_hood_power_error(2.0);
+ superstructure_plant_.set_hood_voltage_offset(2.0);
+
+ superstructure_plant_.set_turret_voltage_offset(-1.5);
+ superstructure_plant_.set_indexer_voltage_offset(2.0);
RunForTime(chrono::seconds(1), false);
+ superstructure_plant_.set_hood_voltage_offset(0.0);
+ RunForTime(chrono::seconds(5), false);
+
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
- EXPECT_EQ(turret::Turret::State::RUNNING, superstructure_.turret().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
+ EXPECT_EQ(column::Column::State::RUNNING, superstructure_.column().state());
+ superstructure_plant_.set_turret_voltage_offset(0.0);
+ superstructure_plant_.set_indexer_voltage_offset(0.0);
RunForTime(chrono::seconds(4), true);
@@ -871,7 +977,7 @@
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
goal->shooter.angular_velocity = 300.0;
goal->indexer.angular_velocity = 20.0;
ASSERT_TRUE(goal.Send());
@@ -885,7 +991,7 @@
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
goal->shooter.angular_velocity = 0.0;
goal->indexer.angular_velocity = 0.0;
ASSERT_TRUE(goal.Send());
@@ -907,7 +1013,7 @@
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
goal->shooter.angular_velocity = 200.0;
goal->indexer.angular_velocity = 20.0;
ASSERT_TRUE(goal.Send());
@@ -927,7 +1033,7 @@
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
goal->shooter.angular_velocity = 0.0;
goal->indexer.angular_velocity = 5.0;
ASSERT_TRUE(goal.Send());
@@ -944,9 +1050,9 @@
RunIteration();
superstructure_queue_.status.FetchLatest();
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
- if (static_cast<indexer::Indexer::State>(
+ if (static_cast<column::Column::IndexerState>(
superstructure_queue_.status->indexer.state) ==
- indexer::Indexer::State::REVERSING) {
+ column::Column::IndexerState::REVERSING) {
break;
}
}
@@ -959,7 +1065,8 @@
// Grab the position we were stuck at.
superstructure_queue_.position.FetchLatest();
ASSERT_TRUE(superstructure_queue_.position.get() != nullptr);
- const double indexer_position = superstructure_queue_.position->theta_indexer;
+ const double indexer_position =
+ superstructure_queue_.position->column.indexer.position;
// Now, unstick it.
superstructure_plant_.set_freeze_indexer(false);
@@ -968,9 +1075,9 @@
RunIteration();
superstructure_queue_.status.FetchLatest();
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
- if (static_cast<indexer::Indexer::State>(
+ if (static_cast<column::Column::IndexerState>(
superstructure_queue_.status->indexer.state) ==
- indexer::Indexer::State::RUNNING) {
+ column::Column::IndexerState::RUNNING) {
break;
}
}
@@ -986,7 +1093,7 @@
superstructure_queue_.position.FetchLatest();
ASSERT_TRUE(superstructure_queue_.position.get() != nullptr);
const double unstuck_indexer_position =
- superstructure_queue_.position->theta_indexer;
+ superstructure_queue_.position->column.indexer.position;
EXPECT_LT(unstuck_indexer_position, indexer_position - 0.1);
// Now, verify that everything works as expected.
@@ -1002,7 +1109,7 @@
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
goal->shooter.angular_velocity = 0.0;
goal->indexer.angular_velocity = 5.0;
ASSERT_TRUE(goal.Send());
@@ -1019,9 +1126,9 @@
RunIteration();
superstructure_queue_.status.FetchLatest();
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
- if (static_cast<indexer::Indexer::State>(
+ if (static_cast<column::Column::IndexerState>(
superstructure_queue_.status->indexer.state) ==
- indexer::Indexer::State::REVERSING) {
+ column::Column::IndexerState::REVERSING) {
break;
}
}
@@ -1037,9 +1144,9 @@
RunIteration();
superstructure_queue_.status.FetchLatest();
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
- if (static_cast<indexer::Indexer::State>(
+ if (static_cast<column::Column::IndexerState>(
superstructure_queue_.status->indexer.state) ==
- indexer::Indexer::State::RUNNING) {
+ column::Column::IndexerState::RUNNING) {
break;
}
}
@@ -1051,6 +1158,8 @@
chrono::milliseconds(600));
EXPECT_TRUE(unstuck_detection_time - unstuck_start_time >
chrono::milliseconds(400));
+ LOG(INFO, "Unstuck time is %ldms",
+ (unstuck_detection_time - unstuck_start_time).count() / 1000000);
// Now, make sure it transitions to stuck again after a delay.
const auto restuck_start_time = monotonic_clock::now();
@@ -1059,9 +1168,9 @@
RunIteration();
superstructure_queue_.status.FetchLatest();
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
- if (static_cast<indexer::Indexer::State>(
+ if (static_cast<column::Column::IndexerState>(
superstructure_queue_.status->indexer.state) ==
- indexer::Indexer::State::REVERSING) {
+ column::Column::IndexerState::REVERSING) {
break;
}
}
@@ -1074,6 +1183,14 @@
chrono::milliseconds(600));
}
+// TODO(austin): What happens if either fuse blows?
+// TODO(austin): What happens if either fuse blows (while zeroing or operating)?
+// TODO(austin): What happens if the indexer stalls while running?
+// Aren tried it and nothing really happens.
+
+// TODO(austin): Indexer zeroing error detection.
+// TODO(austin): Detect detached turret encoder
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2017/control_loops/superstructure/turret/BUILD b/y2017/control_loops/superstructure/turret/BUILD
deleted file mode 100644
index 8969693..0000000
--- a/y2017/control_loops/superstructure/turret/BUILD
+++ /dev/null
@@ -1,46 +0,0 @@
-genrule(
- name = 'genrule_turret',
- cmd = '$(location //y2017/control_loops/python:turret) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:turret',
- ],
- outs = [
- 'turret_plant.h',
- 'turret_plant.cc',
- 'turret_integral_plant.h',
- 'turret_integral_plant.cc',
- ],
-)
-
-cc_library(
- name = 'turret_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'turret_plant.cc',
- 'turret_integral_plant.cc',
- ],
- hdrs = [
- 'turret_plant.h',
- 'turret_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
-)
-
-cc_library(
- name = 'turret',
- visibility = ['//visibility:public'],
- srcs = [
- 'turret.cc',
- ],
- hdrs = [
- 'turret.h',
- ],
- deps = [
- ':turret_plants',
- '//frc971/control_loops:profiled_subsystem',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
-)
diff --git a/y2017/control_loops/superstructure/turret/turret.cc b/y2017/control_loops/superstructure/turret/turret.cc
deleted file mode 100644
index 3a3a126..0000000
--- a/y2017/control_loops/superstructure/turret/turret.cc
+++ /dev/null
@@ -1,126 +0,0 @@
-#include "y2017/control_loops/superstructure/turret/turret.h"
-
-#include "y2017/constants.h"
-#include "y2017/control_loops/superstructure/turret/turret_integral_plant.h"
-
-namespace y2017 {
-namespace control_loops {
-namespace superstructure {
-namespace turret {
-
-constexpr double Turret::kZeroingVoltage;
-constexpr double Turret::kOperatingVoltage;
-
-Turret::Turret()
- : profiled_subsystem_(
- ::std::unique_ptr<
- ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
- new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- 3, 1, 1>(MakeIntegralTurretLoop())),
- constants::GetValues().turret.zeroing,
- constants::Values::kTurretRange, 7.0, 50.0) {}
-
-void Turret::Reset() {
- state_ = State::UNINITIALIZED;
- profiled_subsystem_.Reset();
-}
-
-void Turret::Iterate(
- const control_loops::TurretGoal *unsafe_goal,
- const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
- bool disable = output == nullptr;
- profiled_subsystem_.Correct(*position);
-
- switch (state_) {
- case State::UNINITIALIZED:
- // Wait in the uninitialized state until the turret is initialized.
- LOG(DEBUG, "Uninitialized, waiting for turret\n");
- if (profiled_subsystem_.initialized()) {
- state_ = State::DISABLED_INITIALIZED;
- }
- disable = true;
- break;
-
- case State::DISABLED_INITIALIZED:
- // Wait here until we are either fully zeroed while disabled, or we become
- // enabled.
- if (disable) {
- if (profiled_subsystem_.zeroed()) {
- state_ = State::RUNNING;
- }
- } else {
- state_ = State::ZEROING;
- }
-
- // Set the goals to where we are now so when we start back up, we don't
- // jump.
- profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
- // Set up the profile to be the zeroing profile.
- profiled_subsystem_.AdjustProfile(0.10, 1);
-
- // We are not ready to start doing anything yet.
- disable = true;
- break;
-
- case State::ZEROING:
- // Now, zero by actively holding still.
- if (profiled_subsystem_.zeroed()) {
- state_ = State::RUNNING;
- } else if (disable) {
- state_ = State::DISABLED_INITIALIZED;
- }
- break;
-
- case State::RUNNING: {
- if (disable) {
- // Reset the profile to the current position so it starts from here when
- // we get re-enabled.
- profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
- }
-
- if (unsafe_goal) {
- profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
- profiled_subsystem_.set_unprofiled_goal(unsafe_goal->angle);
- }
-
- // ESTOP if we hit the hard limits.
- if (profiled_subsystem_.CheckHardLimits() ||
- profiled_subsystem_.error()) {
- state_ = State::ESTOP;
- }
- } break;
-
- case State::ESTOP:
- LOG(ERROR, "Estop\n");
- disable = true;
- break;
- }
-
- // Set the voltage limits.
- const double max_voltage =
- (state_ == State::RUNNING) ? kOperatingVoltage : kZeroingVoltage;
-
- profiled_subsystem_.set_max_voltage({{max_voltage}});
-
- // Calculate the loops for a cycle.
- profiled_subsystem_.Update(disable);
-
- // Write out all the voltages.
- if (output) {
- *output = profiled_subsystem_.voltage();
- }
-
- // Save debug/internal state.
- // TODO(austin): Save more.
- status->zeroed = profiled_subsystem_.zeroed();
-
- profiled_subsystem_.PopulateStatus(status);
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
-}
-
-} // namespace turret
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2017
diff --git a/y2017/control_loops/superstructure/turret/turret.h b/y2017/control_loops/superstructure/turret/turret.h
deleted file mode 100644
index 5499d92..0000000
--- a/y2017/control_loops/superstructure/turret/turret.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_TURRET_H_
-#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_TURRET_H_
-
-#include "frc971/control_loops/profiled_subsystem.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
-
-namespace y2017 {
-namespace control_loops {
-namespace superstructure {
-namespace turret {
-
-class Turret {
- public:
- Turret();
- double goal(int row, int col) const {
- return profiled_subsystem_.goal(row, col);
- }
-
- // The zeroing and operating voltages.
- static constexpr double kZeroingVoltage = 2.5;
- static constexpr double kOperatingVoltage = 12.0;
-
- void Iterate(const control_loops::TurretGoal *unsafe_goal,
- const ::frc971::PotAndAbsolutePosition *position,
- double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
-
- void Reset();
-
- enum class State : int32_t{
- UNINITIALIZED,
- DISABLED_INITIALIZED,
- ZEROING,
- RUNNING,
- ESTOP,
- };
-
- State state() const { return state_; }
-
- private:
- State state_;
-
- ::frc971::control_loops::SingleDOFProfiledSubsystem<
- ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator>
- profiled_subsystem_;
-};
-
-} // namespace turret
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2017
-
-#endif // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_TURRET_H_
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 09d36a6..2873d2c 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -334,7 +334,7 @@
CopyPosition(indexer_counter_, &superstructure_message->column.indexer,
Values::kIndexerEncoderCountsPerRevolution,
- Values::kIndexerEncoderRatio, true);
+ Values::kIndexerEncoderRatio, false);
superstructure_message->theta_shooter =
encoder_translate(shooter_encoder_->GetRaw(),
@@ -347,7 +347,7 @@
CopyPosition(turret_counter_, &superstructure_message->column.turret,
Values::kTurretEncoderCountsPerRevolution,
- Values::kTurretEncoderRatio, true);
+ Values::kTurretEncoderRatio, false);
superstructure_message.Send();
}
@@ -530,9 +530,8 @@
-kMaxBringupPower, kMaxBringupPower) /
12.0);
intake_rollers_victor_->SetSpeed(queue->voltage_intake_rollers / 12.0);
- indexer_victor_->SetSpeed(queue->voltage_indexer / 12.0);
- indexer_roller_victor_->SetSpeed(queue->voltage_indexer_rollers /
- 12.0);
+ indexer_victor_->SetSpeed(-queue->voltage_indexer / 12.0);
+ indexer_roller_victor_->SetSpeed(queue->voltage_indexer_rollers / 12.0);
turret_victor_->SetSpeed(::aos::Clip(-queue->voltage_turret,
-kMaxBringupPower, kMaxBringupPower) /
12.0);