Converted hood to only need an index pulse.
We now have a more complicated seek mechanism which runs the hood to
each hard stop, or until we find both index pulses.
Change-Id: I9932cc158beec0bc55dc0e908accb0aea6a73506
diff --git a/y2017/analysis/hood_test b/y2017/analysis/hood_test
index 3bbc0f0..2d88970 100644
--- a/y2017/analysis/hood_test
+++ b/y2017/analysis/hood_test
@@ -2,5 +2,11 @@
superstructure_lib_test status hood estimator_state position
superstructure_lib_test status hood state
+superstructure_lib_test status hood position
+superstructure_lib_test status hood velocity
+superstructure_lib_test status hood goal_position
+superstructure_lib_test status hood goal_velocity
+
+superstructure_lib_test status hood unprofiled_goal_position
superstructure_lib_test goal hood angle
-superstructure_lib_test position hood pot
+superstructure_lib_test position hood encoder
diff --git a/y2017/constants.cc b/y2017/constants.cc
index c716ecf..2ac4875 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -71,45 +71,59 @@
intake->zeroing.average_filter_size = Values::kZeroingSampleSize;
intake->zeroing.one_revolution_distance = Values::kIntakeEncoderIndexDifference;
- intake->zeroing.measured_absolute_position = 0;
intake->zeroing.zeroing_threshold = 0.001;
intake->zeroing.moving_buffer_size = 9;
intake->zeroing.allowable_encoder_error = 0.3;
turret->zeroing.average_filter_size = Values::kZeroingSampleSize;
- turret->zeroing.one_revolution_distance = Values::kTurretEncoderIndexDifference;
- turret->zeroing.measured_absolute_position = 0;
+ turret->zeroing.one_revolution_distance =
+ Values::kTurretEncoderIndexDifference;
turret->zeroing.zeroing_threshold = 0.001;
turret->zeroing.moving_buffer_size = 9;
turret->zeroing.allowable_encoder_error = 0.3;
- hood->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ hood->zeroing.index_pulse_count = 2;
hood->zeroing.index_difference = Values::kHoodEncoderIndexDifference;
- hood->zeroing.measured_index_position = 0.1;
- hood->zeroing.allowable_encoder_error = 0.3;
+ hood->zeroing.known_index_pulse = 0;
switch (team) {
// A set of constants for tests.
case 1:
intake->pot_offset = 0;
+ intake->zeroing.measured_absolute_position = 0;
+
turret->pot_offset = 0;
+ turret->zeroing.measured_absolute_position = 0;
+
hood->pot_offset = 0.1;
+ hood->zeroing.measured_index_position = 0.05;
+
r->down_error = 0;
r->vision_name = "test";
break;
case kCompTeamNumber:
intake->pot_offset = 0;
+ intake->zeroing.measured_absolute_position = 0;
+
turret->pot_offset = 0;
- hood->pot_offset = 0.1;
+ turret->zeroing.measured_absolute_position = 0;
+
+ hood->zeroing.measured_index_position = 0.05;
+
r->down_error = 0;
r->vision_name = "competition";
break;
case kPracticeTeamNumber:
intake->pot_offset = 0;
+ intake->zeroing.measured_absolute_position = 0;
+
turret->pot_offset = 0;
- hood->pot_offset = 0.1;
+ turret->zeroing.measured_absolute_position = 0;
+
+ hood->zeroing.measured_index_position = 0.05;
+
r->down_error = 0;
r->vision_name = "practice";
break;
diff --git a/y2017/constants.h b/y2017/constants.h
index 1ad582b..5305367 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -39,7 +39,7 @@
struct Hood {
double pot_offset;
- ::frc971::constants::PotAndIndexPulseZeroingConstants zeroing;
+ ::frc971::constants::EncoderPlusIndexZeroingConstants zeroing;
};
static const int kZeroingSampleSize = 200;
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index 8684c98..40d3473 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -157,14 +157,14 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- q_pos = 0.12
- q_vel = 2.00
- q_voltage = 3.0
+ q_pos = 0.01
+ q_vel = 4.0
+ q_voltage = 4.0
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0],
[0.0, 0.0, (q_voltage ** 2.0)]])
- r_pos = 0.05
+ r_pos = 0.001
self.R = numpy.matrix([[(r_pos ** 2.0)]])
self.KalmanGain, self.Q_steady = controls.kalman(
@@ -186,6 +186,7 @@
self.t = []
self.x = []
self.v = []
+ self.v_hat = []
self.a = []
self.x_hat = []
self.u = []
@@ -251,6 +252,7 @@
self.v.append(hood.X[1, 0])
self.a.append((self.v[-1] - last_v) / hood.dt)
+ self.v_hat.append(observer_hood.X_hat[1, 0])
offset = 0.0
if i > 100:
@@ -277,6 +279,8 @@
pylab.subplot(3, 1, 1)
pylab.plot(self.t, self.x, label='x')
pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.plot(self.t, self.v, label='v')
+ pylab.plot(self.t, self.v_hat, label='v_hat')
pylab.legend()
pylab.subplot(3, 1, 2)
@@ -301,7 +305,7 @@
# Test moving the hood with constant separation.
initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[numpy.pi/2.0], [0.0], [0.0]])
+ R = numpy.matrix([[numpy.pi/4.0], [0.0], [0.0]])
scenario_plotter.run_test(hood, end_goal=R,
controller_hood=hood_controller,
observer_hood=observer_hood, iterations=200)
diff --git a/y2017/control_loops/superstructure/hood/hood.cc b/y2017/control_loops/superstructure/hood/hood.cc
index 8af4a9e..b5acba0 100644
--- a/y2017/control_loops/superstructure/hood/hood.cc
+++ b/y2017/control_loops/superstructure/hood/hood.cc
@@ -10,9 +10,13 @@
constexpr double Hood::kZeroingVoltage;
constexpr double Hood::kOperatingVoltage;
+// The tracking error to allow before declaring that we are stuck and reversing
+// direction while zeroing.
+constexpr double kStuckZeroingTrackingError = 0.02;
-Hood::Hood()
- : profiled_subsystem_(
+IndexPulseProfiledSubsystem::IndexPulseProfiledSubsystem()
+ : ::frc971::control_loops::SingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PulseIndexZeroingEstimator>(
::std::unique_ptr<
::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
@@ -20,15 +24,40 @@
constants::GetValues().hood.zeroing, constants::Values::kHoodRange,
0.5, 10.0) {}
+void IndexPulseProfiledSubsystem::CapGoal(const char *name,
+ Eigen::Matrix<double, 3, 1> *goal) {
+ if (zeroed()) {
+ ::frc971::control_loops::SingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PulseIndexZeroingEstimator>::CapGoal(name, goal);
+ } 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).
+ if ((*goal)(0, 0) > kMaxRange) {
+ LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+ kMaxRange);
+ (*goal)(0, 0) = kMaxRange;
+ }
+ if ((*goal)(0, 0) < -kMaxRange) {
+ LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+ kMaxRange);
+ (*goal)(0, 0) = -kMaxRange;
+ }
+ }
+}
+
+Hood::Hood() {}
+
void Hood::Reset() {
state_ = State::UNINITIALIZED;
profiled_subsystem_.Reset();
}
void Hood::Iterate(const control_loops::HoodGoal *unsafe_goal,
- const ::frc971::PotAndIndexPosition *position,
- double *output,
- ::frc971::control_loops::ProfiledJointStatus *status) {
+ const ::frc971::IndexPosition *position, double *output,
+ ::frc971::control_loops::IndexProfiledJointStatus *status) {
bool disable = output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -57,7 +86,7 @@
// jump.
profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
// Set up the profile to be the zeroing profile.
- profiled_subsystem_.AdjustProfile(0.10, 1);
+ profiled_subsystem_.AdjustProfile(0.30, 1.0);
// We are not ready to start doing anything yet.
disable = true;
@@ -71,33 +100,36 @@
} else if (disable) {
state_ = State::DISABLED_INITIALIZED;
} else {
- // Seek between the two soft limits.
- if (profiled_subsystem_.position() >
- (profiled_subsystem_.range().lower +
- profiled_subsystem_.range().upper) /
- 2.0) {
+ const double kRange = profiled_subsystem_.range().upper_hard -
+ profiled_subsystem_.range().lower_hard;
+ // Seek +- the range of motion.
+ if (profiled_subsystem_.position() > 0) {
// We are above the middle.
- if (profiled_subsystem_.goal(1, 0) > 0) {
- // And moving up. Keep going to the upper soft limit until we
- // arrive.
- profiled_subsystem_.set_unprofiled_goal(
- profiled_subsystem_.range().upper);
+ if (profiled_subsystem_.goal(1, 0) > 0 &&
+ ::std::abs(profiled_subsystem_.position() -
+ profiled_subsystem_.goal(0, 0)) <
+ kStuckZeroingTrackingError) {
+ // And moving up and not stuck. Keep going until we've gone the
+ // full range of motion or get stuck.
+ profiled_subsystem_.set_unprofiled_goal(kRange);
} else {
- // And no longer moving. Go down to the lower soft limit.
- profiled_subsystem_.set_unprofiled_goal(
- profiled_subsystem_.range().lower);
+ // And no longer moving. Go down to the opposite of the range of
+ // motion.
+ profiled_subsystem_.set_unprofiled_goal(-kRange);
}
} else {
// We are below the middle.
- if (profiled_subsystem_.goal(1, 0) < 0) {
- // And moving down. Keep going to the lower soft limit until we
- // arrive.
- profiled_subsystem_.set_unprofiled_goal(
- profiled_subsystem_.range().lower);
+ if (profiled_subsystem_.goal(1, 0) < 0 &&
+ ::std::abs(profiled_subsystem_.position() -
+ profiled_subsystem_.goal(0, 0)) <
+ kStuckZeroingTrackingError) {
+ // And moving down and not stuck. Keep going until we've gone the
+ // full range of motion or get stuck.
+ profiled_subsystem_.set_unprofiled_goal(-kRange);
} else {
- // And no longer moving. Go up to the upper soft limit.
- profiled_subsystem_.set_unprofiled_goal(
- profiled_subsystem_.range().upper);
+ // And no longer moving. Go up to the opposite of the range of
+ // motion.
+ profiled_subsystem_.set_unprofiled_goal(kRange);
}
}
}
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index b96d384..c58e005 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -9,37 +9,51 @@
namespace superstructure {
namespace hood {
+// Profiled subsystem class with significantly relaxed limits while zeroing. We
+// need relaxed limits, because if you start at the top of the range, you need
+// to go to -range, and if you start at the bottom of the range, you need to go
+// to +range. The standard subsystem doesn't support that.
+class IndexPulseProfiledSubsystem
+ : public ::frc971::control_loops::SingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PulseIndexZeroingEstimator> {
+ public:
+ IndexPulseProfiledSubsystem();
+
+ private:
+ void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) override;
+};
+
class Hood {
public:
- Hood();
- double goal(int row, int col) const {
- return profiled_subsystem_.goal(row, col);
- }
+ Hood();
+ 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;
+ // The zeroing and operating voltages.
+ static constexpr double kZeroingVoltage = 2.5;
+ static constexpr double kOperatingVoltage = 12.0;
- void Iterate(const control_loops::HoodGoal *unsafe_goal,
- const ::frc971::PotAndIndexPosition *position, double *output,
- ::frc971::control_loops::ProfiledJointStatus *status);
+ void Iterate(const control_loops::HoodGoal *unsafe_goal,
+ const ::frc971::IndexPosition *position, double *output,
+ ::frc971::control_loops::IndexProfiledJointStatus *status);
- void Reset();
+ void Reset();
- enum class State : int32_t{
- UNINITIALIZED,
- DISABLED_INITIALIZED,
- ZEROING,
- RUNNING,
- ESTOP,
- };
+ enum class State : int32_t {
+ UNINITIALIZED,
+ DISABLED_INITIALIZED,
+ ZEROING,
+ RUNNING,
+ ESTOP,
+ };
- State state() const { return state_; }
+ State state() const { return state_; }
- private:
- State state_;
+ private:
+ State state_;
- ::frc971::control_loops::SingleDOFProfiledSubsystem<> profiled_subsystem_;
+ IndexPulseProfiledSubsystem profiled_subsystem_;
};
} // namespace hood
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 397a888..b4e6c9b 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -128,7 +128,7 @@
// Each subsystems status.
.frc971.control_loops.AbsoluteProfiledJointStatus intake;
.frc971.control_loops.AbsoluteProfiledJointStatus turret;
- .frc971.control_loops.ProfiledJointStatus hood;
+ .frc971.control_loops.IndexProfiledJointStatus hood;
IndexerStatus indexer;
ShooterStatus shooter;
};
@@ -147,7 +147,7 @@
// The sensor readings for the hood. The units and sign are defined the
// same as what's in the Goal message.
- .frc971.PotAndIndexPosition hood;
+ .frc971.IndexPosition hood;
// Shooter wheel angle in radians.
double theta_shooter;
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index d03c48c..3075f03 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -132,7 +132,7 @@
SuperstructureSimulation()
: hood_plant_(new HoodPlant(
::y2017::control_loops::superstructure::hood::MakeHoodPlant())),
- hood_pot_encoder_(constants::Values::kHoodEncoderIndexDifference),
+ hood_encoder_(constants::Values::kHoodEncoderIndexDifference),
turret_plant_(new TurretPlant(
::y2017::control_loops::superstructure::turret::MakeTurretPlant())),
@@ -173,7 +173,7 @@
hood_plant_->mutable_X(0, 0) = start_pos;
hood_plant_->mutable_X(1, 0) = 0.0;
- hood_pot_encoder_.Initialize(
+ hood_encoder_.Initialize(
start_pos, kNoiseScalar,
constants::GetValues().hood.zeroing.measured_index_position);
}
@@ -201,7 +201,7 @@
::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
superstructure_queue_.position.MakeMessage();
- hood_pot_encoder_.GetSensorValues(&position->hood);
+ 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);
@@ -311,17 +311,33 @@
indexer_plant_->Update();
}
-
- const double angle_hood = hood_plant_->Y(0, 0);
+ 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);
- hood_pot_encoder_.MoveTo(angle_hood);
+ // The hood is special. We don't want to fault when we hit the hard stop.
+ // We want to freeze the hood at the hard stop.
+ if (angle_hood > constants::Values::kHoodRange.upper_hard) {
+ 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;
+ } 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_encoder_.MoveTo(angle_hood);
turret_pot_encoder_.MoveTo(angle_turret);
intake_pot_encoder_.MoveTo(position_intake);
- EXPECT_GE(angle_hood, constants::Values::kHoodRange.lower_hard);
- EXPECT_LE(angle_hood, constants::Values::kHoodRange.upper_hard);
+
EXPECT_GE(angle_turret, constants::Values::kTurretRange.lower_hard);
EXPECT_LE(angle_turret, constants::Values::kTurretRange.upper_hard);
EXPECT_GE(position_intake, constants::Values::kIntakeRange.lower_hard);
@@ -330,7 +346,7 @@
private:
::std::unique_ptr<HoodPlant> hood_plant_;
- PositionSensorSimulator hood_pot_encoder_;
+ PositionSensorSimulator hood_encoder_;
::std::unique_ptr<TurretPlant> turret_plant_;
PositionSensorSimulator turret_pot_encoder_;
@@ -539,6 +555,19 @@
// 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.
TEST_F(SuperstructureTest, SaturationTest) {
+ // Zero it before we move.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.distance = constants::Values::kIntakeRange.upper;
+ goal->turret.angle = constants::Values::kTurretRange.upper;
+ goal->hood.angle = constants::Values::kHoodRange.upper;
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+
+ // Try a low acceleration move with a high max velocity and verify the
+ // acceleration is capped like expected.
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->intake.distance = constants::Values::kIntakeRange.lower;
@@ -560,7 +589,9 @@
set_peak_hood_acceleration(1.1);
RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+ // Now do a high acceleration move with a low velocity limit.
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->intake.distance = constants::Values::kIntakeRange.upper;
@@ -753,7 +784,7 @@
SimulateSensorReset();
RunForTime(chrono::milliseconds(100));
- EXPECT_EQ(hood::Hood::State::UNINITIALIZED, superstructure_.hood().state());
+ EXPECT_EQ(hood::Hood::State::ZEROING, superstructure_.hood().state());
EXPECT_EQ(turret::Turret::State::UNINITIALIZED,
superstructure_.turret().state());
EXPECT_EQ(intake::Intake::State::UNINITIALIZED,
@@ -814,7 +845,7 @@
EXPECT_EQ(intake::Intake::State::RUNNING,
superstructure_.intake().state());
- superstructure_plant_.set_hood_power_error(1.0);
+ superstructure_plant_.set_hood_power_error(2.0);
RunForTime(chrono::seconds(1), false);
@@ -822,7 +853,7 @@
EXPECT_EQ(turret::Turret::State::RUNNING, superstructure_.turret().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
- RunForTime(chrono::seconds(2), true);
+ RunForTime(chrono::seconds(4), true);
VerifyNearGoal();
}