Add climber servo code
Used for traversing to high bar.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I333c581c33aab3f2d960db8452b5b1783aab9a5a
diff --git a/y2022/control_loops/python/climber.py b/y2022/control_loops/python/climber.py
index fd1707b..5f5ae38 100755
--- a/y2022/control_loops/python/climber.py
+++ b/y2022/control_loops/python/climber.py
@@ -17,7 +17,7 @@
kClimber = linear_system.LinearSystemParams(
name='Climber',
motor=control_loop.Falcon(),
- G=(1.0 / 10.0) * (1.0 / 3.0) * (1.0 / 3.0),
+ G=(1.0 / 4.0) * (1.0 / 3.0) * (1.0 / 3.0),
radius=22 * 0.25 / numpy.pi / 2.0 * 0.0254,
mass=2.0,
q_pos=0.10,
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index d23e589..15f9c2a 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -77,6 +77,7 @@
double transfer_roller_speed = 0.0;
double flipper_arms_voltage = 0.0;
bool have_active_intake_request = false;
+ bool climber_servo = false;
if (unsafe_goal != nullptr) {
roller_speed_compensated_front =
@@ -89,6 +90,8 @@
transfer_roller_speed = unsafe_goal->transfer_roller_speed();
+ climber_servo = unsafe_goal->climber_servo();
+
turret_goal =
unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
@@ -498,6 +501,13 @@
output_struct.roller_voltage_back = roller_speed_compensated_back;
output_struct.transfer_roller_voltage = transfer_roller_speed;
output_struct.flipper_arms_voltage = flipper_arms_voltage;
+ if (climber_servo) {
+ output_struct.climber_servo_left = 0.0;
+ output_struct.climber_servo_right = 1.0;
+ } else {
+ output_struct.climber_servo_left = 1.0;
+ output_struct.climber_servo_right = 0.0;
+ }
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 0bb51e1..4d4a81f 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -26,6 +26,10 @@
// Height of the climber above rest point
climber:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 0);
+ // True if the servo should be released.
+ // Positive is moving it out
+ climber_servo:bool (id: 15);
+
// Goal angles of intake joints.
// Positive is out, 0 is up.
intake_front:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 1);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 02fef22..7123a47 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -981,7 +981,7 @@
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_BACK_BALL);
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
- constants::Values::kTurretBackIntakePos(), 0.001);
+ -constants::Values::kTurretBackIntakePos(), 0.001);
// Since the intake beambreak hasn't triggered in a while, it should realize
// the ball was lost.
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index a4460e2..4ba9034 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -5,6 +5,10 @@
// - is down + is up
climber_voltage:double (id: 0);
+ // Position of the climber servo from 0 to 1
+ climber_servo_left:double (id: 10);
+ climber_servo_right:double (id: 11);
+
// Voltage of the flipper arms falcons
// - is feed + is open
flipper_arms_voltage:double (id: 1);