Added units per volt ratio constants
This makes it easier to convert subsystem
measurement units to volts instead of having to
manually multiply the pot ratios.
Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: If0d8d21db50bffd3058c783bbe2b274fd714eeac
Signed-off-by: Nathan Leong <100028864@mvla.net>
diff --git a/y2022/BUILD b/y2022/BUILD
index 5d29f04..bfeaac3 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -214,6 +214,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/wpilib:wpilib_utils",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
"//y2022/control_loops/superstructure/catapult:catapult_plants",
"//y2022/control_loops/superstructure/climber:climber_plants",
diff --git a/y2022/constants.cc b/y2022/constants.cc
index c97058e..af6e829 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -10,6 +10,7 @@
#include "absl/base/call_once.h"
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
+#include "frc971/wpilib/wpilib_utils.h"
#include "glog/logging.h"
#include "y2022/control_loops/superstructure/catapult/integral_catapult_plant.h"
#include "y2022/control_loops/superstructure/climber/integral_climber_plant.h"
@@ -190,21 +191,25 @@
{5, {4.0}},
});
- climber->potentiometer_offset = 0.0;
- intake_front->potentiometer_offset = 0.0;
- intake_front->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.0;
- intake_back->potentiometer_offset = 0.0;
- intake_back->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.0;
- 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;
+ climber->potentiometer_offset = -0.035;
- catapult_params->zeroing_constants.measured_absolute_position = 0.0;
- catapult->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 3.122;
+ intake_front->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.175;
+
+ intake_back->potentiometer_offset = 3.365;
+ intake_back->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.052;
+
+ turret->potentiometer_offset = -10.668;
+ turret->subsystem_params.zeroing_constants.measured_absolute_position =
+ 1.308;
+
+ flipper_arm_left->potentiometer_offset = -6.40;
+ flipper_arm_right->potentiometer_offset = 5.56;
+
+ catapult_params->zeroing_constants.measured_absolute_position = 1.717;
+ catapult->potentiometer_offset = -2.034;
ball_color->reference_red = {0, 0, 1, 1};
ball_color->reference_blue = {0, 0, 1, 1};
@@ -348,6 +353,47 @@
LOG(FATAL) << "unknown team: " << team;
}
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kClimberRange(), climber->potentiometer_offset,
+ [](double meters) { return meters / Values::kClimberPotMetersPerVolt(); },
+ false))
+ << "Couldn't translate climber pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kFlipperArmRange(), flipper_arm_left->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kFlipperArmsPotRadiansPerVolt();
+ },
+ false))
+ << "Couldn't translate flipper left pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kFlipperArmRange(), flipper_arm_right->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kFlipperArmsPotRadiansPerVolt();
+ },
+ true))
+ << "Couldn't translate flipper right pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kIntakeRange(), intake_front->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kIntakePotRadiansPerVolt();
+ },
+ true))
+ << "Couldn't translate front intake pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kIntakeRange(), intake_back->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kIntakePotRadiansPerVolt();
+ },
+ true))
+ << "Couldn't translate back intake pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ *turret_range, turret->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kTurretPotRadiansPerVolt();
+ },
+ false))
+ << "Couldn't translate turret pot";
+
return r;
}
diff --git a/y2022/constants.h b/y2022/constants.h
index 297f814..62b9b4a 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -58,6 +58,11 @@
}
static constexpr double kClimberPotRatio() { return 1.0; }
+ static constexpr double kClimberPotMetersPerVolt() {
+ return kClimberPotRatio() * (5.0 /*turns*/ / 5.0 /*volts*/) *
+ kClimberPotMetersPerRevolution();
+ }
+
struct PotConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::RelativeEncoderZeroingEstimator>
@@ -77,6 +82,11 @@
static constexpr double kIntakePotRatio() { return 16.0 / 64.0; }
+ static constexpr double kIntakePotRadiansPerVolt() {
+ return kIntakePotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+
static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
return control_loops::superstructure::intake::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::intake::kOutputRatio /
@@ -121,6 +131,10 @@
static constexpr double kTurretFrontIntakePos() { return 0; }
static constexpr double kTurretPotRatio() { return 27.0 / 110.0; }
+ static constexpr double kTurretPotRadiansPerVolt() {
+ return kTurretPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
static constexpr double kTurretEncoderRatio() { return kTurretPotRatio(); }
static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
@@ -175,6 +189,11 @@
static constexpr double kFlipperArmsPotRatio() { return 16.0 / 36.0; }
+ static constexpr double kFlipperArmsPotRadiansPerVolt() {
+ return kFlipperArmsPotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+
PotConstants flipper_arm_left;
PotConstants flipper_arm_right;
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 84164bb..8df3a3a 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -82,24 +82,19 @@
}
double climber_pot_translate(double voltage) {
- return voltage * Values::kClimberPotRatio() *
- (5.0 /*turns*/ / 5.0 /*volts*/) *
- Values::kClimberPotMetersPerRevolution();
+ return voltage * Values::kClimberPotMetersPerVolt();
}
double flipper_arms_pot_translate(double voltage) {
- return voltage * Values::kFlipperArmsPotRatio() *
- (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ return voltage * Values::kFlipperArmsPotRadiansPerVolt();
}
double intake_pot_translate(double voltage) {
- return voltage * Values::kIntakePotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
- (2 * M_PI /*radians*/);
+ return voltage * Values::kIntakePotRadiansPerVolt();
}
double turret_pot_translate(double voltage) {
- return voltage * Values::kTurretPotRatio() *
- (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ return voltage * Values::kTurretPotRadiansPerVolt();
}
constexpr double kMaxFastEncoderPulsesPerSecond =