Merge changes I305d235d,I87e99a14
* changes:
Move target mapping to 2023
Solve mapping problem in 3d
diff --git a/frc971/rockpi/build_rootfs.sh b/frc971/rockpi/build_rootfs.sh
index f7abbd5..b6c8338 100755
--- a/frc971/rockpi/build_rootfs.sh
+++ b/frc971/rockpi/build_rootfs.sh
@@ -176,6 +176,7 @@
copyfile root.root 500 root/bin/grow.sh
copyfile root.root 500 root/bin/change_hostname.sh
copyfile root.root 500 root/bin/deploy_kernel.sh
+copyfile root.root 500 root/bin/chrt.sh
copyfile root.root 644 etc/systemd/system/grow-rootfs.service
copyfile root.root 644 etc/sysctl.d/sctp.conf
copyfile root.root 644 etc/systemd/logind.conf
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index 9b4863e..053bfa1 100644
--- a/frc971/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -290,18 +290,21 @@
memset(&format, 0, sizeof(format));
format.type = multiplanar() ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE
: V4L2_BUF_TYPE_VIDEO_CAPTURE;
- format.fmt.pix.width = cols_;
- format.fmt.pix.height = rows_;
+
+ constexpr int kWidth = 640;
+ constexpr int kHeight = 480;
+ format.fmt.pix.width = kWidth;
+ format.fmt.pix.height = kHeight;
format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
// This means we want to capture from a progressive (non-interlaced)
// source.
format.fmt.pix.field = V4L2_FIELD_NONE;
PCHECK(Ioctl(VIDIOC_S_FMT, &format) == 0);
- CHECK_EQ(static_cast<int>(format.fmt.pix.width), cols_);
- CHECK_EQ(static_cast<int>(format.fmt.pix.height), rows_);
+ CHECK_EQ(static_cast<int>(format.fmt.pix.width), kWidth);
+ CHECK_EQ(static_cast<int>(format.fmt.pix.height), kHeight);
CHECK_EQ(static_cast<int>(format.fmt.pix.bytesperline),
- cols_ * 2 /* bytes per pixel */);
- CHECK_EQ(format.fmt.pix.sizeimage, ImageSize());
+ kWidth * 2 /* bytes per pixel */);
+ CHECK_EQ(format.fmt.pix.sizeimage, ImageSize(kHeight, kWidth));
StreamOn();
}
@@ -320,6 +323,8 @@
epoll_->OnReadable(fd().get(), [this]() { OnImageReady(); });
}
+RockchipV4L2Reader::~RockchipV4L2Reader() { epoll_->DeleteFd(fd().get()); }
+
void RockchipV4L2Reader::OnImageReady() {
if (!ReadLatestImage()) {
return;
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
index 182160d..e36fef8 100644
--- a/frc971/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -63,7 +63,10 @@
// TODO(Brian): This concept won't exist once we start using variable-size
// H.264 frames.
- size_t ImageSize() const { return rows_ * cols_ * 2 /* bytes per pixel */; }
+ size_t ImageSize() const { return ImageSize(rows_, cols_); }
+ static size_t ImageSize(int rows, int cols) {
+ return rows * cols * 2 /* bytes per pixel */;
+ }
const aos::ScopedFD &fd() { return fd_; };
@@ -134,10 +137,6 @@
class V4L2Reader : public V4L2ReaderBase {
public:
V4L2Reader(aos::EventLoop *event_loop, const std::string &device_name);
-
- private:
- const int rows_ = 480;
- const int cols_ = 640;
};
// Rockpi specific v4l2 reader. This assumes that the media device has been
@@ -148,6 +147,8 @@
const std::string &device_name,
const std::string &image_sensor_subdev);
+ ~RockchipV4L2Reader();
+
void SetExposure(size_t duration) override;
void SetGain(size_t gain);
diff --git a/y2022/BUILD b/y2022/BUILD
index aba165e..3d4dac3 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -217,6 +217,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/wpilib:wpilib_utils",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
"//y2022/control_loops/superstructure/catapult:catapult_plants",
"//y2022/control_loops/superstructure/climber:climber_plants",
diff --git a/y2022/constants.cc b/y2022/constants.cc
index e9aaa59..21ba9e4 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -10,6 +10,7 @@
#include "absl/base/call_once.h"
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
+#include "frc971/wpilib/wpilib_utils.h"
#include "glog/logging.h"
#include "y2022/control_loops/superstructure/catapult/integral_catapult_plant.h"
#include "y2022/control_loops/superstructure/climber/integral_climber_plant.h"
@@ -141,7 +142,7 @@
{3.60, {0.39, 20.65}},
{4.50, {0.44, 22.3}},
- {4.9, {0.43, 22.75}}, // up to here
+ {4.9, {0.43, 22.75}}, // up to here
{5.4, {0.43, 23.85}},
{6.0, {0.42, 25.3}},
@@ -190,21 +191,21 @@
{5, {4.0}},
});
- climber->potentiometer_offset = 0.0;
- intake_front->potentiometer_offset = 0.0;
+ climber->potentiometer_offset = -0.035;
+ intake_front->potentiometer_offset = 3.122;
intake_front->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.0;
- intake_back->potentiometer_offset = 0.0;
+ .measured_absolute_position = 0.175;
+ intake_back->potentiometer_offset = 3.365;
intake_back->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.0;
- turret->potentiometer_offset = 0.0;
+ .measured_absolute_position = 0.051;
+ turret->potentiometer_offset = -7.932;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.0;
- flipper_arm_left->potentiometer_offset = 0.0;
- flipper_arm_right->potentiometer_offset = 0.0;
+ 1.166;
+ flipper_arm_left->potentiometer_offset = -6.40;
+ flipper_arm_right->potentiometer_offset = 5.56;
catapult_params->zeroing_constants.measured_absolute_position = 0.0;
- catapult->potentiometer_offset = 0.0;
+ catapult->potentiometer_offset = -2.033;
ball_color->reference_red = {0, 0, 1, 1};
ball_color->reference_blue = {0, 0, 1, 1};
@@ -310,8 +311,10 @@
.upper = 2.9 // Front Soft
};
turret_params->range = *turret_range;
- flipper_arm_left->potentiometer_offset = -4.39536583413615 - 0.108401297910291;
- flipper_arm_right->potentiometer_offset = 4.36264091401229 + 0.175896445665755;
+ flipper_arm_left->potentiometer_offset =
+ -4.39536583413615 - 0.108401297910291;
+ flipper_arm_right->potentiometer_offset =
+ 4.36264091401229 + 0.175896445665755;
catapult_params->zeroing_constants.measured_absolute_position =
1.62909518684227;
@@ -361,6 +364,47 @@
LOG(FATAL) << "unknown team: " << team;
}
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kClimberRange(), climber->potentiometer_offset,
+ [](double meters) { return meters / Values::kClimberPotMetersPerVolt(); },
+ false))
+ << "Couldn't translate climber pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kFlipperArmRange(), flipper_arm_left->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kFlipperArmsPotRadiansPerVolt();
+ },
+ false))
+ << "Couldn't translate flipper left pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kFlipperArmRange(), flipper_arm_right->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kFlipperArmsPotRadiansPerVolt();
+ },
+ true))
+ << "Couldn't translate flipper right pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kIntakeRange(), intake_front->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kIntakePotRadiansPerVolt();
+ },
+ true))
+ << "Couldn't translate front intake pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ Values::kIntakeRange(), intake_back->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kIntakePotRadiansPerVolt();
+ },
+ true))
+ << "Couldn't translate back intake pot";
+ CHECK(frc971::wpilib::SafePotVoltageRange(
+ *turret_range, turret->potentiometer_offset,
+ [](double radians) {
+ return radians / Values::kTurretPotRadiansPerVolt();
+ },
+ false))
+ << "Couldn't translate turret pot";
+
return r;
}
diff --git a/y2022/constants.h b/y2022/constants.h
index 297f814..62b9b4a 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -58,6 +58,11 @@
}
static constexpr double kClimberPotRatio() { return 1.0; }
+ static constexpr double kClimberPotMetersPerVolt() {
+ return kClimberPotRatio() * (5.0 /*turns*/ / 5.0 /*volts*/) *
+ kClimberPotMetersPerRevolution();
+ }
+
struct PotConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::RelativeEncoderZeroingEstimator>
@@ -77,6 +82,11 @@
static constexpr double kIntakePotRatio() { return 16.0 / 64.0; }
+ static constexpr double kIntakePotRadiansPerVolt() {
+ return kIntakePotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+
static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
return control_loops::superstructure::intake::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::intake::kOutputRatio /
@@ -121,6 +131,10 @@
static constexpr double kTurretFrontIntakePos() { return 0; }
static constexpr double kTurretPotRatio() { return 27.0 / 110.0; }
+ static constexpr double kTurretPotRadiansPerVolt() {
+ return kTurretPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
static constexpr double kTurretEncoderRatio() { return kTurretPotRatio(); }
static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
@@ -175,6 +189,11 @@
static constexpr double kFlipperArmsPotRatio() { return 16.0 / 36.0; }
+ static constexpr double kFlipperArmsPotRadiansPerVolt() {
+ return kFlipperArmsPotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+
PotConstants flipper_arm_left;
PotConstants flipper_arm_right;
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 84164bb..8df3a3a 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -82,24 +82,19 @@
}
double climber_pot_translate(double voltage) {
- return voltage * Values::kClimberPotRatio() *
- (5.0 /*turns*/ / 5.0 /*volts*/) *
- Values::kClimberPotMetersPerRevolution();
+ return voltage * Values::kClimberPotMetersPerVolt();
}
double flipper_arms_pot_translate(double voltage) {
- return voltage * Values::kFlipperArmsPotRatio() *
- (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ return voltage * Values::kFlipperArmsPotRadiansPerVolt();
}
double intake_pot_translate(double voltage) {
- return voltage * Values::kIntakePotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
- (2 * M_PI /*radians*/);
+ return voltage * Values::kIntakePotRadiansPerVolt();
}
double turret_pot_translate(double voltage) {
- return voltage * Values::kTurretPotRatio() *
- (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ return voltage * Values::kTurretPotRadiansPerVolt();
}
constexpr double kMaxFastEncoderPulsesPerSecond =