Support both the new intake and old intake
The gear ratio is like 2% different, not worth stressing about. The mag
encoder moved though, so it has 2 different ratios.
Calibrate the bot since everything got replaced...
Change-Id: I79b3b01b7bf829bd12a77911f9484b9ccb896205
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/constants.cc b/y2023/constants.cc
index 6a263d3..d46d812 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -25,6 +25,7 @@
auto *const arm_distal = &r.arm_distal;
auto *const wrist = &r.wrist;
auto *const roll_joint = &r.roll_joint;
+ r.wrist_flipped = true;
arm_proximal->zeroing.average_filter_size = Values::kZeroingSampleSize;
arm_proximal->zeroing.one_revolution_distance =
@@ -51,18 +52,18 @@
wrist->subsystem_params.operating_voltage = 12.0;
wrist->subsystem_params.zeroing_profile_params = {0.5, 3.0};
wrist->subsystem_params.default_profile_params = {0.5, 5.0};
- wrist->subsystem_params.range = Values::kWristRange();
+ wrist->subsystem_params.range = Values::kCompWristRange();
wrist->subsystem_params.make_integral_loop =
control_loops::superstructure::wrist::MakeIntegralWristLoop;
wrist->subsystem_params.zeroing_constants.average_filter_size =
Values::kZeroingSampleSize;
wrist->subsystem_params.zeroing_constants.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kWristEncoderRatio();
+ M_PI * 2.0 * constants::Values::kCompWristEncoderRatio();
wrist->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
wrist->subsystem_params.zeroing_constants.moving_buffer_size = 20;
wrist->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
wrist->subsystem_params.zeroing_constants.middle_position =
- Values::kWristRange().middle();
+ Values::kCompWristRange().middle();
switch (team) {
// A set of constants for tests.
@@ -82,17 +83,17 @@
break;
case kCompTeamNumber:
- arm_proximal->zeroing.measured_absolute_position = 0.132182297391884;
+ arm_proximal->zeroing.measured_absolute_position = 0.138453705930275;
arm_proximal->potentiometer_offset =
0.931355973012855 + 8.6743197253382 - 0.101200335326309 -
- 0.0820901660993467 - 0.0703733798337964;
+ 0.0820901660993467 - 0.0703733798337964 - 0.0294645384848748;
- arm_distal->zeroing.measured_absolute_position = 0.597004611319487;
+ arm_distal->zeroing.measured_absolute_position = 0.562947209110251;
arm_distal->potentiometer_offset =
0.436664933370656 + 0.49457213779426 + 6.78213223139724 -
0.0220711555235029 - 0.0162945074111813 + 0.00630344935527365 -
0.0164398318919943 - 0.145833494945215 + 0.234878799868491 +
- 0.125924230298394;
+ 0.125924230298394 + 0.147136306208754;
roll_joint->zeroing.measured_absolute_position = 0.593975883699743;
roll_joint->potentiometer_offset =
@@ -104,7 +105,7 @@
0.0201047336425017 - 1.0173426655158 - 0.186085272847293 - 0.0317706563397807;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 1.54674994866259;
+ 5.78862525947414;
break;
@@ -126,6 +127,13 @@
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
2.51265911579648;
+ wrist->subsystem_params.zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kPracticeWristEncoderRatio();
+ wrist->subsystem_params.range = Values::kPracticeWristRange();
+ wrist->subsystem_params.zeroing_constants.middle_position =
+ Values::kPracticeWristRange().middle();
+ r.wrist_flipped = false;
+
break;
case kCodingRobotTeamNumber:
diff --git a/y2023/constants.h b/y2023/constants.h
index de665d9..8996597 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -121,17 +121,34 @@
// Wrist
static constexpr double kWristEncoderCountsPerRevolution() { return 4096.0; }
- static constexpr double kWristEncoderRatio() {
+ static constexpr double kCompWristEncoderRatio() {
+ return 1.0;
+ }
+ static constexpr double kPracticeWristEncoderRatio() {
return (24.0 / 36.0) * (36.0 / 60.0);
}
- static constexpr double kMaxWristEncoderPulsesPerSecond() {
+ static constexpr double kMaxCompWristEncoderPulsesPerSecond() {
return control_loops::superstructure::wrist::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::wrist::kOutputRatio /
- kWristEncoderRatio() * kWristEncoderCountsPerRevolution();
+ kCompWristEncoderRatio() * kWristEncoderCountsPerRevolution();
+ }
+ static constexpr double kMaxPracticeWristEncoderPulsesPerSecond() {
+ return control_loops::superstructure::wrist::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::wrist::kOutputRatio /
+ kPracticeWristEncoderRatio() * kWristEncoderCountsPerRevolution();
}
- static constexpr ::frc971::constants::Range kWristRange() {
+ static constexpr ::frc971::constants::Range kCompWristRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.10, // Back Hard
+ .upper_hard = 4.90, // Front Hard
+ .lower = 0.0, // Back Soft
+ .upper = 4.0, // Front Soft
+ };
+ }
+
+ static constexpr ::frc971::constants::Range kPracticeWristRange() {
return ::frc971::constants::Range{
.lower_hard = -0.10, // Back Hard
.upper_hard = 2.30, // Front Hard
@@ -210,6 +227,8 @@
ArmJointConstants roll_joint;
AbsEncoderConstants wrist;
+
+ bool wrist_flipped;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2023/control_loops/python/wrist.py b/y2023/control_loops/python/wrist.py
index 60ae8a6..fe2fcf6 100644
--- a/y2023/control_loops/python/wrist.py
+++ b/y2023/control_loops/python/wrist.py
@@ -20,7 +20,7 @@
kWrist = angular_system.AngularSystemParams(
name='Wrist',
motor=control_loop.BAG(),
- G=(6.0 / 48.0) * (20.0 / 100.0) * (24.0 / 36.0) * (36.0 / 60.0),
+ G=(6.0 / 48.0) * (20.0 / 100.0) * (18.0 / 24.0) * (24.0 / 44.0),
# Use parallel axis theorem to get the moment of inertia around
# the joint (I = I_cm + mh^2 = 0.001877 + 0.8332 * 0.0407162^2)
J=0.003258,
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 4eeb0f6..ee440ed 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -181,7 +181,7 @@
PositionSensorSimulator(
values->wrist.subsystem_params.zeroing_constants
.one_revolution_distance),
- values->wrist, constants::Values::kWristRange(),
+ values->wrist, constants::Values::kCompWristRange(),
values->wrist.subsystem_params.zeroing_constants
.measured_absolute_position,
dt_),
@@ -449,7 +449,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kWristRange().middle());
+ *builder.fbb(), constants::Values::kCompWristRange().middle());
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_arm_goal_position(arm::NeutralIndex());
@@ -474,7 +474,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kWristRange().upper);
+ *builder.fbb(), constants::Values::kCompWristRange().upper);
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -505,7 +505,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kWristRange().upper);
+ *builder.fbb(), constants::Values::kCompWristRange().upper);
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -524,7 +524,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kWristRange().lower,
+ *builder.fbb(), constants::Values::kCompWristRange().lower,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 921531e..033d3d7 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -104,7 +104,8 @@
Values::kMaxProximalEncoderPulsesPerSecond(),
Values::kMaxDistalEncoderPulsesPerSecond(),
Values::kMaxRollJointEncoderPulsesPerSecond(),
- Values::kMaxWristEncoderPulsesPerSecond(),
+ Values::kMaxCompWristEncoderPulsesPerSecond(),
+ Values::kMaxPracticeWristEncoderPulsesPerSecond(),
});
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
@@ -461,7 +462,10 @@
frc971::AbsolutePositionT wrist;
CopyPosition(wrist_encoder_, &wrist,
Values::kWristEncoderCountsPerRevolution(),
- Values::kWristEncoderRatio(), false);
+ values_->wrist.subsystem_params.zeroing_constants
+ .one_revolution_distance /
+ (M_PI * 2.0),
+ values_->wrist_flipped);
flatbuffers::Offset<frc971::PotAndAbsolutePosition> proximal_offset =
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &proximal);