Merge "Analyse robot log file to understand stilts lift"
diff --git a/aos/containers/sized_array.h b/aos/containers/sized_array.h
index 6d4209a..34e9206 100644
--- a/aos/containers/sized_array.h
+++ b/aos/containers/sized_array.h
@@ -109,6 +109,8 @@
--size_;
}
+ void clear() { size_ = 0; }
+
// These allow access to the underlying storage. The data here may be outside
// the current logical extents of the container.
const array &backing_array() const { return array_; }
diff --git a/aos/containers/sized_array_test.cc b/aos/containers/sized_array_test.cc
index 04a8e41..bfaba06 100644
--- a/aos/containers/sized_array_test.cc
+++ b/aos/containers/sized_array_test.cc
@@ -135,5 +135,24 @@
}
}
+// Tests various ways of filling up and emptying.
+TEST(SizedArrayTest, FillEmpty) {
+ SizedArray<int, 2> a;
+ EXPECT_TRUE(a.empty());
+ EXPECT_FALSE(a.full());
+ a.push_back(9);
+ EXPECT_FALSE(a.empty());
+ EXPECT_FALSE(a.full());
+ a.push_back(7);
+ EXPECT_FALSE(a.empty());
+ EXPECT_TRUE(a.full());
+
+ a.clear();
+ EXPECT_TRUE(a.empty());
+ EXPECT_FALSE(a.full());
+ a.push_back(1);
+ EXPECT_EQ(1, a.back());
+}
+
} // namespace testing
} // namespace aos
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 29b2956..7661daf 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -135,7 +135,9 @@
} // namespace time
constexpr monotonic_clock::time_point monotonic_clock::min_time;
+constexpr monotonic_clock::time_point monotonic_clock::max_time;
constexpr realtime_clock::time_point realtime_clock::min_time;
+constexpr realtime_clock::time_point realtime_clock::max_time;
monotonic_clock::time_point monotonic_clock::now() noexcept {
#ifdef __linux__
diff --git a/aos/time/time.h b/aos/time/time.h
index 136629c..0333c5c 100644
--- a/aos/time/time.h
+++ b/aos/time/time.h
@@ -37,6 +37,8 @@
static constexpr time_point min_time{
time_point(duration(::std::numeric_limits<duration::rep>::min()))};
+ static constexpr time_point max_time{
+ time_point(duration(::std::numeric_limits<duration::rep>::max()))};
};
class realtime_clock {
@@ -60,6 +62,8 @@
static constexpr time_point min_time{
time_point(duration(::std::numeric_limits<duration::rep>::min()))};
+ static constexpr time_point max_time{
+ time_point(duration(::std::numeric_limits<duration::rep>::max()))};
};
namespace time {
diff --git a/aos/util/BUILD b/aos/util/BUILD
index e712d54..af6faf5 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -1,10 +1,13 @@
package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_library(
name = "bitpacking",
hdrs = [
"bitpacking.h",
],
+ compatible_with = mcu_cpus,
visibility = ["//visibility:public"],
deps = [
"//third_party/GSL",
diff --git a/aos/vision/debug/BUILD b/aos/vision/debug/BUILD
index a60f65e..c550de7 100644
--- a/aos/vision/debug/BUILD
+++ b/aos/vision/debug/BUILD
@@ -27,6 +27,7 @@
name = "debug_framework",
srcs = [
"blob_log-source.cc",
+ "aveugle-source.cc",
"camera-source.cc",
"debug_framework.cc",
"jpeg_list-source.cc",
@@ -45,6 +46,7 @@
"//aos/vision/events:gtk_event",
"//aos/vision/events:tcp_client",
"//aos/vision/image:image_stream",
+ "//aos/vision/image:image_dataset",
"//aos/vision/image:image_types",
"//aos/vision/image:jpeg_routines",
"@usr_repo//:gtk+-3.0",
diff --git a/aos/vision/debug/aveugle-source.cc b/aos/vision/debug/aveugle-source.cc
new file mode 100644
index 0000000..cbe3d0a
--- /dev/null
+++ b/aos/vision/debug/aveugle-source.cc
@@ -0,0 +1,75 @@
+#include "aos/vision/debug/debug_framework.h"
+
+#include <gdk/gdk.h>
+#include <fstream>
+#include <string>
+
+#include "aos/vision/image/camera_params.pb.h"
+#include "aos/vision/image/image_stream.h"
+
+namespace aos {
+namespace vision {
+
+class AveugleImageSource : public ImageSource {
+ public:
+ void Init(const std::string &jpeg_list_filename,
+ DebugFrameworkInterface *interface) override {
+ // TODO: These camera params make this ugly and less generic.
+ image_stream_.reset(new ImageStream(jpeg_list_filename,
+ interface->camera_params(), interface));
+ }
+
+ const char *GetHelpMessage() override {
+ return &R"(
+ format_spec is filename of the camera device.
+ example: jevois:/dev/video1
+ This viewer source will stream video from a jevois camera of your choice.
+)"[1];
+ }
+
+ class ImageStream : public ImageStreamEvent {
+ public:
+ ImageStream(const std::string &fname, aos::vision::CameraParams params,
+ DebugFrameworkInterface *interface)
+ : ImageStreamEvent(fname, params), interface_(interface) {
+ interface_->Loop()->Add(this);
+
+ interface_->InstallKeyPress([this](uint32_t keyval) {
+ // Takes a picture when you press 'a'.
+ // TODO(parker): Allow setting directory.
+ if (keyval == GDK_KEY_a) {
+ std::ofstream ofs(std::string("/tmp/debug_viewer_jpeg_") +
+ std::to_string(i_) + ".yuyv",
+ std::ofstream::out);
+ ofs << prev_data_;
+ ofs.close();
+ ++i_;
+ }
+ });
+ }
+ void ProcessImage(DataRef data, aos::monotonic_clock::time_point) override {
+ prev_data_ = std::string(data);
+ interface_->NewImage({640, 480}, [&](ImagePtr img_data) {
+ for (int y = 0; y < 480; ++y) {
+ for (int x = 0; x < 640; ++x) {
+ uint8_t v = data[y * 640 * 2 + x * 2 + 0];
+ img_data.get_px(x, y) = PixelRef{v, v, v};
+ }
+ }
+ return false;
+ });
+ }
+
+ private:
+ int i_ = 0;
+ std::string prev_data_;
+ DebugFrameworkInterface *interface_;
+ };
+
+ std::unique_ptr<ImageStream> image_stream_;
+};
+
+REGISTER_IMAGE_SOURCE("jevois", AveugleImageSource);
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/image/image_dataset.cc b/aos/vision/image/image_dataset.cc
index cd3ec46..d4dde95 100644
--- a/aos/vision/image/image_dataset.cc
+++ b/aos/vision/image/image_dataset.cc
@@ -45,6 +45,15 @@
}
} // namespace
+DatasetFrame LoadFile(const std::string &jpeg_filename) {
+ bool is_jpeg = true;
+ size_t l = jpeg_filename.size();
+ if (l > 4 && jpeg_filename[l - 1] == 'v') {
+ is_jpeg = false;
+ }
+ return DatasetFrame{is_jpeg, GetFileContents(jpeg_filename)};
+}
+
std::vector<DatasetFrame> LoadDataset(const std::string &jpeg_list_filename) {
std::vector<DatasetFrame> images;
auto contents = GetFileContents(jpeg_list_filename);
@@ -62,17 +71,10 @@
if (jpeg_filename[i] == '#') return;
if (jpeg_filename[i] != ' ') break;
}
- bool is_jpeg = true;
- size_t l = jpeg_filename.size();
- if (l > 4 && jpeg_filename[l - 1] == 'v') {
- is_jpeg = false;
- }
if (jpeg_filename[0] == '/') {
- images.emplace_back(
- DatasetFrame{is_jpeg, GetFileContents(jpeg_filename)});
+ images.emplace_back(LoadFile(jpeg_filename));
} else {
- images.emplace_back(
- DatasetFrame{is_jpeg, GetFileContents(basename + jpeg_filename)});
+ images.emplace_back(LoadFile(basename + jpeg_filename));
}
}();
}
diff --git a/aos/vision/image/image_dataset.h b/aos/vision/image/image_dataset.h
index ce8c841..7801cec 100644
--- a/aos/vision/image/image_dataset.h
+++ b/aos/vision/image/image_dataset.h
@@ -15,6 +15,8 @@
std::vector<DatasetFrame> LoadDataset(const std::string &jpeg_list_filename);
+DatasetFrame LoadFile(const std::string &jpeg_filename);
+
} // namespace vision
} // namespace aos
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 2228231..a0a07d6 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -21,7 +21,7 @@
kalman_q_vel,
kalman_q_voltage,
kalman_r_position,
- dt=0.005):
+ dt=0.00505):
self.name = name
self.motor = motor
self.G = G
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 042bd5c..fe8dd85 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -22,7 +22,7 @@
kalman_q_vel,
kalman_q_voltage,
kalman_r_position,
- dt=0.005):
+ dt=0.00505):
self.name = name
self.motor = motor
self.G = G
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index 6c4a6d1..1107602 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -122,6 +122,8 @@
: spi_(new frc::SPI(port)), dio1_(dio1) {
// 1MHz is the maximum supported for burst reads, but we
// want to go slower to hopefully make it more reliable.
+ // Note that the roboRIO's minimum supported clock rate appears to be
+ // 0.781MHz, so that's what this actually does.
spi_->SetClockRate(1e5);
spi_->SetChipSelectActiveLow();
spi_->SetClockActiveLow();
@@ -298,6 +300,8 @@
if (!message.Send()) {
LOG(WARNING, "sending queue message failed\n");
}
+
+ spi_idle_callback_();
}
}
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index a631fee..1846723 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -45,6 +45,10 @@
// readings.
void operator()();
+ void set_spi_idle_callback(std::function<void()> spi_idle_callback) {
+ spi_idle_callback_ = std::move(spi_idle_callback);
+ }
+
void Quit() { run_ = false; }
double gyro_x_zeroed_offset() const { return gyro_x_zeroed_offset_; }
@@ -89,6 +93,7 @@
frc::DigitalInput *const dio1_;
frc::DigitalOutput *reset_ = nullptr;
+ std::function<void()> spi_idle_callback_ = []() {};
::std::atomic<bool> run_{true};
// The averaged values of the gyro over 6 seconds after power up.
diff --git a/frc971/wpilib/ahal/SPI.cc b/frc971/wpilib/ahal/SPI.cc
index 5e18fc1..84a72db 100644
--- a/frc971/wpilib/ahal/SPI.cc
+++ b/frc971/wpilib/ahal/SPI.cc
@@ -17,11 +17,6 @@
#define HAL_FATAL_WITH_STATUS(status)
-/**
- * Constructor
- *
- * @param SPIport the physical SPI port
- */
SPI::SPI(Port SPIport) {
#ifdef WPILIB2017
m_port = SPIport;
@@ -37,119 +32,58 @@
HAL_Report(HALUsageReporting::kResourceType_SPI, instances);
}
-/**
- * Destructor.
- */
SPI::~SPI() { HAL_CloseSPI(m_port); }
-/**
- * Configure the rate of the generated clock signal.
- *
- * The default value is 500,000Hz.
- * The maximum value is 4,000,000Hz.
- *
- * @param hz The clock rate in Hertz.
- */
void SPI::SetClockRate(double hz) { HAL_SetSPISpeed(m_port, hz); }
-/**
- * Configure the order that bits are sent and received on the wire
- * to be most significant bit first.
- */
void SPI::SetMSBFirst() {
m_msbFirst = true;
HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
}
-/**
- * Configure the order that bits are sent and received on the wire
- * to be least significant bit first.
- */
void SPI::SetLSBFirst() {
m_msbFirst = false;
HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
}
-/**
- * Configure that the data is stable on the falling edge and the data
- * changes on the rising edge.
- */
void SPI::SetSampleDataOnFalling() {
m_sampleOnTrailing = true;
HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
}
-/**
- * Configure that the data is stable on the rising edge and the data
- * changes on the falling edge.
- */
void SPI::SetSampleDataOnRising() {
m_sampleOnTrailing = false;
HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
}
-/**
- * Configure the clock output line to be active low.
- * This is sometimes called clock polarity high or clock idle high.
- */
void SPI::SetClockActiveLow() {
m_clk_idle_high = true;
HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
}
-/**
- * Configure the clock output line to be active high.
- * This is sometimes called clock polarity low or clock idle low.
- */
void SPI::SetClockActiveHigh() {
m_clk_idle_high = false;
HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
}
-/**
- * Configure the chip select line to be active high.
- */
void SPI::SetChipSelectActiveHigh() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveHigh(m_port, &status);
HAL_FATAL_WITH_STATUS(status);
}
-/**
- * Configure the chip select line to be active low.
- */
void SPI::SetChipSelectActiveLow() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveLow(m_port, &status);
HAL_FATAL_WITH_STATUS(status);
}
-/**
- * Write data to the slave device. Blocks until there is space in the
- * output FIFO.
- *
- * If not running in output only mode, also saves the data received
- * on the MISO input during the transfer into the receive FIFO.
- */
int SPI::Write(uint8_t *data, int size) {
int retVal = 0;
retVal = HAL_WriteSPI(m_port, data, size);
return retVal;
}
-/**
- * Read a word from the receive FIFO.
- *
- * Waits for the current transfer to complete if the receive FIFO is empty.
- *
- * If the receive FIFO is empty, there is no active transfer, and initiate
- * is false, errors.
- *
- * @param initiate If true, this function pushes "0" into the transmit buffer
- * and initiates a transfer. If false, this function assumes
- * that data is already in the receive FIFO from a previous
- * write.
- */
int SPI::Read(bool initiate, uint8_t *dataReceived, int size) {
int retVal = 0;
if (initiate) {
@@ -162,13 +96,6 @@
return retVal;
}
-/**
- * Perform a simultaneous read/write transaction with the device
- *
- * @param dataToSend The data to be written out to the device
- * @param dataReceived Buffer to receive data from the device
- * @param size The length of the transaction, in bytes
- */
int SPI::Transaction(uint8_t *dataToSend, uint8_t *dataReceived, int size) {
int retVal = 0;
retVal = HAL_TransactionSPI(m_port, dataToSend, dataReceived, size);
diff --git a/frc971/wpilib/ahal/SPI.h b/frc971/wpilib/ahal/SPI.h
index c1172ce..dbac4ae 100644
--- a/frc971/wpilib/ahal/SPI.h
+++ b/frc971/wpilib/ahal/SPI.h
@@ -36,23 +36,66 @@
SPI(const SPI &) = delete;
SPI &operator=(const SPI &) = delete;
+ // Configure the rate of the generated clock signal.
+ //
+ // The claimed default value is 500,000Hz, and the claimed maximum value is
+ // 4,000,000Hz.
+ //
+ // This appears to have a very inflexible clocking setup. You can get 0.781MHz
+ // or 1.563MHz, but nothing in between. At least it rounds down the requested
+ // value like it should... 0.781MHz also appears to be the minimum.
void SetClockRate(double hz);
+ // Configure the order that bits are sent and received on the wire
+ // to be most significant bit first.
void SetMSBFirst();
+ // Configure the order that bits are sent and received on the wire
+ // to be least significant bit first.
void SetLSBFirst();
+ // Configure that the data is stable on the falling edge and the data
+ // changes on the rising edge.
void SetSampleDataOnFalling();
+ // Configure that the data is stable on the rising edge and the data
+ // changes on the falling edge.
void SetSampleDataOnRising();
+ // Configure the clock output line to be active low.
+ // This is sometimes called clock polarity high or clock idle high.
void SetClockActiveLow();
+ // Configure the clock output line to be active high.
+ // This is sometimes called clock polarity low or clock idle low.
void SetClockActiveHigh();
+ // Configure the chip select line to be active high.
void SetChipSelectActiveHigh();
+ // Configure the chip select line to be active low.
void SetChipSelectActiveLow();
- virtual int Write(uint8_t *data, int size);
- virtual int Read(bool initiate, uint8_t *dataReceived, int size);
- virtual int Transaction(uint8_t *dataToSend, uint8_t *dataReceived, int size);
+ // Write data to the slave device. Blocks until there is space in the
+ // output FIFO.
+ //
+ // If not running in output only mode, also saves the data received
+ // on the MISO input during the transfer into the receive FIFO.
+ int Write(uint8_t *data, int size);
+ // Read a word from the receive FIFO.
+ //
+ // Waits for the current transfer to complete if the receive FIFO is empty.
+ //
+ // If the receive FIFO is empty, there is no active transfer, and initiate
+ // is false, errors.
+ //
+ // @param initiate If true, this function pushes "0" into the transmit buffer
+ // and initiates a transfer. If false, this function assumes
+ // that data is already in the receive FIFO from a previous
+ // write.
+ int Read(bool initiate, uint8_t *dataReceived, int size);
+ // Perform a simultaneous read/write transaction with the device
+ //
+ // @param dataToSend The data to be written out to the device
+ // @param dataReceived Buffer to receive data from the device
+ // @param size The length of the transaction, in bytes
+ int Transaction(uint8_t *dataToSend, uint8_t *dataReceived, int size);
protected:
#ifdef WPILIB2017
diff --git a/y2016/control_loops/python/intake.py b/y2016/control_loops/python/intake.py
index ceb4170..6d0509d 100755
--- a/y2016/control_loops/python/intake.py
+++ b/y2016/control_loops/python/intake.py
@@ -25,6 +25,7 @@
J=0.34 - 0.03757568,
q_pos=0.20,
q_vel=5.0,
+ dt=0.005,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
kalman_q_voltage=4.0,
diff --git a/y2019/constants.cc b/y2019/constants.cc
index d77d2c7..e2d54b3 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -110,7 +110,7 @@
stilts_params->zeroing_voltage = 3.0;
stilts_params->operating_voltage = 12.0;
stilts_params->zeroing_profile_params = {0.1, 0.5};
- stilts_params->default_profile_params = {0.1, 0.5};
+ stilts_params->default_profile_params = {0.15, 0.5};
stilts_params->range = Values::kStiltsRange();
stilts_params->make_integral_loop =
&control_loops::superstructure::stilts::MakeIntegralStiltsLoop;
@@ -138,8 +138,8 @@
break;
case kCompTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.019470;
- elevator->potentiometer_offset = -0.075017;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.024407;
+ elevator->potentiometer_offset = -0.075017 + 0.015837 + 0.009793 - 0.012017;
intake->zeroing_constants.measured_absolute_position = 1.860016;
@@ -151,8 +151,8 @@
break;
case kPracticeTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.160767;
- elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.167722;
+ elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355 - 0.006497;
intake->zeroing_constants.measured_absolute_position = 1.756847;
diff --git a/y2019/constants.h b/y2019/constants.h
index 24d8b92..a3af890 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -93,7 +93,7 @@
static constexpr ::frc971::constants::Range kElevatorRange() {
return ::frc971::constants::Range{
- -0.01, // Bottom Hard
+ -0.02, // Bottom Hard
1.62, // Top Hard
0.01, // Bottom Soft
1.59 // Top Soft
diff --git a/y2019/control_loops/python/elevator.py b/y2019/control_loops/python/elevator.py
index 480ed50..5859814 100755
--- a/y2019/control_loops/python/elevator.py
+++ b/y2019/control_loops/python/elevator.py
@@ -29,8 +29,7 @@
kalman_q_pos=0.12,
kalman_q_vel=2.00,
kalman_q_voltage=35.0,
- kalman_r_position=0.05,
- dt=0.00505)
+ kalman_r_position=0.05)
kElevatorModel = copy.copy(kElevator)
kElevatorModel.mass = carriage_mass + first_stage_mass + 1.0
diff --git a/y2019/control_loops/python/stilts.py b/y2019/control_loops/python/stilts.py
index b07321c..2c4644d 100755
--- a/y2019/control_loops/python/stilts.py
+++ b/y2019/control_loops/python/stilts.py
@@ -27,8 +27,7 @@
kalman_q_pos=0.12,
kalman_q_vel=2.00,
kalman_q_voltage=35.0,
- kalman_r_position=0.05,
- dt=0.00505)
+ kalman_r_position=0.05)
def main(argv):
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 21acb2e..d4c1c6b 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -20,16 +20,17 @@
filtered_pressure_ = kSuctionAlpha * suction_pressure +
(1 - kSuctionAlpha) * filtered_pressure_;
- *has_piece = filtered_pressure_ < kVacuumThreshold;
+ const bool new_has_piece = filtered_pressure_ < kVacuumThreshold;
- if (*has_piece && !had_piece_) {
+ if (new_has_piece && !had_piece_) {
time_at_last_acquisition_ = monotonic_now;
}
+ *has_piece =
+ monotonic_now > time_at_last_acquisition_ + kTimeAtHigherVoltage &&
+ new_has_piece;
// if we've had the piece for enought time, go to lower pump_voltage
- low_pump_voltage =
- *has_piece &&
- monotonic_now > time_at_last_acquisition_ + kTimeAtHigherVoltage;
+ low_pump_voltage = *has_piece;
no_goal_for_a_bit =
monotonic_now > time_at_last_evacuate_goal_ + kTimeToKeepPumpRunning;
@@ -49,7 +50,7 @@
output->intake_suction_top = unsafe_goal->top;
output->intake_suction_bottom = unsafe_goal->bottom;
}
- had_piece_ = *has_piece;
+ had_piece_ = new_has_piece;
}
} // namespace superstructure
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
index 3fd5a49..d2acf97 100644
--- a/y2019/control_loops/superstructure/vacuum.h
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -20,7 +20,7 @@
static constexpr double kPumpVoltage = 8.0;
// Voltage to the vaccum pump when we have a piece
- static constexpr double kPumpHasPieceVoltage = 2.0;
+ static constexpr double kPumpHasPieceVoltage = 2.25;
// Time to continue at the higher pump voltage after getting a gamepiece
static constexpr aos::monotonic_clock::duration kTimeAtHigherVoltage =
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 9304e8e..97a1d61 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -37,6 +37,7 @@
"--output=$(location jevois_crc.c)",
]),
]),
+ compatible_with = mcu_cpus,
tools = [
"//third_party/pycrc:pycrc_main",
],
@@ -50,6 +51,7 @@
hdrs = [
"jevois_crc.h",
],
+ compatible_with = mcu_cpus,
deps = [
"//third_party/GSL",
],
@@ -69,6 +71,19 @@
)
cc_library(
+ name = "structures_mcu",
+ hdrs = [
+ "structures.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//aos/containers:sized_array",
+ "//aos/time:time_mcu",
+ "//third_party/eigen",
+ ],
+)
+
+cc_library(
name = "spi",
srcs = [
"spi.cc",
@@ -88,6 +103,24 @@
)
cc_library(
+ name = "spi_mcu",
+ srcs = [
+ "spi.cc",
+ ],
+ hdrs = [
+ "spi.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":jevois_crc",
+ ":structures_mcu",
+ "//aos/util:bitpacking",
+ "//third_party/GSL",
+ "//third_party/optional",
+ ],
+)
+
+cc_library(
name = "uart",
srcs = [
"uart.cc",
@@ -108,6 +141,26 @@
],
)
+cc_library(
+ name = "uart_mcu",
+ srcs = [
+ "uart.cc",
+ ],
+ hdrs = [
+ "uart.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":cobs_mcu",
+ ":jevois_crc",
+ ":structures_mcu",
+ "//aos/containers:sized_array",
+ "//aos/util:bitpacking",
+ "//third_party/GSL",
+ "//third_party/optional",
+ ],
+)
+
cc_test(
name = "uart_test",
srcs = [
@@ -141,6 +194,17 @@
],
)
+cc_library(
+ name = "cobs_mcu",
+ hdrs = [
+ "cobs.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//third_party/GSL",
+ ],
+)
+
cc_test(
name = "cobs_test",
srcs = [
diff --git a/y2019/jevois/camera/BUILD b/y2019/jevois/camera/BUILD
new file mode 100644
index 0000000..e6e33c5
--- /dev/null
+++ b/y2019/jevois/camera/BUILD
@@ -0,0 +1,23 @@
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+ name = "reader",
+ srcs = ["reader.cc"],
+ hdrs = ["reader.h"],
+ deps = [
+ "//aos/vision/image:reader",
+ "//aos/vision/image:camera_params",
+ "//aos/vision/image:image_types",
+ "//aos/logging",
+ "//aos/time",
+ ],
+)
+
+cc_library(
+ name = "image_stream",
+ hdrs = ["image_stream.h"],
+ deps = [
+ "//aos/vision/events:epoll_events",
+ ":reader",
+ ],
+)
diff --git a/y2019/jevois/camera/image_stream.h b/y2019/jevois/camera/image_stream.h
new file mode 100644
index 0000000..62661c6
--- /dev/null
+++ b/y2019/jevois/camera/image_stream.h
@@ -0,0 +1,55 @@
+#ifndef _AOS_VISION_IMAGE_IMAGE_STREAM_H_
+#define _AOS_VISION_IMAGE_IMAGE_STREAM_H_
+
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/camera_params.pb.h"
+#include "y2019/jevois/camera/reader.h"
+
+#include <memory>
+
+namespace y2019 {
+namespace camera {
+
+// Converts a camera reader into a virtual base class that calls ProcessImage
+// on each new image.
+class ImageStreamEvent : public ::aos::events::EpollEvent {
+ public:
+ static std::unique_ptr<Reader> GetCamera(const std::string &fname,
+ ImageStreamEvent *obj,
+ aos::vision::CameraParams params) {
+ using namespace std::placeholders;
+ std::unique_ptr<Reader> camread(new Reader(
+ fname, std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2),
+ params));
+ camread->StartAsync();
+ return camread;
+ }
+
+ explicit ImageStreamEvent(std::unique_ptr<Reader> reader)
+ : ::aos::events::EpollEvent(reader->fd()), reader_(std::move(reader)) {}
+
+ explicit ImageStreamEvent(const std::string &fname,
+ aos::vision::CameraParams params)
+ : ImageStreamEvent(GetCamera(fname, this, params)) {}
+
+ void ProcessHelper(aos::vision::DataRef data,
+ aos::monotonic_clock::time_point timestamp) {
+ if (data.size() < 300) {
+ LOG(INFO, "got bad img of size(%d)\n", static_cast<int>(data.size()));
+ return;
+ }
+ ProcessImage(data, timestamp);
+ }
+ virtual void ProcessImage(aos::vision::DataRef data,
+ aos::monotonic_clock::time_point timestamp) = 0;
+
+ void ReadEvent() override { reader_->HandleFrame(); }
+
+ private:
+ std::unique_ptr<Reader> reader_;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_DEBUG_IMAGE_STREAM_H_
diff --git a/y2019/jevois/camera/reader.cc b/y2019/jevois/camera/reader.cc
new file mode 100644
index 0000000..25b5a3f
--- /dev/null
+++ b/y2019/jevois/camera/reader.cc
@@ -0,0 +1,303 @@
+#include "y2019/jevois/camera/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/logging/logging.h"
+#include "aos/time/time.h"
+
+#define CLEAR(x) memset(&(x), 0, sizeof(x))
+
+namespace y2019 {
+namespace camera {
+
+using ::camera::xioctl;
+
+struct Reader::Buffer {
+ void *start;
+ 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,
+ 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) {
+ PLOG(FATAL, "Cannot identify '%s'", dev_name.c_str());
+ }
+ if (!S_ISCHR(st.st_mode)) {
+ PLOG(FATAL, "%s is no device\n", dev_name.c_str());
+ }
+
+ fd_ = open(dev_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
+ if (fd_ == -1) {
+ PLOG(FATAL, "Cannot open '%s'", dev_name.c_str());
+ }
+
+ Init();
+
+ InitMMap();
+ LOG(INFO, "Bat Vision Successfully Initialized.\n");
+}
+
+void Reader::QueueBuffer(v4l2_buffer *buf) {
+ if (xioctl(fd_, VIDIOC_QBUF, buf) == -1) {
+ PLOG(WARNING,
+ "ioctl VIDIOC_QBUF(%d, %p)."
+ " losing buf #%" PRIu32 "\n",
+ fd_, &buf, buf->index);
+ } else {
+ ++queued_;
+ }
+}
+
+void Reader::HandleFrame() {
+ v4l2_buffer buf;
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+
+ if (xioctl(fd_, VIDIOC_DQBUF, &buf) == -1) {
+ if (errno != EAGAIN) {
+ PLOG(ERROR, "ioctl VIDIOC_DQBUF(%d, %p)", fd_, &buf);
+ }
+ return;
+ }
+ --queued_;
+
+ ++tick_id_;
+ // Get a timestamp now as proxy for when the image was taken
+ // TODO(ben): the image should come with a timestamp, parker
+ // will know how to get it.
+ auto time = aos::monotonic_clock::now();
+
+ process_(aos::vision::DataRef(
+ reinterpret_cast<const char *>(buffers_[buf.index].start),
+ buf.bytesused),
+ time);
+
+ QueueBuffer(&buf);
+}
+
+void Reader::MMapBuffers() {
+ buffers_ = new Buffer[kNumBuffers];
+ v4l2_buffer buf;
+ for (unsigned int n = 0; n < kNumBuffers; ++n) {
+ memset(&buf, 0, sizeof(buf));
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = n;
+ if (xioctl(fd_, VIDIOC_QUERYBUF, &buf) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_QUERYBUF(%d, %p)", fd_, &buf);
+ }
+ buffers_[n].length = buf.length;
+ buffers_[n].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
+ MAP_SHARED, fd_, buf.m.offset);
+ if (buffers_[n].start == MAP_FAILED) {
+ PLOG(FATAL,
+ "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)",
+ (size_t)buf.length, fd_, static_cast<intmax_t>(buf.m.offset));
+ }
+ }
+}
+
+void Reader::InitMMap() {
+ v4l2_requestbuffers req;
+ CLEAR(req);
+ req.count = kNumBuffers;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+ if (xioctl(fd_, VIDIOC_REQBUFS, &req) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s does not support memory mapping\n", dev_name_.c_str());
+ } else {
+ PLOG(FATAL, "ioctl VIDIOC_REQBUFS(%d, %p)\n", fd_, &req);
+ }
+ }
+ queued_ = kNumBuffers;
+ if (req.count != kNumBuffers) {
+ LOG(FATAL, "Insufficient buffer memory on %s\n", dev_name_.c_str());
+ }
+}
+
+// Sets one of the camera's user-control values.
+// Prints the old and new values.
+// Just prints a message if the camera doesn't support this control or value.
+bool Reader::SetCameraControl(uint32_t id, const char *name, int value) {
+ struct v4l2_control getArg = {id, 0U};
+ int r;
+ // Used to be: r = xioctl(fd_, VIDIOC_S_CTRL, &getArg);
+ // Jevois wants this incorrect number below:.
+ r = xioctl(fd_, 0xc00c561b, &getArg);
+ if (r == 0) {
+ if (getArg.value == value) {
+ LOG(DEBUG, "Camera control %s was already %d\n", name, getArg.value);
+ return true;
+ }
+ } else if (errno == EINVAL) {
+ LOG(DEBUG, "Camera control %s is invalid\n", name);
+ errno = 0;
+ return false;
+ }
+
+ struct v4l2_control setArg = {id, value};
+ // Should be: r = xioctl(fd_, VIDIOC_S_CTRL, &setArg);
+ // Jevois wants this incorrect number below:.
+ r = xioctl(fd_, 0xc00c561c, &setArg);
+ if (r == 0) {
+ LOG(DEBUG, "Set camera control %s from %d to %d\n", name, getArg.value,
+ value);
+ return true;
+ }
+
+ LOG(DEBUG, "Couldn't set camera control %s to %d", name, value);
+ errno = 0;
+ return false;
+}
+
+void Reader::Init() {
+ v4l2_capability cap;
+ if (xioctl(fd_, VIDIOC_QUERYCAP, &cap) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s is no V4L2 device\n", dev_name_.c_str());
+ } else {
+ PLOG(FATAL, "ioctl VIDIOC_QUERYCAP(%d, %p)", fd_, &cap);
+ }
+ }
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+ LOG(FATAL, "%s is no video capture device\n", dev_name_.c_str());
+ }
+ if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+ LOG(FATAL, "%s does not support streaming i/o\n", dev_name_.c_str());
+ }
+
+ int camidx = -1;
+ struct v4l2_input inp = {};
+ while (true) {
+ if (xioctl(fd_, VIDIOC_ENUMINPUT, &inp) == -1) {
+ break;
+ }
+ if (inp.type == V4L2_INPUT_TYPE_CAMERA) {
+ if (camidx == -1) camidx = inp.index;
+ printf("Input %d [ %s ] is a camera sensor\n", inp.index, inp.name);
+ } else
+ printf("Input %d [ %s ] is not a camera sensor\n", inp.index, inp.name);
+ ++inp.index;
+ }
+
+ if (xioctl(fd_, VIDIOC_S_INPUT, &camidx) == -1) {
+ LOG(FATAL, "ioctl VIDIOC_S_INPUT(%d) failed with %d: %s\n", fd_, errno,
+ strerror(errno));
+ }
+ printf("camera idx: %d\n", camidx);
+
+ /* Select video input, video standard and tune here. */
+
+ v4l2_format fmt;
+ CLEAR(fmt);
+
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_G_FMT, &fmt) == -1) {
+ LOG(FATAL, "ioctl VIDIC_G_FMT(%d, %p) failed with %d: %s\n", fd_, &fmt,
+ errno, strerror(errno));
+ }
+
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.width = params_.width();
+ fmt.fmt.pix.height = params_.height();
+ printf("setting format: %d, %d\n", params_.width(), params_.height());
+ fmt.fmt.pix.field = V4L2_FIELD_NONE;
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+ if (xioctl(fd_, VIDIOC_S_FMT, &fmt) == -1) {
+ LOG(FATAL, "ioctl VIDIC_S_FMT(%d, %p) failed with %d: %s\n", fd_, &fmt,
+ errno, strerror(errno));
+ }
+ /* Note VIDIOC_S_FMT may change width and height. */
+
+ /* Buggy driver paranoia. */
+ unsigned int min = fmt.fmt.pix.width * 2;
+ if (fmt.fmt.pix.bytesperline < min) fmt.fmt.pix.bytesperline = min;
+ min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
+ if (fmt.fmt.pix.sizeimage < min) fmt.fmt.pix.sizeimage = min;
+
+ // set framerate
+ struct v4l2_streamparm *setfps;
+ setfps = (struct v4l2_streamparm *)calloc(1, sizeof(struct v4l2_streamparm));
+ 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();
+ if (xioctl(fd_, VIDIOC_S_PARM, setfps) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_S_PARM(%d, %p)\n", fd_, setfps);
+ }
+ LOG(INFO, "framerate ended up at %d/%d\n",
+ setfps->parm.capture.timeperframe.numerator,
+ setfps->parm.capture.timeperframe.denominator);
+
+ for (int j = 0; j < 2; ++j) {
+ if (!SetCameraControl(V4L2_CID_EXPOSURE_AUTO, "V4L2_CID_EXPOSURE_AUTO",
+ V4L2_EXPOSURE_MANUAL)) {
+ LOG(FATAL, "Failed to set exposure\n");
+ }
+
+ if (!SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
+ "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure())) {
+ LOG(FATAL, "Failed to set exposure\n");
+ }
+ sleep(1);
+ }
+}
+
+aos::vision::ImageFormat Reader::get_format() {
+ struct v4l2_format fmt;
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_G_FMT, &fmt) == -1) {
+ PLOG(FATAL, "ioctl VIDIC_G_FMT(%d, %p)\n", fd_, &fmt);
+ }
+
+ return aos::vision::ImageFormat{(int)fmt.fmt.pix.width,
+ (int)fmt.fmt.pix.height};
+}
+
+void Reader::Start() {
+ LOG(DEBUG, "queueing buffers for the first time\n");
+ v4l2_buffer buf;
+ for (unsigned int i = 0; i < kNumBuffers; ++i) {
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+ QueueBuffer(&buf);
+ }
+ LOG(DEBUG, "done with first queue\n");
+
+ v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_STREAMON, &type) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_STREAMON(%d, %p)\n", fd_, &type);
+ }
+}
+
+} // namespace camera
+} // namespace y2019
diff --git a/y2019/jevois/camera/reader.h b/y2019/jevois/camera/reader.h
new file mode 100644
index 0000000..53d83c7
--- /dev/null
+++ b/y2019/jevois/camera/reader.h
@@ -0,0 +1,70 @@
+#ifndef AOS_VISION_IMAGE_READER_H_
+#define AOS_VISION_IMAGE_READER_H_
+
+#include <inttypes.h>
+#include <functional>
+#include <string>
+
+#include "aos/time/time.h"
+#include "aos/vision/image/V4L2.h"
+#include "aos/vision/image/camera_params.pb.h"
+#include "aos/vision/image/image_types.h"
+
+namespace y2019 {
+namespace camera {
+
+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,
+ aos::vision::CameraParams params);
+
+ aos::vision::ImageFormat get_format();
+
+ void HandleFrame();
+ void StartAsync() {
+ MMapBuffers();
+ Start();
+ }
+ int fd() { return fd_; }
+
+ private:
+ void QueueBuffer(v4l2_buffer *buf);
+ void InitMMap();
+ bool SetCameraControl(uint32_t id, const char *name, int value);
+ void Init();
+ void Start();
+ void MMapBuffers();
+ // File descriptor of the camera
+ int fd_;
+ // Name of the camera device.
+ std::string dev_name_;
+
+ ProcessCb process_;
+
+ int tick_id_ = 0;
+ // The number of buffers currently queued in v4l2.
+ uint32_t queued_;
+ struct Buffer;
+ // TODO(parker): This should be a smart pointer, but it cannot
+ // because the buffers are not ummapped.
+ Buffer *buffers_;
+
+ // TODO(parker): The timestamps should be queue insertion timestamps
+ // which will remove the impact of kNumBuffers.
+ // TODO(parker): Flush the queue (or tweak the FPS) if we fall behind.
+ static const unsigned int kNumBuffers = 5;
+
+ // set only at initialize
+ aos::vision::CameraParams params_;
+};
+
+} // namespace camera
+} // namespace y2019
+
+#endif // AOS_VISION_IMAGE_READER_H_
diff --git a/y2019/jevois/cobs.h b/y2019/jevois/cobs.h
index 22590d9..112873d 100644
--- a/y2019/jevois/cobs.h
+++ b/y2019/jevois/cobs.h
@@ -6,8 +6,13 @@
#include <algorithm>
#include <array>
-#include "aos/logging/logging.h"
#include "third_party/GSL/include/gsl/gsl"
+#ifdef __linux__
+#include "aos/logging/logging.h"
+#else
+#define CHECK(...)
+#define CHECK_LE(...)
+#endif
// This file contains code for encoding and decoding Consistent Overhead Byte
// Stuffing data. <http://www.stuartcheshire.org/papers/cobsforton.pdf> has
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
index 28df8cf..abc1780 100644
--- a/y2019/jevois/spi.cc
+++ b/y2019/jevois/spi.cc
@@ -1,9 +1,14 @@
#include "y2019/jevois/spi.h"
-#include "aos/logging/logging.h"
#include "aos/util/bitpacking.h"
#include "third_party/GSL/include/gsl/gsl"
#include "y2019/jevois/jevois_crc.h"
+#ifdef __linux__
+#include "aos/logging/logging.h"
+#else
+#define CHECK(...)
+#define CHECK_GE(...)
+#endif
// SPI transfer format (6x 8 bit frames):
// 1. 1-byte brightness for each beacon channel.
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index fcc268e..28da405 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -11,6 +11,7 @@
#include "aos/containers/sized_array.h"
#include "aos/time/time.h"
+#include "third_party/GSL/include/gsl/gsl"
namespace frc971 {
namespace jevois {
diff --git a/y2019/jevois/uart.cc b/y2019/jevois/uart.cc
index e3520a4..3621da8 100644
--- a/y2019/jevois/uart.cc
+++ b/y2019/jevois/uart.cc
@@ -2,10 +2,15 @@
#include <array>
-#include "aos/logging/logging.h"
#include "aos/util/bitpacking.h"
#include "third_party/GSL/include/gsl/gsl"
#include "y2019/jevois/jevois_crc.h"
+#ifdef __linux__
+#include "aos/logging/logging.h"
+#else
+#define CHECK(...)
+#define CHECK_GE(...)
+#endif
namespace frc971 {
namespace jevois {
@@ -50,8 +55,7 @@
return result;
}
-tl::optional<Frame> UartUnpackToTeensy(
- const UartToTeensyBuffer &encoded_buffer) {
+tl::optional<Frame> UartUnpackToTeensy(gsl::span<const char> encoded_buffer) {
std::array<char, uart_to_teensy_size()> buffer;
if (static_cast<size_t>(
CobsDecode<uart_to_teensy_size()>(encoded_buffer, &buffer).size()) !=
@@ -138,7 +142,7 @@
}
tl::optional<CameraCalibration> UartUnpackToCamera(
- const UartToCameraBuffer &encoded_buffer) {
+ gsl::span<const char> encoded_buffer) {
std::array<char, uart_to_camera_size()> buffer;
if (static_cast<size_t>(
CobsDecode<uart_to_camera_size()>(encoded_buffer, &buffer).size()) !=
diff --git a/y2019/jevois/uart.h b/y2019/jevois/uart.h
index 8244fdb..2a4acec 100644
--- a/y2019/jevois/uart.h
+++ b/y2019/jevois/uart.h
@@ -2,6 +2,7 @@
#define Y2019_JEVOIS_UART_H_
#include "aos/containers/sized_array.h"
+#include "third_party/GSL/include/gsl/gsl"
#include "third_party/optional/tl/optional.hpp"
#include "y2019/jevois/cobs.h"
#include "y2019/jevois/structures.h"
@@ -29,11 +30,11 @@
aos::SizedArray<char, CobsMaxEncodedSize(uart_to_camera_size())>;
UartToTeensyBuffer UartPackToTeensy(const Frame &message);
-tl::optional<Frame> UartUnpackToTeensy(const UartToTeensyBuffer &buffer);
+tl::optional<Frame> UartUnpackToTeensy(gsl::span<const char> buffer);
UartToCameraBuffer UartPackToCamera(const CameraCalibration &message);
tl::optional<CameraCalibration> UartUnpackToCamera(
- const UartToCameraBuffer &buffer);
+ gsl::span<const char> buffer);
} // namespace jevois
} // namespace frc971
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 17cc45f..310f41f 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -45,7 +45,7 @@
const ButtonLocation kElevatorBack5(4, 6);
const ButtonLocation kElevatorIntaking(3, 4);
-const ButtonLocation kElevatorIntakingUp(3, 6);
+const ButtonLocation kElevatorOuttake(3, 6);
const ButtonLocation kRelease(4, 4);
const ButtonLocation kSuctionBall(3, 13);
@@ -186,10 +186,14 @@
}
// TODO(sabina): max height please?
- if (data.IsPressed(kDeployStilt)) {
- new_superstructure_goal->stilts.unsafe_goal = 0.50;
- } else if (data.IsPressed(kFallOver)) {
+ if (data.IsPressed(kFallOver)) {
new_superstructure_goal->stilts.unsafe_goal = 0.71;
+ new_superstructure_goal->stilts.profile_params.max_velocity = 0.45;
+ new_superstructure_goal->stilts.profile_params.max_acceleration = 0.5;
+ } else if (data.IsPressed(kDeployStilt)) {
+ new_superstructure_goal->stilts.unsafe_goal = 0.50;
+ new_superstructure_goal->stilts.profile_params.max_velocity = 0.45;
+ new_superstructure_goal->stilts.profile_params.max_acceleration = 0.5;
} else {
new_superstructure_goal->stilts.unsafe_goal = 0.01;
}
@@ -239,14 +243,18 @@
wrist_angle_ = 0.0;
elevator_height_ = 0.36;
}
- //if (data.IsPressed(kElevatorIntaking)) {
- //}
+
+ if (data.IsPressed(kElevatorOuttake) ||
+ (data.IsPressed(kIntakeOut) &&
+ !superstructure_queue.status->has_piece)) {
+ new_superstructure_goal->intake.unsafe_goal = 0.959327;
+ }
+
if (data.IsPressed(kIntakeOut) && !superstructure_queue.status->has_piece) {
elevator_height_ = 0.29;
wrist_angle_ = 2.14;
- new_superstructure_goal->intake.unsafe_goal = 0.52;
if (data.IsPressed(kElevatorIntaking)) {
- new_superstructure_goal->roller_voltage = 6.0;
+ new_superstructure_goal->roller_voltage = 9.0;
} else {
new_superstructure_goal->roller_voltage = 0.0;
}
@@ -256,9 +264,29 @@
new_superstructure_goal->roller_voltage = 0.0;
}
- if (data.IsPressed(kElevatorIntakingUp)) {
- elevator_height_ = 0.29 + 0.3;
- wrist_angle_ = 2.14;
+ if (data.IsPressed(kElevatorOuttake)) {
+ new_superstructure_goal->roller_voltage = -6.0;
+ }
+
+ if (data.IsPressed(kElevatorBack1)) {
+ wrist_angle_ = -2.451824;
+ elevator_height_ = 0.430478;
+ //new_superstructure_goal->wrist.profile_params.max_velocity = 2.0;
+ //new_superstructure_goal->wrist.profile_params.max_acceleration = 20.0;
+ }
+ if (data.IsPressed(kElevatorBack2)) {
+ wrist_angle_ = -2.400;
+ elevator_height_ = 0.364108;
+ new_superstructure_goal->elevator.profile_params.max_velocity = 2.0;
+ new_superstructure_goal->elevator.profile_params.max_acceleration = 5.0;
+ new_superstructure_goal->wrist.profile_params.max_velocity = 0.35;
+ new_superstructure_goal->wrist.profile_params.max_acceleration = 10.0;
+ }
+ if (data.IsPressed(kElevatorBack3)) {
+ wrist_angle_ = -2.211173;
+ elevator_height_ = 0.25;
+ new_superstructure_goal->wrist.profile_params.max_velocity = 2.0;
+ new_superstructure_goal->wrist.profile_params.max_acceleration = 10.0;
}
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index f727d4b..bdef187 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -10,6 +10,12 @@
]
cc_library(
+ name = "constants",
+ hdrs = ["constants.h"],
+ srcs = ["constants.cc"],
+)
+
+cc_library(
name = "target_finder",
srcs = [
"target_finder.cc",
@@ -21,6 +27,7 @@
],
restricted_to = VISION_TARGETS,
deps = [
+ ":constants",
"//aos/vision/blob:contour",
"//aos/vision/blob:hierarchical_contour_merge",
"//aos/vision/blob:region_alloc",
@@ -60,19 +67,30 @@
"//aos/vision/events:epoll_events",
"//aos/vision/events:socket_types",
"//aos/vision/events:udp",
- "//aos/vision/image:image_stream",
- "//aos/vision/image:reader",
"//y2019/jevois:serial",
"//y2019/jevois:structures",
"//y2019/jevois:uart",
+ "//y2019/jevois/camera:reader",
+ "//y2019/jevois/camera:image_stream",
"@com_google_ceres_solver//:ceres",
],
)
-"""
cc_binary(
- name = "calibration",
- srcs = ["calibration.cc"],
+ name = "debug_serial",
+ srcs = ["debug_serial.cc"],
+ deps = [
+ "//y2019/jevois:serial",
+ "//y2019/jevois:structures",
+ "//y2019/jevois:uart",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ ],
+)
+
+cc_binary(
+ name = "global_calibration",
+ srcs = ["global_calibration.cc"],
deps = [
":target_finder",
"//aos/logging",
@@ -83,9 +101,9 @@
"//aos/vision/events:socket_types",
"//aos/vision/events:udp",
"//aos/vision/image:image_stream",
+ "//aos/vision/image:image_dataset",
"//aos/vision/image:reader",
"@com_google_ceres_solver//:ceres",
],
restricted_to = VISION_TARGETS,
)
-"""
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
new file mode 100644
index 0000000..bf3953c
--- /dev/null
+++ b/y2019/vision/constants.cc
@@ -0,0 +1,33 @@
+#include "y2019/vision/constants.h"
+
+namespace y2019 {
+namespace vision {
+
+constexpr double kInchesToMeters = 0.0254;
+
+CameraCalibration camera_4 = {
+ {
+ 3.50309 / 180.0 * M_PI, 593.557, -0.0487739 / 180.0 * M_PI,
+ },
+ {
+ {{5.56082 / kInchesToMeters, 4.70235 / kInchesToMeters,
+ 33.4998 / kInchesToMeters}},
+ 22.2155 * M_PI / 180.0,
+ },
+ {
+ 4,
+ {{12.5 / kInchesToMeters, 12.0 / kInchesToMeters}},
+ {{kInchesToMeters, 0.0}},
+ 26.0,
+ "cam4_0/debug_viewer_jpeg_",
+ }};
+
+const CameraCalibration *GetCamera(int camera_id) {
+ switch (camera_id) {
+ case 4: return &camera_4;
+ default: return nullptr;
+ }
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/constants.h b/y2019/vision/constants.h
new file mode 100644
index 0000000..cbad8bf
--- /dev/null
+++ b/y2019/vision/constants.h
@@ -0,0 +1,78 @@
+#ifndef _Y2019_VISION_CONSTANTS_H_
+#define _Y2019_VISION_CONSTANTS_H_
+
+#include <math.h>
+#include <array>
+#include <string>
+
+namespace y2019 {
+namespace vision {
+
+// Position of the idealized camera in 3d space.
+struct CameraGeometry {
+ // In Meters from floor under imu center.
+ std::array<double, 3> location{{0, 0, 0}};
+ double heading = 0.0;
+
+ void set(double *data) {
+ location[0] = data[0];
+ location[1] = data[1];
+ location[2] = data[2];
+ heading = data[3];
+ }
+ static CameraGeometry get(const double *data) {
+ CameraGeometry out;
+ out.location[0] = data[0];
+ out.location[1] = data[1];
+ out.location[2] = data[2];
+ out.heading = data[3];
+ return out;
+ }
+};
+
+struct IntrinsicParams {
+ static constexpr size_t kNumParams = 3;
+
+ double mount_angle = 0.819433 / 180.0 * M_PI; // 9.32615 / 180.0 * M_PI;
+ double focal_length = 666.763; // 734.328;
+ // This is a final rotation where the camera isn't straight.
+ double barrel_mount = 2.72086 / 180.0 * M_PI;
+
+ void set(double *data) {
+ data[0] = mount_angle;
+ data[1] = focal_length;
+ data[2] = barrel_mount;
+ }
+ static IntrinsicParams get(const double *data) {
+ IntrinsicParams out;
+ out.mount_angle = data[0];
+ out.focal_length = data[1];
+ out.barrel_mount = data[2];
+ return out;
+ }
+};
+
+// Metadata about the calibration results (Should be good enough to reproduce).
+struct DatasetInfo {
+ int camera_id;
+ // In meters from IMU start.
+ std::array<double, 2> to_tape_measure_start;
+ // In meters,
+ std::array<double, 2> tape_measure_direction;
+ // This will multiply tape_measure_direction and thus has no units.
+ double beginning_tape_measure_reading;
+ const char *filename_prefix;
+};
+
+struct CameraCalibration {
+ IntrinsicParams intrinsics;
+ CameraGeometry geometry;
+ DatasetInfo dataset;
+};
+
+const CameraCalibration *GetCamera(int camera_id);
+
+} // namespace vision
+} // namespace y2019
+
+#endif // _Y2019_VISION_CONSTANTS_H_
diff --git a/y2019/vision/debug_serial.cc b/y2019/vision/debug_serial.cc
new file mode 100644
index 0000000..a095b14
--- /dev/null
+++ b/y2019/vision/debug_serial.cc
@@ -0,0 +1,96 @@
+#include "y2019/jevois/serial.h"
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/logging.h"
+#include "y2019/jevois/cobs.h"
+#include "y2019/jevois/serial.h"
+#include "y2019/jevois/structures.h"
+#include "y2019/jevois/uart.h"
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <thread>
+
+namespace y2019 {
+namespace vision {
+
+void main(int argc, char **argv) {
+ (void)argc;
+ (void)argv;
+ using namespace y2019::vision;
+ using namespace frc971::jevois;
+ // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stderr));
+
+ int flags = fcntl(0, F_GETFL, 0);
+ fcntl(0, F_SETFL, flags | O_NONBLOCK);
+
+ int itsDev = ::y2019::jevois::open_via_terminos("/dev/ttyUSB0");
+
+ CobsPacketizer<uart_to_teensy_size()> cobs;
+ while (true) {
+ {
+ constexpr size_t kBufferSize = uart_to_teensy_size();
+ char data[kBufferSize];
+ ssize_t n = read(itsDev, &data[0], kBufferSize);
+ if (n >= 1) {
+ cobs.ParseData(gsl::span<const char>(&data[0], n));
+ auto packet = cobs.received_packet();
+ if (!packet.empty()) {
+ // One we read data from the serial, Teensy code will return an
+ // optional with success if there is a new frame. unwrap that
+ // and print out any frames from inside.
+ auto frame_optional = UartUnpackToTeensy(packet);
+ if (frame_optional) {
+ printf("----------\n");
+ const auto &frame = *frame_optional;
+ for (const auto &target : frame.targets) {
+ printf("z: %g y: %g, r1: %g, r2: %g\n", target.distance / 0.0254,
+ target.height / 0.0254, target.skew, target.heading);
+ }
+ } else {
+ printf("bad frame\n");
+ }
+ cobs.clear_received_packet();
+ }
+ }
+ }
+ {
+ constexpr size_t kBufferSize = 1024;
+ char data[kBufferSize];
+ // read command char from stdin. 'p' = passthrough else usb.
+ ssize_t n = read(0, &data[0], kBufferSize);
+ if (n >= 1) {
+ CameraCalibration calibration{};
+ if (data[0] == 'p') {
+ calibration.camera_command =
+ CameraCalibration::CameraCommand::kCameraPassthrough;
+ } else {
+ calibration.camera_command = CameraCalibration::CameraCommand::kUsb;
+ }
+ if (write(itsDev, "\0", 1) == 1) {
+ const auto out_data = frc971::jevois::UartPackToCamera(calibration);
+ // We don't really care if this succeeds or not. If it fails for some
+ // reason, we'll just try again with the next frame, and the other end
+ // will find the new packet just fine.
+ if (static_cast<int>(
+ write(itsDev, out_data.data(), out_data.size())) !=
+ static_cast<int>(out_data.size())) {
+ printf("ERROR!!! START OVER FROM FIRST PRINCIPLES!!\n");
+ }
+ }
+ }
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(2));
+ }
+}
+
+} // namespace y2019
+} // namespace vision
+
+int main(int argc, char **argv) { y2019::vision::main(argc, argv); }
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
new file mode 100644
index 0000000..e92ae56
--- /dev/null
+++ b/y2019/vision/global_calibration.cc
@@ -0,0 +1,216 @@
+#include <fstream>
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/logging.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_dataset.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/reader.h"
+#include "y2019/vision/target_finder.h"
+
+#undef CHECK_NOTNULL
+#undef CHECK_OP
+#undef PCHECK
+// CERES Clashes with logging symbols...
+#include "ceres/ceres.h"
+
+using ::aos::events::DataSocket;
+using ::aos::events::RXUdpSocket;
+using ::aos::events::TCPServer;
+using ::aos::vision::DataRef;
+using ::aos::vision::Int32Codec;
+using ::aos::monotonic_clock;
+using aos::vision::Segment;
+
+using ceres::NumericDiffCostFunction;
+using ceres::CENTRAL;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+namespace y2019 {
+namespace vision {
+
+constexpr double kInchesToMeters = 0.0254;
+
+template <size_t k, size_t n, size_t n2, typename T>
+T *MakeFunctor(T &&t) {
+ return new T(std::move(t));
+}
+
+std::array<double, 3> GetError(const double *const extrinsics,
+ const double *const geometry, int i) {
+ auto ex = ExtrinsicParams::get(extrinsics);
+
+ double s = sin(geometry[2] + ex.r2);
+ double c = cos(geometry[2] + ex.r2);
+
+ // TODO: Generalize this from being just for a single calibration.
+ double dx = 12.5 + 26.0 + i - (geometry[0] + c * ex.z) / kInchesToMeters;
+ double dy = 12.0 - (geometry[1] + s * ex.z) / kInchesToMeters;
+ double dz = 28.5 - (geometry[3] + ex.y) / kInchesToMeters;
+ return {{dx, dy, dz}};
+}
+
+void main(int argc, char **argv) {
+ (void)argc;
+ (void)argv;
+ using namespace y2019::vision;
+ // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stderr));
+
+ TargetFinder finder_;
+
+ ceres::Problem problem;
+
+ struct Sample {
+ std::unique_ptr<double[]> extrinsics;
+ int i;
+ };
+ std::vector<Sample> all_extrinsics;
+ double intrinsics[IntrinsicParams::kNumParams];
+ IntrinsicParams().set(&intrinsics[0]);
+
+ // To know the meaning, see the printout below...
+ constexpr size_t GeometrykNumParams = 4;
+ double geometry[GeometrykNumParams] = {0, 0, 0, 0};
+
+ Solver::Options options;
+ options.minimizer_progress_to_stdout = false;
+ Solver::Summary summary;
+
+ std::cout << summary.BriefReport() << "\n";
+ auto intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
+ std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ std::cout << "error = " << summary.final_cost << ";\n";
+
+ int nimgs = 56;
+ for (int i = 0; i < nimgs; ++i) {
+ auto frame = aos::vision::LoadFile(
+ "/home/parker/data/frc/2019_calibration/cam4_0/debug_viewer_jpeg_" +
+ std::to_string(i) + ".yuyv");
+
+ aos::vision::ImageFormat fmt{640, 480};
+ aos::vision::BlobList imgs = aos::vision::FindBlobs(
+ aos::vision::DoThresholdYUYV(fmt, frame.data.data(), 120));
+ finder_.PreFilter(&imgs);
+
+ bool verbose = false;
+ std::vector<std::vector<Segment<2>>> raw_polys;
+ for (const RangeImage &blob : imgs) {
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
+ if (polygon.empty()) {
+ } else {
+ raw_polys.push_back(polygon);
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list =
+ finder_.FindTargetsFromComponents(target_component_list, verbose);
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ auto target_value = target.toPointList();
+ auto template_value = finder_.GetTemplateTarget().toPointList();
+
+ auto *extrinsics = new double[ExtrinsicParams::kNumParams];
+ ExtrinsicParams().set(extrinsics);
+ all_extrinsics.push_back({std::unique_ptr<double[]>(extrinsics), i});
+
+ for (size_t j = 0; j < 8; ++j) {
+ auto temp = template_value[j];
+ auto targ = target_value[j];
+
+ auto ftor = [temp, targ, i](const double *const intrinsics,
+ const double *const extrinsics,
+ double *residual) {
+ auto in = IntrinsicParams::get(intrinsics);
+ auto ex = ExtrinsicParams::get(extrinsics);
+ auto pt = targ - Project(temp, in, ex);
+ residual[0] = pt.x();
+ residual[1] = pt.y();
+ return true;
+ };
+ problem.AddResidualBlock(
+ new NumericDiffCostFunction<decltype(ftor), CENTRAL, 2,
+ IntrinsicParams::kNumParams,
+ ExtrinsicParams::kNumParams>(
+ new decltype(ftor)(std::move(ftor))),
+ NULL, &intrinsics[0], extrinsics);
+ }
+
+ auto ftor = [i](const double *const extrinsics,
+ const double *const geometry, double *residual) {
+ auto err = GetError(extrinsics, geometry, i);
+ residual[0] = 32.0 * err[0];
+ residual[1] = 32.0 * err[1];
+ residual[2] = 32.0 * err[2];
+ return true;
+ };
+
+ problem.AddResidualBlock(
+ new NumericDiffCostFunction<decltype(ftor), CENTRAL, 3,
+ ExtrinsicParams::kNumParams,
+ GeometrykNumParams>(
+ new decltype(ftor)(std::move(ftor))),
+ NULL, extrinsics, &geometry[0]);
+ }
+ }
+ // TODO: Debug solver convergence?
+ Solve(options, &problem, &summary);
+ Solve(options, &problem, &summary);
+ Solve(options, &problem, &summary);
+
+ {
+ std::cout << summary.BriefReport() << "\n";
+ auto intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
+ std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ std::cout << "error = " << summary.final_cost << ";\n";
+
+ std::cout << "camera_height = " << geometry[3] / kInchesToMeters << "\n";
+ std::cout << "camera_angle = " << geometry[2] * 180 / M_PI << "\n";
+ std::cout << "camera_x = " << geometry[0] / kInchesToMeters << "\n";
+ std::cout << "camera_y = " << geometry[1] / kInchesToMeters << "\n";
+ std::cout << "camera_barrel = " << intrinsics_.barrel_mount * 180.0 / M_PI
+ << "\n";
+
+ for (auto &sample : all_extrinsics) {
+ int i = sample.i;
+ double *data = sample.extrinsics.get();
+
+ auto extn = ExtrinsicParams::get(data);
+
+ auto err = GetError(data, &geometry[0], i);
+
+ std::cout << i << ", ";
+ std::cout << extn.z / kInchesToMeters << ", ";
+ std::cout << extn.y / kInchesToMeters << ", ";
+ std::cout << extn.r1 * 180 / M_PI << ", ";
+ std::cout << extn.r2 * 180 / M_PI << ", ";
+ // TODO: Methodology problem: This should really have a separate solve for
+ // extrinsics...
+ std::cout << err[0] << ", ";
+ std::cout << err[1] << ", ";
+ std::cout << err[2] << "\n";
+ }
+ }
+}
+
+} // namespace y2019
+} // namespace vision
+
+int main(int argc, char **argv) { y2019::vision::main(argc, argv); }
diff --git a/y2019/vision/notes.txt b/y2019/vision/notes.txt
new file mode 100644
index 0000000..5c05b07
--- /dev/null
+++ b/y2019/vision/notes.txt
@@ -0,0 +1,5 @@
+This file is a list of unpolished notes.
+
+To convert a jevois that has been flashed over to be a passthrough:
+
+mkdir -p JEVOIS/modules/JeVois/; cp JEVOIS1/modules/JeVois/PassThrough/ JEVOIS/modules/JeVois/ -r; mkdir -p JEVOIS/lib/JeVois/; cp JEVOIS1/lib/JeVois/libjevoisbase.so JEVOIS/lib/JeVois/; mkdir -p JEVOIS/deploy; chmod 777 JEVOIS/deploy; cp /backups/no_backup/971-code-lab/971-Robot-Code/y2019/vision/tools/launch.sh JEVOIS/deploy/
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 77adc83..1c1fb99 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -58,6 +58,7 @@
double r1 = extrinsics.r1;
double r2 = extrinsics.r2;
double rup = intrinsics.mount_angle;
+ double rbarrel = intrinsics.barrel_mount;
double fl = intrinsics.focal_length;
::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
@@ -93,6 +94,11 @@
pts.transpose();
}
+ // TODO: Maybe barrel should be extrinsics to allow rocking?
+ // Also, in this case, barrel should go above the rotation above?
+ pts = ::Eigen::AngleAxis<double>(rbarrel, ::Eigen::Vector3d(0.0, 0.0, 1.0)) *
+ pts.transpose();
+
// TODO: Final image projection.
::Eigen::Matrix<double, 1, 3> res = pts;
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index a4762bb..be56e91 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -6,8 +6,8 @@
#include "aos/vision/blob/find_blob.h"
#include "aos/vision/events/socket_types.h"
#include "aos/vision/events/udp.h"
-#include "aos/vision/image/image_stream.h"
-#include "aos/vision/image/reader.h"
+#include "y2019/jevois/camera/image_stream.h"
+#include "y2019/jevois/camera/reader.h"
#include "y2019/jevois/serial.h"
#include "y2019/jevois/structures.h"
@@ -23,7 +23,7 @@
using ::y2019::jevois::open_via_terminos;
using aos::vision::Segment;
-class CameraStream : public ::aos::vision::ImageStreamEvent {
+class CameraStream : public ::y2019::camera::ImageStreamEvent {
public:
CameraStream(::aos::vision::CameraParams params, const ::std::string &fname)
: ImageStreamEvent(fname, params) {}
@@ -31,22 +31,16 @@
void ProcessImage(DataRef data, monotonic_clock::time_point monotonic_now) {
LOG(INFO, "got frame: %d\n", (int)data.size());
- static unsigned i = 0;
-
- /*
- std::ofstream ofs(std::string("/jevois/data/debug_viewer_jpeg_") +
- std::to_string(i) + ".yuyv",
- std::ofstream::out);
- ofs << data;
- ofs.close();
- */
- if (on_frame) on_frame(data, monotonic_now);
- ++i;
-
- if (i == 200) exit(-1);
+ if (on_frame_) on_frame_(data, monotonic_now);
}
- std::function<void(DataRef, monotonic_clock::time_point)> on_frame;
+ void set_on_frame(const std::function<
+ void(DataRef, monotonic_clock::time_point)> &on_frame) {
+ on_frame_ = on_frame;
+ }
+
+ private:
+ std::function<void(DataRef, monotonic_clock::time_point)> on_frame_;
};
int open_terminos(const char *tty_name) { return open_via_terminos(tty_name); }
@@ -66,24 +60,238 @@
exit(-1);
}
+using aos::vision::ImageRange;
+using aos::vision::RangeImage;
+using aos::vision::ImageFormat;
+
+#define MASH(v0, v1, v2, v3, v4) \
+ ((uint8_t(v0) << 4) | (uint8_t(v1) << 3) | (uint8_t(v2) << 2) | \
+ (uint8_t(v3) << 1) | (uint8_t(v4)))
+
+// YUYV image types:
+inline RangeImage DoThresholdYUYV(ImageFormat fmt, const char *data,
+ uint8_t value) {
+ std::vector<std::vector<ImageRange>> ranges;
+ ranges.reserve(fmt.h);
+ for (int y = 0; y < fmt.h; ++y) {
+ const char *row = fmt.w * y * 2 + data;
+ bool p_score = false;
+ int pstart = -1;
+ std::vector<ImageRange> rngs;
+ for (int x = 0; x < fmt.w / 4; ++x) {
+ uint8_t v[8];
+ memcpy(&v[0], row + x * 4 * 2, 8);
+ uint8_t pattern =
+ MASH(p_score, v[0] > value, v[2] > value, v[4] > value, v[6] > value);
+ switch (pattern) {
+ /*
+# Ruby code to generate the below code:
+32.times do |v|
+ puts "case MASH(#{[v[4], v[3], v[2], v[1], v[0]].join(", ")}):"
+ p_score = v[4]
+ pstart = "pstart"
+ 4.times do |i|
+ if v[3 - i] != p_score
+ if (p_score == 1)
+ puts " rngs.emplace_back(ImageRange(#{pstart},
+x * 4 + #{i}));"
+ else
+ pstart = "x * 4 + #{i}"
+ end
+ p_score = v[3 - i]
+ end
+ end
+ if (pstart != "pstart")
+ puts " pstart = #{pstart};"
+ end
+ if (p_score != v[4])
+ puts " p_score = #{["false", "true"][v[0]]};"
+ end
+ puts " break;"
+end
+*/
+ case MASH(0, 0, 0, 0, 0):
+ break;
+ case MASH(0, 0, 0, 0, 1):
+ pstart = x * 4 + 3;
+ p_score = true;
+ break;
+ case MASH(0, 0, 0, 1, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
+ pstart = x * 4 + 2;
+ break;
+ case MASH(0, 0, 0, 1, 1):
+ pstart = x * 4 + 2;
+ p_score = true;
+ break;
+ case MASH(0, 0, 1, 0, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
+ pstart = x * 4 + 1;
+ break;
+ case MASH(0, 0, 1, 0, 1):
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
+ pstart = x * 4 + 3;
+ p_score = true;
+ break;
+ case MASH(0, 0, 1, 1, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 3));
+ pstart = x * 4 + 1;
+ break;
+ case MASH(0, 0, 1, 1, 1):
+ pstart = x * 4 + 1;
+ p_score = true;
+ break;
+ case MASH(0, 1, 0, 0, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
+ pstart = x * 4 + 0;
+ break;
+ case MASH(0, 1, 0, 0, 1):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
+ pstart = x * 4 + 3;
+ p_score = true;
+ break;
+ case MASH(0, 1, 0, 1, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
+ rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
+ pstart = x * 4 + 2;
+ break;
+ case MASH(0, 1, 0, 1, 1):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
+ pstart = x * 4 + 2;
+ p_score = true;
+ break;
+ case MASH(0, 1, 1, 0, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 2));
+ pstart = x * 4 + 0;
+ break;
+ case MASH(0, 1, 1, 0, 1):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 2));
+ pstart = x * 4 + 3;
+ p_score = true;
+ break;
+ case MASH(0, 1, 1, 1, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 3));
+ pstart = x * 4 + 0;
+ break;
+ case MASH(0, 1, 1, 1, 1):
+ pstart = x * 4 + 0;
+ p_score = true;
+ break;
+ case MASH(1, 0, 0, 0, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ p_score = false;
+ break;
+ case MASH(1, 0, 0, 0, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ pstart = x * 4 + 3;
+ break;
+ case MASH(1, 0, 0, 1, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
+ pstart = x * 4 + 2;
+ p_score = false;
+ break;
+ case MASH(1, 0, 0, 1, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ pstart = x * 4 + 2;
+ break;
+ case MASH(1, 0, 1, 0, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
+ pstart = x * 4 + 1;
+ p_score = false;
+ break;
+ case MASH(1, 0, 1, 0, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
+ pstart = x * 4 + 3;
+ break;
+ case MASH(1, 0, 1, 1, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 3));
+ pstart = x * 4 + 1;
+ p_score = false;
+ break;
+ case MASH(1, 0, 1, 1, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ pstart = x * 4 + 1;
+ break;
+ case MASH(1, 1, 0, 0, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
+ p_score = false;
+ break;
+ case MASH(1, 1, 0, 0, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
+ pstart = x * 4 + 3;
+ break;
+ case MASH(1, 1, 0, 1, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
+ rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
+ pstart = x * 4 + 2;
+ p_score = false;
+ break;
+ case MASH(1, 1, 0, 1, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
+ pstart = x * 4 + 2;
+ break;
+ case MASH(1, 1, 1, 0, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 2));
+ p_score = false;
+ break;
+ case MASH(1, 1, 1, 0, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 2));
+ pstart = x * 4 + 3;
+ break;
+ case MASH(1, 1, 1, 1, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 3));
+ p_score = false;
+ break;
+ case MASH(1, 1, 1, 1, 1):
+ break;
+ }
+
+ for (int i = 0; i < 4; ++i) {
+ if ((v[i * 2] > value) != p_score) {
+ if (p_score) {
+ rngs.emplace_back(ImageRange(pstart, x * 4 + i));
+ } else {
+ pstart = x * 4 + i;
+ }
+ p_score = !p_score;
+ }
+ }
+ }
+ if (p_score) {
+ rngs.emplace_back(ImageRange(pstart, fmt.w));
+ }
+ ranges.push_back(rngs);
+ }
+ return RangeImage(0, std::move(ranges));
+}
+
+#undef MASH
+
int main(int argc, char **argv) {
(void)argc;
(void)argv;
using namespace y2019::vision;
+ using frc971::jevois::CameraCalibration;
// gflags::ParseCommandLineFlags(&argc, &argv, false);
::aos::logging::Init();
::aos::logging::AddImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
int itsDev = open_terminos("/dev/ttyS0");
- //dup2(itsDev, 1);
- //dup2(itsDev, 2);
+ frc971::jevois::CobsPacketizer<frc971::jevois::uart_to_camera_size()> cobs;
+ // Uncomment these to printf directly to stdout to get debug info...
+ // dup2(itsDev, 1);
+ // dup2(itsDev, 2);
TargetFinder finder_;
// Check that our current results match possible solutions.
aos::vision::CameraParams params0;
- params0.set_exposure(0);
+ params0.set_exposure(400);
params0.set_brightness(40);
params0.set_width(640);
// params0.set_fps(10);
@@ -91,11 +299,11 @@
::std::unique_ptr<CameraStream> camera0(
new CameraStream(params0, "/dev/video0"));
- camera0->on_frame = [&](DataRef data,
- monotonic_clock::time_point /*monotonic_now*/) {
+ camera0->set_on_frame([&](DataRef data,
+ monotonic_clock::time_point monotonic_now) {
aos::vision::ImageFormat fmt{640, 480};
- aos::vision::BlobList imgs = aos::vision::FindBlobs(
- aos::vision::DoThresholdYUYV(fmt, data.data(), 120));
+ aos::vision::BlobList imgs =
+ aos::vision::FindBlobs(::DoThresholdYUYV(fmt, data.data(), 120));
finder_.PreFilter(&imgs);
bool verbose = false;
@@ -124,24 +332,70 @@
results = finder_.FilterResults(results);
+ // TODO: Select top 3 (randomly?)
+
+ frc971::jevois::Frame frame{};
+
+ for (size_t i = 0; i < results.size() && i < frame.targets.max_size();
+ ++i) {
+ const auto &result = results[i].extrinsics;
+ frame.targets.push_back(frc971::jevois::Target{
+ static_cast<float>(result.z), static_cast<float>(result.y),
+ static_cast<float>(result.r2), static_cast<float>(result.r1)});
+ }
+
+ frame.age = std::chrono::duration_cast<frc971::jevois::camera_duration>(
+ aos::monotonic_clock::now() - monotonic_now);
+
// If we succeed in writing our delimiter, then write out the rest of the
// frame. If not, no point in continuing.
if (write(itsDev, "\0", 1) == 1) {
- frc971::jevois::Frame frame{};
- // TODO(Parker): Fill out frame appropriately.
const auto serialized_frame = frc971::jevois::UartPackToTeensy(frame);
// We don't really care if this succeeds or not. If it fails for some
// reason, we'll just try again with the next frame, and the other end
// will find the new packet just fine.
- (void)write(itsDev, serialized_frame.data(), serialized_frame.size());
+ ssize_t n =
+ write(itsDev, serialized_frame.data(), serialized_frame.size());
+
+ if (n != (ssize_t)serialized_frame.size()) {
+ LOG(INFO, "Some problem happened");
+ }
}
- };
+ });
aos::events::EpollLoop loop;
- for (int i = 0; i < 100; ++i) {
- std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ while (true) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
camera0->ReadEvent();
+
+ {
+ constexpr size_t kBufferSize = frc971::jevois::uart_to_teensy_size();
+ char data[kBufferSize];
+ ssize_t n = read(itsDev, &data[0], kBufferSize);
+ if (n >= 1) {
+ cobs.ParseData(gsl::span<const char>(&data[0], n));
+ auto packet = cobs.received_packet();
+ if (!packet.empty()) {
+ auto calibration_question =
+ frc971::jevois::UartUnpackToCamera(packet);
+ if (calibration_question) {
+ const auto &calibration = *calibration_question;
+ switch (calibration.camera_command) {
+ case CameraCalibration::CameraCommand::kNormal:
+ break;
+ case CameraCalibration::CameraCommand::kUsb:
+ return 0;
+ case CameraCalibration::CameraCommand::kCameraPassthrough:
+ return system("touch /tmp/do_not_export_sd_card");
+ }
+ } else {
+ printf("bad frame\n");
+ }
+ cobs.clear_received_packet();
+ }
+ }
+ }
}
// TODO: Fix event loop on jevois:
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index 557b04a..a20ecf7 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -3,6 +3,7 @@
#include "aos/vision/math/segment.h"
#include "aos/vision/math/vector.h"
+#include "y2019/vision/constants.h"
namespace y2019 {
namespace vision {
@@ -42,24 +43,6 @@
std::array<aos::vision::Vector<2>, 8> toPointList() const;
};
-struct IntrinsicParams {
- static constexpr size_t kNumParams = 2;
-
- double mount_angle = 10.0481 / 180.0 * M_PI;
- double focal_length = 729.445;
-
- void set(double *data) {
- data[0] = mount_angle;
- data[1] = focal_length;
- }
- static IntrinsicParams get(const double *data) {
- IntrinsicParams out;
- out.mount_angle = data[0];
- out.focal_length = data[1];
- return out;
- }
-};
-
struct ExtrinsicParams {
static constexpr size_t kNumParams = 4;
diff --git a/y2019/vision/tools/Jevois_fstab b/y2019/vision/tools/Jevois_fstab
new file mode 100644
index 0000000..6b44012
--- /dev/null
+++ b/y2019/vision/tools/Jevois_fstab
@@ -0,0 +1,9 @@
+/dev/root / ext4 ro,flush,noatime,noauto 0 0
+devpts /dev/pts devpts defaults,gid=5,mode=620 0 0
+tmpfs /dev/shm tmpfs mode=0777 0 0
+tmpfs /tmp tmpfs mode=1777 0 0
+tmpfs /run tmpfs mode=0755,nosuid,nodev 0 0
+sysfs /sys sysfs defaults 0 0
+debugfs /sys/kernel/debug debugfs defaults 0 0
+/dev/mmcblk0p1 /boot vfat ro,flush,noatime,umask=000 0 0
+/dev/mmcblk0p3 /jevois ext4 rw,noatime 0 0
diff --git a/y2019/vision/tools/austin_cam.sh b/y2019/vision/tools/austin_cam.sh
new file mode 100755
index 0000000..063b7dc
--- /dev/null
+++ b/y2019/vision/tools/austin_cam.sh
@@ -0,0 +1,142 @@
+#!/bin/sh
+
+##############################################################################################################
+# Default settings:
+##############################################################################################################
+
+CAMERA=ov9650
+if [ -f /boot/sensor ]; then CAMERA=`tail -1 /boot/sensor`; fi
+
+use_usbserial=1 # Allow using a serial-over-USB to communicate with JeVois command-line interface
+use_usbsd=1 # Allow exposing the JEVOIS partition of the microSD as a USB drive
+use_serialtty=0 # Use a TTY on the hardware serial and do not use it in jevois-daemon
+use_usbserialtty=0 # Use a TTY on the serial-over-USB and do not use it in jevois-daemon
+use_maxbandwidth=1 # Use 100% of isochronous bandwidth to minimize latency; disable to connect several cameras
+use_quietcmd=0 # Do not say OK after every good command when true
+use_nopython=0 # Disable python to save memory when true
+
+if [ -f /boot/nousbserial ]; then use_usbserial=0; echo "JeVois serial-over-USB disabled"; fi
+if [ -f /boot/nousbsd ]; then use_usbsd=0; echo "JeVois microSD access over USB disabled"; fi
+if [ -f /boot/multicam ]; then use_maxbandwidth=0; echo "JeVois multi-camera mode enabled"; fi
+if [ -f /boot/quietcmd ]; then use_quietcmd=1; echo "JeVois quiet command-line mode enabled"; fi
+if [ -f /boot/nopython ]; then use_nopython=1; echo "JeVois python support disabled"; fi
+
+# Block device we present to the host as a USB drive, or empty to not present it at start:
+usbsdfile=""
+if [ -f /boot/usbsdauto ]; then usbsdfile="/dev/mmcblk0p3"; echo "JeVois microSD access over USB is AUTO"; fi
+
+# Login prompts over hardware serial or serial-over-USB:
+if [ -f /boot/serialtty ]; then use_serialtty=1; echo "Using tty on JeVois hardware serial"; fi
+if [ "X${use_usbserial}" != "X1" ]; then use_usbserialtty=0;
+elif [ -f /boot/usbserialtty ]; then use_usbserialtty=1; echo "Using tty on JeVois serial-over-USB"; fi
+
+##############################################################################################################
+# Load all required kernel modules:
+##############################################################################################################
+
+cd /lib/modules/3.4.39
+
+for m in videobuf-core videobuf-dma-contig videodev vfe_os vfe_subdev v4l2-common v4l2-int-device \
+ cci ${CAMERA} vfe_v4l2 ump disp mali ; do
+ if [ $m = "vfe_v4l2" ]; then
+ echo "### insmod ${m}.ko sensor=${CAMERA} ###"
+ insmod ${m}.ko sensor="${CAMERA}"
+ else
+ echo "### insmod ${m}.ko ###"
+ insmod ${m}.ko
+ fi
+done
+
+##############################################################################################################
+# Install any new packages:
+##############################################################################################################
+
+cd /jevois
+for f in packages/*.jvpkg; do
+ if [ -f "${f}" ]; then
+ echo "### Installing package ${f} ###"
+ bzcat "${f}" | tar xvf -
+ sync
+ rm -f "${f}"
+ sync
+ fi
+done
+
+##############################################################################################################
+# Find any newly unpacked postinstall scripts, run them, and delete them:
+##############################################################################################################
+
+for f in modules/*/*/postinstall; do
+ if [ -f "${f}" ]; then
+ echo "### Running ${f} ###"
+ d=`dirname "${f}"`
+ cd "${d}"
+ sh postinstall
+ sync
+ rm -f postinstall
+ sync
+ cd /jevois
+ fi
+done
+
+##############################################################################################################
+# Build a default videomappings.cfg, if missing:
+##############################################################################################################
+
+if [ ! -f /jevois/config/videomappings.cfg ]; then
+ echo 'YUYV 640 360 30.0 YUYV 320 240 30.0 JeVois JeVoisIntro *' > /jevois/config/videomappings.cfg
+ echo 'YUYV 640 480 30.0 YUYV 320 240 30.0 JeVois JeVoisIntro' >> /jevois/config/videomappings.cfg
+fi
+
+##############################################################################################################
+# Get a list of all our needed library paths:
+##############################################################################################################
+
+LIBPATH="/lib:/usr/lib"
+for d in `find /jevois/lib -type d -print`; do LIBPATH="${LIBPATH}:${d}"; done
+export LD_LIBRARY_PATH=${LIBPATH}
+
+##############################################################################################################
+# Insert the gadget driver:
+##############################################################################################################
+
+echo "### Insert gadget driver ###"
+MODES=`/usr/bin/jevois-module-param ${CAMERA}`
+
+echo $MODES
+
+insmodopts=""
+if [ "X${use_usbsd}" = "X1" ]; then insmodopts="${insmodopts} file=${usbsdfile}"; fi
+
+insmod /lib/modules/3.4.39/g_jevoisa33.ko modes=${MODES} use_serial=${use_usbserial} \
+ use_storage=${use_usbsd} max_bandwidth=${use_maxbandwidth} ${insmodopts}
+echo insmod /lib/modules/3.4.39/g_jevoisa33.ko modes=${MODES} use_serial=${use_usbserial} \
+ use_storage=${use_usbsd} max_bandwidth=${use_maxbandwidth} ${insmodopts}
+
+##############################################################################################################
+# Launch jevois-daemon:
+##############################################################################################################
+
+/jevois/deploy/launch.sh
+
+echo "### Start jevois daemon ###"
+opts=""
+if [ "X${use_usbserial}" != "X1" -o "X${use_usbserialtty}" = "X1" ]; then opts="${opts} --usbserialdev="; fi
+if [ "X${use_serialtty}" = "X1" ]; then opts="${opts} --serialdev="; fi
+if [ "X${use_maxbandwidth}" != "X1" ]; then opts="${opts} --multicam=1"; fi
+if [ "X${use_quietcmd}" = "X1" ]; then opts="${opts} --quietcmd=1"; fi
+if [ "X${use_nopython}" = "X1" ]; then opts="${opts} --python=0"; fi
+if [ "X${CAMERA}" != "X" ]; then opts="${opts} --camerasens=${CAMERA}"; fi
+
+if [ ! -f /tmp/do_not_export_sd_card ]; then
+ sync
+ echo "### FALLBACK remount ###"
+ mount -o remount,ro /jevois || exit
+ echo "### FALLBACK disk publish (Device is unusable after this point...) ###"
+ echo /dev/mmcblk0p3 > /sys/devices/platform/sunxi_usb_udc/gadget/lun0/file
+fi
+
+
+# Start the jevois daemon:
+echo /usr/bin/jevois-daemon ${opts}
+/usr/bin/jevois-daemon ${opts}
diff --git a/y2019/vision/tools/deploy.sh b/y2019/vision/tools/deploy.sh
new file mode 100755
index 0000000..4ad926f
--- /dev/null
+++ b/y2019/vision/tools/deploy.sh
@@ -0,0 +1,20 @@
+#!/bin/sh
+echo "Mount jevois ..."
+./jevois-cmd usbsd
+
+echo "Waiting for fs ..."
+while [ ! -d /media/$USER/JEVOIS ]
+do
+ sleep 1
+done
+echo "OK"
+
+echo "Copying files ..."
+cp ./austin_cam.sh /media/$USER/JEVOIS/
+
+echo "Unmount sd card ..."
+umount /media/$USER/JEVOIS
+echo "OK"
+
+echo "Rebooting Jevois."
+./jevois-cmd restart
diff --git a/y2019/vision/tools/flash.sh b/y2019/vision/tools/flash.sh
new file mode 100755
index 0000000..5daad73
--- /dev/null
+++ b/y2019/vision/tools/flash.sh
@@ -0,0 +1,49 @@
+#!/bin/sh
+
+if [ "$(id -u)" != "0" ]
+then
+ echo "Please run as root: $(id -u)"
+ exit
+fi
+
+DEV_BASE=/dev/mmcblk0p
+
+echo "Please check that ${DEV_BASE}3 is the correct drive to format. This is a destructive command so please be sure."
+read -p "Enter \"yes\" when you are ready: " input
+
+if [ "$input" != "yes" ]
+then
+ echo "Format aborted."
+ exit -1
+fi
+
+# need to disable some new features on 17.x
+sudo mkfs.ext4 -L JEVOIS -O ^64bit,uninit_bg,^metadata_csum ${DEV_BASE}3
+
+echo "Mounting JEVOIS."
+mkdir -p /tmp/JEVOIS
+sudo mount ${DEV_BASE}3 /tmp/JEVOIS
+
+echo "Mounting LINUX."
+mkdir -p /tmp/LINUX
+sudo mount ${DEV_BASE}2 /tmp/LINUX
+
+echo "Make JEVOIS directories."
+mkdir -p /tmp/JEVOIS/packages
+mkdir -p /tmp/JEVOIS/modules
+mkdir -p /tmp/JEVOIS/config
+mkdir -p /tmp/JEVOIS/lib
+
+echo "Copy configs."
+cp ./austin_cam.sh /tmp/JEVOIS
+cp ./videomappings.cfg /tmp/JEVOIS/config/
+cp ./rcS_script.txt /tmp/LINUX/etc/init.d/rcS
+cp Jevois_fstab /tmp/LINUX/etc/fstab
+
+echo "Un-mounting JEVOIS"
+sudo umount /tmp/JEVOIS
+
+echo "Un-mounting LINUX"
+sudo umount /tmp/LINUX
+
+
diff --git a/y2019/vision/tools/jevois-cmd b/y2019/vision/tools/jevois-cmd
new file mode 100755
index 0000000..6fe19d3
--- /dev/null
+++ b/y2019/vision/tools/jevois-cmd
@@ -0,0 +1,39 @@
+#!/bin/bash
+# USAGE: jevois-cmd [-d /dev/ttyACMX] command
+#
+# Send a command to a connected JeVois camera using the serial-over-USB port
+#
+# Tip: If things appear garbled in some way, maybe your TTY settings got messed up. Run screen /dev/ttyACM0 115200 to
+# reset to the expected state.
+#
+# You may need to install: sudo apt install python-serial
+
+ttydev="/dev/ttyACM0"
+if [ "X$1" = "X-d" ]; then ttydev=$2; shift 2; fi
+
+if [ ! -c "$ttydev" ]; then echo "Cannot access $ttydev -- is JeVois plugged in? -- ABORT"; exit 1; fi
+
+# Set the tty flags:
+sudo stty -F "${ttydev}" sane raw igncr clocal cread -echo
+
+# First read any accumulated junk in the serial buffers:
+while true; do
+ sudo bash -c "read -s -t 0.05 < \"${ttydev}\""
+ if [ $? -ne 0 ]; then break; fi
+done
+
+# Make a python program:
+prog="import serial
+ser = serial.Serial('$ttydev', 115200, timeout=1)
+ser.write('$*\n')
+nothing=1
+out = ''
+while nothing or ser.inWaiting() > 0:
+ out += ser.read(1)
+ nothing=0
+if out != '':
+ print out,
+"
+
+# Send the command:
+sudo python2.7 -c "$prog"
diff --git a/y2019/vision/tools/launch.sh b/y2019/vision/tools/launch.sh
new file mode 100755
index 0000000..f2bd68d
--- /dev/null
+++ b/y2019/vision/tools/launch.sh
@@ -0,0 +1 @@
+touch /tmp/do_not_export_sd_card
diff --git a/y2019/vision/tools/rcS_script.txt b/y2019/vision/tools/rcS_script.txt
new file mode 100644
index 0000000..3909a6e
--- /dev/null
+++ b/y2019/vision/tools/rcS_script.txt
@@ -0,0 +1,6 @@
+#!/bin/sh
+hostname jevois
+MODULES_DIR=/lib/modules/3.4.39
+
+if [ ! -f /boot/login ]; then /jevois/austin_cam.sh
+else while true; do /sbin/getty -L ttyS0 115200 vt100; done; fi
diff --git a/y2019/vision/tools/videomappings.cfg b/y2019/vision/tools/videomappings.cfg
new file mode 100644
index 0000000..395cbe4
--- /dev/null
+++ b/y2019/vision/tools/videomappings.cfg
@@ -0,0 +1,2 @@
+YUYV 640 360 30.0 YUYV 640 480 30.0 JeVois PassThrough *
+YUYV 640 480 30.0 YUYV 640 480 30.0 JeVois PassThrough
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 2efb55b..221bb51 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -359,7 +359,7 @@
void set_intake_roller_talon(
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX> t) {
intake_rollers_talon_ = ::std::move(t);
- intake_rollers_talon_->ConfigContinuousCurrentLimit(20.0, 0);
+ intake_rollers_talon_->ConfigContinuousCurrentLimit(10.0, 0);
intake_rollers_talon_->EnableCurrentLimit(true);
}
@@ -383,10 +383,10 @@
if (superstructure_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
- big_suction_cup0_->Set(!superstructure_->intake_suction_top);
- big_suction_cup1_->Set(!superstructure_->intake_suction_top);
- small_suction_cup0_->Set(!superstructure_->intake_suction_bottom);
- small_suction_cup1_->Set(!superstructure_->intake_suction_bottom);
+ big_suction_cup0_->Set(superstructure_->intake_suction_top);
+ big_suction_cup1_->Set(superstructure_->intake_suction_top);
+ small_suction_cup0_->Set(superstructure_->intake_suction_bottom);
+ small_suction_cup1_->Set(superstructure_->intake_suction_bottom);
intake_rollers_talon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,