Add pivot joint in wpilib_interface
Signed-off-by: Maxwell Henderson <maxwell.henderson@mailbox.org>
Change-Id: I4c221a3713cb56c20b8af95417737233c5e9cab2
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index 2dd1144..475a72a 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -90,6 +90,10 @@
control_loops::drivetrain::kWheelRadius;
}
+double pivot_pot_translate(double voltage) {
+ return voltage * Values::kPivotJointPotRadiansPerVolt();
+}
+
constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
Values::kMaxDrivetrainEncoderPulsesPerSecond(),
Values::kMaxPivotJointEncoderPulsesPerSecond(),
@@ -289,12 +293,14 @@
std::shared_ptr<Falcon> right_back,
std::shared_ptr<Falcon> left_front,
std::shared_ptr<Falcon> left_back,
- std::shared_ptr<Falcon> roller_falcon) {
+ std::shared_ptr<Falcon> roller_falcon,
+ std::shared_ptr<Falcon> pivot_falcon) {
right_front_ = std::move(right_front);
right_back_ = std::move(right_back);
left_front_ = std::move(left_front);
left_back_ = std::move(left_back);
roller_falcon_ = std::move(roller_falcon);
+ pivot_falcon_ = std::move(pivot_falcon);
}
std::optional<frc971::control_loops::CANFalconT> roller_falcon_data() {
@@ -314,8 +320,8 @@
auto builder = can_position_sender_.MakeBuilder();
- for (auto falcon :
- {right_front_, right_back_, left_front_, left_back_, roller_falcon_}) {
+ for (auto falcon : {right_front_, right_back_, left_front_, left_back_,
+ roller_falcon_, pivot_falcon_}) {
falcon->RefreshNontimesyncedSignals();
}
@@ -366,7 +372,7 @@
can_position_sender_;
std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_,
- roller_falcon_;
+ roller_falcon_, pivot_falcon_;
std::optional<frc971::control_loops::CANFalconT> roller_falcon_data_;
@@ -422,15 +428,26 @@
flatbuffers::Offset<frc971::control_loops::CANFalcon>
roller_falcon_offset;
+ frc971::PotAndAbsolutePositionT pivot;
+ CopyPosition(pivot_encoder_, &pivot,
+ Values::kPivotJointEncoderCountsPerRevolution(),
+ Values::kPivotJointEncoderRatio(), pivot_pot_translate, true,
+ values_->pivot_joint.potentiometer_offset);
+
auto optional_roller_falcon = can_sensor_reader_->roller_falcon_data();
if (optional_roller_falcon.has_value()) {
roller_falcon_offset = frc971::control_loops::CANFalcon::Pack(
*builder.fbb(), &optional_roller_falcon.value());
}
+
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> pivot_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &pivot);
+
superstructure::Position::Builder position_builder =
builder.MakeBuilder<superstructure::Position>();
position_builder.add_end_effector_cube_beam_break(
!end_effector_cube_beam_break_->Get());
+ position_builder.add_pivot_joint_position(pivot_offset);
if (!roller_falcon_offset.IsNull()) {
position_builder.add_roller_falcon(roller_falcon_offset);
@@ -518,6 +535,21 @@
end_effector_cube_beam_break_ = ::std::move(sensor);
}
+ void set_pivot_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ pivot_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_pivot_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ pivot_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_pivot_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ pivot_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
private:
std::shared_ptr<const Values> values_;
@@ -534,6 +566,8 @@
frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
+ frc971::wpilib::AbsoluteEncoderAndPotentiometer pivot_encoder_;
+
CANSensorReader *can_sensor_reader_;
};
@@ -587,6 +621,7 @@
void HandleCANConfiguration(const frc971::CANConfiguration &configuration) {
roller_falcon_->PrintConfigs();
+ pivot_falcon_->PrintConfigs();
if (configuration.reapply()) {
WriteConfigs();
}
@@ -596,6 +631,10 @@
roller_falcon_ = std::move(roller_falcon);
}
+ void set_pivot_falcon(std::shared_ptr<Falcon> pivot_falcon) {
+ pivot_falcon_ = std::move(pivot_falcon);
+ }
+
private:
void WriteConfigs() { roller_falcon_->WriteRollerConfigs(); }
@@ -605,6 +644,11 @@
roller_control.UpdateFreqHz = 0_Hz;
roller_control.EnableFOC = true;
+ ctre::phoenix6::controls::DutyCycleOut pivot_control(
+ SafeSpeed(-output.roller_voltage()));
+ pivot_control.UpdateFreqHz = 0_Hz;
+ pivot_control.EnableFOC = true;
+
ctre::phoenix::StatusCode status =
roller_falcon_->talon()->SetControl(roller_control);
@@ -612,6 +656,13 @@
AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
status.GetName(), status.GetDescription());
}
+
+ status = pivot_falcon_->talon()->SetControl(pivot_control);
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
}
void Stop() override {
@@ -621,6 +672,7 @@
stop_command.EnableFOC = true;
roller_falcon_->talon()->SetControl(stop_command);
+ pivot_falcon_->talon()->SetControl(stop_command);
}
double SafeSpeed(double voltage) {
@@ -628,6 +680,7 @@
}
std::shared_ptr<Falcon> roller_falcon_;
+ std::shared_ptr<Falcon> pivot_falcon_;
};
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
@@ -773,6 +826,8 @@
std::make_shared<Falcon>(3, "Drivetrain Bus", &signals_registry);
std::shared_ptr<Falcon> roller =
std::make_shared<Falcon>(5, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> pivot =
+ std::make_shared<Falcon>(4, "Drivetrain Bus", &signals_registry);
// Thread 3.
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
@@ -781,7 +836,7 @@
std::move(signals_registry));
can_sensor_reader.set_falcons(right_front, right_back, left_front,
- left_back, roller);
+ left_back, roller, pivot);
AddLoop(&can_sensor_reader_event_loop);
@@ -794,9 +849,14 @@
sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
sensor_reader.set_superstructure_reading(superstructure_reading);
sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
+
sensor_reader.set_end_effector_cube_beam_break(
make_unique<frc::DigitalInput>(22));
+ sensor_reader.set_pivot_encoder(make_encoder(3));
+ sensor_reader.set_pivot_absolute_pwm(make_unique<frc::DigitalInput>(3));
+ sensor_reader.set_pivot_potentiometer(make_unique<frc::AnalogInput>(3));
+
AddLoop(&sensor_reader_event_loop);
// Thread 5.
@@ -844,6 +904,7 @@
// Setup superstructure CAN output.
SuperstructureCANWriter superstructure_can_writer(&can_output_event_loop);
superstructure_can_writer.set_roller_falcon(roller);
+ superstructure_can_writer.set_pivot_falcon(pivot);
can_output_event_loop.MakeWatcher(
"/roborio", [&drivetrain_writer, &superstructure_can_writer](