Add Flipper Arms Wpilib_Interface
Change-Id: I808519619dc271658666b8060d9903239b809718
Signed-off-by: Griffin Bui <griffinbui+gerrit@gmail.com>
diff --git a/y2022/constants.cc b/y2022/constants.cc
index b89ef5a..764e82b 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -92,6 +92,22 @@
climber->subsystem_params.make_integral_loop =
control_loops::superstructure::climber::MakeIntegralClimberLoop;
+ // Flipper arm constants
+ Values::PotConstants flipper_arms;
+ flipper_arms.subsystem_params.zeroing_voltage = 3.0;
+ flipper_arms.subsystem_params.operating_voltage = 12.0;
+ flipper_arms.subsystem_params.zeroing_profile_params = {0.5, 0.1};
+ flipper_arms.subsystem_params.default_profile_params = {6.0, 1.0};
+ flipper_arms.subsystem_params.range = Values::kFlipperArmRange();
+
+ auto *const flipper_arm_right = &r.flipper_arm_left;
+ auto *const flipper_arm_left = &r.flipper_arm_right;
+
+ *flipper_arm_right = flipper_arms;
+ *flipper_arm_left = flipper_arms;
+
+ // No integral loops for flipper arms
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -105,6 +121,8 @@
turret->potentiometer_offset = 0.0;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
0.0;
+ flipper_arm_left->potentiometer_offset = 0.0;
+ flipper_arm_right->potentiometer_offset = 0.0;
break;
case kCompTeamNumber:
@@ -118,6 +136,8 @@
turret->potentiometer_offset = 0.0;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
0.0;
+ flipper_arm_left->potentiometer_offset = 0.0;
+ flipper_arm_right->potentiometer_offset = 0.0;
break;
case kPracticeTeamNumber:
@@ -131,6 +151,8 @@
turret->potentiometer_offset = 0.0;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
0.0;
+ flipper_arm_left->potentiometer_offset = 0.0;
+ flipper_arm_right->potentiometer_offset = 0.0;
break;
case kCodingRobotTeamNumber:
@@ -144,6 +166,8 @@
turret->potentiometer_offset = 0.0;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
0.0;
+ flipper_arm_left->potentiometer_offset = 0.0;
+ flipper_arm_right->potentiometer_offset = 0.0;
break;
default:
diff --git a/y2022/constants.h b/y2022/constants.h
index 65aad44..c0c49e7 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -122,6 +122,21 @@
control_loops::superstructure::turret::kOutputRatio /
kTurretEncoderRatio() * kTurretEncoderCountsPerRevolution();
}
+
+ // Flipper arms
+ static constexpr double kFlipperArmSupplyCurrentLimit() { return 30.0; }
+ static constexpr double kFlipperArmStatorCurrentLimit() { return 40.0; }
+
+ // TODO: (Griffin) this needs to be set
+ static constexpr ::frc971::constants::Range kFlipperArmRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.01, .upper_hard = 0.6, .lower = 0.0, .upper = 0.5};
+ }
+
+ static constexpr double kFlipperArmsPotRatio() { return 16.0 / 36.0; }
+
+ PotConstants flipper_arm_left;
+ PotConstants flipper_arm_right;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index aecc090..a673361 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -5,24 +5,28 @@
// - is down + is up
climber_voltage:double (id: 0);
+ // Voltage of the flipper arms falcons
+ // - is feed + is open
+ flipper_arms_voltage:double (id: 1);
+
// Voltage of the catapult falcon
// Positive lifts the catapult to fire.
- catapult_voltage:double (id: 1);
+ catapult_voltage:double (id: 2);
// Voltage of the turret falcon
// Positive rotates the turret around the Z axis (up) according to the
// right hand rule.
- turret_voltage:double (id: 2);
+ turret_voltage:double (id: 3);
// Intake joint voltages.
- intake_voltage_front:double (id: 3);
- intake_voltage_back:double (id: 4);
+ intake_voltage_front:double (id: 4);
+ intake_voltage_back:double (id: 5);
// Intake roller voltages
- roller_voltage_front:double (id: 5);
- roller_voltage_back:double (id: 6);
+ roller_voltage_front:double (id: 6);
+ roller_voltage_back:double (id: 7);
// One transfer motor for both sides
- transfer_roller_voltage:double (id: 7);
+ transfer_roller_voltage:double (id: 8);
}
root_type Output;
diff --git a/y2022/control_loops/superstructure/superstructure_position.fbs b/y2022/control_loops/superstructure/superstructure_position.fbs
index 38babaf..cd0bc67 100644
--- a/y2022/control_loops/superstructure/superstructure_position.fbs
+++ b/y2022/control_loops/superstructure/superstructure_position.fbs
@@ -9,6 +9,10 @@
intake_front:frc971.PotAndAbsolutePosition (id: 1);
intake_back:frc971.PotAndAbsolutePosition (id: 2);
turret:frc971.PotAndAbsolutePosition (id: 3);
+
+ // Zero is straight and positive is open
+ flipper_arm_left:frc971.RelativePosition (id: 4);
+ flipper_arm_right:frc971.RelativePosition (id: 5);
}
root_type Position;
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 31f2013..bd884a8 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -81,6 +81,11 @@
Values::kClimberPotMetersPerRevolution();
}
+double flipper_arms_pot_translate(double voltage) {
+ return voltage * Values::kFlipperArmsPotRatio() *
+ (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
+
double intake_pot_translate(double voltage) {
return voltage * Values::kIntakePotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
(2 * M_PI /*radians*/);
@@ -142,18 +147,28 @@
flatbuffers::Offset<frc971::RelativePosition> climber_offset =
frc971::RelativePosition::Pack(*builder.fbb(), &climber);
+ frc971::RelativePositionT flipper_arm_left;
+ CopyPosition(*flipper_arm_left_potentiometer_, &flipper_arm_left,
+ flipper_arms_pot_translate, false,
+ values_->flipper_arm_left.potentiometer_offset);
+
+ frc971::RelativePositionT flipper_arm_right;
+ CopyPosition(*flipper_arm_right_potentiometer_, &flipper_arm_right,
+ flipper_arms_pot_translate, false,
+ values_->flipper_arm_right.potentiometer_offset);
+
// Intake
frc971::PotAndAbsolutePositionT intake_front;
- frc971::PotAndAbsolutePositionT intake_back;
- frc971::PotAndAbsolutePositionT turret;
CopyPosition(intake_encoder_front_, &intake_front,
Values::kIntakeEncoderCountsPerRevolution(),
Values::kIntakeEncoderRatio(), intake_pot_translate, false,
values_->intake_front.potentiometer_offset);
+ frc971::PotAndAbsolutePositionT intake_back;
CopyPosition(intake_encoder_back_, &intake_back,
Values::kIntakeEncoderCountsPerRevolution(),
Values::kIntakeEncoderRatio(), intake_pot_translate, false,
values_->intake_back.potentiometer_offset);
+ frc971::PotAndAbsolutePositionT turret;
CopyPosition(turret_encoder_, &turret,
Values::kTurretEncoderCountsPerRevolution(),
Values::kTurretEncoderRatio(), turret_pot_translate, false,
@@ -165,10 +180,16 @@
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake_back);
flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
+ flatbuffers::Offset<frc971::RelativePosition> flipper_arm_left_offset =
+ frc971::RelativePosition::Pack(*builder.fbb(), &flipper_arm_left);
+ flatbuffers::Offset<frc971::RelativePosition> flipper_arm_right_offset =
+ frc971::RelativePosition::Pack(*builder.fbb(), &flipper_arm_right);
superstructure::Position::Builder position_builder =
builder.MakeBuilder<superstructure::Position>();
position_builder.add_climber(climber_offset);
+ position_builder.add_flipper_arm_left(flipper_arm_left_offset);
+ position_builder.add_flipper_arm_right(flipper_arm_right_offset);
position_builder.add_intake_front(intake_offset_front);
position_builder.add_intake_back(intake_offset_back);
position_builder.add_turret(turret_offset);
@@ -218,6 +239,16 @@
climber_potentiometer_ = ::std::move(potentiometer);
}
+ void set_flipper_arm_left_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ flipper_arm_left_potentiometer_ = ::std::move(potentiometer);
+ }
+
+ void set_flipper_arm_right_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ flipper_arm_right_potentiometer_ = ::std::move(potentiometer);
+ }
+
void set_intake_encoder_front(::std::unique_ptr<frc::Encoder> encoder) {
fast_encoder_filter_.Add(encoder.get());
intake_encoder_front_.set_encoder(::std::move(encoder));
@@ -273,7 +304,8 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
- std::unique_ptr<frc::AnalogInput> climber_potentiometer_;
+ std::unique_ptr<frc::AnalogInput> climber_potentiometer_,
+ flipper_arm_right_potentiometer_, flipper_arm_left_potentiometer_;
frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_encoder_front_,
intake_encoder_back_, turret_encoder_;
};
@@ -331,6 +363,17 @@
Values::kIntakeRollerStatorCurrentLimit(), 0});
}
+ void set_flipper_arms_falcon(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ flipper_arms_falcon_ = ::std::move(t);
+ flipper_arms_falcon_->ConfigSupplyCurrentLimit(
+ {true, Values::kFlipperArmSupplyCurrentLimit(),
+ Values::kFlipperArmSupplyCurrentLimit(), 0});
+ flipper_arms_falcon_->ConfigStatorCurrentLimit(
+ {true, Values::kFlipperArmStatorCurrentLimit(),
+ Values::kFlipperArmStatorCurrentLimit(), 0});
+ }
+
void set_transfer_roller_victor(::std::unique_ptr<::frc::VictorSP> t) {
transfer_roller_victor_ = ::std::move(t);
}
@@ -343,6 +386,8 @@
ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
roller_falcon_back_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled,
0);
+ flipper_arms_falcon_->Set(
+ ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
intake_falcon_front_->SetDisabled();
intake_falcon_back_->SetDisabled();
transfer_roller_victor_->SetDisabled();
@@ -353,13 +398,18 @@
void Write(const superstructure::Output &output) override {
WritePwm(output.climber_voltage(), climber_falcon_.get());
+
WritePwm(output.intake_voltage_front(), intake_falcon_front_.get());
WritePwm(output.intake_voltage_back(), intake_falcon_back_.get());
WriteCan(output.roller_voltage_front(), roller_falcon_front_.get());
WriteCan(output.roller_voltage_back(), roller_falcon_back_.get());
WritePwm(output.transfer_roller_voltage(), transfer_roller_victor_.get());
+
+ WriteCan(output.flipper_arms_voltage(), flipper_arms_falcon_.get());
+
WritePwm(output.catapult_voltage(), catapult_falcon_1_.get());
WritePwm(output.catapult_voltage(), catapult_falcon_2_.get());
+
WritePwm(output.turret_voltage(), turret_falcon_.get());
}
@@ -379,7 +429,7 @@
::std::unique_ptr<frc::TalonFX> intake_falcon_front_, intake_falcon_back_;
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- roller_falcon_front_, roller_falcon_back_;
+ roller_falcon_front_, roller_falcon_back_, flipper_arms_falcon_;
::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
catapult_falcon_2_, climber_falcon_;
@@ -435,6 +485,11 @@
sensor_reader.set_climber_potentiometer(make_unique<frc::AnalogInput>(5));
+ sensor_reader.set_flipper_arm_left_potentiometer(
+ make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_flipper_arm_right_potentiometer(
+ make_unique<frc::AnalogInput>(5));
+
AddLoop(&sensor_reader_event_loop);
// Thread 4.
@@ -447,18 +502,20 @@
SuperstructureWriter superstructure_writer(&output_event_loop);
- superstructure_writer.set_turret_falcon(make_unique<::frc::TalonFX>(2));
- superstructure_writer.set_catapult_falcon_1(make_unique<::frc::TalonFX>(3));
- superstructure_writer.set_catapult_falcon_2(make_unique<::frc::TalonFX>(4));
+ superstructure_writer.set_turret_falcon(make_unique<::frc::TalonFX>(0));
+ superstructure_writer.set_catapult_falcon_1(make_unique<::frc::TalonFX>(1));
+ superstructure_writer.set_catapult_falcon_2(make_unique<::frc::TalonFX>(2));
superstructure_writer.set_roller_falcon_front(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(3));
superstructure_writer.set_roller_falcon_back(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(2));
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(4));
superstructure_writer.set_transfer_roller_victor(
- make_unique<::frc::VictorSP>(4));
+ make_unique<::frc::VictorSP>(2));
superstructure_writer.set_intake_falcon_front(make_unique<frc::TalonFX>(5));
superstructure_writer.set_intake_falcon_back(make_unique<frc::TalonFX>(6));
superstructure_writer.set_climber_falcon(make_unique<frc::TalonFX>(7));
+ superstructure_writer.set_flipper_arms_falcon(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(5));
AddLoop(&output_event_loop);