Make new single wheel finisher nominally work
We now have the new 2 motor, heavy flywheel. Update the code to have
the new inertia and also have 2 separate PWM channels to control it.
Change-Id: I2b109a0560ff499729b34a5a7e23293add4aa57c
diff --git a/y2020/constants.h b/y2020/constants.h
index e55a70b..f7d67e0 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -156,7 +156,7 @@
static constexpr double kFinisherEncoderCountsPerRevolution() {
return 2048.0;
}
- static constexpr double kFinisherEncoderRatio() { return 30.0 / 40.0; }
+ static constexpr double kFinisherEncoderRatio() { return 36.0 / 40.0; }
static constexpr double kMaxFinisherEncoderPulsesPerSecond() {
return control_loops::superstructure::finisher::kFreeSpeed / (2.0 * M_PI) *
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 6da1c03..035586a 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -13,27 +13,25 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-# Inertia for a single 4" diameter, 2" wide neopreme wheel.
-J_wheel = 0.000319 * 2.0 * 6.0
# Gear ratio to the final wheel.
# 40 tooth on the flywheel
# 48 for the falcon.
# 60 tooth on the outer wheel.
G = 48.0 / 40.0
# Overall flywheel inertia.
-J = J_wheel * (1.0 + (40.0 / 60.0)**2.0)
+J = 0.00507464
# The position and velocity are measured for the final wheel.
kFinisher = flywheel.FlywheelParams(
name='Finisher',
- motor=control_loop.Falcon(),
+ motor=control_loop.NMotor(control_loop.Falcon(), 2),
G=G,
J=J,
q_pos=0.01,
q_vel=100.0,
q_voltage=6.0,
r_pos=0.05,
- controller_poles=[.92])
+ controller_poles=[.90])
def main(argv):
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index e7b7b86..cc1e1b8 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -262,7 +262,7 @@
// Shooter
y2020::control_loops::superstructure::ShooterPositionT shooter;
shooter.theta_finisher =
- encoder_translate(finisher_encoder_->GetRaw(),
+ encoder_translate(-finisher_encoder_->GetRaw(),
Values::kFinisherEncoderCountsPerRevolution(),
Values::kFinisherEncoderRatio());
@@ -377,8 +377,12 @@
accelerator_right_falcon_ = ::std::move(t);
}
- void set_flywheel_falcon(::std::unique_ptr<::frc::TalonFX> t) {
- finisher_falcon_ = ::std::move(t);
+ void set_finisher_falcon0(::std::unique_ptr<::frc::TalonFX> t) {
+ finisher_falcon0_ = ::std::move(t);
+ }
+
+ void set_finisher_falcon1(::std::unique_ptr<::frc::TalonFX> t) {
+ finisher_falcon1_ = ::std::move(t);
}
void set_climber_falcon(
@@ -414,10 +418,12 @@
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- washing_machine_control_panel_victor_->SetSpeed(
- std::clamp(-output.washing_machine_spinner_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
+ if (washing_machine_control_panel_victor_) {
+ washing_machine_control_panel_victor_->SetSpeed(
+ std::clamp(-output.washing_machine_spinner_voltage(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ }
accelerator_left_falcon_->SetSpeed(
std::clamp(-output.accelerator_left_voltage(), -kMaxBringupPower,
@@ -429,9 +435,14 @@
kMaxBringupPower) /
12.0);
- finisher_falcon_->SetSpeed(std::clamp(output.finisher_voltage(),
- -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ finisher_falcon1_->SetSpeed(std::clamp(output.finisher_voltage(),
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+ finisher_falcon0_->SetSpeed(std::clamp(-output.finisher_voltage(),
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
if (climber_falcon_) {
climber_falcon_->Set(
@@ -448,17 +459,20 @@
intake_joint_victor_->SetDisabled();
turret_victor_->SetDisabled();
feeder_falcon_->SetDisabled();
- washing_machine_control_panel_victor_->SetDisabled();
+ if (washing_machine_control_panel_victor_) {
+ washing_machine_control_panel_victor_->SetDisabled();
+ }
accelerator_left_falcon_->SetDisabled();
accelerator_right_falcon_->SetDisabled();
- finisher_falcon_->SetDisabled();
+ finisher_falcon0_->SetDisabled();
+ finisher_falcon1_->SetDisabled();
}
::std::unique_ptr<::frc::VictorSP> hood_victor_, intake_joint_victor_,
turret_victor_, washing_machine_control_panel_victor_;
::std::unique_ptr<::frc::TalonFX> feeder_falcon_, accelerator_left_falcon_,
- accelerator_right_falcon_, finisher_falcon_;
+ accelerator_right_falcon_, finisher_falcon0_, finisher_falcon1_;
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
intake_roller_falcon_, climber_falcon_;
@@ -555,13 +569,15 @@
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
superstructure_writer.set_turret_victor(make_unique<frc::VictorSP>(7));
superstructure_writer.set_feeder_falcon(make_unique<frc::TalonFX>(6));
- superstructure_writer.set_washing_machine_control_panel_victor(
- make_unique<frc::VictorSP>(3));
+ // TODO(austin): When this goes over to CAN, update it and make it work.
+ //superstructure_writer.set_washing_machine_control_panel_victor(
+ //make_unique<frc::VictorSP>(3));
superstructure_writer.set_accelerator_left_falcon(
make_unique<::frc::TalonFX>(5));
superstructure_writer.set_accelerator_right_falcon(
make_unique<::frc::TalonFX>(4));
- superstructure_writer.set_flywheel_falcon(make_unique<::frc::TalonFX>(9));
+ superstructure_writer.set_finisher_falcon0(make_unique<::frc::TalonFX>(9));
+ superstructure_writer.set_finisher_falcon1(make_unique<::frc::TalonFX>(3));
// TODO: check port
superstructure_writer.set_climber_falcon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));