Merge "Support indexing of MCAP files"
diff --git a/y2022/constants.cc b/y2022/constants.cc
index cb85e94..a6b9a28 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -137,16 +137,42 @@
{1.9, {0.1, 19.0}},
{2.12, {0.15, 18.8}},
{2.9, {0.25, 19.2}},
+ {3.2, {0.28, 20.5}},
- {3.8, {0.35, 20.6}},
- {4.9, {0.4, 21.9}},
- {6.0, {0.40, 24.0}},
- {7.0, {0.40, 25.5}},
+ {3.86, {0.35, 20.9}},
+ {4.9, {0.4, 21.9}},
+ {5.4, {0.4, 23.9}},
+ {6.0, {0.40, 25.0}},
+ {7.0, {0.37, 27.1}},
- {7.8, {0.35, 26.9}},
- {10.0, {0.35, 26.9}},
+ {7.8, {0.35, 28.0}},
+ {10.0, {0.35, 28.0}},
});
+ if (false) {
+ // 1.5 meters -> 2.7
+ // 2.3 meters -> 4.7
+ // 4.5 meters -> 7.0
+ // 7.0 meters -> 9.0
+
+ constexpr double kShotVelocity = 9.0;
+ r.shot_velocity_interpolation_table =
+ InterpolationTable<Values::ShotVelocityParams>({
+ {1.0, {kShotVelocity}},
+ {10.0, {kShotVelocity}},
+ });
+ } else {
+ r.shot_velocity_interpolation_table =
+ InterpolationTable<Values::ShotVelocityParams>({
+ {1.0, {2.7}},
+ {1.5, {2.7}},
+ {2.3, {4.7}},
+ {4.5, {7.0}},
+ {7.0, {9.0}},
+ {10.0, {9.0}},
+ });
+ }
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -155,6 +181,12 @@
{5, {0.6, 10.0}},
});
+ r.shot_velocity_interpolation_table =
+ InterpolationTable<Values::ShotVelocityParams>({
+ {2, {2.0}},
+ {5, {4.0}},
+ });
+
climber->potentiometer_offset = 0.0;
intake_front->potentiometer_offset = 0.0;
intake_front->subsystem_params.zeroing_constants
@@ -173,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;
@@ -188,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;
@@ -224,6 +258,12 @@
{5, {0.6, 10.0}},
});
+ r.shot_velocity_interpolation_table =
+ InterpolationTable<Values::ShotVelocityParams>({
+ {2, {2.0}},
+ {5, {4.0}},
+ });
+
climber->potentiometer_offset = 0.0;
intake_front->potentiometer_offset = 0.0;
intake_front->subsystem_params.zeroing_constants
diff --git a/y2022/constants.h b/y2022/constants.h
index 610ddc0..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
};
}
@@ -218,13 +218,28 @@
static ShotParams BlendY(double coefficient, ShotParams a1, ShotParams a2) {
using ::frc971::shooter_interpolation::Blend;
return ShotParams{
- Blend(coefficient, a1.shot_angle, a2.shot_angle),
- Blend(coefficient, a1.shot_velocity, a2.shot_velocity),
+ .shot_angle = Blend(coefficient, a1.shot_angle, a2.shot_angle),
+ .shot_velocity =
+ Blend(coefficient, a1.shot_velocity, a2.shot_velocity),
};
}
};
+ struct ShotVelocityParams {
+ // Speed over ground to use for shooting-on-the-fly.
+ double shot_speed_over_ground;
+
+ static ShotVelocityParams BlendY(double coefficient, ShotVelocityParams a1,
+ ShotVelocityParams a2) {
+ using ::frc971::shooter_interpolation::Blend;
+ return ShotVelocityParams{Blend(coefficient, a1.shot_speed_over_ground,
+ a2.shot_speed_over_ground)};
+ }
+ };
+
InterpolationTable<ShotParams> shot_interpolation_table;
+
+ InterpolationTable<ShotVelocityParams> shot_velocity_interpolation_table;
};
// Creates and returns a Values instance for the constants.
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 b21dd0d..15f9c2a 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -5,6 +5,9 @@
#include "frc971/zeroing/wrap.h"
#include "y2022/control_loops/superstructure/collision_avoidance.h"
+DEFINE_bool(ignore_distance, false,
+ "If true, ignore distance when shooting and obay joystick_reader");
+
namespace y2022 {
namespace control_loops {
namespace superstructure {
@@ -28,7 +31,8 @@
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
can_position_fetcher_(
- event_loop->MakeFetcher<CANPosition>("/superstructure")) {
+ event_loop->MakeFetcher<CANPosition>("/superstructure")),
+ aimer_(values) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -73,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 =
@@ -85,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();
@@ -92,8 +99,9 @@
constants::Values::ShotParams shot_params;
const double distance_to_goal = aimer_.DistanceToGoal();
- if (unsafe_goal->auto_aim() && values_->shot_interpolation_table.GetInRange(
- distance_to_goal, &shot_params)) {
+ if (!FLAGS_ignore_distance && unsafe_goal->auto_aim() &&
+ values_->shot_interpolation_table.GetInRange(distance_to_goal,
+ &shot_params)) {
flatbuffers::FlatBufferBuilder *catapult_goal_fbb =
catapult_goal_buffer.fbb();
std::optional<flatbuffers::Offset<
@@ -243,7 +251,7 @@
.shooting = true});
// Dont shoot if the robot is moving faster than this
- constexpr double kMaxShootSpeed = 1.7;
+ constexpr double kMaxShootSpeed = 2.7;
const bool moving_too_fast = std::abs(robot_velocity()) > kMaxShootSpeed;
switch (state_) {
@@ -493,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/control_loops/superstructure/turret/aiming.cc b/y2022/control_loops/superstructure/turret/aiming.cc
index 643adbc..5fe8a2c 100644
--- a/y2022/control_loops/superstructure/turret/aiming.cc
+++ b/y2022/control_loops/superstructure/turret/aiming.cc
@@ -1,6 +1,5 @@
#include "y2022/control_loops/superstructure/turret/aiming.h"
-#include "y2022/constants.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
namespace y2022 {
@@ -13,10 +12,6 @@
using frc971::control_loops::aiming::RobotState;
namespace {
-// Average speed-over-ground of the ball on its way to the target. Our current
-// model assumes constant ball velocity regardless of shot distance.
-constexpr double kBallSpeedOverGround = 2.0; // m/s
-
// If the turret is at zero, then it will be at this angle at which the shot
// will leave the robot. I.e., if the turret is at zero, then the shot will go
// straight out the back of the robot.
@@ -43,7 +38,8 @@
}
} // namespace
-Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+Aimer::Aimer(std::shared_ptr<const constants::Values> constants)
+ : constants_(constants), goal_(MakePrefilledGoal()) {}
void Aimer::Update(const Status *status, ShotMode shot_mode) {
const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
@@ -56,15 +52,18 @@
const double xdot = linear_angular(0) * std::cos(status->theta());
const double ydot = linear_angular(0) * std::sin(status->theta());
- current_goal_ =
- frc971::control_loops::aiming::AimerGoal(
- ShotConfig{goal, shot_mode, constants::Values::kTurretRange(),
- kBallSpeedOverGround,
- /*wrap_mode=*/0.0, kTurretZeroOffset},
- RobotState{robot_pose,
- {xdot, ydot},
- linear_angular(1),
- goal_.message().unsafe_goal()});
+ // Use the previous shot distance to estimate the speed-over-ground of the
+ // ball.
+ current_goal_ = frc971::control_loops::aiming::AimerGoal(
+ ShotConfig{goal, shot_mode, constants_->kTurretRange(),
+ constants_->shot_velocity_interpolation_table
+ .Get(current_goal_.target_distance)
+ .shot_speed_over_ground,
+ /*wrap_mode=*/0.0, kTurretZeroOffset},
+ RobotState{robot_pose,
+ {xdot, ydot},
+ linear_angular(1),
+ goal_.message().unsafe_goal()});
goal_.mutable_message()->mutate_unsafe_goal(current_goal_.position);
goal_.mutable_message()->mutate_goal_velocity(
diff --git a/y2022/control_loops/superstructure/turret/aiming.h b/y2022/control_loops/superstructure/turret/aiming.h
index 9494103..4eabe3e 100644
--- a/y2022/control_loops/superstructure/turret/aiming.h
+++ b/y2022/control_loops/superstructure/turret/aiming.h
@@ -2,10 +2,11 @@
#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
#include "aos/flatbuffers.h"
+#include "frc971/control_loops/aiming/aiming.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
-#include "frc971/control_loops/aiming/aiming.h"
+#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
namespace y2022::control_loops::superstructure::turret {
@@ -19,7 +20,7 @@
typedef frc971::control_loops::drivetrain::Status Status;
typedef frc971::control_loops::aiming::ShotMode ShotMode;
- Aimer();
+ Aimer(std::shared_ptr<const constants::Values> constants);
void Update(const Status *status, ShotMode shot_mode);
@@ -32,6 +33,7 @@
flatbuffers::FlatBufferBuilder *fbb) const;
private:
+ std::shared_ptr<const constants::Values> constants_;
aos::FlatbufferDetachedBuffer<Goal> goal_;
frc971::control_loops::aiming::TurretGoal current_goal_;
};
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index baaa2ee..05f3008 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)) {
@@ -221,9 +227,9 @@
catapult_return_pos = -0.908;
}
- constexpr double kRollerSpeed = 8.0;
+ constexpr double kRollerSpeed = 12.0;
constexpr double kTransferRollerSpeed = 12.0;
- constexpr double kIntakePosition = -0.02;
+ constexpr double kIntakePosition = -0.12;
constexpr size_t kIntakeCounterIterations = 25;
// Extend the intakes and spin the rollers.
@@ -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/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 3f8e13d..783d368 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -642,7 +642,7 @@
H_model(1, kY) = 1.0;
H_accel(0, kX) = 1.0;
H_accel(1, kY) = 1.0;
- R.diagonal() << 1e-0, 1e-0;
+ R.diagonal() << 1e-2, 1e-2;
const Eigen::Matrix<double, kNModelStates, 2> K_model =
P_model_ * H_model.transpose() *
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index 60af1f8..93c72d7 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -133,7 +133,7 @@
}
// Threshold for mean distance from a blob centroid to a circle.
- constexpr double kCircleDistanceThreshold = 10.0;
+ constexpr double kCircleDistanceThreshold = 1.0;
// We should only expect to see blobs between these angles on a circle.
constexpr double kDegToRad = M_PI / 180.0;
constexpr double kMinBlobAngle = 50.0 * kDegToRad;
diff --git a/y2022/vision/camera_definition.py b/y2022/vision/camera_definition.py
index 61789cb..f59f2cf 100644
--- a/y2022/vision/camera_definition.py
+++ b/y2022/vision/camera_definition.py
@@ -105,7 +105,7 @@
camera_yaw = 0.0
T = np.array([-7.5 * 0.0254, -3.5 * 0.0254, 34.0 * 0.0254])
elif pi_number == "pi3":
- camera_yaw = 179.0 * np.pi / 180.0
+ camera_yaw = 178.5 * np.pi / 180.0
T = np.array([-1.0 * 0.0254, 8.5 * 0.0254, 34.25 * 0.0254])
elif pi_number == "pi4":
camera_yaw = -90.0 * np.pi / 180.0
diff --git a/y2022/vision/camera_reader_main.cc b/y2022/vision/camera_reader_main.cc
index 22794d3..9320152 100644
--- a/y2022/vision/camera_reader_main.cc
+++ b/y2022/vision/camera_reader_main.cc
@@ -10,7 +10,7 @@
DEFINE_double(duty_cycle, 0.6, "Duty cycle of the LEDs");
DEFINE_uint32(exposure, 5,
"Exposure time, in 100us increments; 0 implies auto exposure");
-DEFINE_uint32(outdoors_exposure, 2,
+DEFINE_uint32(outdoors_exposure, 20,
"Exposure time when using --use_outdoors, in 100us increments; 0 "
"implies auto exposure");
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);