Merge "Tune hood"
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 7fe24bf..463919a 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -102,7 +102,7 @@
break;
case kCompTeamNumber:
- hood->zeroing_constants.measured_absolute_position = 0.432541963515072;
+ hood->zeroing_constants.measured_absolute_position = 0.266691815725476;
intake->zeroing_constants.measured_absolute_position =
1.42977866919024 - Values::kIntakeZero();
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index b4d8c84..98ddfe4 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -31,13 +31,13 @@
kHood = angular_system.AngularSystemParams(
name='Hood',
motor=control_loop.BAG(),
- G=radians_per_turn,
- J=0.08389,
- q_pos=0.55,
- q_vel=14.0,
+ G=radians_per_turn / (2.0 * numpy.pi),
+ J=4.0,
+ q_pos=0.15,
+ q_vel=5.0,
kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=2.5,
+ kalman_q_vel=10.0,
+ kalman_q_voltage=15.0,
kalman_r_position=0.05)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index b9ba172..d16c692 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -64,24 +64,34 @@
climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
+ const AbsoluteEncoderProfiledJointStatus *const hood_status =
+ GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
+
const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
if (output != nullptr) {
// Friction is a pain and putting a really high burden on the integrator.
- double velocity_sign = turret_status->velocity() * kTurretFrictionGain;
+ const double turret_velocity_sign = turret_status->velocity() * kTurretFrictionGain;
output_struct.turret_voltage +=
- std::clamp(velocity_sign, -kTurretFrictionVoltageLimit,
+ std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
kTurretFrictionVoltageLimit);
+
+ // Friction is a pain and putting a really high burden on the integrator.
+ const double hood_velocity_sign = hood_status->velocity() * kHoodFrictionGain;
+ output_struct.hood_voltage +=
+ std::clamp(hood_velocity_sign, -kHoodFrictionVoltageLimit,
+ kHoodFrictionVoltageLimit);
+
+ // And dither the output.
+ time_ += 0.00505;
+ output_struct.hood_voltage += 1.3 * std::sin(time_ * 2.0 * M_PI * 30.0);
}
bool zeroed;
bool estopped;
{
- const AbsoluteEncoderProfiledJointStatus *const hood_status =
- GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
-
const AbsoluteEncoderProfiledJointStatus *const intake_status =
GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index d5f4235..8f7cd8c 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -26,6 +26,9 @@
static constexpr double kTurretFrictionGain = 10.0;
static constexpr double kTurretFrictionVoltageLimit = 1.5;
+ static constexpr double kHoodFrictionGain = 40.0;
+ static constexpr double kHoodFrictionVoltageLimit = 1.5;
+
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
@@ -52,6 +55,9 @@
shooter::Shooter shooter_;
Climber climber_;
+
+ double time_ = 0.0;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index f74b111..5932d2d 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -235,14 +235,21 @@
EXPECT_NEAR(superstructure_output_fetcher_->turret_voltage(), 0.0,
voltage_check_turret);
+ // Invert the friction model.
+ const double hood_velocity_sign =
+ hood_plant_->X(1) * Superstructure::kHoodFrictionGain;
::Eigen::Matrix<double, 1, 1> hood_U;
hood_U << superstructure_output_fetcher_->hood_voltage() +
- hood_plant_->voltage_offset();
+ hood_plant_->voltage_offset() -
+ std::clamp(hood_velocity_sign,
+ -Superstructure::kHoodFrictionVoltageLimit,
+ Superstructure::kHoodFrictionVoltageLimit);
::Eigen::Matrix<double, 1, 1> intake_U;
intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
intake_plant_->voltage_offset();
+ // Invert the friction model.
const double turret_velocity_sign =
turret_plant_->X(1) * Superstructure::kTurretFrictionGain;
::Eigen::Matrix<double, 1, 1> turret_U;
@@ -672,7 +679,8 @@
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_hood_velocity(23.0);
- superstructure_plant_.set_peak_hood_acceleration(0.2);
+ // 30 hz sin wave on the hood causes acceleration to be ignored.
+ superstructure_plant_.set_peak_hood_acceleration(5.5);
superstructure_plant_.set_peak_intake_velocity(23.0);
superstructure_plant_.set_peak_intake_acceleration(0.2);