Added units per volt ratio constants
This reverts commit 00c730d45e241e494d8c22b52340769ea9a83a9a.
Reason for revert: Works again.
Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: Ia1f340c69586cc0ef417408b99eee7fc6b4fa4fb
diff --git a/y2022/BUILD b/y2022/BUILD
index 9d3acab..69bba3e 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -217,6 +217,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 913525f..21ba9e4 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"
@@ -363,6 +364,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 =