Add y2023 wrist and roll constants
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I801842f0852a0e93c10b16cd83621de3b05b039c
diff --git a/y2023/BUILD b/y2023/BUILD
index d5f85ca..ad82c2b 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -220,6 +220,8 @@
"//frc971/shooter_interpolation:interpolation",
"//y2023/control_loops/drivetrain:polydrivetrain_plants",
"//y2023/control_loops/superstructure/arm:arm_constants",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
+ "//y2023/control_loops/superstructure/wrist:wrist_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
diff --git a/y2023/constants.cc b/y2023/constants.cc
index 89d9f49..123bf2b 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -11,6 +11,8 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "glog/logging.h"
+#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
+#include "y2023/control_loops/superstructure/wrist/integral_wrist_plant.h"
namespace y2023 {
namespace constants {
@@ -19,9 +21,10 @@
LOG(INFO) << "creating a Constants for team: " << team;
Values r;
-
auto *const arm_proximal = &r.arm_proximal;
auto *const arm_distal = &r.arm_distal;
+ auto *const wrist = &r.wrist;
+ auto *const roll_joint = &r.roll_joint;
arm_proximal->zeroing.average_filter_size = Values::kZeroingSampleSize;
arm_proximal->zeroing.one_revolution_distance =
@@ -37,6 +40,40 @@
arm_distal->zeroing.moving_buffer_size = 20;
arm_distal->zeroing.allowable_encoder_error = 0.9;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ *const roll_joint_params = &roll_joint->subsystem_params;
+
+ roll_joint_params->zeroing_voltage = 3.0;
+ roll_joint_params->operating_voltage = 12.0;
+ roll_joint_params->zeroing_profile_params = {0.5, 3.0};
+ roll_joint_params->default_profile_params = {6.0, 30.0};
+ roll_joint_params->range = Values::kRollJointRange();
+ roll_joint_params->make_integral_loop =
+ control_loops::superstructure::roll::MakeIntegralRollLoop;
+ roll_joint_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ roll_joint_params->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kRollJointEncoderRatio();
+ roll_joint_params->zeroing_constants.zeroing_threshold = 0.0005;
+ roll_joint_params->zeroing_constants.moving_buffer_size = 20;
+ roll_joint_params->zeroing_constants.allowable_encoder_error = 0.9;
+
+ wrist->zeroing_voltage = 3.0;
+ wrist->operating_voltage = 12.0;
+ wrist->zeroing_profile_params = {0.5, 3.0};
+ wrist->default_profile_params = {6.0, 30.0};
+ wrist->range = Values::kWristRange();
+ wrist->make_integral_loop =
+ control_loops::superstructure::wrist::MakeIntegralWristLoop;
+ wrist->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
+ wrist->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kWristEncoderRatio();
+ wrist->zeroing_constants.zeroing_threshold = 0.0005;
+ wrist->zeroing_constants.moving_buffer_size = 20;
+ wrist->zeroing_constants.allowable_encoder_error = 0.9;
+ wrist->zeroing_constants.middle_position = Values::kWristRange().middle();
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -45,6 +82,12 @@
arm_distal->zeroing.measured_absolute_position = 0.0;
arm_distal->potentiometer_offset = 0.0;
+
+ roll_joint_params->zeroing_constants.measured_absolute_position = 0.0;
+ roll_joint->potentiometer_offset = 0.0;
+
+ wrist->zeroing_constants.measured_absolute_position = 0.0;
+
break;
case kCompTeamNumber:
@@ -53,6 +96,12 @@
arm_distal->zeroing.measured_absolute_position = 0.0;
arm_distal->potentiometer_offset = 0.0;
+
+ roll_joint_params->zeroing_constants.measured_absolute_position = 0.0;
+ roll_joint->potentiometer_offset = 0.0;
+
+ wrist->zeroing_constants.measured_absolute_position = 0.0;
+
break;
case kPracticeTeamNumber:
@@ -61,6 +110,12 @@
arm_distal->zeroing.measured_absolute_position = 0.0;
arm_distal->potentiometer_offset = 0.0;
+
+ roll_joint_params->zeroing_constants.measured_absolute_position = 0.0;
+ roll_joint->potentiometer_offset = 0.0;
+
+ wrist->zeroing_constants.measured_absolute_position = 0.0;
+
break;
case kCodingRobotTeamNumber:
@@ -69,10 +124,18 @@
arm_distal->zeroing.measured_absolute_position = 0.0;
arm_distal->potentiometer_offset = 0.0;
+
+ roll_joint_params->zeroing_constants.measured_absolute_position = 0.0;
+ roll_joint->potentiometer_offset = 0.0;
+
+ wrist->zeroing_constants.measured_absolute_position = 0.0;
+
break;
default:
LOG(FATAL) << "unknown team: " << team;
+
+ // TODO(milind): add pot range checks once we add ranges
}
return r;
diff --git a/y2023/constants.h b/y2023/constants.h
index 64153af..f8e766c 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -10,6 +10,8 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2023/control_loops/superstructure/arm/arm_constants.h"
+#include "y2023/control_loops/superstructure/roll/roll_plant.h"
+#include "y2023/control_loops/superstructure/wrist/wrist_plant.h"
namespace y2023 {
namespace constants {
@@ -65,17 +67,78 @@
return (24.0 / 36.0) * (24.0 / 58.0) * (15.0 / 95.0);
}
+ static constexpr double kProximalPotRadiansPerVolt() {
+ return kProximalPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+
static constexpr double kDistalEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kDistalEncoderRatio() { return (15.0 / 95.0); }
static constexpr double kMaxDistalEncoderPulsesPerSecond() {
return control_loops::superstructure::arm::kArmConstants.free_speed /
(2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g2 /
- kDistalEncoderRatio() * kProximalEncoderCountsPerRevolution();
+ kDistalEncoderRatio() * kDistalEncoderCountsPerRevolution();
}
static constexpr double kDistalPotRatio() {
return (24.0 / 36.0) * (18.0 / 66.0) * (15.0 / 95.0);
}
+ static constexpr double kDistalPotRadiansPerVolt() {
+ return kDistalPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+
+ // Roll joint
+ static constexpr double kRollJointEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ static constexpr double kRollJointEncoderRatio() { return (18.0 / 48.0); }
+
+ static constexpr double kRollJointPotRatio() { return (18.0 / 48.0); }
+
+ static constexpr double kRollJointPotRadiansPerVolt() {
+ return kRollJointPotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+
+ static constexpr double kMaxRollJointEncoderPulsesPerSecond() {
+ return control_loops::superstructure::roll::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::roll::kOutputRatio /
+ kRollJointEncoderRatio() * kRollJointEncoderCountsPerRevolution();
+ }
+
+ static constexpr ::frc971::constants::Range kRollJointRange() {
+ return ::frc971::constants::Range{
+ -1.05, // Back Hard
+ 1.44, // Front Hard
+ -0.89, // Back Soft
+ 1.26 // Front Soft
+ };
+ }
+
+ // Wrist
+ static constexpr double kWristEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kWristEncoderRatio() {
+ return (24.0 / 36.0) * (36.0 / 60.0);
+ }
+
+ static constexpr double kMaxWristEncoderPulsesPerSecond() {
+ return control_loops::superstructure::wrist::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::wrist::kOutputRatio /
+ kWristEncoderRatio() * kWristEncoderCountsPerRevolution();
+ }
+
+ static constexpr ::frc971::constants::Range kWristRange() {
+ return ::frc971::constants::Range{
+ -1.05, // Back Hard
+ 1.44, // Front Hard
+ -0.89, // Back Soft
+ 1.26 // Front Soft
+ };
+ }
+
struct PotConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::RelativeEncoderZeroingEstimator>
@@ -97,6 +160,12 @@
ArmJointConstants arm_proximal;
ArmJointConstants arm_distal;
+
+ PotAndAbsEncoderConstants roll_joint;
+
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ wrist;
};
// Creates and returns a Values instance for the constants.