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/constants.cc b/y2022/constants.cc
index e7c0ca3..a6b9a28 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -205,8 +205,9 @@
break;
case kCompTeamNumber:
- climber->potentiometer_offset =
- -0.0463847608752 - 0.0376876182111 + 0.0629263851579;
+ climber->potentiometer_offset = -0.0463847608752 - 0.0376876182111 +
+ 0.0629263851579 - 0.00682128836400001 +
+ 0.0172237531191;
intake_front->potentiometer_offset =
2.79628370453323 - 0.0250288114832881 + 0.577152542437606;
@@ -220,9 +221,10 @@
turret->potentiometer_offset = -9.99970387166721 + 0.06415943 +
0.073290115367682 - 0.0634440443622909 +
- 0.213601224728352 + 0.0657973101027296;
+ 0.213601224728352 + 0.0657973101027296 -
+ 0.114726411377978;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.27787064956636;
+ 0.39190961531060;
flipper_arm_left->potentiometer_offset = -6.4;
flipper_arm_right->potentiometer_offset = 5.56;
diff --git a/y2022/constants.h b/y2022/constants.h
index 31f8a0c..4b5351e 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -45,7 +45,7 @@
// Climber
static constexpr ::frc971::constants::Range kClimberRange() {
return ::frc971::constants::Range{
- .lower_hard = -0.01, .upper_hard = 0.55, .lower = 0.005, .upper = 0.48};
+ .lower_hard = -0.01, .upper_hard = 0.59, .lower = 0.003, .upper = 0.555};
}
static constexpr double kClimberPotMetersPerRevolution() {
return 22 * 0.25 * 0.0254;
@@ -114,9 +114,9 @@
static constexpr ::frc971::constants::Range kTurretRange() {
return ::frc971::constants::Range{
.lower_hard = -7.0, // Back Hard
- .upper_hard = 3.0, // Front Hard
+ .upper_hard = 3.4, // Front Hard
.lower = -6.5, // Back Soft
- .upper = 2.5 // Front Soft
+ .upper = 3.15 // Front Soft
};
}
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);
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index baaa2ee..7551564 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -58,8 +58,9 @@
const ButtonLocation kTurret(4, 15);
const ButtonLocation kAutoAim(4, 16);
-const ButtonLocation kClimberExtend(4, 6);
+const ButtonLocation kClimberExtend(1, 2);
const ButtonLocation kClimberIntakes(4, 5);
+const ButtonLocation kClimberServo(4, 4);
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
@@ -177,6 +178,7 @@
std::optional<double> turret_pos = std::nullopt;
double climber_position = 0.01;
+ bool climber_servo = false;
double catapult_pos = 0.03;
double catapult_speed = 18.0;
@@ -195,9 +197,13 @@
}
if (data.IsPressed(kClimberExtend)) {
- climber_position = 0.50;
+ climber_position = 0.54;
} else {
- climber_position = 0.01;
+ climber_position = 0.005;
+ }
+
+ if (data.IsPressed(kClimberServo)) {
+ climber_servo = true;
}
if (data.IsPressed(kTurret)) {
@@ -304,7 +310,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), climber_position,
- frc971::CreateProfileParameters(*builder.fbb(), 1.0, 5.0));
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 1.0));
superstructure::CatapultGoal::Builder catapult_builder =
builder.MakeBuilder<superstructure::CatapultGoal>();
@@ -330,6 +336,7 @@
superstructure_goal_builder.add_roller_speed_back(roller_back_speed);
superstructure_goal_builder.add_transfer_roller_speed(
transfer_roller_speed);
+ superstructure_goal_builder.add_climber_servo(climber_servo);
superstructure_goal_builder.add_auto_aim(data.IsPressed(kAutoAim));
if (requested_intake.has_value()) {
superstructure_goal_builder.add_turret_intake(requested_intake.value());
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 56eba95..84164bb 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -17,6 +17,7 @@
#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
#include "frc971/wpilib/ahal/DriverStation.h"
#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/Servo.h"
#include "frc971/wpilib/ahal/TalonFX.h"
#include "frc971/wpilib/ahal/VictorSP.h"
#undef ERROR
@@ -447,6 +448,13 @@
event_loop, "/superstructure"),
catapult_reversal_(make_unique<frc::DigitalOutput>(0)) {}
+ void set_climber_servo_left(::std::unique_ptr<::frc::Servo> t) {
+ climber_servo_left_ = ::std::move(t);
+ }
+ void set_climber_servo_right(::std::unique_ptr<::frc::Servo> t) {
+ climber_servo_right_ = ::std::move(t);
+ }
+
void set_climber_falcon(std::unique_ptr<frc::TalonFX> t) {
climber_falcon_ = std::move(t);
}
@@ -545,6 +553,9 @@
void Stop() override {
AOS_LOG(WARNING, "Superstructure output too old.\n");
climber_falcon_->SetDisabled();
+ climber_servo_left_->SetRaw(0);
+ climber_servo_right_->SetRaw(0);
+
roller_falcon_front_->Set(
ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
roller_falcon_back_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled,
@@ -568,6 +579,8 @@
void Write(const superstructure::Output &output) override {
WritePwm(-output.climber_voltage(), climber_falcon_.get());
+ climber_servo_left_->SetPosition(output.climber_servo_left());
+ climber_servo_right_->SetPosition(output.climber_servo_right());
WritePwm(output.intake_voltage_front(), intake_falcon_front_.get());
WritePwm(output.intake_voltage_back(), intake_falcon_back_.get());
@@ -623,6 +636,8 @@
::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_;
std::unique_ptr<frc::DigitalOutput> catapult_reversal_;
+
+ ::std::unique_ptr<::frc::Servo> climber_servo_left_, climber_servo_right_;
};
class CANSensorReader {
@@ -767,6 +782,8 @@
superstructure_writer.set_intake_falcon_front(make_unique<frc::TalonFX>(2));
superstructure_writer.set_intake_falcon_back(make_unique<frc::TalonFX>(4));
superstructure_writer.set_climber_falcon(make_unique<frc::TalonFX>(8));
+ superstructure_writer.set_climber_servo_left(make_unique<frc::Servo>(7));
+ superstructure_writer.set_climber_servo_right(make_unique<frc::Servo>(6));
superstructure_writer.set_flipper_arms_falcon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(2));
superstructure_writer.set_superstructure_reading(superstructure_reading);