Tuned intake.
Change-Id: I028f6cbb8df55d281734ca39abb4f62b7fd27793
diff --git a/y2018/control_loops/superstructure/intake/intake.cc b/y2018/control_loops/superstructure/intake/intake.cc
index 7212fd3..080720a 100644
--- a/y2018/control_loops/superstructure/intake/intake.cc
+++ b/y2018/control_loops/superstructure/intake/intake.cc
@@ -71,7 +71,7 @@
goal_velocity = 0.0;
} else {
goal_velocity = ::aos::Clip(
- ((goal_angle(unsafe_goal) - loop_->X_hat(0, 0)) * 6.0), -10.0, 10.0);
+ ((goal_angle(unsafe_goal) - loop_->X_hat(0, 0)) * 12.0), -16.0, 16.0);
}
// Computes the goal.
loop_->mutable_R() << 0.0, goal_velocity, 0.0, goal_velocity,
@@ -84,8 +84,8 @@
const double *unsafe_goal) {
status->goal_position = goal_angle(unsafe_goal);
status->goal_velocity = loop_->R(1, 0);
- status->spring_position = loop_->X_hat(0);
- status->spring_velocity = loop_->X_hat(1);
+ status->spring_position = loop_->X_hat(0) - loop_->X_hat(2);
+ status->spring_velocity = loop_->X_hat(1) - loop_->X_hat(3);
status->motor_position = loop_->X_hat(2);
status->motor_velocity = loop_->X_hat(3);
status->delayed_voltage = loop_->X_hat(4);
@@ -102,7 +102,6 @@
const control_loops::IntakeElasticSensors *position,
control_loops::IntakeVoltage *output,
control_loops::IntakeSideStatus *status) {
- double intake_last_position_ = status->estimator_state.position;
zeroing_estimator_.UpdateEstimate(position->motor_position);
switch (state_) {
@@ -164,6 +163,7 @@
status->zeroed = zeroing_estimator_.zeroed();
status->estopped = (state_ == State::ESTOP);
status->state = static_cast<int32_t>(state_);
+ intake_last_position_ = status->estimator_state.position;
}
} // namespace intake
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index d46d90e..b154334 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -97,6 +97,8 @@
State state_ = State::UNINITIALIZED;
::frc971::zeroing::PotAndAbsEncoderZeroingEstimator zeroing_estimator_;
+
+ double intake_last_position_ = 0.0;
};
} // namespace intake
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index f53a6db..1c5a4e5 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -63,12 +63,66 @@
status->arm.zeroed;
if (output && unsafe_goal) {
- output->intake.left.voltage_rollers = ::std::max(
+ const double roller_voltage = ::std::max(
-kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
kMaxIntakeRollerVoltage));
- output->intake.right.voltage_rollers = ::std::max(
- -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
- kMaxIntakeRollerVoltage));
+ constexpr int kReverseTime = 15;
+ if (unsafe_goal->intake.roller_voltage < 0.0) {
+ output->intake.left.voltage_rollers = roller_voltage;
+ output->intake.right.voltage_rollers = roller_voltage;
+ rotation_state_ = RotationState::NOT_ROTATING;
+ rotation_count_ = 0;
+ } else {
+ switch (rotation_state_) {
+ case RotationState::NOT_ROTATING:
+ if (position->intake.left.beam_break) {
+ rotation_state_ = RotationState::ROTATING_RIGHT;
+ rotation_count_ = kReverseTime;
+ break;
+ } else if (position->intake.right.beam_break) {
+ rotation_state_ = RotationState::ROTATING_LEFT;
+ rotation_count_ = kReverseTime;
+ break;
+ } else {
+ break;
+ }
+ case RotationState::ROTATING_LEFT:
+ if (position->intake.right.beam_break) {
+ rotation_count_ = kReverseTime;
+ } else {
+ --rotation_count_;
+ }
+ if (rotation_count_ == 0) {
+ rotation_state_ = RotationState::NOT_ROTATING;
+ }
+ break;
+ case RotationState::ROTATING_RIGHT:
+ if (position->intake.left.beam_break) {
+ rotation_count_ = kReverseTime;
+ } else {
+ --rotation_count_;
+ }
+ if (rotation_count_ == 0) {
+ rotation_state_ = RotationState::NOT_ROTATING;
+ }
+ break;
+ }
+
+ switch (rotation_state_) {
+ case RotationState::NOT_ROTATING:
+ output->intake.left.voltage_rollers = roller_voltage;
+ output->intake.right.voltage_rollers = roller_voltage;
+ break;
+ case RotationState::ROTATING_LEFT:
+ output->intake.left.voltage_rollers = roller_voltage;
+ output->intake.right.voltage_rollers = -roller_voltage;
+ break;
+ case RotationState::ROTATING_RIGHT:
+ output->intake.left.voltage_rollers = -roller_voltage;
+ output->intake.right.voltage_rollers = roller_voltage;
+ break;
+ }
+ }
}
}
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index ade5047..2081e6c 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -36,6 +36,15 @@
intake::IntakeSide intake_right_;
arm::Arm arm_;
+ enum class RotationState {
+ NOT_ROTATING = 0,
+ ROTATING_LEFT = 1,
+ ROTATING_RIGHT = 2
+ };
+
+ RotationState rotation_state_ = RotationState::NOT_ROTATING;
+ int rotation_count_ = 0;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 09809e6..601cec3 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -279,14 +279,16 @@
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
// Left side test.
EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
- superstructure_queue_.status->left_intake.spring_position,
+ superstructure_queue_.status->left_intake.spring_position +
+ superstructure_queue_.status->left_intake.motor_position,
0.001);
EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
superstructure_plant_.left_intake().spring_position(), 0.001);
// Right side test.
EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
- superstructure_queue_.status->right_intake.spring_position,
+ superstructure_queue_.status->right_intake.spring_position +
+ superstructure_queue_.status->right_intake.motor_position,
0.001);
EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
superstructure_plant_.right_intake().spring_position(), 0.001);
@@ -395,12 +397,17 @@
// Check that we are near our soft limit.
superstructure_queue_.status.FetchLatest();
+ EXPECT_NEAR(0.0, superstructure_queue_.status->left_intake.spring_position,
+ 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_queue_.status->left_intake.spring_position,
+ superstructure_queue_.status->left_intake.spring_position +
+ superstructure_queue_.status->left_intake.motor_position,
0.001);
+ EXPECT_NEAR(0.0, superstructure_queue_.status->right_intake.spring_position,
+ 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_queue_.status->right_intake.spring_position,
+ superstructure_queue_.status->right_intake.motor_position,
0.001);
// Set some ridiculous goals to test lower limits.
@@ -419,12 +426,15 @@
superstructure_queue_.status.FetchLatest();
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_queue_.status->left_intake.spring_position,
+ superstructure_queue_.status->left_intake.motor_position, 0.001);
+ EXPECT_NEAR(0.0, superstructure_queue_.status->left_intake.spring_position,
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_queue_.status->right_intake.spring_position,
- 0.001);}
+ superstructure_queue_.status->right_intake.motor_position, 0.001);
+ EXPECT_NEAR(0.0, superstructure_queue_.status->right_intake.spring_position,
+ 0.001);
+}
TEST_F(SuperstructureTest, DISABLED_LowerHardstopStartup) {
superstructure_plant_.InitializeIntakePosition(