Tune the wrist for y2023 comp
Works better when the sensor is plugged in too...
Change-Id: I14b97ed0137a5b112ce82178254f0a224433822a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/constants.cc b/y2023/constants.cc
index dbe8025..71e8682 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -50,7 +50,7 @@
wrist->subsystem_params.zeroing_voltage = 3.0;
wrist->subsystem_params.operating_voltage = 12.0;
wrist->subsystem_params.zeroing_profile_params = {0.5, 3.0};
- wrist->subsystem_params.default_profile_params = {6.0, 30.0};
+ wrist->subsystem_params.default_profile_params = {0.5, 5.0};
wrist->subsystem_params.range = Values::kWristRange();
wrist->subsystem_params.make_integral_loop =
control_loops::superstructure::wrist::MakeIntegralWristLoop;
@@ -101,7 +101,7 @@
0.5935210745062;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.0;
+ 0.183283543884167;
break;
diff --git a/y2023/constants.h b/y2023/constants.h
index 816de02..d2caca6 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -132,10 +132,10 @@
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
+ .lower_hard = -0.10, // Back Hard
+ .upper_hard = 2.30, // Front Hard
+ .lower = 0.0, // Back Soft
+ .upper = 2.0, // Front Soft
};
}
diff --git a/y2023/control_loops/python/wrist.py b/y2023/control_loops/python/wrist.py
index 1970c5b..60ae8a6 100644
--- a/y2023/control_loops/python/wrist.py
+++ b/y2023/control_loops/python/wrist.py
@@ -24,11 +24,11 @@
# 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,
- q_pos=0.40,
- q_vel=20.0,
+ q_pos=0.80,
+ q_vel=80.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
- kalman_q_voltage=4.0,
+ kalman_q_voltage=0.5,
kalman_r_position=0.05,
radius=5.71 * 0.0254)
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index adeda2b..4e65dc4 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -37,10 +37,12 @@
namespace joysticks {
// TODO(milind): add correct locations
-const ButtonLocation kIntake(3, 3);
-const ButtonLocation kScore(3, 3);
+const ButtonLocation kIntake(4, 5);
+const ButtonLocation kScore(4, 4);
const ButtonLocation kSpit(3, 3);
+const ButtonLocation kWrist(4, 10);
+
namespace superstructure = y2023::control_loops::superstructure;
namespace arm = superstructure::arm;
@@ -78,15 +80,32 @@
arm_goal_position_ = arm::ScorePosIndex();
} else if (data.IsPressed(kScore)) {
arm_goal_position_ = arm::ScorePosIndex();
+ } else {
+ arm_goal_position_ = arm::NeutralPosIndex();
+ }
+
+ double wrist_goal = 0.1;
+
+ if (data.IsPressed(kWrist)) {
+ wrist_goal = 1.5;
+ } else {
+ wrist_goal = 0.1;
}
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset =
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), wrist_goal,
+ CreateProfileParameters(*builder.fbb(), 12.0, 90.0));
+
superstructure::Goal::Builder superstructure_goal_builder =
builder.MakeBuilder<superstructure::Goal>();
superstructure_goal_builder.add_arm_goal_position(arm_goal_position_);
superstructure_goal_builder.add_roller_goal(roller_goal);
+ superstructure_goal_builder.add_wrist(wrist_offset);
if (builder.Send(superstructure_goal_builder.Finish()) !=
aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");