Add shooter to wpilib_interface
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I56e53cbfdd1a4e0cf20f95967b39da6f32e262ca
diff --git a/y2024/BUILD b/y2024/BUILD
index d8d0db7..4aec7d2 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -211,9 +211,12 @@
"//frc971/zeroing:absolute_encoder",
"//frc971/zeroing:pot_and_absolute_encoder",
"//y2024/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2024/control_loops/superstructure/altitude:altitude_plants",
+ "//y2024/control_loops/superstructure/catapult:catapult_plants",
"//y2024/control_loops/superstructure/climber:climber_plants",
"//y2024/control_loops/superstructure/extend:extend_plants",
"//y2024/control_loops/superstructure/intake_pivot:intake_pivot_plants",
+ "//y2024/control_loops/superstructure/turret:turret_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
diff --git a/y2024/constants.h b/y2024/constants.h
index 2e482e6..2eff408 100644
--- a/y2024/constants.h
+++ b/y2024/constants.h
@@ -11,9 +11,12 @@
#include "frc971/zeroing/absolute_encoder.h"
#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024/control_loops/superstructure/altitude/altitude_plant.h"
+#include "y2024/control_loops/superstructure/catapult/catapult_plant.h"
#include "y2024/control_loops/superstructure/climber/climber_plant.h"
#include "y2024/control_loops/superstructure/extend/extend_plant.h"
#include "y2024/control_loops/superstructure/intake_pivot/intake_pivot_plant.h"
+#include "y2024/control_loops/superstructure/turret/turret_plant.h"
namespace y2024::constants {
@@ -91,6 +94,25 @@
}
static constexpr double kExtendEncoderCountsPerRevolution() { return 4096.0; }
+ // TODO: (niko) add the gear ratios for the intake once we have them
+ static constexpr double kCatapultEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ static constexpr double kCatapultEncoderRatio() { return 12.0 / 24.0; }
+
+ static constexpr double kCatapultPotRatio() { return 12.0 / 24.0; }
+
+ static constexpr double kCatapultPotRadiansPerVolt() {
+ return kCatapultPotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+
+ static constexpr double kMaxCatapultEncoderPulsesPerSecond() {
+ return control_loops::superstructure::catapult::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::catapult::kOutputRatio /
+ kCatapultEncoderRatio() * kCatapultEncoderCountsPerRevolution();
+ }
static constexpr double kExtendEncoderRatio() { return 1.0; }
@@ -109,6 +131,42 @@
kExtendEncoderRatio() * kExtendEncoderCountsPerRevolution();
}
+ static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kTurretPotRatio() {
+ return (22.0 / 100.0) * (28.0 / 48.0) * (36.0 / 24.0);
+ }
+
+ static constexpr double kTurretEncoderRatio() { return 22.0 / 100.0; }
+
+ static constexpr double kTurretPotRadiansPerVolt() {
+ return kTurretPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+ static constexpr double kMaxTurretEncoderPulsesPerSecond() {
+ return control_loops::superstructure::turret::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::turret::kOutputRatio /
+ kTurretEncoderRatio() * kTurretEncoderCountsPerRevolution();
+ }
+
+ static constexpr double kAltitudeEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ static constexpr double kAltitudeEncoderRatio() { return 16.0 / 162.0; }
+
+ static constexpr double kAltitudePotRatio() { return 16.0 / 162.0; }
+
+ static constexpr double kAltitudePotRadiansPerVolt() {
+ return kAltitudePotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+ static constexpr double kMaxAltitudeEncoderPulsesPerSecond() {
+ return control_loops::superstructure::altitude::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::altitude::kOutputRatio /
+ kAltitudeEncoderRatio() * kAltitudeEncoderCountsPerRevolution();
+ }
+
// 20 -> 28 reduction to a 0.5" radius roller
static constexpr double kTransferRollerOutputRatio = (20.0 / 28.0) * 0.0127;
// 20 -> 34 reduction, and the 34 is on a 0.625" radius roller
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 4ad6cd7..e444f8d 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -55,7 +55,11 @@
"extend_supply_current_limit": 35,
"extend_stator_current_limit": 300,
"extend_roller_supply_current_limit": 35,
- "extend_roller_stator_current_limit": 60
+ "extend_roller_stator_current_limit": 60,
+ "turret_supply_current_limit": 35,
+ "turret_stator_current_limit": 60,
+ "altitude_supply_current_limit": 35,
+ "altitude_stator_current_limit": 60
},
"transfer_roller_voltages": {
"transfer_in": 12.0,
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 6f6e455..633dc2e 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -75,6 +75,10 @@
extend_stator_current_limit:double (id: 11);
extend_roller_supply_current_limit:double (id: 12);
extend_roller_stator_current_limit:double (id: 13);
+ turret_supply_current_limit:double (id: 14);
+ turret_stator_current_limit:double (id: 15);
+ altitude_supply_current_limit:double (id: 16);
+ altitude_stator_current_limit:double (id: 17);
}
table TransferRollerVoltages {
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index dd8f437..bce5451 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -85,6 +85,18 @@
return voltage * Values::kExtendPotMetersPerVolt();
}
+double catapult_pot_translate(double voltage) {
+ return voltage * Values::kCatapultPotRadiansPerVolt();
+}
+
+double turret_pot_translate(double voltage) {
+ return voltage * Values::kTurretPotRadiansPerVolt();
+}
+
+double altitude_pot_translate(double voltage) {
+ return voltage * Values::kAltitudePotRadiansPerVolt();
+}
+
double drivetrain_velocity_translate(double in) {
return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
(2.0 * M_PI)) *
@@ -97,7 +109,9 @@
Values::kMaxIntakePivotEncoderPulsesPerSecond(),
Values::kMaxClimberEncoderPulsesPerSecond(),
Values::kMaxExtendEncoderPulsesPerSecond(),
+ Values::kMaxCatapultEncoderPulsesPerSecond(),
});
+
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
@@ -159,6 +173,29 @@
->extend_constants()
->potentiometer_offset());
+ CopyPosition(catapult_encoder_, builder->add_catapult(),
+ Values::kCatapultEncoderCountsPerRevolution(),
+ Values::kCatapultEncoderRatio(), catapult_pot_translate,
+ true,
+ robot_constants_->robot()
+ ->catapult_constants()
+ ->potentiometer_offset());
+
+ CopyPosition(turret_encoder_, builder->add_turret(),
+ Values::kTurretEncoderCountsPerRevolution(),
+ Values::kTurretEncoderRatio(), turret_pot_translate, true,
+ robot_constants_->robot()
+ ->turret_constants()
+ ->potentiometer_offset());
+
+ CopyPosition(altitude_encoder_, builder->add_altitude(),
+ Values::kAltitudeEncoderCountsPerRevolution(),
+ Values::kAltitudeEncoderRatio(), altitude_pot_translate,
+ true,
+ robot_constants_->robot()
+ ->altitude_constants()
+ ->potentiometer_offset());
+
builder->set_transfer_beambreak(transfer_beam_break_->Get());
builder.CheckOk(builder.Send());
}
@@ -246,6 +283,33 @@
extend_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ void set_catapult(::std::unique_ptr<frc::Encoder> encoder,
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm,
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ fast_encoder_filter_.Add(encoder.get());
+ catapult_encoder_.set_encoder(::std::move(encoder));
+ catapult_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ catapult_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_turret(::std::unique_ptr<frc::Encoder> encoder,
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm,
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ fast_encoder_filter_.Add(encoder.get());
+ turret_encoder_.set_encoder(::std::move(encoder));
+ turret_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ turret_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_altitude(::std::unique_ptr<frc::Encoder> encoder,
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm,
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ fast_encoder_filter_.Add(encoder.get());
+ altitude_encoder_.set_encoder(::std::move(encoder));
+ altitude_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ altitude_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
private:
const Constants *robot_constants_;
@@ -260,8 +324,8 @@
std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_, transfer_beam_break_;
frc971::wpilib::AbsoluteEncoder intake_pivot_encoder_;
- frc971::wpilib::AbsoluteEncoderAndPotentiometer climber_encoder_;
- frc971::wpilib::AbsoluteEncoderAndPotentiometer extend_encoder_;
+ frc971::wpilib::AbsoluteEncoderAndPotentiometer climber_encoder_,
+ catapult_encoder_, turret_encoder_, altitude_encoder_, extend_encoder_;
frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
};
@@ -310,9 +374,16 @@
sensor_reader.set_climber(make_encoder(5),
make_unique<frc::DigitalInput>(5),
make_unique<frc::AnalogInput>(5));
-
- sensor_reader.set_extend(make_encoder(6), make_unique<frc::DigitalInput>(6),
- make_unique<frc::AnalogInput>(6));
+ sensor_reader.set_extend(make_encoder(7), make_unique<frc::DigitalInput>(7),
+ make_unique<frc::AnalogInput>(7));
+ sensor_reader.set_catapult(make_encoder(2),
+ make_unique<frc::DigitalInput>(2),
+ make_unique<frc::AnalogInput>(2));
+ sensor_reader.set_turret(make_encoder(3), make_unique<frc::DigitalInput>(3),
+ make_unique<frc::AnalogInput>(3));
+ sensor_reader.set_altitude(make_encoder(6),
+ make_unique<frc::DigitalInput>(6),
+ make_unique<frc::AnalogInput>(6));
AddLoop(&sensor_reader_event_loop);
@@ -349,26 +420,34 @@
4, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->intake_pivot_stator_current_limit(),
current_limits->intake_pivot_supply_current_limit());
+ std::shared_ptr<TalonFX> altitude = std::make_shared<TalonFX>(
+ 5, false, "Drivetrain Bus", &canivore_signal_registry,
+ current_limits->altitude_stator_current_limit(),
+ current_limits->altitude_supply_current_limit());
+ std::shared_ptr<TalonFX> turret = std::make_shared<TalonFX>(
+ 6, false, "Drivetrain Bus", &canivore_signal_registry,
+ current_limits->turret_stator_current_limit(),
+ current_limits->turret_supply_current_limit());
std::shared_ptr<TalonFX> intake_roller = std::make_shared<TalonFX>(
- 5, false, "rio", &rio_signal_registry,
+ 7, false, "rio", &rio_signal_registry,
current_limits->intake_roller_stator_current_limit(),
current_limits->intake_roller_supply_current_limit());
std::shared_ptr<TalonFX> transfer_roller = std::make_shared<TalonFX>(
- 6, false, "rio", &rio_signal_registry,
+ 8, false, "rio", &rio_signal_registry,
current_limits->transfer_roller_stator_current_limit(),
current_limits->transfer_roller_supply_current_limit());
std::shared_ptr<TalonFX> climber = std::make_shared<TalonFX>(
- 7, false, "rio", &rio_signal_registry,
+ 9, false, "rio", &rio_signal_registry,
current_limits->climber_stator_current_limit(),
current_limits->climber_supply_current_limit());
std::shared_ptr<TalonFX> extend = std::make_shared<TalonFX>(
- 8, false, "Drivetrain Bus", &rio_signal_registry,
+ 8, false, "rio", &rio_signal_registry,
current_limits->extend_stator_current_limit(),
current_limits->extend_supply_current_limit());
std::shared_ptr<TalonFX> extend_roller = std::make_shared<TalonFX>(
- 9, false, "Drivetrain Bus", &rio_signal_registry,
+ 9, false, "rio", &rio_signal_registry,
current_limits->extend_roller_stator_current_limit(),
current_limits->extend_roller_supply_current_limit());
@@ -393,7 +472,7 @@
canivore_talonfxs.push_back(talonfx);
}
- for (auto talonfx : {intake_pivot}) {
+ for (auto talonfx : {intake_pivot, altitude, turret}) {
canivore_talonfxs.push_back(talonfx);
}
@@ -417,8 +496,8 @@
frc971::wpilib::CANSensorReader canivore_can_sensor_reader(
&can_sensor_reader_event_loop, std::move(canivore_signal_registry),
canivore_talonfxs,
- [drivetrain_talonfxs, &intake_pivot, &drivetrain_can_position_sender,
- &superstructure_can_position_sender](
+ [drivetrain_talonfxs, &intake_pivot, &altitude, &turret,
+ &drivetrain_can_position_sender, &superstructure_can_position_sender](
ctre::phoenix::StatusCode status) {
aos::Sender<frc971::control_loops::drivetrain::CANPositionStatic>::
StaticBuilder drivetrain_can_builder =
@@ -447,7 +526,13 @@
intake_pivot->SerializePosition(
superstructure_can_builder->add_intake_pivot(),
- superstructure::intake_pivot::kOutputRatio);
+ control_loops::drivetrain::kHighOutputRatio);
+ altitude->SerializePosition(
+ superstructure_can_builder->add_altitude(),
+ control_loops::drivetrain::kHighOutputRatio);
+ turret->SerializePosition(
+ superstructure_can_builder->add_turret(),
+ control_loops::drivetrain::kHighOutputRatio);
superstructure_can_builder->set_timestamp(
intake_pivot->GetTimestamp());
@@ -519,6 +604,10 @@
output.extend_voltage());
talonfx_map.find("extend_roller")
->second->WriteVoltage(output.extend_roller_voltage());
+ talonfx_map.find("altitude")
+ ->second->WriteVoltage(output.altitude_voltage());
+ talonfx_map.find("turret")->second->WriteVoltage(
+ output.turret_voltage());
});
can_drivetrain_writer.set_talonfxs({right_front, right_back},
@@ -530,6 +619,8 @@
can_superstructure_writer.add_talonfx("climber", climber);
can_superstructure_writer.add_talonfx("extend", extend);
can_superstructure_writer.add_talonfx("extend_roller", extend_roller);
+ can_superstructure_writer.add_talonfx("altitude", altitude);
+ can_superstructure_writer.add_talonfx("turret", turret);
can_output_event_loop.MakeWatcher(
"/roborio", [&can_drivetrain_writer, &can_superstructure_writer](