Merge changes Ie821842f,Ie8a999cc,I4a07dc92
* changes:
Split UpdateY from Update in StateFeedbackLoop
Decouple profiled subsystem axis from inputs and outputs
Add -m flag to scale signals when plotting
diff --git a/NO_BUILD_ROBORIO b/NO_BUILD_ROBORIO
index 58dd528..e7d0de1 100644
--- a/NO_BUILD_ROBORIO
+++ b/NO_BUILD_ROBORIO
@@ -9,6 +9,7 @@
-//y2015_bot3/control_loops/python/...
-//y2016/control_loops/python/...
-//y2016/vision/...
+-//y2017/vision/...
-//y2016_bot3/control_loops/python/...
-//y2016_bot4/control_loops/python/...
-//third_party/protobuf/...
diff --git a/aos/common/BUILD b/aos/common/BUILD
index 658a2e6..f4b57d5 100644
--- a/aos/common/BUILD
+++ b/aos/common/BUILD
@@ -393,3 +393,21 @@
'//aos/common/util:death_test_log_implementation',
],
)
+
+cc_library(
+ name = 'ring_buffer',
+ hdrs = [
+ 'ring_buffer.h',
+ ],
+)
+
+cc_test(
+ name = 'ring_buffer_test',
+ srcs = [
+ 'ring_buffer_test.cc',
+ ],
+ deps = [
+ ':ring_buffer',
+ '//aos/testing:googletest',
+ ],
+)
diff --git a/aos/common/ring_buffer.h b/aos/common/ring_buffer.h
new file mode 100644
index 0000000..293c817
--- /dev/null
+++ b/aos/common/ring_buffer.h
@@ -0,0 +1,59 @@
+#ifndef AOS_COMMON_RING_BUFFER_H_
+#define AOS_COMMON_RING_BUFFER_H_
+
+#include <array>
+
+namespace aos {
+
+// This is a helper to keep track of some amount of recent data. As you push
+// data into the ring buffer, it gets stored. If the buffer becomes full, it
+// will start overwriting the oldest data.
+template <typename Data, size_t buffer_size>
+class RingBuffer {
+ public:
+ RingBuffer() {}
+
+ // Add an item to the RingBuffer, overwriting the oldest element if necessary
+ void Push(const Data &data) {
+ if (full()) {
+ data_[oldest_] = data;
+ oldest_ = (oldest_ + 1) % buffer_size;
+ } else {
+ data_[(oldest_ + size_) % buffer_size] = data;
+ ++size_;
+ }
+ }
+
+ // Return the value of the index requested, adjusted so that the RingBuffer
+ // contians the oldest element first and the newest last.
+ Data &operator[](size_t index) {
+ return data_[(oldest_ + index) % buffer_size];
+ }
+
+ const Data &operator[](size_t index) const {
+ return data_[(oldest_ + index) % buffer_size];
+ }
+
+ // Returns the capacity of the RingBuffer
+ size_t capacity() const { return buffer_size; }
+
+ // Returns the number of elements stored in the RingBuffer
+ size_t size() const { return size_; }
+
+ // Is the RingBuffer empty or full?
+ bool empty() const { return size_ == 0; }
+
+ bool full() const { return size_ == buffer_size; }
+
+ private:
+ ::std::array<Data, buffer_size> data_;
+
+ // Oldest contains the oldest item added to the RingBuffer which will be the
+ // next one to be overwritten
+ size_t oldest_ = 0;
+ size_t size_ = 0;
+};
+
+} // namespace aos
+
+#endif // AOS_COMMON_RING_BUFFER_H_
diff --git a/aos/common/ring_buffer_test.cc b/aos/common/ring_buffer_test.cc
new file mode 100644
index 0000000..ca3f35d
--- /dev/null
+++ b/aos/common/ring_buffer_test.cc
@@ -0,0 +1,71 @@
+#include "aos/common/ring_buffer.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+class RingBufferTest : public ::testing::Test {
+ public:
+ RingBufferTest() {}
+
+ protected:
+ RingBuffer<int, 10> buffer_;
+};
+
+// Test if the RingBuffer is empty when initialized properly
+TEST_F(RingBufferTest, DefaultIsEmpty) {
+ // The RingBuffer should have a size of 0, a capacity of 10 (note that it was
+ // initialized as 10), have no items, and not be full
+ ASSERT_EQ(0u, buffer_.size());
+ ASSERT_EQ(10u, buffer_.capacity());
+ ASSERT_TRUE(buffer_.empty());
+ ASSERT_FALSE(buffer_.full());
+}
+
+// Test that the RingBuffer can fill it's entire capacity and read back the data
+TEST_F(RingBufferTest, CanAddData) {
+ ASSERT_TRUE(buffer_.empty());
+
+ // Add sequential numbers to the RingBuffer
+ // (the value of each item is it's index #)
+ for (size_t i = 0; i < buffer_.capacity(); ++i) {
+ // The buffer shouldn't be full yet, and it's size should be how many items
+ // we've added so far. Once that happens, we add another item
+ ASSERT_FALSE(buffer_.full());
+ ASSERT_EQ(i, buffer_.size());
+ buffer_.Push(i);
+
+ // The buffer shouldn't be empty and it's size should be 1 more since we
+ // just
+ // added an item. Also, the last item in the buffer should equal the one we
+ // just added
+ ASSERT_FALSE(buffer_.empty());
+ ASSERT_EQ(i + 1, buffer_.size());
+ ASSERT_EQ(i, buffer_[i]);
+ }
+
+ ASSERT_TRUE(buffer_.full());
+}
+
+// Tests that the RingBuffer properly loops back and starts overwriting from the
+// first element after being filled up
+TEST_F(RingBufferTest, OverfillData) {
+ // Add numbers 0-24 to the RingBuffer
+ for (int i = 0; i < 25; ++i) {
+ buffer_.Push(i);
+ }
+
+ // It should now be full
+ ASSERT_TRUE(buffer_.full());
+
+ // Since the buffer is a size of 10 and has been filled up 2.5 times, it
+ // should
+ // now contain the numbers 15-24
+ for (size_t i = 0; i < buffer_.size(); ++i) {
+ ASSERT_EQ(15 + i, buffer_[i]);
+ }
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/vision/debug/camera-source.cc b/aos/vision/debug/camera-source.cc
index ef48a11..77bd6ed 100644
--- a/aos/vision/debug/camera-source.cc
+++ b/aos/vision/debug/camera-source.cc
@@ -4,6 +4,7 @@
#include <fstream>
#include <string>
+#include "aos/vision/image/camera_params.pb.h"
#include "aos/vision/image/image_stream.h"
namespace aos {
@@ -15,26 +16,20 @@
DebugFrameworkInterface *interface) override {
// TODO: Get these params from a config file passed in through the
// constructor.
- camera::CameraParams params = {.width = 640 * 2,
- .height = 480 * 2,
- .exposure = 10,
- .brightness = 128,
- .gain = 0,
- .fps = 30};
- image_stream_.reset(new ImageStream(jpeg_list_filename, params, interface));
+ image_stream_.reset(new ImageStream(
+ jpeg_list_filename, aos::vision::CameraParams(), interface));
}
const char *GetHelpMessage() override {
return &R"(
format_spec is filename of the camera device.
example: camera:/dev/video0
- This viewer source will stream video from a usb camera of your choice.
-)"[1];
+ This viewer source will stream video from a usb camera of your choice.)"[1];
}
class ImageStream : public ImageStreamEvent {
public:
- ImageStream(const std::string &fname, camera::CameraParams params,
+ ImageStream(const std::string &fname, aos::vision::CameraParams params,
DebugFrameworkInterface *interface)
: ImageStreamEvent(fname, params), interface_(interface) {
interface_->Loop()->Add(this);
diff --git a/aos/vision/image/BUILD b/aos/vision/image/BUILD
index 662addc..27646a5 100644
--- a/aos/vision/image/BUILD
+++ b/aos/vision/image/BUILD
@@ -1,4 +1,5 @@
package(default_visibility = ['//visibility:public'])
+load('/tools/build_rules/protobuf', 'proto_cc_library')
cc_library(
name = 'image_types',
@@ -8,6 +9,11 @@
],
)
+proto_cc_library(
+ name = 'camera_params',
+ src = 'camera_params.proto',
+)
+
cc_library(
name = 'reader',
srcs = ['reader.cc'],
@@ -16,6 +22,7 @@
'//aos/common:time',
'//aos/common/logging:logging',
':image_types',
+ ':camera_params',
],
)
diff --git a/aos/vision/image/camera_params.proto b/aos/vision/image/camera_params.proto
new file mode 100644
index 0000000..c109154
--- /dev/null
+++ b/aos/vision/image/camera_params.proto
@@ -0,0 +1,23 @@
+syntax = "proto2";
+
+package aos.vision;
+
+message CameraParams {
+ // Width of the image.
+ optional int32 width = 1 [default = 1280];
+
+ // Height of the image.
+ optional int32 height = 2 [default = 960];
+
+ // Exposure setting.
+ optional int32 exposure = 3 [default = 10];
+
+ // Brightness setting.
+ optional int32 brightness = 4 [default = 128];
+
+ // Hardware gain multiplier on pixel values.
+ optional int32 gain = 5 [default = 0];
+
+ // Frames per second to run camera.
+ optional int32 fps = 6 [default = 30];
+}
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
index 5a1ad21..308d3ec 100644
--- a/aos/vision/image/image_stream.h
+++ b/aos/vision/image/image_stream.h
@@ -2,6 +2,7 @@
#define _AOS_VISION_IMAGE_IMAGE_STREAM_H_
#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/camera_params.pb.h"
#include "aos/vision/image/reader.h"
#include <memory>
@@ -15,7 +16,7 @@
public:
static std::unique_ptr<::camera::Reader> GetCamera(
const std::string &fname, ImageStreamEvent *obj,
- camera::CameraParams params) {
+ aos::vision::CameraParams params) {
using namespace std::placeholders;
std::unique_ptr<::camera::Reader> camread(new ::camera::Reader(
fname, std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2),
@@ -28,7 +29,7 @@
: ::aos::events::EpollEvent(reader->fd()), reader_(std::move(reader)) {}
explicit ImageStreamEvent(const std::string &fname,
- camera::CameraParams params)
+ aos::vision::CameraParams params)
: ImageStreamEvent(GetCamera(fname, this, params)) {}
void ProcessHelper(DataRef data, aos::monotonic_clock::time_point timestamp) {
diff --git a/aos/vision/image/reader.cc b/aos/vision/image/reader.cc
index 03e2fc8..57766e2 100644
--- a/aos/vision/image/reader.cc
+++ b/aos/vision/image/reader.cc
@@ -1,7 +1,19 @@
-#include "aos/common/time.h"
+#include "aos/vision/image/reader.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <malloc.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
#include "aos/common/logging/logging.h"
-#include "aos/vision/image/reader.h"
+#include "aos/common/time.h"
#define CLEAR(x) memset(&(x), 0, sizeof(x))
@@ -12,8 +24,21 @@
size_t length; // for munmap
};
+aos::vision::CameraParams MakeCameraParams(int32_t width, int32_t height,
+ int32_t exposure, int32_t brightness,
+ int32_t gain, int32_t fps) {
+ aos::vision::CameraParams cam;
+ cam.set_width(width);
+ cam.set_height(height);
+ cam.set_exposure(exposure);
+ cam.set_brightness(brightness);
+ cam.set_gain(gain);
+ cam.set_fps(fps);
+ return cam;
+}
+
Reader::Reader(const std::string &dev_name, ProcessCb process,
- CameraParams params)
+ aos::vision::CameraParams params)
: dev_name_(dev_name), process_(std::move(process)), params_(params) {
struct stat st;
if (stat(dev_name.c_str(), &st) == -1) {
@@ -67,7 +92,7 @@
}
if (!SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
- "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure)) {
+ "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure())) {
LOG(FATAL, "Failed to set exposure\n");
}
}
@@ -200,8 +225,8 @@
v4l2_format fmt;
CLEAR(fmt);
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- fmt.fmt.pix.width = params_.width;
- fmt.fmt.pix.height = params_.height;
+ fmt.fmt.pix.width = params_.width();
+ fmt.fmt.pix.height = params_.height();
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
fmt.fmt.pix.field = V4L2_FIELD_ANY;
// fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
@@ -224,16 +249,16 @@
}
if (!SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
- "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure)) {
+ "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure())) {
LOG(FATAL, "Failed to set exposure\n");
}
if (!SetCameraControl(V4L2_CID_BRIGHTNESS, "V4L2_CID_BRIGHTNESS",
- params_.brightness)) {
+ params_.brightness())) {
LOG(FATAL, "Failed to set up camera\n");
}
- if (!SetCameraControl(V4L2_CID_GAIN, "V4L2_CID_GAIN", params_.gain)) {
+ if (!SetCameraControl(V4L2_CID_GAIN, "V4L2_CID_GAIN", params_.gain())) {
LOG(FATAL, "Failed to set up camera\n");
}
@@ -243,7 +268,7 @@
memset(setfps, 0, sizeof(struct v4l2_streamparm));
setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
setfps->parm.capture.timeperframe.numerator = 1;
- setfps->parm.capture.timeperframe.denominator = params_.fps;
+ setfps->parm.capture.timeperframe.denominator = params_.fps();
if (xioctl(fd_, VIDIOC_S_PARM, setfps) == -1) {
PLOG(FATAL, "ioctl VIDIOC_S_PARM(%d, %p)\n", fd_, setfps);
}
diff --git a/aos/vision/image/reader.h b/aos/vision/image/reader.h
index 28735fb..e913ad1 100644
--- a/aos/vision/image/reader.h
+++ b/aos/vision/image/reader.h
@@ -1,16 +1,5 @@
#ifndef AOS_VISION_IMAGE_READER_H_
#define AOS_VISION_IMAGE_READER_H_
-#include <errno.h>
-#include <fcntl.h>
-#include <malloc.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/mman.h>
-#include <sys/stat.h>
-#include <sys/time.h>
-#include <sys/types.h>
-#include <unistd.h>
#include <inttypes.h>
#include <functional>
@@ -18,24 +7,21 @@
#include "aos/common/time.h"
#include "aos/vision/image/V4L2.h"
+#include "aos/vision/image/camera_params.pb.h"
#include "aos/vision/image/image_types.h"
namespace camera {
-struct CameraParams {
- int32_t width;
- int32_t height;
- int32_t exposure;
- int32_t brightness;
- int32_t gain;
- int32_t fps;
-};
+aos::vision::CameraParams MakeCameraParams(int32_t width, int32_t height,
+ int32_t exposure, int32_t brightness,
+ int32_t gain, int32_t fps);
class Reader {
public:
using ProcessCb = std::function<void(
aos::vision::DataRef data, aos::monotonic_clock::time_point timestamp)>;
- Reader(const std::string &dev_name, ProcessCb process, CameraParams params);
+ Reader(const std::string &dev_name, ProcessCb process,
+ aos::vision::CameraParams params);
aos::vision::ImageFormat get_format();
@@ -74,7 +60,7 @@
static const unsigned int kNumBuffers = 5;
// set only at initialize
- CameraParams params_;
+ aos::vision::CameraParams params_;
};
} // namespace camera
diff --git a/aos/vision/tools/camera_primer.cc b/aos/vision/tools/camera_primer.cc
index a431ac8..3ba24bc 100644
--- a/aos/vision/tools/camera_primer.cc
+++ b/aos/vision/tools/camera_primer.cc
@@ -5,7 +5,7 @@
class ImageStream : public aos::vision::ImageStreamEvent {
public:
- ImageStream(const std::string &fname, camera::CameraParams params)
+ ImageStream(const std::string &fname, aos::vision::CameraParams params)
: ImageStreamEvent(fname, params) {}
void ProcessImage(aos::vision::DataRef /*data*/,
aos::monotonic_clock::time_point) override {
@@ -30,12 +30,7 @@
::aos::logging::AddImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
- camera::CameraParams params = {.width = 640 * 2,
- .height = 480 * 2,
- .exposure = 10,
- .brightness = 128,
- .gain = 0,
- .fps = 30};
+ aos::vision::CameraParams params;
if (argc != 2) {
fprintf(stderr, "usage: %s path_to_camera\n", argv[0]);
diff --git a/aos/vision/tools/jpeg_vision_test.cc b/aos/vision/tools/jpeg_vision_test.cc
index 52dd22f..059166e 100644
--- a/aos/vision/tools/jpeg_vision_test.cc
+++ b/aos/vision/tools/jpeg_vision_test.cc
@@ -42,7 +42,7 @@
class ChannelImageStream : public ImageStreamEvent {
public:
ChannelImageStream(const std::string &fname,
- const camera::CameraParams ¶ms)
+ const aos::vision::CameraParams ¶ms)
: ImageStreamEvent(fname, params), view_(false) {
// Lambda to record image data to a file on key press.
view_.view()->key_press_event = [this](uint32_t keyval) {
@@ -117,12 +117,7 @@
aos::events::EpollLoop loop;
gtk_init(&argc, &argv);
- camera::CameraParams params = {.width = 640 * 2,
- .height = 480 * 2,
- .exposure = 10,
- .brightness = 128,
- .gain = 0,
- .fps = 25};
+ aos::vision::CameraParams params;
aos::vision::ChannelImageStream strm1("/dev/video1", params);
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index cdbff1d..d49b05d 100644
--- a/frc971/wpilib/dma_edge_counting.h
+++ b/frc971/wpilib/dma_edge_counting.h
@@ -43,6 +43,10 @@
public:
DMAEdgeCounter(Encoder *encoder, DigitalInput *input)
: encoder_(encoder), input_(input) {}
+ DMAEdgeCounter() = default;
+
+ void set_encoder(Encoder *encoder) { encoder_ = encoder; }
+ void set_input(DigitalInput *input) { input_ = input; }
int positive_count() const { return pos_edge_count_; }
int negative_count() const { return neg_edge_count_; }
@@ -74,8 +78,8 @@
bool ExtractValue(const DMASample &sample) const;
- Encoder *const encoder_;
- DigitalInput *const input_;
+ Encoder *encoder_ = nullptr;
+ DigitalInput *input_ = nullptr;
// The last DMA reading we got.
DMASample prev_sample_;
diff --git a/tools/bazel b/tools/bazel
index f6f4c52..25266b8 100755
--- a/tools/bazel
+++ b/tools/bazel
@@ -81,11 +81,14 @@
ENVIRONMENT_VARIABLES+=(SHELL="${SHELL}")
ENVIRONMENT_VARIABLES+=(USER="${USER}")
ENVIRONMENT_VARIABLES+=(PATH="${PATH}")
-ENVIRONMENT_VARIABLES+=(LANG="${LANG}")
ENVIRONMENT_VARIABLES+=(HOME="${HOME}")
ENVIRONMENT_VARIABLES+=(LOGNAME="${LOGNAME}")
ENVIRONMENT_VARIABLES+=(TERM="${TERM}")
+if [[ ! -z "${LANG+x}" ]]; then
+ ENVIRONMENT_VARIABLES+=(LANG="${LANG}")
+fi
+
if [[ ! -z "${DISPLAY+x}" ]]; then
ENVIRONMENT_VARIABLES+=(DISPLAY="${DISPLAY}")
fi
diff --git a/tools/build_rules/protobuf.bzl b/tools/build_rules/protobuf.bzl
index c9bb9ef..f6a98b8 100644
--- a/tools/build_rules/protobuf.bzl
+++ b/tools/build_rules/protobuf.bzl
@@ -72,7 +72,7 @@
name = '%s__proto_srcs' % name,
srcs = [src],
deps = [('%s__proto_srcs' % o_name) for o_name in deps],
- visibility = ['//visibility:private'],
+ visibility = visibility,
)
basename = src[:-len('.proto')]
native.cc_library(
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index e8d0b17..d8ca87c 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -143,7 +143,7 @@
"//aos/vision/events:tcp_client",
"//aos/vision/events:epoll_events",
"//aos/vision/events:gtk_event",
- "//aos/vision/debug:debug_viewer",
+ "//aos/vision/debug:debug_window",
"//aos/vision/blob:range_image",
"//aos/vision/blob:codec",
"//aos/vision/blob:stream_view",
diff --git a/y2016/vision/debug_receiver.cc b/y2016/vision/debug_receiver.cc
index 7039c53..d3c527c 100644
--- a/y2016/vision/debug_receiver.cc
+++ b/y2016/vision/debug_receiver.cc
@@ -12,7 +12,6 @@
#include "aos/vision/events/socket_types.h"
#include "aos/vision/events/tcp_client.h"
#include "aos/vision/events/epoll_events.h"
-#include "aos/vision/debug/debug_viewer.h"
#include "aos/vision/blob/range_image.h"
#include "aos/vision/blob/codec.h"
#include "aos/vision/blob/stream_view.h"
@@ -69,9 +68,9 @@
private:
void DrawCross(PixelLinesOverlay &overlay, Vector<2> center, PixelRef color) {
- overlay.add_line(Vector<2>(center.x() - 50, center.y()),
+ overlay.AddLine(Vector<2>(center.x() - 50, center.y()),
Vector<2>(center.x() + 50, center.y()), color);
- overlay.add_line(Vector<2>(center.x(), center.y() - 50),
+ overlay.AddLine(Vector<2>(center.x(), center.y() - 50),
Vector<2>(center.x(), center.y() + 50), color);
}
diff --git a/y2016/vision/target_sender.cc b/y2016/vision/target_sender.cc
index 2790a83..85e62c4 100644
--- a/y2016/vision/target_sender.cc
+++ b/y2016/vision/target_sender.cc
@@ -1,5 +1,6 @@
#include <stdio.h>
#include <stdlib.h>
+#include <sys/stat.h>
#include <fstream>
#include <iostream>
#include <memory>
@@ -37,13 +38,15 @@
using aos::events::DataSocket;
using aos::vision::ImageFormat;
-::camera::CameraParams GetCameraParams(const Calibration &calibration) {
- return ::camera::CameraParams{.width = calibration.camera_image_width(),
- .height = calibration.camera_image_height(),
- .exposure = calibration.camera_exposure(),
- .brightness = calibration.camera_brightness(),
- .gain = calibration.camera_gain(),
- .fps = calibration.camera_fps()};
+::aos::vision::CameraParams GetCameraParams(const Calibration &calibration) {
+ ::aos::vision::CameraParams params;
+ params.set_width(calibration.camera_image_width());
+ params.set_height(calibration.camera_image_height());
+ params.set_exposure(calibration.camera_exposure());
+ params.set_brightness(calibration.camera_brightness());
+ params.set_gain(calibration.camera_gain());
+ params.set_fps(calibration.camera_fps());
+ return params;
}
int64_t Nanos(aos::monotonic_clock::duration time_diff) {
@@ -62,13 +65,14 @@
class ImageSender : public ImageStreamEvent {
public:
- ImageSender(int camera_index, camera::CameraParams params,
+ ImageSender(int camera_index, aos::vision::CameraParams params,
const std::string &fname, const std::string &ipadder, int port)
: ImageStreamEvent(fname, params),
camera_index_(camera_index),
udp_serv_(ipadder, 8080),
tcp_serv_(port),
- blob_filt_(ImageFormat(params.width, params.height), 40, 750, 250000),
+ blob_filt_(ImageFormat(params.width(), params.height()), 40, 750,
+ 250000),
finder_(0.25, 35) {
int index = 0;
while (true) {
@@ -94,9 +98,8 @@
DecodeJpeg(data, &image_);
auto fmt = image_.fmt();
- RangeImage rimg = DoThreshold(image_.get(), [](PixelRef &px) {
- return (px.g > 88);
- });
+ RangeImage rimg =
+ DoThreshold(image_.get(), [](PixelRef &px) { return (px.g > 88); });
// flip the right image as this camera is mount backward
if (camera_index_ == 0) {
@@ -202,11 +205,11 @@
private:
};
-void RunCamera(int camera_index, camera::CameraParams params,
+void RunCamera(int camera_index, aos::vision::CameraParams params,
const std::string &device, const std::string &ip_addr,
int port) {
- printf("Creating camera %d (%d, %d).\n", camera_index, params.width,
- params.height);
+ printf("Creating camera %d (%d, %d).\n", camera_index, params.width(),
+ params.height());
ImageSender strm(camera_index, params, device, ip_addr, port);
aos::events::EpollLoop loop;
diff --git a/y2016/vision/tools/BUILD b/y2016/vision/tools/BUILD
index a9e25ac..a7824c7 100644
--- a/y2016/vision/tools/BUILD
+++ b/y2016/vision/tools/BUILD
@@ -9,7 +9,7 @@
"//aos/vision/events:epoll_events",
"//aos/vision/events:gtk_event",
"//aos/vision/events:tcp_server",
- "//aos/vision/debug:debug_viewer",
+ "//aos/vision/debug:debug_window",
"//aos/vision/blob:range_image",
"//aos/vision/blob:stream_view",
"//y2016/vision:blob_filters",
diff --git a/y2016/vision/tools/blob_stream_replay.cc b/y2016/vision/tools/blob_stream_replay.cc
index f972fae..acf4d4b 100644
--- a/y2016/vision/tools/blob_stream_replay.cc
+++ b/y2016/vision/tools/blob_stream_replay.cc
@@ -4,6 +4,7 @@
#include <vector>
#include <memory>
#include <endian.h>
+#include <sys/stat.h>
#include <fstream>
#include <gdk/gdk.h>
#include <gtk/gtk.h>
@@ -13,7 +14,6 @@
#include "aos/vision/image/image_stream.h"
#include "aos/vision/events/epoll_events.h"
#include "aos/vision/events/tcp_server.h"
-#include "aos/vision/debug/debug_viewer.h"
#include "aos/vision/blob/stream_view.h"
#include "y2016/vision/blob_filters.h"
// #include "y2016/vision/process_targets.h"
@@ -362,57 +362,57 @@
void DrawSuperSpeed() {
PixelRef color = {0, 255, 255};
// S
- overlay_.add_line(Vector<2>(200, 100), Vector<2>(100, 100), color);
- overlay_.add_line(Vector<2>(100, 100), Vector<2>(100, 300), color);
- overlay_.add_line(Vector<2>(100, 300), Vector<2>(200, 300), color);
- overlay_.add_line(Vector<2>(200, 300), Vector<2>(200, 500), color);
- overlay_.add_line(Vector<2>(200, 500), Vector<2>(100, 500), color);
+ overlay_.AddLine(Vector<2>(200, 100), Vector<2>(100, 100), color);
+ overlay_.AddLine(Vector<2>(100, 100), Vector<2>(100, 300), color);
+ overlay_.AddLine(Vector<2>(100, 300), Vector<2>(200, 300), color);
+ overlay_.AddLine(Vector<2>(200, 300), Vector<2>(200, 500), color);
+ overlay_.AddLine(Vector<2>(200, 500), Vector<2>(100, 500), color);
// U
- overlay_.add_line(Vector<2>(250, 100), Vector<2>(250, 500), color);
- overlay_.add_line(Vector<2>(250, 500), Vector<2>(350, 500), color);
- overlay_.add_line(Vector<2>(350, 500), Vector<2>(350, 100), color);
+ overlay_.AddLine(Vector<2>(250, 100), Vector<2>(250, 500), color);
+ overlay_.AddLine(Vector<2>(250, 500), Vector<2>(350, 500), color);
+ overlay_.AddLine(Vector<2>(350, 500), Vector<2>(350, 100), color);
// P
- overlay_.add_line(Vector<2>(400, 100), Vector<2>(400, 500), color);
- overlay_.add_line(Vector<2>(400, 100), Vector<2>(500, 100), color);
- overlay_.add_line(Vector<2>(500, 100), Vector<2>(500, 300), color);
- overlay_.add_line(Vector<2>(500, 300), Vector<2>(400, 300), color);
+ overlay_.AddLine(Vector<2>(400, 100), Vector<2>(400, 500), color);
+ overlay_.AddLine(Vector<2>(400, 100), Vector<2>(500, 100), color);
+ overlay_.AddLine(Vector<2>(500, 100), Vector<2>(500, 300), color);
+ overlay_.AddLine(Vector<2>(500, 300), Vector<2>(400, 300), color);
// E
- overlay_.add_line(Vector<2>(550, 100), Vector<2>(550, 500), color);
- overlay_.add_line(Vector<2>(550, 100), Vector<2>(650, 100), color);
- overlay_.add_line(Vector<2>(550, 300), Vector<2>(650, 300), color);
- overlay_.add_line(Vector<2>(550, 500), Vector<2>(650, 500), color);
+ overlay_.AddLine(Vector<2>(550, 100), Vector<2>(550, 500), color);
+ overlay_.AddLine(Vector<2>(550, 100), Vector<2>(650, 100), color);
+ overlay_.AddLine(Vector<2>(550, 300), Vector<2>(650, 300), color);
+ overlay_.AddLine(Vector<2>(550, 500), Vector<2>(650, 500), color);
// R
- overlay_.add_line(Vector<2>(700, 100), Vector<2>(700, 500), color);
- overlay_.add_line(Vector<2>(700, 100), Vector<2>(800, 100), color);
- overlay_.add_line(Vector<2>(800, 100), Vector<2>(800, 300), color);
- overlay_.add_line(Vector<2>(800, 300), Vector<2>(700, 300), color);
- overlay_.add_line(Vector<2>(700, 350), Vector<2>(800, 500), color);
+ overlay_.AddLine(Vector<2>(700, 100), Vector<2>(700, 500), color);
+ overlay_.AddLine(Vector<2>(700, 100), Vector<2>(800, 100), color);
+ overlay_.AddLine(Vector<2>(800, 100), Vector<2>(800, 300), color);
+ overlay_.AddLine(Vector<2>(800, 300), Vector<2>(700, 300), color);
+ overlay_.AddLine(Vector<2>(700, 350), Vector<2>(800, 500), color);
// S
- overlay_.add_line(Vector<2>(200, 550), Vector<2>(100, 550), color);
- overlay_.add_line(Vector<2>(100, 550), Vector<2>(100, 750), color);
- overlay_.add_line(Vector<2>(100, 750), Vector<2>(200, 750), color);
- overlay_.add_line(Vector<2>(200, 750), Vector<2>(200, 950), color);
- overlay_.add_line(Vector<2>(200, 950), Vector<2>(100, 950), color);
+ overlay_.AddLine(Vector<2>(200, 550), Vector<2>(100, 550), color);
+ overlay_.AddLine(Vector<2>(100, 550), Vector<2>(100, 750), color);
+ overlay_.AddLine(Vector<2>(100, 750), Vector<2>(200, 750), color);
+ overlay_.AddLine(Vector<2>(200, 750), Vector<2>(200, 950), color);
+ overlay_.AddLine(Vector<2>(200, 950), Vector<2>(100, 950), color);
// P
- overlay_.add_line(Vector<2>(250, 550), Vector<2>(250, 950), color);
- overlay_.add_line(Vector<2>(250, 550), Vector<2>(350, 550), color);
- overlay_.add_line(Vector<2>(350, 550), Vector<2>(350, 750), color);
- overlay_.add_line(Vector<2>(350, 750), Vector<2>(250, 750), color);
+ overlay_.AddLine(Vector<2>(250, 550), Vector<2>(250, 950), color);
+ overlay_.AddLine(Vector<2>(250, 550), Vector<2>(350, 550), color);
+ overlay_.AddLine(Vector<2>(350, 550), Vector<2>(350, 750), color);
+ overlay_.AddLine(Vector<2>(350, 750), Vector<2>(250, 750), color);
// E
- overlay_.add_line(Vector<2>(400, 550), Vector<2>(400, 950), color);
- overlay_.add_line(Vector<2>(400, 550), Vector<2>(500, 550), color);
- overlay_.add_line(Vector<2>(400, 750), Vector<2>(500, 750), color);
- overlay_.add_line(Vector<2>(400, 950), Vector<2>(500, 950), color);
+ overlay_.AddLine(Vector<2>(400, 550), Vector<2>(400, 950), color);
+ overlay_.AddLine(Vector<2>(400, 550), Vector<2>(500, 550), color);
+ overlay_.AddLine(Vector<2>(400, 750), Vector<2>(500, 750), color);
+ overlay_.AddLine(Vector<2>(400, 950), Vector<2>(500, 950), color);
// E
- overlay_.add_line(Vector<2>(550, 550), Vector<2>(550, 950), color);
- overlay_.add_line(Vector<2>(550, 550), Vector<2>(650, 550), color);
- overlay_.add_line(Vector<2>(550, 750), Vector<2>(650, 750), color);
- overlay_.add_line(Vector<2>(550, 950), Vector<2>(650, 950), color);
+ overlay_.AddLine(Vector<2>(550, 550), Vector<2>(550, 950), color);
+ overlay_.AddLine(Vector<2>(550, 550), Vector<2>(650, 550), color);
+ overlay_.AddLine(Vector<2>(550, 750), Vector<2>(650, 750), color);
+ overlay_.AddLine(Vector<2>(550, 950), Vector<2>(650, 950), color);
// D
- overlay_.add_line(Vector<2>(700, 550), Vector<2>(700, 950), color);
- overlay_.add_line(Vector<2>(700, 550), Vector<2>(800, 575), color);
- overlay_.add_line(Vector<2>(800, 575), Vector<2>(800, 925), color);
- overlay_.add_line(Vector<2>(800, 925), Vector<2>(700, 950), color);
+ overlay_.AddLine(Vector<2>(700, 550), Vector<2>(700, 950), color);
+ overlay_.AddLine(Vector<2>(700, 550), Vector<2>(800, 575), color);
+ overlay_.AddLine(Vector<2>(800, 575), Vector<2>(800, 925), color);
+ overlay_.AddLine(Vector<2>(800, 925), Vector<2>(700, 950), color);
}
void UpdateNewTime(int new_delta) {
@@ -524,9 +524,9 @@
}
void DrawCross(PixelLinesOverlay &overlay, Vector<2> center, PixelRef color) {
- overlay.add_line(Vector<2>(center.x() - 25, center.y()),
+ overlay.AddLine(Vector<2>(center.x() - 25, center.y()),
Vector<2>(center.x() + 25, center.y()), color);
- overlay.add_line(Vector<2>(center.x(), center.y() - 25),
+ overlay.AddLine(Vector<2>(center.x(), center.y() - 25),
Vector<2>(center.x(), center.y() + 25), color);
}
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 9822b0a..008e08b 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -47,12 +47,9 @@
constexpr ::frc971::constants::Range Values::kHoodRange;
constexpr double Values::kTurretEncoderCountsPerRevolution,
- Values::kTurretEncoderRatio, Values::kTurretPotRatio,
- Values::kTurretEncoderIndexDifference,
- Values::kMaxTurretEncoderPulsesPerSecond;
-constexpr ::frc971::constants::Range Values::kTurretRange;
+ Values::kTurretEncoderRatio, Values::kMaxTurretEncoderPulsesPerSecond;
-constexpr double Values::kMaxIndexerEncoderCountsPerRevolution,
+constexpr double Values::kIndexerEncoderCountsPerRevolution,
Values::kIndexerEncoderRatio, Values::kIndexerEncoderIndexDifference,
Values::kMaxIndexerEncoderPulsesPerSecond;
@@ -64,7 +61,6 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
Values::Intake *const intake = &r->intake;
- Values::Turret *const turret = &r->turret;
Values::Hood *const hood = &r->hood;
r->drivetrain_max_speed = 5;
@@ -75,13 +71,6 @@
intake->zeroing.moving_buffer_size = 20;
intake->zeroing.allowable_encoder_error = 0.3;
- turret->zeroing.average_filter_size = Values::kZeroingSampleSize;
- turret->zeroing.one_revolution_distance =
- Values::kTurretEncoderIndexDifference;
- turret->zeroing.zeroing_threshold = 0.001;
- turret->zeroing.moving_buffer_size = 9;
- turret->zeroing.allowable_encoder_error = 0.3;
-
hood->zeroing.index_pulse_count = 2;
hood->zeroing.index_difference = Values::kHoodEncoderIndexDifference;
hood->zeroing.known_index_pulse = 0;
@@ -92,9 +81,6 @@
intake->pot_offset = 0;
intake->zeroing.measured_absolute_position = 0;
- turret->pot_offset = 0;
- turret->zeroing.measured_absolute_position = 0;
-
hood->pot_offset = 0.1;
hood->zeroing.measured_index_position = 0.05;
@@ -106,9 +92,6 @@
intake->pot_offset = 0.26712;
intake->zeroing.measured_absolute_position = 0.008913;
- turret->pot_offset = 0;
- turret->zeroing.measured_absolute_position = 0;
-
hood->zeroing.measured_index_position = 0.652898 - 0.488117;
r->down_error = 0;
@@ -119,9 +102,6 @@
intake->pot_offset = 0.2921 + 0.00039 + 0.012236 - 0.023602;
intake->zeroing.measured_absolute_position = 0.031437;
- turret->pot_offset = -5.45 - 0.026111;
- turret->zeroing.measured_absolute_position = 0.2429;
-
hood->zeroing.measured_index_position = 0.655432 - 0.460505;
r->down_error = 0;
diff --git a/y2017/constants.h b/y2017/constants.h
index e011501..ec8de5d 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -89,21 +89,15 @@
-0.39 * M_PI / 180.0, 37.11 * M_PI / 180.0, (-0.39 + 1.0) * M_PI / 180.0,
(37.11 - 1.0) * M_PI / 180.0};
- static constexpr double kTurretEncoderCountsPerRevolution = 1024 * 4;
- static constexpr double kTurretEncoderRatio = 16.0 / 92.0;
- static constexpr double kTurretPotRatio = 16.0 / 92.0;
- static constexpr double kTurretEncoderIndexDifference =
- 2.0 * M_PI * kTurretEncoderRatio;
+ static constexpr double kTurretEncoderCountsPerRevolution = 256 * 4;
+ static constexpr double kTurretEncoderRatio = 11.0 / 94.0;
static constexpr double kMaxTurretEncoderPulsesPerSecond =
control_loops::superstructure::turret::kFreeSpeed *
control_loops::superstructure::turret::kOutputRatio /
constants::Values::kTurretEncoderRatio *
kTurretEncoderCountsPerRevolution;
- static constexpr ::frc971::constants::Range kTurretRange{
- -460.0 / 2.0 * M_PI / 180.0, 460.0 / 2.0 * M_PI / 180.0,
- -450.0 / 2.0 * M_PI / 180.0, 450.0 / 2.0 * M_PI / 180.0};
- static constexpr double kMaxIndexerEncoderCountsPerRevolution = 256 * 4;
+ static constexpr double kIndexerEncoderCountsPerRevolution = 256 * 4;
static constexpr double kIndexerEncoderRatio = (18.0 / 36.0) * (12.0 / 84.0);
static constexpr double kIndexerEncoderIndexDifference =
2.0 * M_PI * kIndexerEncoderRatio;
@@ -111,14 +105,12 @@
control_loops::superstructure::indexer::kFreeSpeed *
control_loops::superstructure::indexer::kOutputRatio /
constants::Values::kIndexerEncoderRatio *
- kMaxIndexerEncoderCountsPerRevolution;
+ kIndexerEncoderCountsPerRevolution;
double drivetrain_max_speed;
Intake intake;
- Turret turret;
-
Hood hood;
double down_error;
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 427dadb..5a50a06 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -107,6 +107,15 @@
double position_error;
};
+struct ColumnPosition {
+ // Indexer angle in radians relative to the base. Positive is according to
+ // the right hand rule around +z.
+ .frc971.HallEffectAndPosition indexer;
+ // Turret angle in radians relative to the indexer. Positive is the same as
+ // the indexer.
+ .frc971.HallEffectAndPosition turret;
+};
+
queue_group SuperstructureQueue {
implements aos.control_loops.ControlLoop;
@@ -138,12 +147,8 @@
// out.
.frc971.PotAndAbsolutePosition intake;
- // Indexer angle in radians.
- double theta_indexer;
-
- // The sensor readings for the turret. The units and sign are defined the
- // same as what's in the Goal message.
- .frc971.PotAndAbsolutePosition turret;
+ // The position of the column.
+ ColumnPosition column;
// The sensor readings for the hood. The units and sign are defined the
// same as what's in the Goal message.
diff --git a/y2017/vision/BUILD b/y2017/vision/BUILD
index b70e67f..14811d9 100644
--- a/y2017/vision/BUILD
+++ b/y2017/vision/BUILD
@@ -20,6 +20,9 @@
proto_cc_library(
name = 'vision_config',
src = 'vision_config.proto',
+ deps = [
+ '//aos/vision/image:camera_params',
+ ],
)
cc_binary(
@@ -61,6 +64,7 @@
'//aos/vision/events:udp',
':vision_queue',
':vision_result',
+ ':target_finder',
'//aos/common:mutex',
],
)
diff --git a/y2017/vision/target_finder.cc b/y2017/vision/target_finder.cc
index e39fe73..22943ef 100644
--- a/y2017/vision/target_finder.cc
+++ b/y2017/vision/target_finder.cc
@@ -105,7 +105,6 @@
aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
return aos::vision::DoThreshold(image, [&](aos::vision::PixelRef &px) {
if (px.g > 88) {
- return true;
uint8_t min = std::min(px.b, px.r);
uint8_t max = std::max(px.b, px.r);
if (min >= px.g || max >= px.g) return false;
@@ -234,5 +233,49 @@
return true;
}
+namespace {
+
+constexpr double kInchesToMeters = 0.0254;
+
+} // namespace
+
+void RotateAngle(aos::vision::Vector<2> vec, double angle, double *rx,
+ double *ry) {
+ double cos_ang = std::cos(angle);
+ double sin_ang = std::sin(angle);
+ *rx = vec.x() * cos_ang - vec.y() * sin_ang;
+ *ry = vec.x() * sin_ang + vec.y() * cos_ang;
+}
+
+void TargetFinder::GetAngleDist(const aos::vision::Vector<2>& target,
+ double down_angle, double *dist,
+ double *angle) {
+ // TODO(ben): Will put all these numbers in a config file before
+ // the first competition. I hope.
+ double focal_length = 1418.6;
+ double mounted_angle_deg = 33.5;
+ double camera_angle = mounted_angle_deg * M_PI / 180.0 - down_angle;
+ double window_height = 960.0;
+ double window_width = 1280.0;
+
+ double target_height = 78.0;
+ double camera_height = 21.5;
+ double tape_width = 2;
+ double world_height = tape_width + target_height - camera_height;
+
+ double target_to_boiler = 9.5;
+ double robot_to_camera = 9.5;
+ double added_dist = target_to_boiler + robot_to_camera;
+
+ double px = target.x() - window_width / 2.0;
+ double py = target.y() - window_height / 2.0;
+ double pz = focal_length;
+ RotateAngle(aos::vision::Vector<2>(pz, -py), camera_angle, &pz, &py);
+ double pl = std::sqrt(pz * pz + px * px);
+
+ *dist = kInchesToMeters * (world_height * pl / py - added_dist);
+ *angle = std::atan2(px, pz);
+}
+
} // namespace vision
} // namespace y2017
diff --git a/y2017/vision/target_finder.h b/y2017/vision/target_finder.h
index 79417d1..5ee143d 100644
--- a/y2017/vision/target_finder.h
+++ b/y2017/vision/target_finder.h
@@ -70,6 +70,10 @@
// Get the local overlay for debug if we are doing that.
aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
+ // Convert target location into meters and radians.
+ void GetAngleDist(const aos::vision::Vector<2>& target, double down_angle,
+ double *dist, double *angle);
+
private:
// Find a loosly connected target.
double DetectConnectedTarget(const RangeImage &img);
diff --git a/y2017/vision/target_receiver.cc b/y2017/vision/target_receiver.cc
index 851d3a5..f96a95e 100644
--- a/y2017/vision/target_receiver.cc
+++ b/y2017/vision/target_receiver.cc
@@ -5,28 +5,18 @@
#include "aos/common/time.h"
#include "aos/linux_code/init.h"
#include "aos/vision/events/udp.h"
+#include "y2017/vision/target_finder.h"
#include "y2017/vision/vision.q.h"
#include "y2017/vision/vision_result.pb.h"
using aos::monotonic_clock;
-namespace y2017 {
-namespace vision {
-
-void ComputeDistanceAngle(const TargetResult &target, double *distance,
- double *angle) {
- // TODO: fix this.
- *distance = target.y();
- *angle = target.x();
-}
-
-} // namespace vision
-} // namespace y2017
-
int main() {
using namespace y2017::vision;
::aos::events::RXUdpSocket recv(8080);
char raw_data[65507];
+ // TODO(parker): Have this pull in a config from somewhere.
+ TargetFinder finder;
while (true) {
// TODO(austin): Don't malloc.
@@ -52,8 +42,10 @@
target_time.time_since_epoch())
.count();
- ComputeDistanceAngle(target.target(), &new_vision_status->distance,
- &new_vision_status->angle);
+ finder.GetAngleDist(
+ aos::vision::Vector<2>(target.target().x(), target.target().y()),
+ /* TODO: Insert down estimate here in radians: */ 0.0,
+ &new_vision_status->distance, &new_vision_status->angle);
}
LOG_STRUCT(DEBUG, "vision", *new_vision_status);
diff --git a/y2017/vision/target_sender.cc b/y2017/vision/target_sender.cc
index 548d01d..8eb13f5 100644
--- a/y2017/vision/target_sender.cc
+++ b/y2017/vision/target_sender.cc
@@ -2,6 +2,7 @@
#include <google/protobuf/text_format.h>
#include <stdio.h>
#include <stdlib.h>
+#include <sys/stat.h>
#include <fstream>
#include <iostream>
#include <memory>
@@ -76,13 +77,9 @@
std::ofstream ofst_;
};
-ImageFormat GetImageFormat(const CameraSettings ¶ms) {
- return ImageFormat{params.width(), params.height()};
-}
-
class ImageSender : public ImageStreamEvent {
public:
- ImageSender(camera::CameraParams params, GameSpecific game_cfg,
+ ImageSender(aos::vision::CameraParams params, GameSpecific game_cfg,
const std::string &fname, const std::string &ipadder, int port)
: ImageStreamEvent(fname, params),
game_cfg_(game_cfg),
@@ -181,17 +178,11 @@
private:
};
-void RunCamera(CameraSettings settings, GameSpecific game_cfg,
+void RunCamera(aos::vision::CameraParams settings, GameSpecific game_cfg,
const std::string &device, const std::string &ip_addr,
int port) {
printf("Creating camera (%dx%d).\n", settings.width(), settings.height());
- camera::CameraParams params = {settings.width(),
- settings.height(),
- settings.exposure(),
- settings.brightness(),
- 0,
- (int32_t)settings.fps()};
- ImageSender strm(params, game_cfg, device, ip_addr, port);
+ ImageSender strm(settings, game_cfg, device, ip_addr, port);
aos::events::EpollLoop loop;
loop.Add(strm.GetTCPServ());
diff --git a/y2017/vision/vision_config.proto b/y2017/vision/vision_config.proto
index 0bb7279..5501507 100644
--- a/y2017/vision/vision_config.proto
+++ b/y2017/vision/vision_config.proto
@@ -1,31 +1,9 @@
syntax = "proto2";
+import "aos/vision/image/camera_params.proto";
+
package y2017.vision;
-// Stores configuration for camera related settings and specs.
-message CameraSettings {
- // The focal length of the camera in pixels.
- optional double focal_length = 1 [default = 1418.6];
-
- // Width of the image.
- optional int32 width = 2 [default = 1280];
-
- // Height of the image.
- optional int32 height = 3 [default = 960];
-
- // Exposure setting.
- optional int32 exposure = 4 [default = 10];
-
- // Brightness setting.
- optional int32 brightness = 5 [default = 128];
-
- // Hardware gain multiplier on pixel values.
- optional double gain = 6 [default = 1.0];
-
- // Frames per second to run camera.
- optional double fps = 7 [default = 30.0];
-}
-
message GameSpecific {
// Needs more woojy.
optional int32 woojy = 1;
@@ -51,9 +29,9 @@
// Map robot name to the robot dependent configuration.
map<string, RobotConfig> robot_configs = 1;
- // Parameters for camera bringup.
- optional CameraSettings camera_params = 2;
+ // Year independent camera parameters.
+ optional aos.vision.CameraParams camera_params = 3;
// Parameters for this specific game
- optional GameSpecific game_params = 3;
+ optional GameSpecific game_params = 4;
}
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 1343640..09d36a6 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -104,11 +104,6 @@
(2 * M_PI /*radians*/);
}
-double turret_pot_translate(double voltage) {
- return -voltage * Values::kTurretPotRatio * (10.0 /*turns*/ / 5.0 /*volts*/) *
- (2 * M_PI /*radians*/);
-}
-
constexpr double kMaxFastEncoderPulsesPerSecond =
max(Values::kMaxDrivetrainEncoderPulsesPerSecond,
Values::kMaxShooterEncoderPulsesPerSecond);
@@ -124,6 +119,8 @@
Values::kMaxHoodEncoderPulsesPerSecond;
static_assert(kMaxSlowEncoderPulsesPerSecond <= 100000,
"slow encoders are too fast");
+static_assert(kMaxSlowEncoderPulsesPerSecond < kMaxMediumEncoderPulsesPerSecond,
+ "slow encoders are faster than medium?");
// Handles reading the duty cycle on a DigitalInput.
class DutyCycleReader {
@@ -203,10 +200,7 @@
static_cast<int>(1 / 4.0 /* built-in tolerance */ /
kMaxMediumEncoderPulsesPerSecond * 1e9 +
0.5));
- slow_encoder_filter_.SetPeriodNanoSeconds(
- static_cast<int>(1 / 4.0 /* built-in tolerance */ /
- kMaxSlowEncoderPulsesPerSecond * 1e9 +
- 0.5));
+ hall_filter_.SetPeriodNanoSeconds(100000);
}
void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
@@ -239,29 +233,35 @@
void set_indexer_encoder(::std::unique_ptr<Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
+ indexer_counter_.set_encoder(encoder.get());
indexer_encoder_ = ::std::move(encoder);
}
+ void set_indexer_hall(::std::unique_ptr<DigitalInput> input) {
+ hall_filter_.Add(input.get());
+ indexer_counter_.set_input(input.get());
+ indexer_hall_ = ::std::move(input);
+ }
+
void set_turret_encoder(::std::unique_ptr<Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
- turret_encoder_.set_encoder(::std::move(encoder));
+ turret_counter_.set_encoder(encoder.get());
+ turret_encoder_ = ::std::move(encoder);
}
- void set_turret_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
- turret_encoder_.set_potentiometer(::std::move(potentiometer));
- }
-
- void set_turret_absolute(::std::unique_ptr<DigitalInput> input) {
- turret_encoder_.set_absolute_pwm(::std::move(input));
+ void set_turret_hall(::std::unique_ptr<DigitalInput> input) {
+ hall_filter_.Add(input.get());
+ turret_counter_.set_input(input.get());
+ turret_hall_ = ::std::move(input);
}
void set_hood_encoder(::std::unique_ptr<Encoder> encoder) {
- slow_encoder_filter_.Add(encoder.get());
+ medium_encoder_filter_.Add(encoder.get());
hood_encoder_.set_encoder(::std::move(encoder));
}
void set_hood_index(::std::unique_ptr<DigitalInput> index) {
- slow_encoder_filter_.Add(index.get());
+ medium_encoder_filter_.Add(index.get());
hood_encoder_.set_index(::std::move(index));
}
@@ -274,7 +274,9 @@
void set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(
new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
+ dma_synchronizer_->Add(&indexer_counter_);
dma_synchronizer_->Add(&hood_encoder_);
+ dma_synchronizer_->Add(&turret_counter_);
}
void operator()() {
@@ -330,10 +332,9 @@
Values::kIntakeEncoderRatio, intake_pot_translate, true,
values.intake.pot_offset);
- superstructure_message->theta_indexer =
- -encoder_translate(indexer_encoder_->GetRaw(),
- Values::kMaxIndexerEncoderCountsPerRevolution,
- Values::kIndexerEncoderRatio);
+ CopyPosition(indexer_counter_, &superstructure_message->column.indexer,
+ Values::kIndexerEncoderCountsPerRevolution,
+ Values::kIndexerEncoderRatio, true);
superstructure_message->theta_shooter =
encoder_translate(shooter_encoder_->GetRaw(),
@@ -344,10 +345,9 @@
Values::kHoodEncoderCountsPerRevolution,
Values::kHoodEncoderRatio, true);
- CopyPosition(turret_encoder_, &superstructure_message->turret,
+ CopyPosition(turret_counter_, &superstructure_message->column.turret,
Values::kTurretEncoderCountsPerRevolution,
- Values::kTurretEncoderRatio, turret_pot_translate, true,
- values.turret.pot_offset);
+ Values::kTurretEncoderRatio, true);
superstructure_message.Send();
}
@@ -410,13 +410,35 @@
encoder_ratio * (2.0 * M_PI);
}
+ void CopyPosition(const ::frc971::wpilib::DMAEdgeCounter &counter,
+ ::frc971::HallEffectAndPosition *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->position =
+ multiplier * encoder_translate(counter.polled_encoder(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ position->current = !counter.polled_value();
+ position->posedge_count = counter.negative_count();
+ position->negedge_count = counter.positive_count();
+ position->posedge_value =
+ multiplier * encoder_translate(counter.last_negative_encoder_value(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ position->negedge_value =
+ multiplier * encoder_translate(counter.last_positive_encoder_value(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ }
+
int32_t my_pid_;
DriverStation *ds_;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_,
- slow_encoder_filter_;
+ hall_filter_;
::std::unique_ptr<Encoder> drivetrain_left_encoder_,
drivetrain_right_encoder_;
@@ -424,9 +446,13 @@
AbsoluteEncoderAndPotentiometer intake_encoder_;
::std::unique_ptr<Encoder> indexer_encoder_;
- ::std::unique_ptr<AnalogInput> indexer_hall_;
+ ::std::unique_ptr<DigitalInput> indexer_hall_;
+ ::frc971::wpilib::DMAEdgeCounter indexer_counter_;
- AbsoluteEncoderAndPotentiometer turret_encoder_;
+ ::std::unique_ptr<Encoder> turret_encoder_;
+ ::std::unique_ptr<DigitalInput> turret_hall_;
+ ::frc971::wpilib::DMAEdgeCounter turret_counter_;
+
::frc971::wpilib::DMAEncoder hood_encoder_;
::std::unique_ptr<Encoder> shooter_encoder_;
@@ -559,10 +585,10 @@
reader.set_intake_potentiometer(make_unique<AnalogInput>(4));
reader.set_indexer_encoder(make_encoder(5));
+ reader.set_indexer_hall(make_unique<DigitalInput>(4));
reader.set_turret_encoder(make_encoder(6));
- reader.set_turret_absolute(make_unique<DigitalInput>(2));
- reader.set_turret_potentiometer(make_unique<AnalogInput>(5));
+ reader.set_turret_hall(make_unique<DigitalInput>(2));
reader.set_hood_encoder(make_encoder(4));
reader.set_hood_index(make_unique<DigitalInput>(1));