Added a zeroing procedure.
Change-Id: Ib329f1a02772e36d737a0a1d0f5114b2b5acc029
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 214fc6c..c4ef468 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -14,6 +14,13 @@
namespace superstructure {
namespace {
+constexpr double kIntakeEncoderIndexDifference =
+ constants::Values::kIntakeEncoderIndexDifference;
+constexpr double kWristEncoderIndexDifference =
+ constants::Values::kWristEncoderIndexDifference;
+constexpr double kShoulderEncoderIndexDifference =
+ constants::Values::kShoulderEncoderIndexDifference;
+
constexpr double kZeroingVoltage = 4.0;
} // namespace
@@ -22,25 +29,74 @@
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
superstructure_queue) {}
-void Superstructure::UpdateZeroingState() {
- // TODO(austin): Explicit state transitions instead of this.
- // TODO(adam): Change this once we have zeroing written.
- if (!arm_.initialized() || !intake_.initialized()) {
- state_ = INITIALIZING;
- } else if (!intake_.zeroed()) {
- state_ = ZEROING_INTAKE;
- } else if (!arm_.zeroed()) {
- state_ = ZEROING_ARM;
+bool Superstructure::IsArmNear(double shoulder_tolerance,
+ double wrist_tolerance) {
+ return ((arm_.unprofiled_goal() - arm_.X_hat())
+ .block<2, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < shoulder_tolerance) &&
+ ((arm_.unprofiled_goal() - arm_.X_hat())
+ .block<4, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < wrist_tolerance) &&
+ ((arm_.unprofiled_goal() - arm_.goal())
+ .block<4, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < 1e-6);
+}
+
+bool Superstructure::IsArmNear(double tolerance) {
+ return ((arm_.unprofiled_goal() - arm_.X_hat())
+ .block<4, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < tolerance) &&
+ ((arm_.unprofiled_goal() - arm_.goal())
+ .block<4, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < 1e-6);
+}
+
+bool Superstructure::IsIntakeNear(double tolerance) {
+ return ((intake_.unprofiled_goal() - intake_.X_hat())
+ .block<2, 1>(0, 0)
+ .lpNorm<Eigen::Infinity>() < tolerance);
+}
+
+double Superstructure::MoveButKeepAbove(double reference_angle,
+ double current_angle,
+ double move_distance) {
+ return -MoveButKeepBelow(-reference_angle, -current_angle, -move_distance);
+}
+
+double Superstructure::MoveButKeepBelow(double reference_angle,
+ double current_angle,
+ double move_distance) {
+ // There are 3 interesting places to move to.
+ const double small_negative_move = current_angle - move_distance;
+ const double small_positive_move = current_angle + move_distance;
+ // And the reference angle.
+
+ // Move the the highest one that is below reference_angle.
+ if (small_negative_move > reference_angle) {
+ return reference_angle;
+ } else if (small_positive_move > reference_angle) {
+ return small_negative_move;
} else {
- state_ = RUNNING;
+ return small_positive_move;
}
}
+constexpr double Superstructure::kShoulderMiddleAngle;
+constexpr double Superstructure::kLooseTolerance;
+constexpr double Superstructure::kIntakeUpperClear;
+constexpr double Superstructure::kIntakeLowerClear;
+constexpr double Superstructure::kShoulderUpAngle;
+constexpr double Superstructure::kShoulderLanded;
+constexpr double Superstructure::kTightTolerance;
+constexpr double Superstructure::kWristAlmostLevel;
+constexpr double Superstructure::kShoulderWristClearAngle;
+
void Superstructure::RunIteration(
const control_loops::SuperstructureQueue::Goal *unsafe_goal,
const control_loops::SuperstructureQueue::Position *position,
control_loops::SuperstructureQueue::Output *output,
control_loops::SuperstructureQueue::Status *status) {
+ const State state_before_switch = state_;
if (WasReset()) {
LOG(ERROR, "WPILib reset, restarting\n");
arm_.Reset();
@@ -54,58 +110,221 @@
arm_.Correct(position->shoulder, position->wrist);
intake_.Correct(position->intake);
- // Zeroing will work as follows:
- // Start with the intake. Move it towards the center. Once zeroed, move it
- // back to the bottom. Rotate the shoulder towards the center. Once zeroed,
- // move it up enough to rotate the wrist towards the center.
-
- // We'll then need code to do sanity checking on values.
+ // There are 2 main zeroing paths, HIGH_ARM_ZERO and LOW_ARM_ZERO.
+ //
+ // HIGH_ARM_ZERO works by lifting the arm all the way up so it is clear,
+ // moving the shooter to be horizontal, moving the intake out, and then moving
+ // the arm back down.
+ //
+ // LOW_ARM_ZERO works by moving the intake out of the way, lifting the arm up,
+ // leveling the shooter, and then moving back down.
switch (state_) {
case UNINITIALIZED:
- LOG(DEBUG, "Uninitialized\n");
- state_ = INITIALIZING;
+ // Wait in the uninitialized state until both the arm and intake are
+ // initialized.
+ LOG(DEBUG, "Uninitialized, waiting for intake and arm\n");
+ if (arm_.initialized() && intake_.initialized()) {
+ state_ = DISABLED_INITIALIZED;
+ }
disable = true;
break;
- case INITIALIZING:
- LOG(DEBUG, "Waiting for accurate initial position.\n");
+ case DISABLED_INITIALIZED:
+ // Wait here until we are either fully zeroed while disabled, or we become
+ // enabled. At that point, figure out if we should HIGH_ARM_ZERO or
+ // LOW_ARM_ZERO.
+ if (disable) {
+ if (arm_.zeroed() && intake_.zeroed()) {
+ state_ = SLOW_RUNNING;
+ }
+ } else {
+ if (arm_.shoulder_angle() >= kShoulderMiddleAngle) {
+ state_ = HIGH_ARM_ZERO_LIFT_ARM;
+ } else {
+ state_ = LOW_ARM_ZERO_LOWER_INTAKE;
+ }
+ }
+
+ // Set the goals to where we are now so when we start back up, we don't
+ // jump.
+ intake_.ForceGoal(intake_.angle());
+ arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
+ // Set up the profile to be the zeroing profile.
+ intake_.AdjustProfile(0.5, 10);
+ arm_.AdjustProfile(0.5, 10, 0.5, 10);
+
+ // We are not ready to start doing anything yet.
disable = true;
- // Update state_ to accurately represent the state of the zeroing
- // estimators.
- UpdateZeroingState();
- if (state_ != INITIALIZING) {
- // Set the goals to where we are now.
- intake_.ForceGoal(intake_.angle());
- arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
+ break;
+
+ case HIGH_ARM_ZERO_LIFT_ARM:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Raise the shoulder up out of the way.
+ arm_.set_unprofiled_goal(kShoulderUpAngle, arm_.wrist_angle());
+ if (IsArmNear(kLooseTolerance)) {
+ // Close enough, start the next move.
+ state_ = HIGH_ARM_ZERO_LEVEL_SHOOTER;
+ }
}
break;
- case ZEROING_INTAKE:
- case ZEROING_ARM:
- // TODO(adam): Add your magic here.
- state_ = RUNNING;
+ case HIGH_ARM_ZERO_LEVEL_SHOOTER:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Move the shooter to be level.
+ arm_.set_unprofiled_goal(kShoulderUpAngle, 0.0);
+
+ if (IsArmNear(kLooseTolerance)) {
+ // Close enough, start the next move.
+ state_ = HIGH_ARM_ZERO_MOVE_INTAKE_OUT;
+ }
+ }
break;
+ case HIGH_ARM_ZERO_MOVE_INTAKE_OUT:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // If we were just asked to move the intake, make sure it moves far
+ // enough.
+ if (last_state_ != HIGH_ARM_ZERO_MOVE_INTAKE_OUT) {
+ intake_.set_unprofiled_goal(
+ MoveButKeepBelow(kIntakeUpperClear, intake_.angle(),
+ kIntakeEncoderIndexDifference * 2.5));
+ }
+
+ if (IsIntakeNear(kLooseTolerance)) {
+ // Close enough, start the next move.
+ state_ = HIGH_ARM_ZERO_LOWER_ARM;
+ }
+ }
+ break;
+
+ case HIGH_ARM_ZERO_LOWER_ARM:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Land the shooter in the belly-pan. It should be zeroed by the time
+ // it gets there. If not, just estop.
+ arm_.set_unprofiled_goal(kShoulderLanded, 0.0);
+ if (arm_.zeroed() && intake_.zeroed()) {
+ state_ = RUNNING;
+ } else if (IsArmNear(kLooseTolerance)) {
+ LOG(ERROR,
+ "Failed to zero while executing the HIGH_ARM_ZERO sequence. "
+ "Arm: %d Intake %d\n",
+ arm_.zeroed(), intake_.zeroed());
+ state_ = ESTOP;
+ }
+ }
+ break;
+
+ case LOW_ARM_ZERO_LOWER_INTAKE:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Move the intake down out of the way of the arm. Make sure to move it
+ // far enough to zero.
+ if (last_state_ != LOW_ARM_ZERO_LOWER_INTAKE) {
+ intake_.set_unprofiled_goal(
+ MoveButKeepBelow(kIntakeLowerClear, intake_.angle(),
+ kIntakeEncoderIndexDifference * 2.5));
+ }
+ if (IsIntakeNear(kLooseTolerance)) {
+ if (::std::abs(arm_.wrist_angle()) < kWristAlmostLevel) {
+ state_ = LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER;
+ } else {
+ state_ = LOW_ARM_ZERO_LIFT_SHOULDER;
+ }
+ }
+ }
+ break;
+
+ case LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // If we are supposed to level the shooter, set it to level, and wait
+ // until it is very close to level.
+ arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), 0.0);
+ if (IsArmNear(kLooseTolerance, kTightTolerance)) {
+ state_ = LOW_ARM_ZERO_LIFT_SHOULDER;
+ }
+ }
+ break;
+
+ case LOW_ARM_ZERO_LIFT_SHOULDER:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Decide where to move to. We need to move far enough to see an index
+ // pulse, but must also get high enough that we can safely level the
+ // shooter.
+ if (last_state_ != LOW_ARM_ZERO_LIFT_SHOULDER) {
+ arm_.set_unprofiled_goal(
+ MoveButKeepAbove(kShoulderWristClearAngle, arm_.shoulder_angle(),
+ ::std::max(kWristEncoderIndexDifference,
+ kShoulderEncoderIndexDifference) *
+ 2.5),
+ arm_.unprofiled_goal(2, 0));
+ }
+
+ // Wait until we are level and then go for it.
+ if (IsArmNear(kLooseTolerance)) {
+ state_ = LOW_ARM_ZERO_LEVEL_SHOOTER;
+ }
+ }
+ break;
+
+ case LOW_ARM_ZERO_LEVEL_SHOOTER:
+ if (disable) {
+ state_ = DISABLED_INITIALIZED;
+ } else {
+ // Move the shooter level (and keep the same height). We don't want to
+ // got to RUNNING until we are completely level so that we don't
+ // give control back in a weird case where we might crash.
+ arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), 0.0);
+ if (IsArmNear(kLooseTolerance)) {
+ if (arm_.zeroed() && intake_.zeroed()) {
+ state_ = RUNNING;
+ } else {
+ LOG(ERROR,
+ "Failed to zero while executing the LOW_ARM_ZERO sequence. "
+ "Arm: %d Intake %d\n",
+ arm_.zeroed(), intake_.zeroed());
+ state_ = ESTOP;
+ }
+ }
+ }
+ break;
+
+ case SLOW_RUNNING:
case RUNNING:
+ // TODO(austin): Exit SLOW_RUNNING if we are not collided.
if (unsafe_goal) {
arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
- unsafe_goal->max_angular_acceleration_shoulder,
- unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_wrist);
+ unsafe_goal->max_angular_acceleration_shoulder,
+ unsafe_goal->max_angular_velocity_wrist,
+ unsafe_goal->max_angular_acceleration_wrist);
intake_.AdjustProfile(unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_intake);
+ unsafe_goal->max_angular_acceleration_intake);
arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
- unsafe_goal->angle_wrist);
+ unsafe_goal->angle_wrist);
intake_.set_unprofiled_goal(unsafe_goal->angle_intake);
}
- // Update state_ to accurately represent the state of the zeroing
- // estimators.
-
- if (state_ != RUNNING && state_ != ESTOP) {
- state_ = UNINITIALIZED;
+ // ESTOP if we hit any of the limits. It is safe(ish) to hit the limits
+ // while zeroing since we use such low power.
+ if (state_ == RUNNING || state_ == SLOW_RUNNING) {
+ // ESTOP if we hit the hard limits.
+ if ((arm_.CheckHardLimits() || intake_.CheckHardLimits()) && output) {
+ state_ = ESTOP;
+ }
}
break;
@@ -115,15 +334,6 @@
break;
}
- // ESTOP if we hit any of the limits. It is safe(ish) to hit the limits while
- // zeroing since we use such low power.
- if (state_ == RUNNING) {
- // ESTOP if we hit the hard limits.
- if ((arm_.CheckHardLimits() || intake_.CheckHardLimits()) && output) {
- state_ = ESTOP;
- }
- }
-
// Set the voltage limits.
const double max_voltage = state_ == RUNNING ? 12.0 : kZeroingVoltage;
arm_.set_max_voltage(max_voltage, max_voltage);
@@ -174,7 +384,7 @@
status->state = state_;
- last_state_ = state_;
+ last_state_ = state_before_switch;
}
} // namespace superstructure
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 9a04c3c..1350a15 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -24,23 +24,80 @@
explicit Superstructure(
control_loops::SuperstructureQueue *my_superstructure =
&control_loops::superstructure_queue);
+
+ // This is the angle above which we will do a HIGH_ARM_ZERO, and below which
+ // we will do a LOW_ARM_ZERO.
+ static constexpr double kShoulderMiddleAngle = M_PI / 4.0;
+ // This is the large scale movement tolerance.
+ static constexpr double kLooseTolerance = 0.05;
+
+ // This is the small scale movement tolerance.
+ static constexpr double kTightTolerance = 0.01;
+
+ // This is the angle such that the intake will clear the arm when the shooter
+ // is level.
+ static constexpr double kIntakeUpperClear = 1.1;
+ // This is the angle such that the intake will clear the arm when the shooter
+ // is at almost any position.
+ static constexpr double kIntakeLowerClear = 0.5;
+
+ // This is the angle that the shoulder will go to when doing the
+ // HIGH_ARM_ZERO.
+ static constexpr double kShoulderUpAngle = M_PI / 2.0;
+
+ // This is the angle that the shoulder will go down to when landing in the
+ // bellypan.
+ static constexpr double kShoulderLanded = -0.02;
+
+ // This is the angle below which we consider the wrist close enough to level
+ // that we should move it to level before doing anything.
+ static constexpr double kWristAlmostLevel = 0.10;
+
+ // This is the angle that the shoulder will go down to when raising up before
+ // leveling the shooter for calibration.
+ static constexpr double kShoulderWristClearAngle = 0.6;
+
enum State {
- // Waiting to receive data before doing anything.
+ // Wait for all the filters to be ready before starting the initialization
+ // process.
UNINITIALIZED = 0,
- // Estimating the starting location.
- INITIALIZING = 1,
- // Moving the intake to find an index pulse.
- ZEROING_INTAKE = 2,
- // Moving the arm to find an index pulse.
- ZEROING_ARM = 3,
- // All good!
- RUNNING = 4,
+
+ // We now are ready to decide how to zero. Decide what to do once we are
+ // enabled.
+ DISABLED_INITIALIZED = 1,
+
+ // Lift the arm up out of the way.
+ HIGH_ARM_ZERO_LIFT_ARM = 2,
+
+ HIGH_ARM_ZERO_LEVEL_SHOOTER = 3,
+
+ HIGH_ARM_ZERO_MOVE_INTAKE_OUT = 4,
+
+ HIGH_ARM_ZERO_LOWER_ARM = 6,
+
+ LOW_ARM_ZERO_LOWER_INTAKE = 7,
+ LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER = 8,
+ LOW_ARM_ZERO_LIFT_SHOULDER = 9,
+ LOW_ARM_ZERO_LEVEL_SHOOTER = 11,
+ // Run, but limit power to zeroing voltages.
+ SLOW_RUNNING = 12,
+ // Run with full power.
+ RUNNING = 13,
// Internal error caused the superstructure to abort.
- ESTOP = 5,
+ ESTOP = 14,
};
State state() const { return state_; }
+ // Returns the value to move the joint to such that it will stay below
+ // reference_angle starting at current_angle, but move at least move_distance
+ static double MoveButKeepBelow(double reference_angle, double current_angle,
+ double move_distance);
+ // Returns the value to move the joint to such that it will stay above
+ // reference_angle starting at current_angle, but move at least move_distance
+ static double MoveButKeepAbove(double reference_angle, double current_angle,
+ double move_distance);
+
protected:
virtual void RunIteration(
const control_loops::SuperstructureQueue::Goal *unsafe_goal,
@@ -56,9 +113,11 @@
State state_ = UNINITIALIZED;
State last_state_ = UNINITIALIZED;
- // Sets state_ to the correct state given the current state of the zeroing
- // estimators.
- void UpdateZeroingState();
+ // Returns true if the profile has finished, and the joint is within the
+ // specified tolerance.
+ bool IsArmNear(double tolerance);
+ bool IsArmNear(double shoulder_tolerance, double wrist_tolerance);
+ bool IsIntakeNear(double tolerance);
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index f3d8752..d20b345 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -27,21 +27,19 @@
// Updates our estimator with the latest position.
void Correct(::frc971::PotAndIndexPosition position);
+ // Runs the controller and profile generator for a cycle.
+ void Update(bool disabled);
+ // Sets the maximum voltage that will be commanded by the loop.
+ void set_max_voltage(double voltage);
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal);
// Sets the unprofiled goal. The profiler will generate a profile to go to
// this goal.
void set_unprofiled_goal(double unprofiled_goal);
-
- // Runs the controller and profile generator for a cycle.
- void Update(bool disabled);
-
// Limits our profiles to a max velocity and acceleration for proper motion.
void AdjustProfile(double max_angular_velocity,
double max_angular_acceleration);
- // Sets the maximum voltage that will be commanded by the loop.
- void set_max_voltage(double voltage);
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 1bd4eca..78fae9d 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -80,7 +80,7 @@
static constexpr double kNoiseScalar = 0.1;
SuperstructureSimulation()
: intake_plant_(new IntakePlant(MakeIntakePlant())),
- plant_arm_(new ArmPlant(MakeArmPlant())),
+ arm_plant_(new ArmPlant(MakeArmPlant())),
pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
pot_encoder_shoulder_(
constants::Values::kShoulderEncoderIndexDifference),
@@ -103,16 +103,16 @@
}
void InitializeShoulderPosition(double start_pos) {
- plant_arm_->mutable_X(0, 0) = start_pos;
- plant_arm_->mutable_X(1, 0) = 0.0;
+ arm_plant_->mutable_X(0, 0) = start_pos;
+ arm_plant_->mutable_X(1, 0) = 0.0;
pot_encoder_shoulder_.Initialize(start_pos, kNoiseScalar);
}
// Must be called after any changes to InitializeShoulderPosition.
void InitializeWristPosition(double start_pos) {
- plant_arm_->mutable_X(2, 0) = start_pos + plant_arm_->X(0, 0);
- plant_arm_->mutable_X(3, 0) = 0.0;
+ arm_plant_->mutable_X(2, 0) = start_pos + arm_plant_->X(0, 0);
+ arm_plant_->mutable_X(3, 0) = 0.0;
pot_encoder_wrist_.Initialize(start_pos, kNoiseScalar);
}
@@ -129,13 +129,17 @@
position.Send();
}
+ double shoulder_angle() const { return arm_plant_->X(0, 0); }
+ double wrist_angle() const { return arm_plant_->X(2, 0); }
+ double intake_angle() const { return intake_plant_->X(0, 0); }
+
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
void set_power_error(double power_error_intake, double power_error_shoulder,
double power_error_wrist) {
intake_plant_->set_voltage_offset(power_error_intake);
- plant_arm_->set_shoulder_voltage_offset(power_error_shoulder);
- plant_arm_->set_wrist_voltage_offset(power_error_wrist);
+ arm_plant_->set_shoulder_voltage_offset(power_error_shoulder);
+ arm_plant_->set_wrist_voltage_offset(power_error_wrist);
}
// Simulates for a single timestep.
@@ -146,19 +150,19 @@
intake_plant_->mutable_U() << superstructure_queue_.output->voltage_intake +
intake_plant_->voltage_offset();
- plant_arm_->mutable_U() << superstructure_queue_.output->voltage_shoulder +
- plant_arm_->shoulder_voltage_offset(),
+ arm_plant_->mutable_U() << superstructure_queue_.output->voltage_shoulder +
+ arm_plant_->shoulder_voltage_offset(),
superstructure_queue_.output->voltage_wrist +
- plant_arm_->wrist_voltage_offset();
+ arm_plant_->wrist_voltage_offset();
// Use the plant to generate the next physical state given the voltages to
// the motors.
intake_plant_->Update();
- plant_arm_->Update();
+ arm_plant_->Update();
const double angle_intake = intake_plant_->Y(0, 0);
- const double angle_shoulder = plant_arm_->Y(0, 0);
- const double angle_wrist = plant_arm_->Y(1, 0);
+ const double angle_shoulder = arm_plant_->Y(0, 0);
+ const double angle_wrist = arm_plant_->Y(1, 0);
// Use the physical state to simulate sensor readings.
pot_encoder_intake_.MoveTo(angle_intake);
@@ -176,7 +180,7 @@
private:
::std::unique_ptr<IntakePlant> intake_plant_;
- ::std::unique_ptr<ArmPlant> plant_arm_;
+ ::std::unique_ptr<ArmPlant> arm_plant_;
PositionSensorSimulator pot_encoder_intake_;
PositionSensorSimulator pot_encoder_shoulder_;
@@ -209,6 +213,13 @@
superstructure_queue_.status->shoulder.angle, 0.001);
EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
superstructure_queue_.status->wrist.angle, 0.001);
+
+ EXPECT_NEAR(superstructure_queue_.goal->angle_intake,
+ superstructure_plant_.intake_angle(), 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->angle_shoulder,
+ superstructure_plant_.shoulder_angle(), 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
+ superstructure_plant_.wrist_angle(), 0.001);
}
// Runs one iteration of the whole simulation and checks that separation
@@ -349,7 +360,7 @@
.max_angular_acceleration_wrist(20)
.Send());
- RunForTime(Time::InSeconds(5));
+ RunForTime(Time::InSeconds(10));
VerifyNearGoal();
}
@@ -375,7 +386,7 @@
.angle_wrist(constants::Values::kWristRange.lower)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
- RunForTime(Time::InSeconds(5));
+ RunForTime(Time::InSeconds(10));
VerifyNearGoal();
}
@@ -394,7 +405,7 @@
.angle_wrist(constants::Values::kWristRange.lower)
.Send());
// We have to wait for it to put the elevator in a safe position as well.
- RunForTime(Time::InSeconds(5));
+ RunForTime(Time::InSeconds(10));
VerifyNearGoal();
}
@@ -413,14 +424,14 @@
.angle_shoulder(constants::Values::kShoulderRange.lower + 0.3)
.angle_wrist(constants::Values::kWristRange.lower + 0.3)
.Send());
- RunForTime(Time::InSeconds(5));
+ RunForTime(Time::InSeconds(10));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
SimulateSensorReset();
RunForTime(Time::InMS(100));
EXPECT_NE(Superstructure::RUNNING, superstructure_.state());
- RunForTime(Time::InMS(6000));
+ RunForTime(Time::InMS(10000));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
}
@@ -446,6 +457,14 @@
EXPECT_NE(0.0, superstructure_.arm_.goal(2, 0));
}
+// Tests that MoveButKeepBelow returns sane values.
+TEST_F(SuperstructureTest, MoveButKeepBelowTest) {
+ EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 10.0, 1.0));
+ EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 2.0, 1.0));
+ EXPECT_EQ(0.0, Superstructure::MoveButKeepBelow(1.0, 1.0, 1.0));
+ EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 0.0, 1.0));
+}
+
// Tests that the integrators works.
TEST_F(SuperstructureTest, IntegratorTest) {
superstructure_plant_.InitializeIntakePosition(