Merge "Clean up new autonomous base code"
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/analysis/logreader.py b/frc971/analysis/logreader.py
index c59487b..c3a789c 100644
--- a/frc971/analysis/logreader.py
+++ b/frc971/analysis/logreader.py
@@ -51,7 +51,7 @@
self.HandleLine(line)
except Exception as ex:
# It's common for the last line of the file to be malformed.
- print("Ignoring malformed log entry: ", line)
+ print("Ignoring malformed log entry: ", line, ex)
def HandleLine(self, line):
"""
@@ -70,13 +70,19 @@
binary = key[0]
struct_instance_name = key[1]
data_search_path = key[2]
- boolean_multiplier = None
+ boolean_multiplier = False
+ multiplier = 1.0
# If the plot definition line ends with a "-b X" where X is a number then
# that number gets drawn when the value is True. Zero gets drawn when the
# value is False.
if len(data_search_path) >= 2 and data_search_path[-2] == '-b':
- boolean_multiplier = float(data_search_path[-1])
+ multiplier = float(data_search_path[-1])
+ boolean_multiplier = True
+ data_search_path = data_search_path[:-2]
+
+ if len(data_search_path) >= 2 and data_search_path[-2] == '-m':
+ multiplier = float(data_search_path[-1])
data_search_path = data_search_path[:-2]
# Make sure that we're looking at the right binary structure instance.
@@ -88,10 +94,10 @@
for path in data_search_path:
data = data[path]
- if boolean_multiplier is not None:
+ if boolean_multiplier:
if data == 'T':
- value.Add(pline.time, boolean_multiplier)
+ value.Add(pline.time, multiplier)
else:
value.Add(pline.time, 0)
else:
- value.Add(pline.time, data)
+ value.Add(pline.time, float(data) * multiplier)
diff --git a/frc971/constants.h b/frc971/constants.h
index 7b4cb47..0171e47 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -6,6 +6,21 @@
namespace frc971 {
namespace constants {
+struct HallEffectZeroingConstants {
+ // The absolute position of the lower edge of the hall effect sensor.
+ double lower_hall_position;
+ // The absolute position of the upper edge of the hall effect sensor.
+ double upper_hall_position;
+ // The difference in scaled units between two hall effect edges. This is the
+ // number of units/cycle.
+ double index_difference;
+ // Number of cycles we need to see the hall effect high.
+ size_t hall_trigger_zeroing_length;
+ // Direction the system must be moving in order to zero. True is positive,
+ // False is negative direction.
+ bool zeroing_move_direction;
+};
+
struct PotAndIndexPulseZeroingConstants {
// The number of samples in the moving average filter.
size_t average_filter_size;
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 4d87587..0f248f1 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -98,6 +98,20 @@
int32_t index_pulses_seen;
};
+struct HallEffectAndPositionEstimatorState {
+ // If error.
+ bool error;
+ // If we've found a positive edge while moving backwards and is zeroed.
+ bool zeroed;
+ // Encoder angle relative to where we started.
+ double encoder;
+ // The positions of the extreme posedges we've seen.
+ // If we've gotten enough samples where the hall effect is high before can be
+ // certain it is not a false positive.
+ bool high_long_enough;
+ double offset;
+};
+
// A left/right pair of PotAndIndexPositions.
struct PotAndIndexPair {
PotAndIndexPosition left;
@@ -113,6 +127,21 @@
double negedge_value;
};
+// Records the hall effect sensor and encoder values.
+struct HallEffectAndPosition {
+ // The current hall effect state.
+ bool current;
+ // The current encoder position.
+ double position;
+ // The number of positive and negative edges we've seen on the hall effect
+ // sensor.
+ int32_t posedge_count;
+ int32_t negedge_count;
+ // The values corresponding to the last hall effect sensor reading.
+ double posedge_value;
+ double negedge_value;
+};
+
// Records the positions for a mechanism with edge-capturing sensors on it.
struct HallEffectPositions {
double current;
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 9412ea3..12e72cb 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -56,105 +56,132 @@
* remainder of the graph.
*/
-PositionSensorSimulator::PositionSensorSimulator(double index_diff,
+PositionSensorSimulator::PositionSensorSimulator(double index_difference,
unsigned int noise_seed)
- : index_diff_(index_diff), pot_noise_(noise_seed, 0.0) {
+ : lower_index_edge_(index_difference, noise_seed),
+ upper_index_edge_(index_difference, noise_seed),
+ index_difference_(index_difference) {
Initialize(0.0, 0.0);
}
void PositionSensorSimulator::Initialize(
double start_position, double pot_noise_stddev,
- double known_index_pos /* = 0*/,
+ double known_index_position /* = 0*/,
double known_absolute_encoder_pos /* = 0*/) {
- // We're going to make the index pulse we know "segment zero".
- cur_index_segment_ = floor((start_position - known_index_pos) / index_diff_);
- known_index_pos_ = known_index_pos;
+ InitializeHallEffectAndPosition(start_position, known_index_position,
+ known_index_position);
+
known_absolute_encoder_ = known_absolute_encoder_pos;
- cur_index_ = 0;
- index_count_ = 0;
- cur_pos_ = start_position;
- start_position_ = start_position;
- pot_noise_.set_standard_deviation(pot_noise_stddev);
+
+ lower_index_edge_.mutable_pot_noise()->set_standard_deviation(pot_noise_stddev);
+ upper_index_edge_.mutable_pot_noise()->set_standard_deviation(pot_noise_stddev);
}
-void PositionSensorSimulator::MoveTo(double new_pos) {
- // Compute which index segment we're in. In other words, compute between
- // which two index pulses we are.
- const int new_index_segment =
- floor((new_pos - known_index_pos_) / index_diff_);
+void PositionSensorSimulator::InitializeHallEffectAndPosition(
+ double start_position, double known_index_lower, double known_index_upper) {
+ current_position_ = start_position;
+ start_position_ = start_position;
- if (new_index_segment < cur_index_segment_) {
- // We've crossed an index pulse in the negative direction. That means the
- // index pulse we just crossed is the higher end of the current index
- // segment. For example, if the mechanism moved from index segment four to
- // index segment three, then we just crossed index pulse 4.
- cur_index_ = new_index_segment + 1;
- index_count_++;
- } else if (new_index_segment > cur_index_segment_) {
- // We've crossed an index pulse in the positive direction. That means the
- // index pulse we just crossed is the lower end of the index segment. For
- // example, if the mechanism moved from index segment seven to index
- // segment eight, then we just crossed index pulse eight.
- cur_index_ = new_index_segment;
- index_count_++;
+ lower_index_edge_.Initialize(start_position, known_index_lower);
+ upper_index_edge_.Initialize(start_position, known_index_upper);
+
+ posedge_count_ = 0;
+ negedge_count_ = 0;
+ posedge_value_ = start_position;
+ negedge_value_ = start_position;
+}
+
+void PositionSensorSimulator::MoveTo(double new_position) {
+ {
+ const int lower_start_segment = lower_index_edge_.current_index_segment();
+ lower_index_edge_.MoveTo(new_position);
+ const int lower_end_segment = lower_index_edge_.current_index_segment();
+ if (lower_end_segment > lower_start_segment) {
+ // Moving up past the lower edge.
+ ++posedge_count_;
+ posedge_value_ = lower_index_edge_.IndexPulsePosition();
+ }
+ if (lower_end_segment < lower_start_segment) {
+ // Moved down.
+ ++negedge_count_;
+ negedge_value_ = lower_index_edge_.IndexPulsePosition();
+ }
}
- if (new_index_segment != cur_index_segment_) {
- latched_pot_ = pot_noise_.AddNoiseToSample(cur_index_ * index_diff_ +
- known_index_pos_);
+ {
+ const int upper_start_segment = upper_index_edge_.current_index_segment();
+ upper_index_edge_.MoveTo(new_position);
+ const int upper_end_segment = upper_index_edge_.current_index_segment();
+ if (upper_end_segment > upper_start_segment) {
+ // Moving up past the upper edge.
+ ++negedge_count_;
+ negedge_value_ = upper_index_edge_.IndexPulsePosition();
+ }
+ if (upper_end_segment < upper_start_segment) {
+ // Moved down.
+ ++posedge_count_;
+ posedge_value_ = upper_index_edge_.IndexPulsePosition();
+ }
}
- cur_index_segment_ = new_index_segment;
- cur_pos_ = new_pos;
+ current_position_ = new_position;
}
void PositionSensorSimulator::GetSensorValues(IndexPosition *values) {
- values->encoder = cur_pos_ - start_position_;
+ values->encoder = current_position_ - start_position_;
- if (index_count_ == 0) {
+ values->index_pulses = lower_index_edge_.index_count();
+ if (values->index_pulses == 0) {
values->latched_encoder = 0.0;
} else {
- // Determine the position of the index pulse relative to absolute zero.
- double index_pulse_position = cur_index_ * index_diff_ + known_index_pos_;
-
// Populate the latched encoder samples.
- values->latched_encoder = index_pulse_position - start_position_;
+ values->latched_encoder =
+ lower_index_edge_.IndexPulsePosition() - start_position_;
}
-
- values->index_pulses = index_count_;
}
void PositionSensorSimulator::GetSensorValues(PotAndIndexPosition *values) {
- values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
- values->encoder = cur_pos_ - start_position_;
+ values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+ current_position_);
+ values->encoder = current_position_ - start_position_;
- if (index_count_ == 0) {
+ if (lower_index_edge_.index_count() == 0) {
values->latched_pot = 0.0;
values->latched_encoder = 0.0;
} else {
- // Determine the position of the index pulse relative to absolute zero.
- double index_pulse_position = cur_index_ * index_diff_ + known_index_pos_;
-
// Populate the latched pot/encoder samples.
- values->latched_pot = latched_pot_;
- values->latched_encoder = index_pulse_position - start_position_;
+ values->latched_pot = lower_index_edge_.latched_pot();
+ values->latched_encoder =
+ lower_index_edge_.IndexPulsePosition() - start_position_;
}
- values->index_pulses = index_count_;
+ values->index_pulses = lower_index_edge_.index_count();
}
void PositionSensorSimulator::GetSensorValues(PotAndAbsolutePosition *values) {
- values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
- values->encoder = cur_pos_ - start_position_;
+ values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+ current_position_);
+ values->encoder = current_position_ - start_position_;
// TODO(phil): Create some lag here since this is a PWM signal it won't be
// instantaneous like the other signals. Better yet, its lag varies
// randomly with the distribution varying depending on the reading.
- values->absolute_encoder =
- ::std::remainder(cur_pos_ + known_absolute_encoder_, index_diff_);
+ values->absolute_encoder = ::std::remainder(
+ current_position_ + known_absolute_encoder_, index_difference_);
if (values->absolute_encoder < 0) {
- values->absolute_encoder += index_diff_;
+ values->absolute_encoder += index_difference_;
}
}
+void PositionSensorSimulator::GetSensorValues(HallEffectAndPosition *values) {
+ values->current = lower_index_edge_.current_index_segment() !=
+ upper_index_edge_.current_index_segment();
+ values->position = current_position_ - start_position_;
+
+ values->posedge_count = posedge_count_;
+ values->negedge_count = negedge_count_;
+ values->posedge_value = posedge_value_ - start_position_;
+ values->negedge_value = negedge_value_ - start_position_;
+}
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 3de96c1..6e4f017 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -22,9 +22,9 @@
// interval between when the absolute encoder reads 0.
// noise_seed: The seed to feed into the random number generator for the
// potentiometer values.
- // TODO(danielp): Allow for starting with a non-zero encoder value.
- PositionSensorSimulator(double index_diff, unsigned int noise_seed =
- ::aos::testing::RandomSeed());
+ PositionSensorSimulator(
+ double index_difference,
+ unsigned int noise_seed = ::aos::testing::RandomSeed());
// Set new parameters for the sensors. This is useful for unit tests to change
// the simulated sensors' behavior on the fly.
@@ -35,11 +35,17 @@
// This specifies the standard deviation of that
// distribution.
// known_index_pos: The absolute position of an index pulse.
- void Initialize(double start_position,
- double pot_noise_stddev,
+ void Initialize(double start_position, double pot_noise_stddev,
double known_index_pos = 0.0,
double known_absolute_encoder_pos = 0.0);
+ // Initializes a sensor simulation which is pretending to be a hall effect +
+ // encoder setup. This is assuming that the hall effect sensor triggers once
+ // per cycle, and a cycle is index_difference_ long;
+ void InitializeHallEffectAndPosition(double start_position,
+ double known_index_lower,
+ double known_index_upper);
+
// Simulate the structure moving to a new position. The new value is measured
// relative to absolute zero. This will update the simulated sensors with new
// readings.
@@ -50,51 +56,112 @@
// values: The target structure will be populated with simulated sensor
// readings. The readings will be in SI units. For example the units
// can be given in radians, meters, etc.
- void GetSensorValues(IndexPosition* values);
-
- // Get the current values of the simulated sensors.
- // values: The target structure will be populated with simulated sensor
- // readings. The readings will be in SI units. For example the units
- // can be given in radians, meters, etc.
- void GetSensorValues(PotAndIndexPosition* values);
-
- // Get the current values of the simulated sensors.
- // values: The target structure will be populated with simulated sensor
- // readings. The readings will be in SI units. For example the units
- // can be given in radians, meters, etc.
- void GetSensorValues(PotAndAbsolutePosition* values);
+ void GetSensorValues(IndexPosition *values);
+ void GetSensorValues(PotAndIndexPosition *values);
+ void GetSensorValues(PotAndAbsolutePosition *values);
+ void GetSensorValues(HallEffectAndPosition *values);
private:
- // The absolute segment between two index pulses the simulation is on. For
- // example, when the current position is betwen index pulse zero and one,
- // the current index segment is considered to be zero. Index segment one is
- // between index pulses 1 and 2, etc.
- int cur_index_segment_;
- // Index pulse to use for calculating latched sensor values, relative to
- // absolute zero. In other words this always holds the index pulse that was
- // encountered most recently.
- int cur_index_;
- // How many index pulses we've seen.
- int index_count_;
- // The pot position at the most recent index pulse with noise added.
- double latched_pot_;
+ // It turns out that we can re-use a lot of the same logic to count the index
+ // pulses as we can use to count the hall effect edges. The trick is to add 2
+ // "index pulses" spaced apart by the width of the hall effect sensor, and
+ // call the sensor "high" when the index segments disagree for the two
+ // IndexEdge classes disagree.
+ class IndexEdge {
+ public:
+ explicit IndexEdge(double index_difference, unsigned int noise_seed)
+ : index_difference_(index_difference), pot_noise_(noise_seed, 0.0) {}
+
+ void Initialize(double start_position, double segment_position) {
+ current_index_segment_ =
+ ::std::floor((start_position - segment_position) / index_difference_);
+ known_index_position_ = segment_position;
+ last_index_ = 0;
+ index_count_ = 0;
+ }
+
+ double index_count() const { return index_count_; }
+ double latched_pot() const { return latched_pot_; }
+ int current_index_segment() const { return current_index_segment_; }
+
+ double IndexPulsePosition() const {
+ return last_index_ * index_difference_ + known_index_position_;
+ }
+
+ void MoveTo(double new_position) {
+ const int new_index_segment = ::std::floor(
+ (new_position - known_index_position_) / index_difference_);
+
+ if (new_index_segment < current_index_segment_) {
+ // We've crossed an index pulse in the negative direction. That means
+ // the index pulse we just crossed is the higher end of the current
+ // index segment. For example, if the mechanism moved from index segment
+ // four to index segment three, then we just crossed index pulse 4.
+ last_index_ = new_index_segment + 1;
+ index_count_++;
+ } else if (new_index_segment > current_index_segment_) {
+ // We've crossed an index pulse in the positive direction. That means
+ // the index pulse we just crossed is the lower end of the index
+ // segment. For example, if the mechanism moved from index segment seven
+ // to index segment eight, then we just crossed index pulse eight.
+ last_index_ = new_index_segment;
+ index_count_++;
+ }
+
+ if (new_index_segment != current_index_segment_) {
+ latched_pot_ = pot_noise_.AddNoiseToSample(
+ last_index_ * index_difference_ + known_index_position_);
+ }
+
+ current_index_segment_ = new_index_segment;
+ }
+
+ GaussianNoise *mutable_pot_noise() { return &pot_noise_; }
+
+ private:
+ // The absolute segment between two index pulses the simulation is on. For
+ // example, when the current position is betwen index pulse zero and one,
+ // the current index segment is considered to be zero. Index segment one is
+ // between index pulses 1 and 2, etc.
+ int current_index_segment_;
+ // Index pulse to use for calculating latched sensor values, relative to
+ // absolute zero. In other words this always holds the index pulse that was
+ // encountered most recently.
+ int last_index_;
+ // How many index pulses we've seen.
+ int index_count_;
+
+ // Absolute position of a known index pulse.
+ double known_index_position_;
+ // Distance between index pulses on the mechanism.
+ double index_difference_;
+
+ // The pot position at the most recent index pulse with noise added.
+ double latched_pot_;
+
+ // Gaussian noise to add to pot readings.
+ GaussianNoise pot_noise_;
+ };
+
+ IndexEdge lower_index_edge_;
+ IndexEdge upper_index_edge_;
+
// Distance between index pulses on the mechanism.
- double index_diff_;
- // Absolute position of a known index pulse.
- // OR
- // Absolute position of the absolute encoder's reading stored in
- // known_absolute_encoder_.
- double known_index_pos_;
+ double index_difference_;
+
// The readout of the absolute encoder when the robot's mechanism is at
- // known_index_pos_.
+ // zero.
double known_absolute_encoder_;
// Current position of the mechanism relative to absolute zero.
- double cur_pos_;
+ double current_position_;
// Starting position of the mechanism relative to absolute zero. See the
// `starting_position` parameter in the constructor for more info.
double start_position_;
- // Gaussian noise to add to pot readings.
- GaussianNoise pot_noise_;
+
+ int posedge_count_;
+ int negedge_count_;
+ double posedge_value_;
+ double negedge_value_;
};
} // namespace control_loops
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 4a3ac1e..0f09f7f 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -303,5 +303,87 @@
// TODO(philipp): Test negative values.
}
+// Tests that we get the right number of edges with the HallEffectAndPosition
+// sensor value.
+TEST_F(PositionSensorSimTest, HallEffectAndPosition) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+ sim.InitializeHallEffectAndPosition(-0.25, 0.0, 0.5);
+ HallEffectAndPosition position;
+
+ // Go over only the lower edge rising.
+ sim.MoveTo(0.25);
+ sim.GetSensorValues(&position);
+ EXPECT_TRUE(position.current);
+ EXPECT_DOUBLE_EQ(0.50, position.position);
+ EXPECT_EQ(1, position.posedge_count);
+ EXPECT_EQ(0.25, position.posedge_value);
+ EXPECT_EQ(0, position.negedge_count);
+ EXPECT_EQ(0, position.negedge_value);
+
+ // Now, go over the upper edge, falling.
+ sim.MoveTo(0.75);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_DOUBLE_EQ(1.0, position.position);
+ EXPECT_EQ(1, position.posedge_count);
+ EXPECT_DOUBLE_EQ(0.25, position.posedge_value);
+ EXPECT_EQ(1, position.negedge_count);
+ EXPECT_DOUBLE_EQ(0.75, position.negedge_value);
+
+ // Now, jump a whole cycle.
+ sim.MoveTo(1.75);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_DOUBLE_EQ(2.0, position.position);
+ EXPECT_EQ(2, position.posedge_count);
+ EXPECT_DOUBLE_EQ(1.25, position.posedge_value);
+ EXPECT_EQ(2, position.negedge_count);
+ EXPECT_DOUBLE_EQ(1.75, position.negedge_value);
+
+ // Now, jump a whole cycle backwards.
+ sim.MoveTo(0.75);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_DOUBLE_EQ(1.0, position.position);
+ EXPECT_EQ(3, position.posedge_count);
+ EXPECT_DOUBLE_EQ(1.75, position.posedge_value);
+ EXPECT_EQ(3, position.negedge_count);
+ EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+
+ // Now, go over the upper edge, rising.
+ sim.MoveTo(0.25);
+ sim.GetSensorValues(&position);
+ EXPECT_TRUE(position.current);
+ EXPECT_DOUBLE_EQ(0.5, position.position);
+ EXPECT_EQ(4, position.posedge_count);
+ EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
+ EXPECT_EQ(3, position.negedge_count);
+ EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+
+ // Now, go over the lower edge, falling.
+ sim.MoveTo(-0.25);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_DOUBLE_EQ(0.0, position.position);
+ EXPECT_EQ(4, position.posedge_count);
+ EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
+ EXPECT_EQ(4, position.negedge_count);
+ EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+
+ for (int i = 0; i < 10; ++i) {
+ // Now, go over the lower edge, falling.
+ sim.MoveTo(-0.25 - i * 1.0e-6);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_NEAR(-i * 1.0e-6, position.position, 1e-8);
+ EXPECT_EQ(4, position.posedge_count);
+ EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
+ EXPECT_EQ(4, position.negedge_count);
+ EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+ }
+}
+
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 4d09bbc..6ebba72 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -24,12 +24,15 @@
// styles.
template <int number_of_states, int number_of_axes,
class ZeroingEstimator =
- ::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator,
+ int number_of_inputs = number_of_axes,
+ int number_of_outputs = number_of_axes>
class ProfiledSubsystem {
public:
ProfiledSubsystem(
::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>> loop,
+ number_of_states, number_of_inputs, number_of_outputs>>
+ loop,
::std::array<ZeroingEstimator, number_of_axes> &&estimators)
: loop_(::std::move(loop)), estimators_(::std::move(estimators)) {
zeroed_.fill(false);
@@ -55,7 +58,7 @@
}
// Returns the controller.
- const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes> &
+ const StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs> &
controller() const {
return *loop_;
}
@@ -102,8 +105,8 @@
}
// Sets the maximum voltage that will be commanded by the loop.
- void set_max_voltage(::std::array<double, number_of_axes> voltages) {
- for (int i = 0; i < number_of_axes; ++i) {
+ void set_max_voltage(::std::array<double, number_of_inputs> voltages) {
+ for (int i = 0; i < number_of_inputs; ++i) {
loop_->set_max_voltage(i, voltages[i]);
}
}
@@ -114,7 +117,8 @@
// TODO(austin): It's a bold assumption to assume that we will have the same
// number of sensors as axes. So far, that's been fine.
::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>> loop_;
+ number_of_states, number_of_inputs, number_of_outputs>>
+ loop_;
// The goal that the profile tries to reach.
Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 6d05444..891aaa6 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -160,6 +160,11 @@
// that the plant should deal with.
CheckU(U);
X_ = Update(X(), U);
+ UpdateY(U);
+ }
+
+ // Computes the new Y given the control input.
+ void UpdateY(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
Y_ = C() * X() + D() * U;
}
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index ec219cc..d551667 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -127,6 +127,12 @@
}
void ADIS16448::operator()() {
+ // NI's SPI driver defaults to SCHED_OTHER. Find it's PID with ps, and change
+ // it to a RT priority of 33.
+ PCHECK(system(
+ "ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
+ "33") == 0);
+
::aos::SetCurrentThreadName("IMU");
// Try to initialize repeatedly as long as we're supposed to be running.
@@ -135,6 +141,8 @@
}
LOG(INFO, "IMU initialized successfully\n");
+ ::aos::SetCurrentThreadRealtimePriority(33);
+
// Rounded to approximate the 204.8 Hz.
constexpr size_t kImuSendRate = 205;
diff --git a/frc971/wpilib/dma_edge_counting.cc b/frc971/wpilib/dma_edge_counting.cc
index 7f87956..afb880f 100644
--- a/frc971/wpilib/dma_edge_counting.cc
+++ b/frc971/wpilib/dma_edge_counting.cc
@@ -11,7 +11,7 @@
void DMAEdgeCounter::UpdateFromSample(const DMASample &sample) {
const bool previous_value =
- have_prev_sample_ ? ExtractValue(prev_sample_) : polled_value_;
+ have_prev_sample_ ? ExtractValue(prev_sample_) : previous_polled_value_;
have_prev_sample_ = true;
prev_sample_ = sample;
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index cdbff1d..64722ca 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_; }
@@ -59,10 +63,12 @@
private:
void UpdateFromSample(const DMASample &sample) override;
void UpdatePolledValue() override {
+ previous_polled_value_ = polled_value_;
polled_value_ = input_->Get();
polled_encoder_ = encoder_->GetRaw();
}
void PollFromSample(const DMASample &sample) override {
+ previous_polled_value_ = polled_value_;
polled_value_ = ExtractValue(sample);
polled_encoder_ = sample.GetRaw(encoder_);
}
@@ -74,8 +80,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_;
@@ -93,6 +99,7 @@
int neg_last_encoder_ = 0;
bool polled_value_ = false;
+ bool previous_polled_value_ = false;
int polled_encoder_ = 0;
DISALLOW_COPY_AND_ASSIGN(DMAEdgeCounter);
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 947a15c..4fe0236 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -142,6 +142,118 @@
return r;
}
+HallEffectAndPositionZeroingEstimator::HallEffectAndPositionZeroingEstimator(
+ const ZeroingConstants &constants)
+ : constants_(constants) {
+ Reset();
+}
+
+void HallEffectAndPositionZeroingEstimator::Reset() {
+ offset_ = 0.0;
+ min_low_position_ = ::std::numeric_limits<double>::max();
+ max_low_position_ = ::std::numeric_limits<double>::lowest();
+ zeroed_ = false;
+ initialized_ = false;
+ last_used_posedge_count_ = 0;
+ cycles_high_ = 0;
+ high_long_enough_ = false;
+ first_start_pos_ = 0.0;
+ error_ = false;
+ current_ = 0.0;
+ first_start_pos_ = 0.0;
+}
+
+void HallEffectAndPositionZeroingEstimator::TriggerError() {
+ if (!error_) {
+ LOG(ERROR, "Manually triggered zeroing error.\n");
+ error_ = true;
+ }
+}
+
+void HallEffectAndPositionZeroingEstimator::StoreEncoderMaxAndMin(
+ const HallEffectAndPosition &info) {
+ // If we have a new posedge.
+ if (!info.current) {
+ if (last_hall_) {
+ min_low_position_ = max_low_position_ = info.position;
+ } else {
+ min_low_position_ = ::std::min(min_low_position_, info.position);
+ max_low_position_ = ::std::max(max_low_position_, info.position);
+ }
+ }
+ last_hall_ = info.current;
+}
+
+void HallEffectAndPositionZeroingEstimator::UpdateEstimate(
+ const HallEffectAndPosition &info) {
+ // We want to make sure that we encounter at least one posedge while zeroing.
+ // So we take the posedge count from the first sample after reset and wait for
+ // that count to change and for the hall effect to stay high before we
+ // consider ourselves zeroed.
+ if (!initialized_) {
+ last_used_posedge_count_ = info.posedge_count;
+ initialized_ = true;
+ last_hall_ = info.current;
+ }
+
+ StoreEncoderMaxAndMin(info);
+
+ if (info.current) {
+ cycles_high_++;
+ } else {
+ cycles_high_ = 0;
+ last_used_posedge_count_ = info.posedge_count;
+ }
+
+ high_long_enough_ = cycles_high_ >= constants_.hall_trigger_zeroing_length;
+
+ bool moving_backward = false;
+ if (constants_.zeroing_move_direction) {
+ moving_backward = info.position > min_low_position_;
+ } else {
+ moving_backward = info.position < max_low_position_;
+ }
+
+ // If there are no posedges to use or we don't have enough samples yet to
+ // have a well-filtered starting position then we use the filtered value as
+ // our best guess.
+ if (last_used_posedge_count_ != info.posedge_count && high_long_enough_ &&
+ moving_backward) {
+ // Note the offset and the current posedge count so that we only run this
+ // logic once per posedge. That should be more resilient to corrupted
+ // intermediate data.
+ offset_ = -info.posedge_value;
+ if (constants_.zeroing_move_direction) {
+ offset_ += constants_.lower_hall_position;
+ } else {
+ offset_ += constants_.upper_hall_position;
+ }
+ last_used_posedge_count_ = info.posedge_count;
+
+ // Save the first starting position.
+ if (!zeroed_) {
+ first_start_pos_ = offset_;
+ LOG(INFO, "latching start position %f\n", first_start_pos_);
+ }
+
+ // Now that we have an accurate starting position we can consider ourselves
+ // zeroed.
+ zeroed_ = true;
+ }
+
+ position_ = info.position - offset_;
+}
+
+HallEffectAndPositionZeroingEstimator::State
+HallEffectAndPositionZeroingEstimator::GetEstimatorState() const {
+ State r;
+ r.error = error_;
+ r.zeroed = zeroed_;
+ r.encoder = position_;
+ r.high_long_enough = high_long_enough_;
+ r.offset = offset_;
+ return r;
+}
PotAndAbsEncoderZeroingEstimator::PotAndAbsEncoderZeroingEstimator(
const constants::PotAndAbsoluteEncoderZeroingConstants &constants)
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 4b21bc4..91d5b8c 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -121,6 +121,80 @@
double first_start_pos_;
};
+// Estimates the position with an incremental encoder with an index pulse and a
+// potentiometer.
+class HallEffectAndPositionZeroingEstimator : public ZeroingEstimator {
+ public:
+ using Position = HallEffectAndPosition;
+ using ZeroingConstants = constants::HallEffectZeroingConstants;
+ using State = HallEffectAndPositionEstimatorState;
+
+ explicit HallEffectAndPositionZeroingEstimator(const ZeroingConstants &constants);
+
+ // Update the internal logic with the next sensor values.
+ void UpdateEstimate(const Position &info);
+
+ // Reset the internal logic so it needs to be re-zeroed.
+ void Reset();
+
+ // Manually trigger an internal error. This is used for testing the error
+ // logic.
+ void TriggerError();
+
+ bool error() const override { return error_; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ // Returns information about our current state.
+ State GetEstimatorState() const;
+
+ private:
+ // Sets the minimum and maximum posedge position values.
+ void StoreEncoderMaxAndMin(const HallEffectAndPosition &info);
+
+ // The zeroing constants used to describe the configuration of the system.
+ const ZeroingConstants constants_;
+
+ // The estimated state of the hall effect.
+ double current_ = 0.0;
+ // The estimated position.
+ double position_ = 0.0;
+ // The smallest and largest positions of the last set of encoder positions
+ // while the hall effect was low.
+ double min_low_position_;
+ double max_low_position_;
+ // If we've seen the hall effect high for enough times without going low, then
+ // we can be sure it isn't a false positive.
+ bool high_long_enough_;
+ size_t cycles_high_;
+
+ bool last_hall_ = false;
+
+ // The estimated starting position of the mechanism. We also call this the
+ // 'offset' in some contexts.
+ double offset_;
+ // Flag for triggering logic that takes note of the current posedge count
+ // after a reset. See `last_used_posedge_count_'.
+ bool initialized_;
+ // After a reset we keep track of the posedge count with this. Only after the
+ // posedge count changes (i.e. increments at least once or wraps around) will
+ // we consider the mechanism zeroed. We also use this to store the most recent
+ // `HallEffectAndPosition::posedge_count' value when the start position
+ // was calculated. It helps us calculate the start position only on posedges
+ // to reject corrupted intermediate data.
+ int32_t last_used_posedge_count_;
+ // Marker to track whether we're fully zeroed yet or not.
+ bool zeroed_;
+ // Marker to track whether an error has occurred. This gets reset to false
+ // whenever Reset() is called.
+ bool error_ = false;
+ // Stores the position "start_pos" variable the first time the program
+ // is zeroed.
+ double first_start_pos_;
+};
+
// Estimates the position with an absolute encoder which also reports
// incremental counts, and a potentiometer.
class PotAndAbsEncoderZeroingEstimator : public ZeroingEstimator {
@@ -226,6 +300,8 @@
// Returns information about our current state.
State GetEstimatorState() const;
+ void TriggerError() { error_ = true; }
+
private:
// Returns the current real position using the relative encoder offset.
double CalculateCurrentPosition(const IndexPosition &info);
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 32ca049..a4ba3d7 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -55,6 +55,15 @@
estimator->UpdateEstimate(sensor_values_);
}
+ void MoveTo(PositionSensorSimulator *simulator,
+ HallEffectAndPositionZeroingEstimator *estimator,
+ double new_position) {
+ HallEffectAndPosition sensor_values_;
+ simulator->MoveTo(new_position);
+ simulator->GetSensorValues(&sensor_values_);
+ estimator->UpdateEstimate(sensor_values_);
+ }
+
::aos::testing::TestSharedMemory my_shm_;
};
@@ -438,5 +447,88 @@
estimator.GetEstimatorState().position);
}
+// Tests that an error is detected when the starting position changes too much.
+TEST_F(ZeroingTest, TestHallEffectZeroing) {
+ constants::HallEffectZeroingConstants constants;
+ constants.lower_hall_position = 0.25;
+ constants.upper_hall_position = 0.75;
+ constants.index_difference = 1.0;
+ constants.hall_trigger_zeroing_length = 2;
+ constants.zeroing_move_direction = false;
+
+ PositionSensorSimulator sim(constants.index_difference);
+
+ const double start_pos = 1.0;
+
+ sim.InitializeHallEffectAndPosition(start_pos, constants.lower_hall_position,
+ constants.upper_hall_position);
+
+ HallEffectAndPositionZeroingEstimator estimator(constants);
+
+ // Should not be zeroed when we stand still.
+ for (int i = 0; i < 300; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, 0.9);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move to where the hall effect is triggered and make sure it becomes zeroed.
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ ASSERT_TRUE(estimator.zeroed());
+
+ // Check that the offset is calculated correctly.
+ EXPECT_DOUBLE_EQ(-0.25, estimator.offset());
+
+ // Make sure triggering errors works.
+ estimator.TriggerError();
+ ASSERT_TRUE(estimator.error());
+
+ // Ensure resetting resets the state of the estimator.
+ estimator.Reset();
+ ASSERT_FALSE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+
+ // Make sure we don't become zeroed if the hall effect doesn't trigger for
+ // long enough.
+ MoveTo(&sim, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+
+ // Make sure we can zero moving in the opposite direction as before and stay
+ // zeroed once the hall effect is no longer triggered.
+
+ MoveTo(&sim, &estimator, 0.0);
+ ASSERT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.4);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.6);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+
+ // Check that the offset is calculated correctly.
+ EXPECT_DOUBLE_EQ(-0.75, estimator.offset());
+
+ // Make sure we don't zero if we start in the hall effect's range, before we
+ // reset, we also check that there were no errors.
+ MoveTo(&sim, &estimator, 0.5);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+ estimator.Reset();
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+}
+
+
} // namespace zeroing
} // namespace frc971
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/bazel.rc b/tools/bazel.rc
index c8d93d0..0e3001f 100644
--- a/tools/bazel.rc
+++ b/tools/bazel.rc
@@ -53,3 +53,4 @@
test --test_output=errors
build --experimental_sandbox_shm
+build --experimental_multi_threaded_digest
diff --git a/tools/build_rules/protobuf.bzl b/tools/build_rules/protobuf.bzl
index c9bb9ef..11753c2 100644
--- a/tools/build_rules/protobuf.bzl
+++ b/tools/build_rules/protobuf.bzl
@@ -1,14 +1,12 @@
def _do_proto_cc_library_impl(ctx):
- srcs = ctx.files.srcs
- deps = []
- deps += ctx.files.srcs
+ deps = [ctx.file.src]
for dep in ctx.attr.deps:
deps += dep.proto.deps
message = 'Building %s and %s from %s' % (ctx.outputs.pb_h.short_path,
ctx.outputs.pb_cc.short_path,
- ctx.files.srcs[0].short_path)
+ ctx.file.src.short_path)
ctx.action(
inputs = deps + ctx.files._well_known_protos,
executable = ctx.executable._protoc,
@@ -16,7 +14,8 @@
'--cpp_out=%s' % ctx.configuration.genfiles_dir.path,
'-I.',
'-Ithird_party/protobuf/src',
- ] + [s.path for s in srcs],
+ ctx.file.src.path,
+ ],
mnemonic = 'ProtocCc',
progress_message = message,
outputs = [
@@ -27,13 +26,12 @@
return struct(
proto = struct(
- srcs = srcs,
deps = deps,
)
)
-def _do_proto_cc_library_outputs(srcs):
- basename = srcs[0].name[:-len('.proto')]
+def _do_proto_cc_library_outputs(src):
+ basename = src.name[:-len('.proto')]
return {
'pb_h': '%s.pb.h' % basename,
'pb_cc': '%s.pb.cc' % basename,
@@ -42,8 +40,10 @@
_do_proto_cc_library = rule(
implementation = _do_proto_cc_library_impl,
attrs = {
- 'srcs': attr.label_list(
+ 'src': attr.label(
allow_files = FileType(['.proto']),
+ mandatory = True,
+ single_file = True,
),
'deps': attr.label_list(providers = ["proto"]),
'_protoc': attr.label(
@@ -70,9 +70,9 @@
_do_proto_cc_library(
name = '%s__proto_srcs' % name,
- srcs = [src],
+ src = 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/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index 9dd93d6..5fbb1e3 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -25,8 +25,8 @@
.count();
}
-const ProfileParameters kSlowDrive = {0.8, 2.5};
-const ProfileParameters kSlowTurn = {0.8, 3.0};
+const ProfileParameters kSlowDrive = {2.0, 2.0};
+const ProfileParameters kSlowTurn = {3.0, 3.0};
} // namespace
@@ -39,20 +39,43 @@
const ::frc971::autonomous::AutonomousActionParams ¶ms) {
monotonic_clock::time_point start_time = monotonic_clock::now();
LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
+ InitializeEncoders();
+ ResetDrivetrain();
switch (params.mode) {
case 0:
- // Test case autonomous mode.
- // Drives forward 1.0 meters and then turns 180 degrees.
- StartDrive(1.1, 0.0, kSlowDrive, kSlowTurn);
- if (!WaitForDriveNear(1.0, 0.0)) return true;
- StartDrive(0.0, M_PI / 2, kSlowDrive, kSlowTurn);
- if (!WaitForDriveDone()) return true;
+ default:
+ while (true) {
+ constexpr auto kDelayTime = chrono::milliseconds(1);
+ // Test case autonomous mode.
+ // Drives forward 1.0 meters and then turns 180 degrees.
+ StartDrive(1.0, 0.0, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveDone()) return true;
+
+ this_thread::sleep_for(kDelayTime);
+ if (ShouldCancel()) return true;
+
+ StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveDone()) return true;
+
+ this_thread::sleep_for(kDelayTime);
+ if (ShouldCancel()) return true;
+
+ StartDrive(1.0, 0.0, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveDone()) return true;
+
+ this_thread::sleep_for(kDelayTime);
+ if (ShouldCancel()) return true;
+
+ StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveDone()) return true;
+
+ this_thread::sleep_for(kDelayTime);
+ if (ShouldCancel()) return true;
+ }
+
break;
- default:
- LOG(ERROR, "Invalid auto mode %d\n", params.mode);
- return true;
}
LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 9822b0a..b9361c7 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,8 +61,8 @@
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;
+ Values::Column *const column = &r->column;
r->drivetrain_max_speed = 5;
@@ -75,12 +72,8 @@
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;
+ column->indexer_zeroing.index_difference = 2.0 * M_PI;
+ column->turret_zeroing.index_difference = 2.0 * M_PI;
hood->zeroing.index_pulse_count = 2;
hood->zeroing.index_difference = Values::kHoodEncoderIndexDifference;
@@ -92,8 +85,11 @@
intake->pot_offset = 0;
intake->zeroing.measured_absolute_position = 0;
- turret->pot_offset = 0;
- turret->zeroing.measured_absolute_position = 0;
+ column->indexer_zeroing.lower_hall_position = 2.0;
+ column->indexer_zeroing.upper_hall_position = 2.1;
+
+ column->turret_zeroing.lower_hall_position = 0;
+ column->turret_zeroing.upper_hall_position = 0.1;
hood->pot_offset = 0.1;
hood->zeroing.measured_index_position = 0.05;
@@ -106,8 +102,11 @@
intake->pot_offset = 0.26712;
intake->zeroing.measured_absolute_position = 0.008913;
- turret->pot_offset = 0;
- turret->zeroing.measured_absolute_position = 0;
+ column->indexer_zeroing.lower_hall_position = 2.0;
+ column->indexer_zeroing.upper_hall_position = 2.1;
+
+ column->turret_zeroing.lower_hall_position = 0;
+ column->turret_zeroing.upper_hall_position = 0.1;
hood->zeroing.measured_index_position = 0.652898 - 0.488117;
@@ -119,8 +118,11 @@
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;
+ column->indexer_zeroing.lower_hall_position = 2.0;
+ column->indexer_zeroing.upper_hall_position = 2.1;
+
+ column->turret_zeroing.lower_hall_position = 0;
+ column->turret_zeroing.upper_hall_position = 0.1;
hood->zeroing.measured_index_position = 0.655432 - 0.460505;
diff --git a/y2017/constants.h b/y2017/constants.h
index e011501..e8faae1 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -32,16 +32,16 @@
::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
};
- struct Turret {
- double pot_offset;
- ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
- };
-
struct Hood {
double pot_offset;
::frc971::constants::EncoderPlusIndexZeroingConstants zeroing;
};
+ struct Column {
+ ::frc971::constants::HallEffectZeroingConstants indexer_zeroing;
+ ::frc971::constants::HallEffectZeroingConstants turret_zeroing;
+ };
+
static const int kZeroingSampleSize = 200;
static constexpr double kDrivetrainCyclesPerRevolution = 256;
@@ -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,13 +105,13 @@
control_loops::superstructure::indexer::kFreeSpeed *
control_loops::superstructure::indexer::kOutputRatio /
constants::Values::kIndexerEncoderRatio *
- kMaxIndexerEncoderCountsPerRevolution;
+ kIndexerEncoderCountsPerRevolution;
double drivetrain_max_speed;
Intake intake;
- Turret turret;
+ Column column;
Hood hood;
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.cc b/y2017/control_loops/drivetrain/drivetrain_base.cc
index 1e30e08..3367fc1 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_base.cc
@@ -22,7 +22,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::IMU_Y_GYRO,
+ ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
::y2017::control_loops::drivetrain::MakeDrivetrainLoop,
::y2017::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2017/control_loops/python/BUILD b/y2017/control_loops/python/BUILD
index cb599a3..dc5e021 100644
--- a/y2017/control_loops/python/BUILD
+++ b/y2017/control_loops/python/BUILD
@@ -100,3 +100,42 @@
'//frc971/control_loops/python:controls',
]
)
+
+py_library(
+ name = 'turret_lib',
+ srcs = [
+ 'turret.py',
+ ],
+ deps = [
+ '//aos/common/util:py_trapezoid_profile',
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
+
+py_library(
+ name = 'indexer_lib',
+ srcs = [
+ 'indexer.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
+
+py_binary(
+ name = 'column',
+ srcs = [
+ 'column.py',
+ ],
+ deps = [
+ ':turret_lib',
+ ':indexer_lib',
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
diff --git a/y2017/control_loops/python/column.py b/y2017/control_loops/python/column.py
new file mode 100644
index 0000000..ebb2f60
--- /dev/null
+++ b/y2017/control_loops/python/column.py
@@ -0,0 +1,377 @@
+#!/usr/bin/python
+
+from aos.common.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+from y2017.control_loops.python import turret
+from y2017.control_loops.python import indexer
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+
+# TODO(austin): Shut down with no counts on the turret.
+
+class ColumnController(control_loop.ControlLoop):
+ def __init__(self, name='Column'):
+ super(ColumnController, self).__init__(name)
+ self.turret = turret.Turret(name + 'Turret')
+ self.indexer = indexer.Indexer(name + 'Indexer')
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # State is [position_indexer,
+ # velocity_indexer,
+ # position_shooter,
+ # velocity_shooter]
+ # Input is [volts_indexer, volts_shooter]
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 2)))
+
+ self.A_continuous[1 - 1, 1 - 1] = -(self.indexer.Kt / self.indexer.Kv / (self.indexer.J * self.indexer.resistance * self.indexer.G * self.indexer.G) +
+ self.turret.Kt / self.turret.Kv / (self.indexer.J * self.turret.resistance * self.turret.G * self.turret.G))
+ self.A_continuous[1 - 1, 3 - 1] = self.turret.Kt / self.turret.Kv / (self.indexer.J * self.turret.resistance * self.turret.G * self.turret.G)
+ self.B_continuous[1 - 1, 0] = self.indexer.Kt / (self.indexer.J * self.indexer.resistance * self.indexer.G)
+ self.B_continuous[1 - 1, 1] = -self.turret.Kt / (self.indexer.J * self.turret.resistance * self.turret.G)
+
+ self.A_continuous[2 - 1, 3 - 1] = 1
+
+ self.A_continuous[3 - 1, 1 - 1] = self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
+ self.A_continuous[3 - 1, 3 - 1] = -self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
+
+ self.B_continuous[3 - 1, 1] = self.turret.Kt / (self.turret.J * self.turret.resistance * self.turret.G)
+
+ self.C = numpy.matrix([[1, 0, 0], [0, 1, 0]])
+ self.D = numpy.matrix([[0, 0], [0, 0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ q_pos = 0.015
+ q_vel = 0.3
+ self.Q = numpy.matrix([[(1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ q_vel_indexer_ff = 0.000005
+ q_pos_ff = 0.0000005
+ q_vel_ff = 0.00008
+ self.Qff = numpy.matrix([[(1.0 / (q_vel_indexer_ff ** 2.0)), 0.0, 0.0],
+ [0.0, (1.0 / (q_pos_ff ** 2.0)), 0.0],
+ [0.0, 0.0, (1.0 / (q_vel_ff ** 2.0))]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
+
+class Column(ColumnController):
+ def __init__(self, name='Column'):
+ super(Column, self).__init__(name)
+ A_continuous = numpy.matrix(numpy.zeros((4, 4)))
+ B_continuous = numpy.matrix(numpy.zeros((4, 2)))
+
+ A_continuous[0, 1] = 1
+ A_continuous[1:, 1:] = self.A_continuous
+ B_continuous[1:, :] = self.B_continuous
+
+ self.A_continuous = A_continuous
+ self.B_continuous = B_continuous
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ glog.debug('Eig is ' + repr(numpy.linalg.eig(self.A_continuous)))
+
+ self.C = numpy.matrix([[1, 0, 0, 0], [-1, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0], [0, 0]])
+
+ orig_K = self.K
+ self.K = numpy.matrix(numpy.zeros((2, 4)))
+ self.K[:, 1:] = orig_K
+
+ orig_Kff = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((2, 4)))
+ self.Kff[:, 1:] = orig_Kff
+
+ q_pos = 0.12
+ q_vel = 2.00
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0, 0.0],
+ [0.0, 0.0, (q_pos ** 2.0), 0.0],
+ [0.0, 0.0, 0.0, (q_vel ** 2.0)]])
+
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos ** 2.0), 0.0],
+ [0.0, (r_pos ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.InitializeState()
+
+
+class IntegralColumn(Column):
+ def __init__(self, name='IntegralColumn', voltage_error_noise=None):
+ super(IntegralColumn, self).__init__(name)
+
+ A_continuous = numpy.matrix(numpy.zeros((6, 6)))
+ A_continuous[0:4, 0:4] = self.A_continuous
+ A_continuous[0:4:, 4:6] = self.B_continuous
+
+ B_continuous = numpy.matrix(numpy.zeros((6, 2)))
+ B_continuous[0:4, :] = self.B_continuous
+
+ self.A_continuous = A_continuous
+ self.B_continuous = B_continuous
+ glog.debug('A_continuous: ' + repr(self.A_continuous))
+ glog.debug('B_continuous: ' + repr(self.B_continuous))
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ glog.debug('Eig is ' + repr(numpy.linalg.eig(self.A_continuous)))
+
+ C = numpy.matrix(numpy.zeros((2, 6)))
+ C[0:2, 0:4] = self.C
+ self.C = C
+ glog.debug('C is ' + repr(self.C))
+
+ self.D = numpy.matrix([[0, 0], [0, 0]])
+
+ orig_K = self.K
+ self.K = numpy.matrix(numpy.zeros((2, 6)))
+ self.K[:, 0:4] = orig_K
+ self.K[0, 4] = 1
+ self.K[1, 5] = 1
+
+ orig_Kff = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((2, 6)))
+ self.Kff[:, 0:4] = orig_Kff
+
+ q_pos = 0.12
+ q_vel = 2.00
+ q_voltage = 4.0
+ if voltage_error_noise is not None:
+ q_voltage = voltage_error_noise
+
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, (q_pos ** 2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, (q_vel ** 2.0), 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
+
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos ** 2.0), 0.0],
+ [0.0, (r_pos ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.InitializeState()
+
+
+class ScenarioPlotter(object):
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.xi = []
+ self.xt = []
+ self.vi = []
+ self.vt = []
+ self.ai = []
+ self.at = []
+ self.x_hat = []
+ self.ui = []
+ self.ut = []
+ self.ui_fb = []
+ self.ut_fb = []
+ self.offseti = []
+ self.offsett = []
+ self.turret_error = []
+
+ def run_test(self, column, end_goal,
+ controller_column,
+ observer_column=None,
+ iterations=200):
+ """Runs the column plant with an initial condition and goal.
+
+ Args:
+ column: column object to use.
+ end_goal: end_goal state.
+ controller_column: Intake object to get K from, or None if we should
+ use column.
+ observer_column: Intake object to use for the observer, or None if we should
+ use the actual state.
+ iterations: Number of timesteps to run the model for.
+ """
+
+ if controller_column is None:
+ controller_column = column
+
+ vbat = 12.0
+
+ if self.t:
+ initial_t = self.t[-1] + column.dt
+ else:
+ initial_t = 0
+
+ goal = numpy.concatenate((column.X, numpy.matrix(numpy.zeros((2, 1)))), axis=0)
+
+ profile = TrapezoidProfile(column.dt)
+ profile.set_maximum_acceleration(10.0)
+ profile.set_maximum_velocity(3.0)
+ profile.SetGoal(goal[2, 0])
+
+ U_last = numpy.matrix(numpy.zeros((2, 1)))
+ for i in xrange(iterations):
+ observer_column.Y = column.Y
+ observer_column.CorrectObserver(U_last)
+
+ self.offseti.append(observer_column.X_hat[4, 0])
+ self.offsett.append(observer_column.X_hat[5, 0])
+ self.x_hat.append(observer_column.X_hat[0, 0])
+
+ next_goal = numpy.concatenate(
+ (end_goal[0:2, :],
+ profile.Update(end_goal[2, 0], end_goal[3, 0]),
+ end_goal[4:6, :]),
+ axis=0)
+
+ ff_U = controller_column.Kff * (next_goal - observer_column.A * goal)
+ fb_U = controller_column.K * (goal - observer_column.X_hat)
+ self.turret_error.append((goal[2, 0] - column.X[2, 0]) * 100.0)
+ self.ui_fb.append(fb_U[0, 0])
+ self.ut_fb.append(fb_U[1, 0])
+
+ U_uncapped = ff_U + fb_U
+
+ U = U_uncapped.copy()
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ U[1, 0] = numpy.clip(U[1, 0], -vbat, vbat)
+ self.xi.append(column.X[0, 0])
+ self.xt.append(column.X[2, 0])
+
+ if self.vi:
+ last_vi = self.vi[-1]
+ else:
+ last_vi = 0
+ if self.vt:
+ last_vt = self.vt[-1]
+ else:
+ last_vt = 0
+
+ self.vi.append(column.X[1, 0])
+ self.vt.append(column.X[3, 0])
+ self.ai.append((self.vi[-1] - last_vi) / column.dt)
+ self.at.append((self.vt[-1] - last_vt) / column.dt)
+
+ offset = 0.0
+ if i > 100:
+ offset = 1.0
+ column.Update(U + numpy.matrix([[offset], [0.0]]))
+
+ observer_column.PredictObserver(U)
+
+ self.t.append(initial_t + i * column.dt)
+ self.ui.append(U[0, 0])
+ self.ut.append(U[1, 0])
+
+ ff_U -= U_uncapped - U
+ goal = controller_column.A * goal + controller_column.B * ff_U
+
+ if U[1, 0] != U_uncapped[1, 0]:
+ profile.MoveCurrentState(
+ numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
+
+ glog.debug('Time: %f', self.t[-1])
+ glog.debug('goal_error %s', repr(end_goal - goal))
+ glog.debug('error %s', repr(observer_column.X_hat - end_goal))
+
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.xi, label='xi')
+ pylab.plot(self.t, self.xt, label='xt')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.plot(self.t, self.turret_error, label='turret_error')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.ui, label='ui')
+ pylab.plot(self.t, self.ui_fb, label='ui_fb')
+ pylab.plot(self.t, self.ut, label='ut')
+ pylab.plot(self.t, self.ut_fb, label='ut_fb')
+ pylab.plot(self.t, self.offseti, label='voltage_offseti')
+ pylab.plot(self.t, self.offsett, label='voltage_offsett')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.ai, label='ai')
+ pylab.plot(self.t, self.at, label='at')
+ pylab.plot(self.t, self.vi, label='vi')
+ pylab.plot(self.t, self.vt, label='vt')
+ pylab.legend()
+
+ pylab.show()
+
+
+def main(argv):
+ scenario_plotter = ScenarioPlotter()
+
+ column = Column()
+ column_controller = IntegralColumn()
+ observer_column = IntegralColumn()
+
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [10.0], [5.0], [0.0], [0.0], [0.0]])
+ scenario_plotter.run_test(column, end_goal=R, controller_column=column_controller,
+ observer_column=observer_column, iterations=600)
+
+ if FLAGS.plot:
+ scenario_plotter.Plot()
+
+ if len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file names')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'column']
+ column = Column('Column')
+ loop_writer = control_loop.ControlLoopWriter('Column', [column],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
+
+ integral_column = IntegralColumn('IntegralColumn')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralColumn', [integral_column], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
+ stuck_integral_column = IntegralColumn('StuckIntegralColumn', voltage_error_noise=8.0)
+ stuck_integral_loop_writer = control_loop.ControlLoopWriter(
+ 'StuckIntegralColumn', [stuck_integral_column], namespaces=namespaces)
+ stuck_integral_loop_writer.Write(argv[5], argv[6])
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/drivetrain.py b/y2017/control_loops/python/drivetrain.py
index 8a65128..e722c86 100755
--- a/y2017/control_loops/python/drivetrain.py
+++ b/y2017/control_loops/python/drivetrain.py
@@ -31,7 +31,7 @@
# Moment of inertia of the drivetrain in kg m^2
self.J = 2.0
# Mass of the robot, in kg.
- self.m = 24
+ self.m = 50
# Radius of the robot, in meters (requires tuning by hand)
self.rb = 0.59055 / 2.0
# Radius of the wheels, in meters.
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
index 1818d62..f6a2379 100755
--- a/y2017/control_loops/python/indexer.py
+++ b/y2017/control_loops/python/indexer.py
@@ -11,7 +11,10 @@
FLAGS = gflags.FLAGS
-gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
gflags.DEFINE_bool('stall', False, 'If true, stall the indexer.')
@@ -40,8 +43,8 @@
self.G_inner = (12.0 / 48.0) * (18.0 / 36.0) * (12.0 / 84.0)
self.G_outer = (12.0 / 48.0) * (18.0 / 36.0) * (24.0 / 420.0)
- # Motor inertia in kg * m^2
- self.motor_inertia = 0.000006
+ # Motor inertia in kg m^2
+ self.motor_inertia = 0.00001187
# The output coordinate system is in radians for the inner part of the
# indexer.
@@ -51,14 +54,13 @@
self.J_inner * self.G_inner * self.G_inner +
self.J_outer * self.G_outer * self.G_outer) / (self.G_inner * self.G_inner) + \
self.motor_inertia * ((1.0 / self.G_inner) ** 2.0)
- glog.debug('J is %f', self.J)
self.G = self.G_inner
# Resistance of the motor, divided by 2 to account for the 2 motors
- self.R = 12.0 / self.stall_current
+ self.resistance = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
+ (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Control loop time step
@@ -67,9 +69,9 @@
# State feedback matrices
# [angular velocity]
self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.resistance)]])
self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.G * self.R)]])
+ [[self.Kt / (self.J * self.G * self.resistance)]])
self.C = numpy.matrix([[1]])
self.D = numpy.matrix([[0]])
@@ -77,10 +79,6 @@
self.A_continuous, self.B_continuous, self.dt)
self.PlaceControllerPoles([.75])
- glog.debug('K: %s', repr(self.K))
-
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.PlaceObserverPoles([0.3])
diff --git a/y2017/control_loops/python/intake.py b/y2017/control_loops/python/intake.py
index 712dfa4..ae209c3 100755
--- a/y2017/control_loops/python/intake.py
+++ b/y2017/control_loops/python/intake.py
@@ -99,13 +99,6 @@
glog.debug('Poles are %s',
repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.rpl = 0.30
- self.ipl = 0.10
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl])
-
- glog.debug('L is %s', repr(self.L))
-
q_pos = 0.10
q_vel = 1.65
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
diff --git a/y2017/control_loops/python/polydrivetrain.py b/y2017/control_loops/python/polydrivetrain.py
index df1f18f..f20ec2f 100755
--- a/y2017/control_loops/python/polydrivetrain.py
+++ b/y2017/control_loops/python/polydrivetrain.py
@@ -129,7 +129,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.85, 0.85])
+ self.PlaceControllerPoles([0.90, 0.90])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 0858e45..917470a 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -39,7 +39,7 @@
# Moment of inertia of the shooter wheel in kg m^2
# 1400.6 grams/cm^2
# 1.407 *1e-4 kg m^2
- self.J = 0.00080
+ self.J = 0.00120
# Resistance of the motor, divided by 2 to account for the 2 motors
self.R = 12.0 / self.stall_current
# Motor velocity constant
@@ -64,7 +64,7 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([.90])
+ self.PlaceControllerPoles([.75])
glog.debug('K %s', repr(self.K))
glog.debug('Poles are %s',
@@ -147,12 +147,12 @@
q_pos = 0.01
q_vel = 2.0
- q_voltage = 0.2
+ q_voltage = 0.3
self.Q_continuous = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0],
[0.0, 0.0, (q_voltage ** 2.0)]])
- r_pos = 0.001
+ r_pos = 0.0003
self.R_continuous = numpy.matrix([[(r_pos ** 2.0)]])
_, _, self.Q, self.R = controls.kalmd(
@@ -256,6 +256,7 @@
self.u.append(U[0, 0])
def Plot(self):
+ pylab.figure()
pylab.subplot(3, 1, 1)
pylab.plot(self.t, self.v, label='x')
pylab.plot(self.t, self.x_hat, label='x_hat')
@@ -270,7 +271,6 @@
pylab.plot(self.t, self.a, label='a')
pylab.legend()
- pylab.figure()
pylab.draw()
diff --git a/y2017/control_loops/python/turret.py b/y2017/control_loops/python/turret.py
index a69e3e1..4bfa245 100755
--- a/y2017/control_loops/python/turret.py
+++ b/y2017/control_loops/python/turret.py
@@ -21,27 +21,27 @@
def __init__(self, name='Turret'):
super(Turret, self).__init__(name)
# Stall Torque in N m
- self.stall_torque = 0.43
+ self.stall_torque = 0.71
# Stall Current in Amps
- self.stall_current = 53
- self.free_speed_rpm = 13180
+ self.stall_current = 134
+ self.free_speed_rpm = 18730.0
# Free Speed in rotations/second.
self.free_speed = self.free_speed_rpm / 60.0
# Free Current in Amps
- self.free_current = 1.8
+ self.free_current = 0.7
# Resistance of the motor
- self.R = 12.0 / self.stall_current
+ self.resistance = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
+ (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
- self.G = (1.0 / 7.0) * (1.0 / 5.0) * (16.0 / 92.0)
+ self.G = (12.0 / 60.0) * (11.0 / 94.0)
# Motor inertia in kg * m^2
- self.motor_inertia = 0.000006
+ self.motor_inertia = 0.00001187
# Moment of inertia, measured in CAD.
# Extra mass to compensate for friction is added on.
@@ -53,8 +53,8 @@
# State is [position, velocity]
# Input is [Voltage]
- C1 = self.Kt / (self.R * self.J * self.Kv * self.G * self.G)
- C2 = self.Kt / (self.J * self.R * self.G)
+ C1 = self.Kt / (self.resistance * self.J * self.Kv * self.G * self.G)
+ C2 = self.Kt / (self.J * self.resistance * self.G)
self.A_continuous = numpy.matrix(
[[0, 1],
@@ -93,10 +93,6 @@
self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- glog.debug('K %s', repr(self.K))
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
-
q_pos = 0.10
q_vel = 1.65
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
@@ -107,10 +103,7 @@
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
-
- glog.debug('Kal %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
- glog.debug('KalL is %s', repr(self.L))
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
diff --git a/y2017/control_loops/superstructure/column/BUILD b/y2017/control_loops/superstructure/column/BUILD
new file mode 100644
index 0000000..5a79dbc
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/BUILD
@@ -0,0 +1,64 @@
+genrule(
+ name = 'genrule_column',
+ cmd = '$(location //y2017/control_loops/python:column) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:column',
+ ],
+ outs = [
+ 'column_plant.h',
+ 'column_plant.cc',
+ 'column_integral_plant.h',
+ 'column_integral_plant.cc',
+ 'stuck_column_integral_plant.cc',
+ 'stuck_column_integral_plant.h',
+ ],
+)
+
+cc_library(
+ name = 'column_plants',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'column_plant.cc',
+ 'column_integral_plant.cc',
+ ],
+ hdrs = [
+ 'column_plant.h',
+ 'column_integral_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'column_zeroing',
+ srcs = [
+ 'column_zeroing.cc',
+ ],
+ hdrs = [
+ 'column_zeroing.h',
+ ],
+ deps = [
+ '//frc971/control_loops:queues',
+ '//frc971/zeroing:wrap',
+ '//frc971/zeroing:zeroing',
+ '//frc971:constants',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017:constants',
+ ],
+)
+
+cc_test(
+ name = 'column_zeroing_test',
+ srcs = [
+ 'column_zeroing_test.cc',
+ ],
+ deps = [
+ ':column_zeroing',
+ '//aos/testing:test_shm',
+ '//frc971/control_loops:position_sensor_sim',
+ '//frc971/control_loops:team_number_test_environment',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017:constants',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.cc b/y2017/control_loops/superstructure/column/column_zeroing.cc
new file mode 100644
index 0000000..e26d33e
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/column_zeroing.cc
@@ -0,0 +1,81 @@
+#include "y2017/control_loops/superstructure/column/column_zeroing.h"
+
+#include "frc971/zeroing/wrap.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace column {
+
+ColumnZeroingEstimator::ColumnZeroingEstimator(
+ const ZeroingConstants &column_constants)
+ : indexer_(column_constants.indexer_zeroing),
+ turret_(column_constants.turret_zeroing),
+ turret_zeroed_distance_(column_constants.turret_zeroed_distance) {
+ Reset();
+}
+
+void ColumnZeroingEstimator::Reset() {
+ zeroed_ = false;
+ error_ = false;
+ offset_ready_ = false;
+ indexer_.Reset();
+ turret_.Reset();
+}
+
+void ColumnZeroingEstimator::TriggerError() {
+ if (!error_) {
+ LOG(ERROR, "Manually triggered zeroing error.\n");
+ error_ = true;
+ }
+}
+
+void ColumnZeroingEstimator::UpdateEstimate(const ColumnPosition &position) {
+ indexer_.UpdateEstimate(position.indexer);
+ turret_.UpdateEstimate(position.turret);
+
+ if (indexer_.zeroed() && turret_.zeroed()) {
+ indexer_offset_ = indexer_.offset();
+
+ // Compute the current turret position.
+ const double current_turret = indexer_offset_ + position.indexer.position +
+ turret_.offset() + position.turret.position;
+
+ // Now, we can compute the turret position which is closest to 0 radians
+ // (within +- M_PI).
+ const double adjusted_turret =
+ ::frc971::zeroing::Wrap(0.0, current_turret, M_PI * 2.0);
+
+ // Now, compute the actual turret offset.
+ turret_offset_ = adjusted_turret - position.turret.position -
+ (indexer_offset_ + position.indexer.position);
+ offset_ready_ = true;
+
+ // If we are close enough to 0, we are zeroed. Otherwise, we don't know
+ // which revolution we are on and need more info. We will always report the
+ // turret position as within +- M_PI from 0 with the provided offset.
+ if (::std::abs(position.indexer.position + position.turret.position +
+ indexer_offset_ + turret_offset_) <
+ turret_zeroed_distance_) {
+ zeroed_ = true;
+ }
+
+ // TODO(austin): Latch the offset when we get zeroed for the first time and
+ // see if we get conflicting information further on.
+ }
+}
+
+ColumnZeroingEstimator::State ColumnZeroingEstimator::GetEstimatorState()
+ const {
+ State r;
+ r.error = error();
+ r.zeroed = zeroed();
+ r.indexer = indexer_.GetEstimatorState();
+ r.turret = turret_.GetEstimatorState();
+ return r;
+}
+
+} // column
+} // superstructure
+} // control_loops
+} // y2017
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.h b/y2017/control_loops/superstructure/column/column_zeroing.h
new file mode 100644
index 0000000..f2ac94f
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/column_zeroing.h
@@ -0,0 +1,74 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/zeroing/zeroing.h"
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace column {
+
+class ColumnZeroingEstimator {
+ public:
+ using ZeroingConstants = ::y2017::constants::Values::Column;
+ using SubZeroingConstants = ::frc971::constants::HallEffectZeroingConstants;
+ using State = ColumnEstimatorState;
+ using SubEstimator = ::frc971::zeroing::HallEffectAndPositionZeroingEstimator;
+
+ ColumnZeroingEstimator(const ZeroingConstants &column_constants);
+
+ void UpdateEstimate(const ColumnPosition &position);
+
+ void Reset();
+
+ void TriggerError();
+
+ bool offset_ready() const { return offset_ready_; }
+
+ bool error() const {
+ return error_ || indexer_.error() || turret_.error();
+ }
+
+ bool zeroed() const {
+ return zeroed_ && indexer_.zeroed() && turret_.zeroed();
+ }
+
+ double indexer_offset() const { return indexer_offset_; }
+ double turret_offset() const { return turret_offset_; }
+
+ // Returns information about our current state.
+ State GetEstimatorState() const;
+
+ private:
+ // We are ensuring that two subsystems are zeroed, so here they are!
+ SubEstimator indexer_, turret_;
+ // The offset in positions between the zero indexer and zero turret.
+ double indexer_offset_ = 0.0;
+ double turret_offset_ = 0.0;
+ // Marker to track whether we're fully zeroed yet or not.
+ bool zeroed_ = false;
+ // Marker to track whether an error has occurred. This gets reset to false
+ // whenever Reset() is called.
+ bool error_ = false;
+
+ // True if we have seen both edges the first time, but have not seen the
+ // region close enough to zero to be convinced which ambiguous start position
+ // we started in.
+ bool offset_ready_ = false;
+
+ // The max absolute value of the turret angle that we need to get to to be
+ // classified as zeroed. Otherwise, we may be ambiguous on which wrap we
+ // are on.
+ const double turret_zeroed_distance_;
+};
+
+} // column
+} // superstructure
+} // control_loops
+} // y2017
+
+#endif // y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
diff --git a/y2017/control_loops/superstructure/column/column_zeroing_test.cc b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
new file mode 100644
index 0000000..e6739fc
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
@@ -0,0 +1,125 @@
+#include <unistd.h>
+#include <memory>
+#include <random>
+
+#include "aos/common/die.h"
+#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "frc971/zeroing/zeroing.h"
+#include "gtest/gtest.h"
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/column/column_zeroing.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace column {
+
+using ::frc971::HallEffectAndPosition;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::zeroing::HallEffectAndPositionZeroingEstimator;
+
+class ZeroingTest : public ::testing::Test {
+ public:
+ explicit ZeroingTest()
+ : indexer_sensor_(2.0 * M_PI),
+ turret_sensor_(2.0 * M_PI),
+ column_zeroing_estimator_(constants::GetValues().column) {}
+
+ protected:
+ void SetUp() override { aos::SetDieTestMode(true); }
+ // Initializes logging and provides a tmp shmem.
+ ::aos::testing::TestSharedMemory my_shm_;
+
+ PositionSensorSimulator indexer_sensor_;
+ PositionSensorSimulator turret_sensor_;
+ ColumnZeroingEstimator column_zeroing_estimator_;
+
+ void InitializeHallEffectAndPosition(double indexer_start_position,
+ double turret_start_position) {
+ indexer_sensor_.InitializeHallEffectAndPosition(
+ indexer_start_position,
+ constants::GetValues().column.indexer_zeroing.lower_hall_position,
+ constants::GetValues().column.indexer_zeroing.upper_hall_position);
+ turret_sensor_.InitializeHallEffectAndPosition(
+ turret_start_position - indexer_start_position,
+ constants::GetValues().column.turret_zeroing.lower_hall_position,
+ constants::GetValues().column.turret_zeroing.upper_hall_position);
+ }
+
+ void MoveTo(double indexer, double turret) {
+ ColumnPosition column_position;
+ indexer_sensor_.MoveTo(indexer);
+ turret_sensor_.MoveTo(turret - indexer);
+
+ indexer_sensor_.GetSensorValues(&column_position.indexer);
+ turret_sensor_.GetSensorValues(&column_position.turret);
+
+ column_zeroing_estimator_.UpdateEstimate(column_position);
+ }
+};
+
+// Tests that starting at 0 and spinning around the indexer in a full circle
+// zeroes it (skipping the only offset_ready state).
+TEST_F(ZeroingTest, TestZeroStartingPoint) {
+ InitializeHallEffectAndPosition(0.0, 0.0);
+
+ for (double i = 0; i < 2.0 * M_PI; i += M_PI / 100) {
+ MoveTo(i, 0.0);
+ EXPECT_EQ(column_zeroing_estimator_.zeroed(),
+ column_zeroing_estimator_.offset_ready());
+ }
+ EXPECT_NEAR(column_zeroing_estimator_.indexer_offset(), 0.0, 1e-6);
+ EXPECT_NEAR(column_zeroing_estimator_.turret_offset(), 0.0, 1e-6);
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+}
+
+// Tests that starting the turret at M_PI + a bit stays offset_ready until we
+// move the turret closer to 0.0
+TEST_F(ZeroingTest, TestPiStartingPoint) {
+ constexpr double kStartingPosition = M_PI + 0.1;
+ InitializeHallEffectAndPosition(0.0, kStartingPosition);
+
+ for (double i = 0; i < 2.0 * M_PI; i += M_PI / 100) {
+ MoveTo(i, kStartingPosition);
+ EXPECT_FALSE(column_zeroing_estimator_.zeroed());
+ }
+ for (double i = kStartingPosition; i > 0.0; i -= M_PI / 100) {
+ MoveTo(2.0 * M_PI, i);
+ }
+ EXPECT_TRUE(column_zeroing_estimator_.offset_ready());
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+
+ EXPECT_NEAR(column_zeroing_estimator_.indexer_offset(), 0.0, 1e-6);
+ EXPECT_NEAR(column_zeroing_estimator_.turret_offset(), kStartingPosition,
+ 1e-6);
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+}
+
+// Tests that starting the turret at -M_PI - a bit stays offset_ready until we
+// move the turret closer to 0.0
+TEST_F(ZeroingTest, TestMinusPiStartingPoint) {
+ constexpr double kStartingPosition = -M_PI - 0.1;
+ InitializeHallEffectAndPosition(0.0, kStartingPosition);
+
+ for (double i = 0; i < 2.0 * M_PI; i += M_PI / 100) {
+ MoveTo(i, kStartingPosition);
+ EXPECT_FALSE(column_zeroing_estimator_.zeroed());
+ }
+ for (double i = kStartingPosition; i < 0.0; i += M_PI / 100) {
+ MoveTo(2.0 * M_PI, i);
+ }
+ EXPECT_TRUE(column_zeroing_estimator_.offset_ready());
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+
+ EXPECT_NEAR(column_zeroing_estimator_.indexer_offset(), 0.0, 1e-6);
+ EXPECT_NEAR(column_zeroing_estimator_.turret_offset(), kStartingPosition,
+ 1e-6);
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+}
+
+} // namespace column
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/hood/hood.cc b/y2017/control_loops/superstructure/hood/hood.cc
index b5acba0..b0c02de 100644
--- a/y2017/control_loops/superstructure/hood/hood.cc
+++ b/y2017/control_loops/superstructure/hood/hood.cc
@@ -149,7 +149,8 @@
}
// ESTOP if we hit the hard limits.
- if (profiled_subsystem_.CheckHardLimits()) {
+ if (profiled_subsystem_.CheckHardLimits() ||
+ profiled_subsystem_.error()) {
state_ = State::ESTOP;
}
} break;
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
index c66e493..9d56542 100644
--- a/y2017/control_loops/superstructure/intake/intake.cc
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -85,7 +85,8 @@
}
// ESTOP if we hit the hard limits.
- if (profiled_subsystem_.CheckHardLimits()) {
+ if (profiled_subsystem_.CheckHardLimits() ||
+ profiled_subsystem_.error()) {
state_ = State::ESTOP;
}
} break;
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index 8991658..18fc795 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -42,10 +42,7 @@
history_[history_position_] = current_position;
history_position_ = (history_position_ + 1) % kHistoryLength;
- dt_velocity_ = (current_position - last_position_) /
- chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ dt_position_ = current_position - last_position_;
last_position_ = current_position;
}
@@ -53,7 +50,7 @@
void ShooterController::Reset() { reset_ = true; }
-void ShooterController::Update(bool disabled) {
+void ShooterController::Update(bool disabled, chrono::nanoseconds dt) {
loop_->mutable_R() = loop_->next_R();
if (::std::abs(loop_->R(1, 0)) < 1.0) {
// Kill power at low angular velocities.
@@ -102,8 +99,11 @@
X_hat_current_ = loop_->X_hat();
position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
+ dt_velocity_ = dt_position_ /
+ chrono::duration_cast<chrono::duration<double>>(dt).count();
+ fixed_dt_velocity_ = dt_position_ / 0.005;
- loop_->Update(disabled);
+ loop_->Update(disabled, dt);
}
void ShooterController::SetStatus(ShooterStatus *status) {
@@ -115,13 +115,15 @@
status->voltage_error = X_hat_current_(2, 0);
status->position_error = position_error_;
status->instantaneous_velocity = dt_velocity_;
+ status->fixed_instantaneous_velocity = fixed_dt_velocity_;
}
void Shooter::Reset() { wheel_.Reset(); }
void Shooter::Iterate(const control_loops::ShooterGoal *goal,
- const double *position, double *output,
- control_loops::ShooterStatus *status) {
+ const double *position,
+ ::aos::monotonic_clock::time_point position_time,
+ double *output, control_loops::ShooterStatus *status) {
if (goal) {
// Update position/goal for our wheel.
wheel_.set_goal(goal->angular_velocity);
@@ -129,7 +131,13 @@
wheel_.set_position(*position);
- wheel_.Update(output == nullptr);
+ chrono::nanoseconds dt = ::aos::controls::kLoopFrequency;
+ if (last_time_ != ::aos::monotonic_clock::min_time) {
+ dt = position_time - last_time_;
+ }
+ last_time_ = position_time;
+
+ wheel_.Update(output == nullptr, dt);
wheel_.SetStatus(status);
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index 7dc44bf..d9b6f45 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -40,7 +40,7 @@
double error() const { return error_; }
// Executes the control loop for a cycle.
- void Update(bool disabled);
+ void Update(bool disabled, ::std::chrono::nanoseconds dt);
// Resets the kalman filter and any other internal state.
void Reset();
@@ -59,7 +59,9 @@
ptrdiff_t history_position_ = 0;
double error_ = 0.0;
+ double dt_position_ = 0.0;
double dt_velocity_ = 0.0;
+ double fixed_dt_velocity_ = 0.0;
double last_position_ = 0.0;
double average_angular_velocity_ = 0.0;
double min_velocity_ = 0.0;
@@ -83,7 +85,8 @@
// never be nullptr. goal can be nullptr if no goal exists, and output should
// be nullptr if disabled.
void Iterate(const control_loops::ShooterGoal *goal, const double *position,
- double *output, control_loops::ShooterStatus *status);
+ ::aos::monotonic_clock::time_point position_time, double *output,
+ control_loops::ShooterStatus *status);
// Sets the shooter up to reset the kalman filter next time Iterate is called.
void Reset();
@@ -93,6 +96,8 @@
bool last_ready_ = false;
double min_ = 0.0;
+ ::aos::monotonic_clock::time_point last_time_ =
+ ::aos::monotonic_clock::min_time;
DISALLOW_COPY_AND_ASSIGN(Shooter);
};
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 0b5f2d9..14beff5 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -44,19 +44,18 @@
&(position->turret),
output != nullptr ? &(output->voltage_turret) : nullptr,
&(status->turret));
-
intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
&(position->intake),
output != nullptr ? &(output->voltage_intake) : nullptr,
&(status->intake));
shooter_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->shooter) : nullptr,
- &(position->theta_shooter),
+ &(position->theta_shooter), position->sent_time,
output != nullptr ? &(output->voltage_shooter) : nullptr,
&(status->shooter));
indexer_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->indexer) : nullptr,
- &(position->theta_indexer),
- output != nullptr ? &(output->voltage_indexer) : nullptr,
- &(status->indexer));
+ &(position->theta_indexer),
+ output != nullptr ? &(output->voltage_indexer) : nullptr,
+ &(status->indexer));
if (output && unsafe_goal) {
output->voltage_intake_rollers =
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 427dadb..99989b3 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -102,11 +102,66 @@
// The current velocity measured as delta x / delta t in radians/sec.
double instantaneous_velocity;
+ double fixed_instantaneous_velocity;
// The error between our measurement and expected measurement in radians.
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;
+};
+
+struct ColumnEstimatorState {
+ bool error;
+ bool zeroed;
+ .frc971.HallEffectAndPositionEstimatorState intake;
+ .frc971.HallEffectAndPositionEstimatorState turret;
+};
+
+struct TurretProfiledSubsystemStatus {
+ // Is the subsystem zeroed?
+ bool zeroed;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ int32_t state;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // Position of the joint.
+ float position;
+ // Velocity of the joint in units/second.
+ float velocity;
+ // Profiled goal position of the joint.
+ float goal_position;
+ // Profiled goal velocity of the joint in units/second.
+ float goal_velocity;
+ // Unprofiled goal position from absoulte zero of the joint.
+ float unprofiled_goal_position;
+ // Unprofiled goal velocity of the joint in units/second.
+ float unprofiled_goal_velocity;
+
+ // The estimated voltage error.
+ float voltage_error;
+
+ // The calculated velocity with delta x/delta t
+ float calculated_velocity;
+
+ // Components of the control loop output
+ float position_power;
+ float velocity_power;
+ float feedforwards_power;
+
+ // State of the estimator.
+ ColumnEstimatorState estimator_state;
+};
+
queue_group SuperstructureQueue {
implements aos.control_loops.ControlLoop;
@@ -127,10 +182,11 @@
// Each subsystems status.
.frc971.control_loops.AbsoluteProfiledJointStatus intake;
- .frc971.control_loops.AbsoluteProfiledJointStatus turret;
.frc971.control_loops.IndexProfiledJointStatus hood;
- IndexerStatus indexer;
ShooterStatus shooter;
+
+ TurretProfiledSubsystemStatus turret;
+ IndexerStatus indexer;
};
message Position {
@@ -138,12 +194,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/control_loops/superstructure/turret/turret.cc b/y2017/control_loops/superstructure/turret/turret.cc
index 9616dff..3a3a126 100644
--- a/y2017/control_loops/superstructure/turret/turret.cc
+++ b/y2017/control_loops/superstructure/turret/turret.cc
@@ -85,7 +85,8 @@
}
// ESTOP if we hit the hard limits.
- if (profiled_subsystem_.CheckHardLimits()) {
+ if (profiled_subsystem_.CheckHardLimits() ||
+ profiled_subsystem_.error()) {
state_ = State::ESTOP;
}
} break;
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));