Merge "Add support for serializing and deserializing the other SPI messages"
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/robot_state/robot_state.q b/aos/robot_state/robot_state.q
index 394d574..7eb10f5 100644
--- a/aos/robot_state/robot_state.q
+++ b/aos/robot_state/robot_state.q
@@ -12,7 +12,7 @@
};
message JoystickState {
- Joystick[4] joysticks;
+ Joystick[6] joysticks;
bool test_mode;
bool fms_attached;
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/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/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/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index e005475..7b1b029 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -1,9 +1,9 @@
#include "frc971/wpilib/joystick_sender.h"
-#include "aos/robot_state/robot_state.q.h"
#include "aos/init.h"
-#include "aos/network/team_number.h"
#include "aos/logging/queue_logging.h"
+#include "aos/network/team_number.h"
+#include "aos/robot_state/robot_state.q.h"
#include "HAL/HAL.h"
#include "frc971/wpilib/ahal/DriverStation.h"
@@ -41,13 +41,16 @@
new_state->team_id = team_id;
new_state->fake = false;
- for (int i = 0; i < 4; ++i) {
+ for (uint8_t i = 0;
+ i < sizeof(new_state->joysticks) / sizeof(::aos::Joystick); ++i) {
new_state->joysticks[i].buttons = ds->GetStickButtons(i);
for (int j = 0; j < 6; ++j) {
new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
}
+ if (ds->GetStickPOVCount(i) > 0) {
+ new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
+ }
LOG_STRUCT(DEBUG, "joystick_state", *new_state);
-
}
if (!new_state.Send()) {
LOG(WARNING, "sending joystick_state failed\n");
diff --git a/y2019/control_loops/superstructure/stilts/process_log_file.rb b/y2019/control_loops/superstructure/stilts/process_log_file.rb
new file mode 100755
index 0000000..4925ce2
--- /dev/null
+++ b/y2019/control_loops/superstructure/stilts/process_log_file.rb
@@ -0,0 +1,263 @@
+#!/usr/bin/env ruby
+#
+# Analyse robot log file to understand stilts lift
+#
+# The robot lift time was much longer than expected when it was
+# first tested. This script reads the log file and creates plot
+# files and a gnuplot script for viewing the results. From
+# viewing these results, it was determined that the lift
+# control loop profile acceleration needed to be increased
+# from 0.5 m/s^2 to 2.0 m/s^2.
+#
+# Michael wrote this script and James and Austin helped
+# with understanding the results and selecting what to plot.
+# The script was written Feb 23, 2019 while sorting out
+# why the robot was lifting slowly.
+#
+# The log files used with this script are at
+# https://robotics.mvla.net/frc971/2019/robotLogFiles/2019-02-23_MVHS_lab_lift_speed_debugging/
+#
+usage = <<EOF
+
+USAGE: #{__FILE__} [-h] aos_log-number [[aos_log-number2] ...]
+
+ -h print this help message
+
+ Processes a log file and create data plot files and a gnuplot script
+ for viewing the results. Be sure and uncompress the log files
+ before processing them if needed. i.e. gunzip aos_log-number.gz
+
+ To easily view a subset of the log data,
+ change the 'xrange' in the top of the aos_log-number.gnuplot file.
+ View the plots with
+
+ gnuplot aos_log-number.gnuplot
+
+ Use the "q" key followed by "enter" to advance from one plot to
+ the next.
+
+ Click in the plot with the right mouse button to select the start
+ of a region to zoom in on. Move the mouse to the end of the region
+ of interest and then right click again to complete the selection.
+ Doing so on the first plot is a good way to look at a time slice
+ of the log file. Subsequent plots will have the same time range.
+ This can also be set by updating the "set xrange" near the top of
+ the aos_log-number file.
+
+ The log files used with this script are at
+ https://robotics.mvla.net/frc971/2019/robotLogFiles/2019-02-23_MVHS_lab_lift_speed_debugging/
+
+EOF
+
+# print the usage message if there are no arguements or the -h
+# flag is given.
+if ( (ARGV.length == 0) || (ARGV.length > 0 && ARGV[0].match(/^-*h/)) ) then
+ puts usage
+ exit
+end
+
+#
+def run_cmd(cmd)
+ puts "Running: "+cmd
+ `tcsh -c "#{cmd}"`
+end
+#
+puts "ARGV = #{ARGV.inspect}"
+ARGV.each do |log_file|
+puts "# Processing log file #{log_file}"
+
+file_currents = log_file + ".currents" # PDP currents and battery voltage
+file_voltages = log_file + ".voltages" # control loop output voltage
+file_goal_status = log_file + ".goal_status" # status for stilts position, velocity,
+ # and control loop target speed and voltage error
+file_goal_goal = log_file + ".goal_goal" # control loop target accleration
+file_gnuplot = log_file + ".gnuplot" # gnuplot plot file.
+file_debug = log_file + ".debug" # log_displayer debug level output
+
+run_cmd("/home/michael/bin/log_displayer -l DEBUG #{log_file} > #{file_debug}")
+run_cmd("grep currents #{file_debug} | sed -e 's/[,:\{\}]/ /g' -e 's/^.*at 000000//' -e 's/s//' > #{file_currents}")
+run_cmd("egrep '^superstructure.*stilts_voltage' #{file_debug} | sed -e 's/[,:\{\}]/ /g' -e 's/^.*at 000000//' -e 's/s//' -e 's/y2019.*stilts_voltage/stilts_voltage/' > #{file_voltages}")
+run_cmd("egrep '^superstructure.*status' #{file_debug} | sed -e 's/[,:\{\}]/ /g' -e 's/^.*at 000000//' -e 's/s//' -e 's/y2019.* stilts/stilts/' > #{file_goal_status}")
+run_cmd("egrep '^superstructure.*goal:' #{file_debug} | sed -e 's/[,:\{\}]/ /g' -e 's/^.*at 000000//' -e 's/s//' -e 's/y2019.* stilts/stilts/' > #{file_goal_goal}")
+
+puts "Making the plot file #{file_gnuplot}"
+plot_commands = <<EOF
+#!/bin/env gnuplot
+#
+# gnuplot script for viewing the results. To easily view
+# a subset of the log data, change the 'xrange' in the top
+# of the aos_log-number.gnuplot file. View the plots with
+#
+# gnuplot #{file_gnuplot}
+#
+# Use the "q" key followed by "enter" to advance from one plot to
+# the next.
+#
+# Click in the plot with the right mouse button to select the start
+# of a region to zoom in on. Move the mouse to the end of the region
+# of interest and then right click again to complete the selection.
+# Doing so on the first plot is a good way to look at a time slice
+# of the log file. Subsequent plots will have the same time range.
+# This can also be set by updating the "set xrange" near the top of
+# the aos_log-number.gnuplot file.
+#
+
+# Set the terminal size for the plots in pixels.
+set term qt size 1800, 900
+
+# Set the default line with for the plot lines.
+do for [i=1:8] { set linetype i linewidth 4 }
+
+# Set the xrange for the plots to be the full length of the data.
+# Edit this to limit the plot range. i.e. use [9.71:17] to limit
+# the plot time to 9.71 seconds to 17 seconds.
+set xrange [*:*]
+
+set yrange [*:*]
+set y2tics
+set ytics nomirror
+set y2label "Voltage (Volts)"
+set ylabel "Position (m)"
+set xlabel "Time (seconds)"
+set grid
+volts = "#{file_voltages}"
+currents = "#{file_currents}"
+goal_status = "#{file_goal_status}"
+goal_goal = "#{file_goal_goal}"
+
+print ""
+print "# Show the stilts position, motor voltage from the control loop,"
+print "# and the battery voltage from the CAN power distribution board data."
+print ""
+print "# Enter a \\"q\\" key followed by \\"Enter\\" to advance to the next plot."
+print ""
+print "# Click in the plot with the right mouse button to select the start"
+print "# of a region to zoom in on. Move the mouse to the end of the region"
+print "# of interest and then right click again to complete the selection."
+print "# Doing so on the first plot is a good way to look at a time slice"
+print "# of the log file. Subsequent plots will have the same time range."
+print "# This can also be set by updating the \\"set xrange\\" near the top of"
+print "# the aos_log-number.gnuplot file."
+print ""
+#
+
+set title "#{log_file}" noenhanced
+plot \\
+ goal_status using 1:14 title "Position" with lines, \\
+ volts using 1:($6 > 0.4 || $6 < -0.4 ? $6 : 1/0) axis x1y2 with lines lw 2 tit "Motor Voltage", \\
+ currents using 1:9 axis x1y2 tit "Battery Voltage" with lines
+pause -1
+set multiplot layout 4, 1 title "#{log_file}" noenhanced
+unset title
+set ylabel "Position (m)"
+set xlabel "Time (seconds)"
+unset y2label
+unset y2tics
+plot \
+ goal_status using 1:14 title "Position" with lines
+
+set ylabel "Voltage (Volts)"
+set yrange [*:12.8]
+plot \\
+ volts using 1:($6 > 0.4 || $6 < -0.4 ? $6 : 1/0) with lines lw 2 tit "Motor Voltage", \\
+ currents using 1:9 tit "Battery Voltage" with lines
+
+set ylabel "Current (Amps)"
+set yrange [*:*]
+plot \\
+ currents using 1:18 tit "Motor Current" with lines lw 2
+
+set ylabel "Speed (meters/second)"
+plot \\
+ 0.0 with lines lw 3 notitle, \\
+ goal_status using 1:16 title "Control Loop Status Lift Speed" with lines lw 2, \\
+ goal_status using 1:20 title "Control Loop Goal Lift Speed" with lines lw 2
+
+
+unset multiplot
+pause -1
+
+Kv = 163.96 # rad/(volts*seconds)
+Kt = 0.00530 # N*m/Amps
+Resistance = 0.0896 # Ohms
+pulley_radius = 0.0142 # Meters
+gear_ratio = 30.95
+omega(v,i) = Kv * ( v - i * Resistance )
+speed(v,i) = pulley_radius * omega(v, i) / gear_ratio
+set y2label "Voltage (Volts)"
+set y2tics
+set ytics nomirror
+
+print "# The \\"Voltage and Current Lift Speed\\" line shows"
+print "# the predicted speed of the lift using the measured"
+print "# motor current and the battery voltage at the Power"
+print "# Distribution Board. It should match the \\"status"
+print "# Control Loop Goal Lift Speed\\" line from the log"
+print "# file. It uses the equation:"
+print "# omega(v,i) = Kv * ( v - i * Resistance )"
+print "# speed(v,i) = pulley_radius * omega(v, i) / gear_ratio"
+print ""
+
+set title "#{log_file}" noenhanced
+set ylabel "Lift Speed (meters/second)"
+plot \\
+ currents using 1:(v=$9, i=$18, speed(v,i)) tit "Voltage and Current Lift Speed" with lines lw 2, \\
+ goal_status using 1:16 title "Control Loop Status Lift Speed" with lines lw 2, \\
+ goal_status using 1:20 title "status Control Loop Goal Lift Speed" with lines lw 2, \\
+ goal_goal using 1:12 title "max\\\\_speed Control Loop Goal Lift Speed" with lines lw 2, \\
+ currents using 1:9 axis x1y2 tit "Battery Voltage" with lines lw 2, \\
+ goal_status using 1:26 title "Control Loop Voltage Error" axis x1y2 with lines lw 2, \\
+ volts using 1:($6 > 0.4 || $6 < -0.4 ? $6 : 1/0) axis x1y2 with lines lw 2 tit "Motor Voltage"
+pause -1
+
+set multiplot layout 3, 1 title "#{log_file}" noenhanced
+unset title
+
+set ylabel "Position (m)"
+set y2label "Current (Amps)"
+set y2range [*:*]
+plot \\
+ goal_status using 1:14 title "Position" with lines, \\
+ currents using 1:18 axis x1y2 tit "Motor Current" with lines lw 2
+
+set ylabel "Lift Speed Acceleration (meters/second^2)"
+unset y2label
+unset y2tics
+set ytics mirror
+plot \\
+ goal_goal using 1:14 title "Control Loop Lift Speed max\\\\_acceleraton Goal" with lines lw 2
+
+set y2label "Voltage (Volts)"
+set y2tics
+set ytics nomirror
+
+set ylabel "Lift Speed (meters/second)"
+plot \\
+ currents using 1:(v=$9, i=$18, speed(v,i)) tit "Voltage and Current Lift Speed" with lines lw 2, \\
+ goal_status using 1:16 title "Control Loop Status Lift Speed" with lines lw 2, \\
+ goal_status using 1:20 title "Control Loop Goal Lift Speed" with lines lw 2, \\
+ goal_goal using 1:12 title "max\\\\_speed Control Loop Goal Lift Speed" with lines lw 2, \\
+ currents using 1:9 axis x1y2 tit "Battery Voltage" with lines lw 2, \\
+ goal_status using 1:26 title "Control Loop Voltage Error" axis x1y2 with lines lw 2, \\
+ volts using 1:($6 > 0.4 || $6 < -0.4 ? $6 : 1/0) axis x1y2 with lines lw 2 tit "Motor Voltage"
+unset multiplot
+
+pause -1
+EOF
+
+File.open(file_gnuplot, 'w') { |f| f.write(plot_commands) }
+
+puts <<EOF
+
+Done making data files and plot file.
+
+View the plots with
+
+ gnuplot #{file_gnuplot}
+
+Use the "q" key followed by "enter" to advance from one
+plot to the next.
+
+EOF
+
+end
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/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 3223742..2d3e2f2 100644
--- a/y2019/jevois/spi.cc
+++ b/y2019/jevois/spi.cc
@@ -1,8 +1,13 @@
#include "y2019/jevois/spi.h"
-#include "aos/logging/logging.h"
#include "aos/util/bitpacking.h"
#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 0db59a5..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(
- gsl::span<const char> 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()) !=
diff --git a/y2019/jevois/uart.h b/y2019/jevois/uart.h
index 87dcc97..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"
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index c879eaa..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,11 +67,11 @@
"//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",
],
)
@@ -81,10 +88,9 @@
],
)
-"""
cc_binary(
- name = "calibration",
- srcs = ["calibration.cc"],
+ name = "global_calibration",
+ srcs = ["global_calibration.cc"],
deps = [
":target_finder",
"//aos/logging",
@@ -95,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/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/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;