Merged in the stuff most recently on the robot.
diff --git a/aos/build/aos.gyp b/aos/build/aos.gyp
index 12ac2a1..a0412c3 100644
--- a/aos/build/aos.gyp
+++ b/aos/build/aos.gyp
@@ -40,6 +40,7 @@
],
'dependencies': [
'<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:queue',
+ '<(AOS)/common/common.gyp:time',
],
'export_dependent_settings': [
'<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:queue',
diff --git a/aos/build/aos.gypi b/aos/build/aos.gypi
index c2c0813..4cd9a9d 100644
--- a/aos/build/aos.gypi
+++ b/aos/build/aos.gypi
@@ -15,10 +15,6 @@
'so_dir': '<(PRODUCT_DIR)/lib',
# the directory that executables that depend on <(EXTERNALS):gtest get put into
'test_dir': '<(PRODUCT_DIR)/tests',
-# 'executable' for linux and 'static_library' for the cRIO
-# Useful for targets that should either be an executable or get compiled into
-# a .out file depending on the current platform.
-# 'aos_target': platform-dependent,
},
'conditions': [
['OS=="crio"', {
@@ -26,9 +22,6 @@
['CC', '<!(readlink -f <(AOS)/build/crio_cc)'],
['CXX', '<!(readlink -f <(AOS)/build/crio_cxx)'],
],
- 'variables': {
- 'aos_target': 'static_library',
- },
}, {
'conditions': [
['PLATFORM!="linux-amd64"', {
@@ -39,9 +32,6 @@
},
],
],
- 'variables': {
- 'aos_target': 'executable',
- },
}
],
],
@@ -111,8 +101,6 @@
'cflags': [
'-mcpu=cortex-a8',
'-mfpu=neon',
-
- '-fstack-protector-all',
],
}],
['PLATFORM=="linux-amd64"', {
diff --git a/aos/build/build.sh b/aos/build/build.sh
index cf79fc3..6f1bf02 100755
--- a/aos/build/build.sh
+++ b/aos/build/build.sh
@@ -76,11 +76,11 @@
fi
${NINJA} -C ${OUTDIR} ${NINJA_ACTION} "$@"
if [[ ${ACTION} == deploy ]]; then
- [[ ${PLATFORM} == linux ]] && \
+ [[ ${PLATFORM} == linux ]] && ( \
rsync --progress -c -r \
${OUTDIR}/outputs/* \
driver@`${AOS}/build/get_ip prime`:/home/driver/robot_code/bin
- ssh driver@`${AOS}/build/get_ip prime` "sync; sync; sync"
+ ssh driver@`${AOS}/build/get_ip prime` "sync; sync; sync" )
[ ${PLATFORM} == crio ] && \
ncftpput `${AOS}/build/get_ip robot` / \
${OUTDIR}/lib/FRC_UserProgram.out
diff --git a/aos/build/externals.gyp b/aos/build/externals.gyp
index 4165574..863df98 100644
--- a/aos/build/externals.gyp
+++ b/aos/build/externals.gyp
@@ -2,6 +2,7 @@
# download_externals.sh makes sure that all of them have been downloaded.
{
'variables': {
+ # TODO(brians): Would we not have to do this hackery if we named it externals_path etc?
'externals': '<(AOS)/../output/downloaded',
'externals_abs': '<!(readlink -f ../../output/downloaded)',
'conditions': [['PLATFORM=="linux-amd64"', {
diff --git a/aos/build/queues.gypi b/aos/build/queues.gypi
index 843272d..7c0343a 100644
--- a/aos/build/queues.gypi
+++ b/aos/build/queues.gypi
@@ -29,6 +29,7 @@
'output_cc': '<(out_dir)/<(RULE_INPUT_ROOT).q.cc',
'output_main': '<(out_dir)/<(RULE_INPUT_ROOT)_main.cc',
'no_rsync': 1,
+ 'aos_q_dependent_paths%': [],
},
'rules': [
{
@@ -46,6 +47,7 @@
'<!@(find <(AOS)/build/queues/ -name *.rb)',
'<(AOS)/common/queue.h',
'<(AOS)/common/time.h',
+ '>@(aos_q_dependent_paths)',
],
'action': ['ruby', '<(script)',
'-I', '<(DEPTH)',
@@ -54,29 +56,7 @@
'<(header_path)',
'-cpp_base',
'<(prefix_dir)/<(_target_name)'],
- 'message': 'Generating C++ code from <(RULE_INPUT_DIRNAME)/<(RULE_INPUT_ROOT).q',
- 'process_outputs_as_sources': 1,
- },
- {
- 'variables': {
- 'script': '<(AOS)/build/act_builder.rb',
- },
- 'rule_name': 'genact',
- 'extension': 'act',
- 'outputs': [
- '<(output_h)',
- '<(output_cc)',
- '<(output_main)',
- ],
- 'inputs': [
- '<(script)',
- ],
- 'action': ['ruby', '<(script)',
- '<(gen_namespace)',
- '<(RULE_INPUT_PATH)',
- '<(DEPTH)',
- '<(out_dir)', 'header', 'cpp', 'main'],
- #'message': 'Generating C++ code from <(RULE_INPUT_DIRNAME)/<(RULE_INPUT_ROOT).act',
+ 'message': 'Generating C++ code for <(header_path)/<(RULE_INPUT_PATH)',
'process_outputs_as_sources': 1,
},
],
@@ -88,7 +68,7 @@
'<(prefix_dir)/<(_target_name)',
],
'variables': {
- 'gen_srcdir_parents': ['<(out_dir)'],
+ 'aos_q_dependent_paths': ['<@(_sources)'],
},
},
'dependencies': [
diff --git a/aos/build/queues/compiler.rb b/aos/build/queues/compiler.rb
index 899ba85..e4312fd 100644
--- a/aos/build/queues/compiler.rb
+++ b/aos/build/queues/compiler.rb
@@ -62,7 +62,7 @@
read_in, write_in = IO.pipe()
child = Process.spawn('clang-format-3.4 --style=google',
{:in=>read_in, write_in=>:close,
- :out=>output})
+ :out=>output.fileno})
read_in.close
[child, write_in]
end
@@ -91,14 +91,14 @@
cpp_tree.add_cc_using("::aos::to_network")
cpp_tree.add_cc_using("::aos::to_host")
- header_file = File.open(h_file_path,"w+")
- cc_file = File.open(cc_file_path,"w+")
+ header_file = WriteIffChanged.new(h_file_path)
+ cc_file = WriteIffChanged.new(cc_file_path)
header_child, header_output = format_pipeline(header_file)
cc_child, cc_output = format_pipeline(cc_file)
cpp_tree.write_header_file($cpp_base,header_output)
cpp_tree.write_cc_file($cpp_base,cc_output)
- cc_output.close()
header_output.close()
+ cc_output.close()
if !Process.wait2(cc_child)[1].success?
$stderr.puts "Formatting cc file failed."
exit 1
@@ -107,6 +107,8 @@
$stderr.puts "Formatting header file failed."
exit 1
end
+ header_file.close()
+ cc_file.close()
end
begin
args = ARGV.dup
diff --git a/aos/build/queues/load.rb b/aos/build/queues/load.rb
index c299d80..59a4590 100644
--- a/aos/build/queues/load.rb
+++ b/aos/build/queues/load.rb
@@ -10,5 +10,6 @@
["q_file.rb","message_dec.rb","queue_dec.rb", "q_struct.rb"].each do |name|
require File.dirname(__FILE__) + "/output/" + name
end
+require File.dirname(__FILE__) + '/write_iff_changed.rb'
require "fileutils"
require "pathname"
diff --git a/aos/build/queues/output/message_dec.rb b/aos/build/queues/output/message_dec.rb
index fdc03ee..59e2f35 100644
--- a/aos/build/queues/output/message_dec.rb
+++ b/aos/build/queues/output/message_dec.rb
@@ -187,7 +187,7 @@
type_class.set_parent("public ::aos::Message")
ts = self.simpleStr()
self.msg_hash = "0x#{Digest::SHA1.hexdigest(ts)[-8..-1]}"
- type_class.add_member("enum {kQueueLength = 1234, kHash = #{self.msg_hash}}")
+ type_class.add_member("enum {kQueueLength = 200, kHash = #{self.msg_hash}}")
@members.each do |elem|
type_class.add_member(elem.create_usage(cpp_tree))
end
@@ -284,9 +284,9 @@
f_call.args.dont_wrap = true
end
def getTypeID()
- Digest::SHA1.hexdigest(@type)[0..7].to_i(16) |
+ '0x' + ((Digest::SHA1.hexdigest(@type)[0..3].to_i(16) << 16) |
0x2000 | # marks it as primitive
- size
+ size).to_s(16)
end
def simpleStr()
"#{@type} #{@name}"
diff --git a/aos/build/queues/print_field.rb b/aos/build/queues/print_field.rb
index 7a9fbb4..a86140f 100644
--- a/aos/build/queues/print_field.rb
+++ b/aos/build/queues/print_field.rb
@@ -4,7 +4,7 @@
["uint#{size}_t", "int#{size}_t"]
end.flatten + ['bool', 'float', 'char', 'double']
-File.open(ARGV[0], 'w') do |output|
+WriteIffChanged.open(ARGV[0]) do |output|
output.puts <<END
// This file is generated by #{File.expand_path(__FILE__)}.
// DO NOT EDIT BY HAND!
diff --git a/aos/build/queues/queue_primitives.rb b/aos/build/queues/queue_primitives.rb
new file mode 100644
index 0000000..2c9594e
--- /dev/null
+++ b/aos/build/queues/queue_primitives.rb
@@ -0,0 +1,51 @@
+require File.dirname(__FILE__) + '/load.rb'
+
+require 'fileutils'
+
+TypeNames = [8, 16, 32, 64].collect do |size|
+ ["uint#{size}_t", "int#{size}_t"]
+end.flatten + ['bool', 'float', 'char', 'double']
+
+FileUtils.mkdir_p(File.dirname(ARGV[0]))
+WriteIffChanged.open(ARGV[0]) do |output|
+ output.puts <<END
+// This file is generated by #{File.expand_path(__FILE__)}.
+// DO NOT EDIT BY HAND!
+
+#include <stdint.h>
+
+namespace aos {
+namespace queue_primitive_types {
+#{TypeNames.collect do |name|
+ message_element = Target::MessageElement.new(name, 'value')
+ statement = MessageElementStmt.new(name, 'value')
+ message_element.size = statement.size
+ next <<END2
+ static const uint32_t #{name}_p = #{message_element.getTypeID()};
+END2
+end.join('')}
+} // namespace queue_primitive_types
+
+// A class for mapping an actual type to a type ID.
+// There are specializations for all of the actual primitive types.
+template<typename T>
+class TypeID {
+ public:
+ static const uint32_t id;
+};
+
+#{TypeNames.collect do |name|
+ message_element = Target::MessageElement.new(name, 'value')
+ statement = MessageElementStmt.new(name, 'value')
+ message_element.size = statement.size
+ next <<END2
+template<>
+class TypeID<#{name}> {
+ public:
+ static const uint32_t id = #{message_element.getTypeID()};
+};
+END2
+end.join('')}
+} // namespace aos
+END
+end
diff --git a/aos/build/queues/write_iff_changed.rb b/aos/build/queues/write_iff_changed.rb
new file mode 100644
index 0000000..74373a9
--- /dev/null
+++ b/aos/build/queues/write_iff_changed.rb
@@ -0,0 +1,30 @@
+require 'tempfile'
+
+# Writes a file like normal except doesn't do anything if the new contents are
+# the same as what's already there. Useful for helping build tools not rebuild
+# if generated files are re-generated but don't change.
+# Usable as a standard File, including to redirect to with IO.popen etc.
+class WriteIffChanged < Tempfile
+ def initialize(filename)
+ super('write_iff_changed')
+ @filename = filename
+ end
+ def WriteIffChanged.open(filename)
+ out = WriteIffChanged.new(filename)
+ yield out
+ out.close
+ end
+ def close
+ flush
+ contents = File.open(path, 'r') do |f|
+ f.readlines(nil)
+ end
+ if !File.exists?(@filename) ||
+ File.new(@filename, 'r').readlines(nil) != contents
+ File.open(@filename, 'w') do |out|
+ out.write(contents[0])
+ end
+ end
+ super
+ end
+end
diff --git a/aos/common/byteorder.h b/aos/common/byteorder.h
index 87ef39a..fb65685 100644
--- a/aos/common/byteorder.h
+++ b/aos/common/byteorder.h
@@ -11,35 +11,33 @@
// Also gives a nice templated interface to these functions.
namespace aos {
-
-#ifndef __VXWORKS__
namespace {
-template<typename int_type, typename T, int_type (*function)(int_type)> static inline void copier(const void *in, void *out) {
- static_assert(sizeof(T) == sizeof(int_type), "bad template args");
+template <typename int_type, int_type (*function)(int_type)>
+static inline void copier(const void *in, void *out) {
// Have confirmed by looking at the assembly code that this generates the same
// code as the (undefined by the c++ standard) way of using a union to do it.
// (which is pretty efficient)
int_type temp;
- memcpy(&temp, in, sizeof(T));
+ memcpy(&temp, in, sizeof(int_type));
temp = function(temp);
- memcpy(out, &temp, sizeof(T));
+ memcpy(out, &temp, sizeof(int_type));
}
-template<typename int_type, typename T, int_type (*function)(int_type)> static inline T memcpier(T in) {
- copier<int_type, T, function>(&in, &in);
+template <typename int_type, typename T, int_type (*function)(int_type)>
+static inline T memcpier(T in) {
+ copier<int_type, function>(&in, &in);
return in;
}
-// Can't make them static because they have to have external linkage to be used as a
-// template parameter.
-// Needed because be64toh etc are macros. (not that the manpage says anything...)
+// Needed because be64toh etc are macros (not that the manpage says
+// anything...).
// These are used instead of ntohs etc because gcc didn't inline those.
-inline uint64_t _be64toh(uint64_t in) { return be64toh(in); }
-inline uint64_t _htobe64(uint64_t in) { return htobe64(in); }
-inline uint32_t _be32toh(uint32_t in) { return be32toh(in); }
-inline uint32_t _htobe32(uint32_t in) { return htobe32(in); }
-inline uint16_t _be16toh(uint16_t in) { return be16toh(in); }
-inline uint16_t _htobe16(uint16_t in) { return htobe16(in); }
+static inline uint64_t _be64toh(uint64_t in) { return be64toh(in); }
+static inline uint64_t _htobe64(uint64_t in) { return htobe64(in); }
+static inline uint32_t _be32toh(uint32_t in) { return be32toh(in); }
+static inline uint32_t _htobe32(uint32_t in) { return htobe32(in); }
+static inline uint16_t _be16toh(uint16_t in) { return be16toh(in); }
+static inline uint16_t _htobe16(uint16_t in) { return htobe16(in); }
template<int bytes, typename T> class do_ntoh {
public:
@@ -54,18 +52,52 @@
template<typename T> class do_ntoh<2, T> {
public:
static inline T apply(T net) { return memcpier<uint16_t, T, _be16toh>(net); }
- static inline void copy(const char *in, T *host) { copier<uint16_t, T, _be16toh>(in, host); }
+ static inline void copy(const char *in, T *host) {
+ copier<uint16_t, _be16toh>(in, host);
+ }
};
template<typename T> class do_ntoh<4, T> {
public:
static inline T apply(T net) { return memcpier<uint32_t, T, _be32toh>(net); }
- static inline void copy(const char *in, T *host) { copier<uint32_t, T, _be32toh>(in, host); }
+ static inline void copy(const char *in, T *host) {
+ copier<uint32_t, _be32toh>(in, host);
+ }
};
template<typename T> class do_ntoh<8, T> {
public:
static inline T apply(T net) { return memcpier<uint64_t, T, _be64toh>(net); }
- static inline void copy(const char *in, T *host) { copier<uint64_t, T, _be64toh>(in, host); }
+ static inline void copy(const char *in, T *host) {
+ copier<uint64_t, _be64toh>(in, host);
+ }
};
+
+template <size_t Size>
+struct do_ntoh_int {
+ static inline void copy(const char *in, char *host);
+};
+template <>
+struct do_ntoh_int<1> {
+ static inline void copy(const char *in, char *host) { host[0] = in[0]; }
+};
+template <>
+struct do_ntoh_int<2> {
+ static inline void copy(const char *in, char *host) {
+ copier<uint16_t, _be16toh>(in, host);
+ }
+};
+template <>
+struct do_ntoh_int<4> {
+ static inline void copy(const char *in, char *host) {
+ copier<uint32_t, _be32toh>(in, host);
+ }
+};
+template <>
+struct do_ntoh_int<8> {
+ static inline void copy(const char *in, char *host) {
+ copier<uint64_t, _be64toh>(in, host);
+ }
+};
+
template<int bytes, typename T> class do_hton {
public:
static inline T apply(T host);
@@ -79,55 +111,83 @@
template<typename T> class do_hton<2, T> {
public:
static inline T apply(T host) { return memcpier<uint16_t, T, _htobe16>(host); }
- static inline void copy(const T *host, char *out) { copier<uint16_t, T, _htobe16>(host, out); }
+ static inline void copy(const T *host, char *out) {
+ copier<uint16_t, _htobe16>(host, out);
+ }
};
template<typename T> class do_hton<4, T> {
public:
static inline T apply(T host) { return memcpier<uint32_t, T, _htobe32>(host); }
- static inline void copy(const T *host, char *out) { copier<uint32_t, T, _htobe32>(host, out); }
+ static inline void copy(const T *host, char *out) {
+ copier<uint32_t, _htobe32>(host, out);
+ }
};
template<typename T> class do_hton<8, T> {
public:
static inline T apply(T host) { return memcpier<uint64_t, T, _htobe64>(host); }
- static inline void copy(const T *host, char *out) { copier<uint64_t, T, _htobe64>(host, out); }
+ static inline void copy(const T *host, char *out) {
+ copier<uint64_t, _htobe64>(host, out);
+ }
};
-} // namespace
-#endif // ifndef __VXWORKS__
+template <size_t Size>
+struct do_hton_int {
+ static inline void copy(const char *host, char *out);
+};
+template <>
+struct do_hton_int<1> {
+ static inline void copy(const char *host, char *out) { out[0] = host[0]; }
+};
+template <>
+struct do_hton_int<2> {
+ static inline void copy(const char *host, char *out) {
+ copier<uint16_t, _htobe16>(host, out);
+ }
+};
+template <>
+struct do_hton_int<4> {
+ static inline void copy(const char *host, char *out) {
+ copier<uint32_t, _htobe32>(host, out);
+ }
+};
+template <>
+struct do_hton_int<8> {
+ static inline void copy(const char *host, char *out) {
+ copier<uint64_t, _htobe64>(host, out);
+ }
+};
+
+} // namespace
// Converts T from network to host byte order.
-template<typename T> static inline T ntoh(T net) {
-#ifndef __VXWORKS__
+template <typename T>
+static inline T ntoh(T net) {
return do_ntoh<sizeof(net), T>::apply(net);
-#else
- return net;
-#endif
}
// Converts T from host to network byte order.
-template<typename T> static inline T hton(T host) {
-#ifndef __VXWORKS__
+template <typename T>
+static inline T hton(T host) {
return do_hton<sizeof(host), T>::apply(host);
-#else
- return host;
-#endif
}
-template<typename T> static inline void to_host(const char *input, T *host) {
-#ifndef __VXWORKS__
+template <typename T>
+static inline void to_host(const char *input, T *host) {
do_ntoh<sizeof(*host), T>::copy(input, host);
-#else
- memcpy(host, input, sizeof(*host));
-#endif
}
-template<typename T> static inline void to_network(const T *host, char *output) {
-#ifndef __VXWORKS__
+template <typename T>
+static inline void to_network(const T *host, char *output) {
do_hton<sizeof(*host), T>::copy(host, output);
-#else
- memcpy(output, host, sizeof(*host));
-#endif
+}
+
+template <size_t Size>
+static inline void to_host(const char *input, char *host) {
+ do_ntoh_int<Size>::copy(input, host);
+}
+template <size_t Size>
+static inline void to_network(const char *host, char *output) {
+ do_hton_int<Size>::copy(host, output);
}
} // namespace aos
#endif
-
diff --git a/aos/common/common.gyp b/aos/common/common.gyp
index 088c882..8847d40 100644
--- a/aos/common/common.gyp
+++ b/aos/common/common.gyp
@@ -46,6 +46,7 @@
'type': 'static_library',
'variables': {
'print_field_cc': '<(SHARED_INTERMEDIATE_DIR)/print_field.cc',
+ 'queue_primitives_h': '<(SHARED_INTERMEDIATE_DIR)/aos_queue_primitives/aos/queue_primitives.h',
},
'sources': [
'queue_types.cc',
@@ -73,7 +74,28 @@
'action': ['ruby', '<(script)', '<(print_field_cc)'],
'message': 'Generating print_field.cc',
},
+ {
+ 'variables': {
+ 'script': '<(AOS)/build/queues/queue_primitives.rb',
+ },
+ 'action_name': 'gen_queue_primitives',
+ 'inputs': [
+ '<(script)',
+ '<!@(find <(AOS)/build/queues/ -name *.rb)',
+ ],
+ 'outputs': [
+ '<(queue_primitives_h)',
+ ],
+ 'action': ['ruby', '<(script)', '<(queue_primitives_h)'],
+ 'message': 'Generating queue_primitives.h',
+ },
],
+ 'direct_dependent_settings': {
+ 'include_dirs': [
+ '<(SHARED_INTERMEDIATE_DIR)/aos_queue_primitives',
+ ],
+ },
+ 'hard_dependency': 1,
},
{
'target_name': 'queue_types_test',
@@ -177,7 +199,7 @@
'control_loop/ControlLoop.cc',
],
'dependencies': [
- '<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
'<(AOS)/build/aos.gyp:logging',
'timing',
'time',
@@ -187,7 +209,7 @@
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:sensor_generation',
],
'export_dependent_settings': [
- '<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
'<(AOS)/build/aos.gyp:logging',
'timing',
'time',
@@ -213,7 +235,7 @@
},
{
'target_name': 'type_traits_test',
- 'type': '<(aos_target)',
+ 'type': 'executable',
'sources': [
'type_traits_test.cpp',
],
@@ -243,7 +265,7 @@
},
{
'target_name': 'once_test',
- 'type': '<(aos_target)',
+ 'type': 'executable',
'sources': [
'once_test.cc',
],
@@ -254,7 +276,7 @@
},
{
'target_name': 'time_test',
- 'type': '<(aos_target)',
+ 'type': 'executable',
'sources': [
'time_test.cc',
],
@@ -313,7 +335,7 @@
},
{
'target_name': 'mutex_test',
- 'type': '<(aos_target)',
+ 'type': 'executable',
'sources': [
'mutex_test.cpp',
],
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index 22712f0..6323df5 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -2,7 +2,7 @@
#include "aos/common/logging/logging.h"
#include "aos/common/control_loop/Timing.h"
-#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/messages/robot_state.q.h"
#include "aos/common/logging/queue_logging.h"
#include "bbb/sensor_generation.q.h"
@@ -12,20 +12,28 @@
// TODO(aschuh): Tests.
-template <class T, bool has_position, bool fail_no_position>
-constexpr ::aos::time::Time
- ControlLoop<T, has_position, fail_no_position>::kStaleLogInterval;
+template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+constexpr ::aos::time::Time ControlLoop<T, has_position, fail_no_position,
+ fail_no_goal>::kStaleLogInterval;
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::ZeroOutputs() {
+template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+void
+ControlLoop<T, has_position, fail_no_position, fail_no_goal>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::Iterate() {
+template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+void ControlLoop<T, has_position, fail_no_position, fail_no_goal>::Iterate() {
+ no_prior_goal_.Print();
+ no_sensor_generation_.Print();
+ very_stale_position_.Print();
+ no_prior_position_.Print();
+ driver_station_old_.Print();
+ no_driver_station_.Print();
+
// Fetch the latest control loop goal and position. If there is no new
// goal, we will just reuse the old one.
// If there is no goal, we haven't started up fully. It isn't worth
@@ -36,8 +44,10 @@
const GoalType *goal = control_loop_->goal.get();
if (goal == NULL) {
LOG_INTERVAL(no_prior_goal_);
- ZeroOutputs();
- return;
+ if (fail_no_goal) {
+ ZeroOutputs();
+ return;
+ }
}
::bbb::sensor_generation.FetchLatest();
@@ -49,18 +59,18 @@
if (!has_sensor_reset_counters_ ||
::bbb::sensor_generation->reader_pid != reader_pid_ ||
::bbb::sensor_generation->cape_resets != cape_resets_) {
- char buffer[128];
- size_t characters = ::bbb::sensor_generation->Print(buffer, sizeof(buffer));
- LOG(INFO, "new sensor_generation message %.*s\n",
- static_cast<int>(characters), buffer);
+ LOG_STRUCT(INFO, "new sensor_generation message",
+ *::bbb::sensor_generation.get());
reader_pid_ = ::bbb::sensor_generation->reader_pid;
cape_resets_ = ::bbb::sensor_generation->cape_resets;
has_sensor_reset_counters_ = true;
reset_ = true;
}
-
- LOG_STRUCT(DEBUG, "goal", *goal);
+
+ if (goal) {
+ LOG_STRUCT(DEBUG, "goal", *goal);
+ }
// Only pass in a position if we got one this cycle.
const PositionType *position = NULL;
@@ -134,8 +144,8 @@
status.Send();
}
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::Run() {
+template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+void ControlLoop<T, has_position, fail_no_position, fail_no_goal>::Run() {
while (true) {
time::SleepUntil(NextLoopTime());
Iterate();
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index f1109e6..0e5c144 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -51,7 +51,8 @@
// If has_position is false, the control loop will always use NULL as the
// position and not check the queue. This is used for "loops" that control
// motors open loop.
-template <class T, bool has_position = true, bool fail_no_position = true>
+template <class T, bool has_position = true, bool fail_no_position = true,
+ bool fail_no_goal = true>
class ControlLoop : public SerializableControlLoop {
public:
// Maximum age of position packets before the loop will be disabled due to
@@ -79,9 +80,12 @@
decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
OutputType;
- // Constructs and sends a message on the output queue which will stop all the
- // motors. Calls Zero to clear all the state.
- void ZeroOutputs();
+ // Constructs and sends a message on the output queue which sets everything to
+ // a safe state (generally motors off). For some subclasses, this will be a
+ // bit different (ie pistons).
+ // The implementation here creates a new Output message, calls Zero() on it,
+ // and then sends it.
+ virtual void ZeroOutputs();
// Returns true if the device reading the sensors reset and potentially lost
// track of encoder counts. Calling this read method clears the flag. After
@@ -139,6 +143,9 @@
OutputType *output,
StatusType *status) = 0;
+ T *queue_group() { return control_loop_; }
+ const T *queue_group() const { return control_loop_; }
+
private:
// Pointer to the queue group
T *control_loop_;
diff --git a/aos/common/debugging-tips.txt b/aos/common/debugging-tips.txt
index af2d37c..718f7d9 100644
--- a/aos/common/debugging-tips.txt
+++ b/aos/common/debugging-tips.txt
@@ -2,15 +2,15 @@
Check the logs! Very few things will fail without putting something in the logs.
If they do, that is a bug unless our code is never getting run (there are
innumerable ways that could happen, but it generally doesn't).
- To check the logs, run `LogDisplayer` if `BinaryLogReader` has been started or
- just run `LogStreamer` if you want to do simple testing without writing logs
- to a file. See `LogDisplayer --help` for options.
+ To check the logs, run `log_displayer` if `binary_log_writer` has been started
+ or just run `log_streamer` if you want to do simple testing without writing
+ logs to a file. See `log_displayer --help` for options.
All of the binaries get put in the same place. That is
src/outputs/prime/outputs on the build machine and
/home/driver/robot_code/bin on linux.
[Startup]
-Low level startup errors often end up in /aos_fatal_error.* on the cRIO and
+Low level startup errors often end up in
/tmp/aos_fatal_error.* under linux. Also helpful are the /tmp/starter*_std*
files (if the standard start scripts are being used) and
aos/crio/bin/netconsole.sh for reading cRIO stdout and stderr.
@@ -22,25 +22,23 @@
on startup and being restarted.
[Control Loop(s) Not Working]
-Are robot_state messages going out? An aos::JoystickInput (often JoystickReader)
- should be sending them.
+Are robot_state messages going out? An aos::JoystickInput (often
+ joystick_reader) should be sending them.
+ Also, kFakeJoysticks in aos/prime/input/joystick_reader.cc has to be set to
+ false in order for anything to get output.
Is it being fed goal messages?
Is it getting position messages?
Is something looking at the output and doing something useful with it?
[No Driver Station Packets]
+TODO(brians): This is out of date.
Check to make sure that JSR (a cRIO-side task) is saying that it's sending
messages. Also check JoystickReader (or whatever inherits from
aos::JoystickInput) is running and see if it's receiving anything.
[Attaching a Debugger]
-In order to make attaching a debugger very useful, you have to compile the code
- with debugging symbols. The way to do that is to modify the appropriate
- build.sh to pass "yes" instead of "no" as an argument to the aos build.sh. You
- have to modify a .gyp file (touching it is sufficient) to get it to use the
- new build flag.
Attaching GDB to an existing robot code process works like normal (you have to
- root first because that's what all of the code currently runs as).
+ su root first because that's what all of the code currently runs as).
If a process is dying repeatedly, the easiest way to debug it is to kill
starter_loop.sh and (it has to be after that) starter_exe. After doing that,
NOTHING will get restarted (including the normal restart after changing a
diff --git a/aos/common/logging/logging.gyp b/aos/common/logging/logging.gyp
index e230e47..36d58b7 100644
--- a/aos/common/logging/logging.gyp
+++ b/aos/common/logging/logging.gyp
@@ -2,7 +2,7 @@
'targets': [
{
'target_name': 'logging_impl_test',
- 'type': '<(aos_target)',
+ 'type': 'executable',
'sources': [
'logging_impl_test.cc',
],
@@ -24,6 +24,26 @@
],
'export_dependent_settings': [
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:die',
+ ],
+ },
+ {
+ 'target_name': 'matrix_logging',
+ 'type': 'static_library',
+ 'sources': [
+ 'matrix_logging.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:die',
+ '<(AOS)/common/common.gyp:queue_types',
+ '<(EXTERNALS):eigen',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:die',
+ '<(AOS)/common/common.gyp:queue_types',
+ '<(EXTERNALS):eigen',
],
},
],
diff --git a/aos/common/logging/logging_impl.cc b/aos/common/logging/logging_impl.cc
index a0c4e56..b74b69d 100644
--- a/aos/common/logging/logging_impl.cc
+++ b/aos/common/logging/logging_impl.cc
@@ -99,6 +99,35 @@
message->type = LogMessage::Type::kStruct;
}
+void FillInMessageMatrix(log_level level,
+ const ::std::string &message_string, uint32_t type_id,
+ int rows, int cols, const void *data,
+ LogMessage *message) {
+ CHECK(MessageType::IsPrimitive(type_id));
+ message->matrix.type = type_id;
+
+ const auto element_size = MessageType::Sizeof(type_id);
+
+ FillInMessageBase(level, message);
+
+ message->message_length = rows * cols * element_size;
+ if (message_string.size() + message->message_length >
+ sizeof(message->matrix.data)) {
+ LOG(FATAL, "%dx%d matrix of type %" PRIu32
+ " (size %u) and message %s is too big\n",
+ rows, cols, type_id, element_size, message_string.c_str());
+ }
+ message->matrix.string_length = message_string.size();
+ memcpy(message->matrix.data, message_string.data(),
+ message->matrix.string_length);
+
+ message->matrix.rows = rows;
+ message->matrix.cols = cols;
+ SerializeMatrix(type_id, &message->matrix.data[message->matrix.string_length],
+ data, rows, cols);
+ message->type = LogMessage::Type::kMatrix;
+}
+
void FillInMessage(log_level level, const char *format, va_list ap,
LogMessage *message) {
FillInMessageBase(level, message);
@@ -109,18 +138,20 @@
}
void PrintMessage(FILE *output, const LogMessage &message) {
-#define BASE_FORMAT \
- "%.*s(%" PRId32 ")(%05" PRIu16 "): %s at %010" PRId32 ".%09" PRId32 "s: "
+#define NSECONDS_DIGITS 5
+#define BASE_FORMAT \
+ "%.*s(%" PRId32 ")(%05" PRIu16 "): %-7s at %010" PRId32 \
+ ".%0" STRINGIFY(NSECONDS_DIGITS) PRId32 "s: "
#define BASE_ARGS \
static_cast<int>(message.name_length), message.name, \
static_cast<int32_t>(message.source), message.sequence, \
- log_str(message.level), message.seconds, message.nseconds
- switch (message.type) {
+ log_str(message.level), message.seconds, (message.nseconds + 5000) / 10000
+ switch (message.type) {
case LogMessage::Type::kString:
fprintf(output, BASE_FORMAT "%.*s", BASE_ARGS,
static_cast<int>(message.message_length), message.message);
break;
- case LogMessage::Type::kStruct:
+ case LogMessage::Type::kStruct: {
char buffer[1024];
size_t output_length = sizeof(buffer);
size_t input_length = message.message_length;
@@ -142,8 +173,34 @@
static_cast<int>(message.structure.string_length),
message.structure.serialized,
static_cast<int>(sizeof(buffer) - output_length), buffer);
- break;
+ } break;
+ case LogMessage::Type::kMatrix: {
+ char buffer[1024];
+ size_t output_length = sizeof(buffer);
+ if (message.message_length !=
+ static_cast<size_t>(message.matrix.rows * message.matrix.cols *
+ MessageType::Sizeof(message.matrix.type))) {
+ LOG(FATAL, "expected %d bytes of matrix data but have %zu\n",
+ message.matrix.rows * message.matrix.cols *
+ MessageType::Sizeof(message.matrix.type),
+ message.message_length);
+ }
+ if (!PrintMatrix(buffer, &output_length,
+ message.matrix.data + message.matrix.string_length,
+ message.matrix.type, message.matrix.rows,
+ message.matrix.cols)) {
+ LOG(FATAL, "printing %dx%d matrix of type %" PRIu32 " failed\n",
+ message.matrix.rows, message.matrix.cols, message.matrix.type);
+ }
+ fprintf(output, BASE_FORMAT "%.*s: %.*s\n", BASE_ARGS,
+ static_cast<int>(message.matrix.string_length),
+ message.matrix.data,
+ static_cast<int>(sizeof(buffer) - output_length), buffer);
+ } break;
}
+#undef NSECONDS_DIGITS
+#undef BASE_FORMAT
+#undef BASE_ARGS
}
} // namespace internal
@@ -169,6 +226,28 @@
static_cast<int>(sizeof(printed) - printed_bytes), printed);
}
+void LogImplementation::LogMatrix(
+ log_level level, const ::std::string &message, uint32_t type_id,
+ int rows, int cols, const void *data) {
+ char serialized[1024];
+ if (static_cast<size_t>(rows * cols * MessageType::Sizeof(type_id)) >
+ sizeof(serialized)) {
+ LOG(FATAL, "matrix of size %u too big to serialize\n",
+ rows * cols * MessageType::Sizeof(type_id));
+ }
+ SerializeMatrix(type_id, serialized, data, rows, cols);
+ char printed[1024];
+ size_t printed_bytes = sizeof(printed);
+ if (!PrintMatrix(printed, &printed_bytes, serialized, type_id, rows, cols)) {
+ LOG(FATAL, "PrintMatrix(%p, %p(=%zd), %p, %" PRIu32 ", %d, %d) failed\n",
+ printed, &printed_bytes, printed_bytes, serialized, type_id, rows,
+ cols);
+ }
+ DoLogVariadic(level, "%.*s: %.*s\n", static_cast<int>(message.size()),
+ message.data(),
+ static_cast<int>(sizeof(printed) - printed_bytes), printed);
+}
+
StreamLogImplementation::StreamLogImplementation(FILE *stream)
: stream_(stream) {}
diff --git a/aos/common/logging/logging_impl.h b/aos/common/logging/logging_impl.h
index 3d5b387..440e3fe 100644
--- a/aos/common/logging/logging_impl.h
+++ b/aos/common/logging/logging_impl.h
@@ -42,11 +42,12 @@
// The struct that the code uses for making logging calls.
struct LogMessage {
enum class Type : uint8_t {
- kString, kStruct,
+ kString, kStruct, kMatrix
};
int32_t seconds, nseconds;
- // message_length is the length of everything in message for all types.
+ // message_length is just the length of the actual data (which member depends
+ // on the type).
size_t message_length, name_length;
pid_t source;
static_assert(sizeof(source) == 4, "that's how they get printed");
@@ -63,6 +64,15 @@
// The message string and then the serialized structure.
char serialized[LOG_MESSAGE_LEN - sizeof(type) - sizeof(string_length)];
} structure;
+ struct {
+ // The type ID of the element type.
+ uint32_t type;
+ int rows, cols;
+ size_t string_length;
+ // The message string and then the serialized matrix.
+ char
+ data[LOG_MESSAGE_LEN - sizeof(type) - sizeof(rows) - sizeof(cols)];
+ } matrix;
};
};
static_assert(shm_ok<LogMessage>::value, "it's going in a queue");
@@ -102,9 +112,12 @@
void LogNext(log_level level, const char *format, ...)
__attribute__((format(LOG_PRINTF_FORMAT_TYPE, 2, 3)));
-// Will take a structure and log it.
+// Takes a structure and log it.
template <class T>
void DoLogStruct(log_level, const ::std::string &, const T &);
+// Takes a matrix and logs it.
+template <class T>
+void DoLogMatrix(log_level, const ::std::string &, const T &);
// Represents a system that can actually take log messages and do something
// useful with them.
@@ -144,6 +157,12 @@
virtual void LogStruct(log_level level, const ::std::string &message,
size_t size, const MessageType *type,
const ::std::function<size_t(char *)> &serialize);
+ // Similiar to LogStruct, except for matrixes.
+ // type_id is the type of the elements of the matrix.
+ // data points to rows*cols*type_id.Size() bytes of data in row-major order.
+ virtual void LogMatrix(log_level level, const ::std::string &message,
+ uint32_t type_id, int rows, int cols,
+ const void *data);
// These functions call similar methods on the "current" LogImplementation or
// Die if they can't find one.
@@ -154,12 +173,18 @@
size_t size, const MessageType *type,
const ::std::function<size_t(char *)> &serialize,
int levels);
+ // This one is implemented in matrix_logging.cc.
+ static void DoLogMatrix(log_level level, const ::std::string &message,
+ uint32_t type_id, int rows, int cols,
+ const void *data, int levels);
// Friends so that they can access the static Do* functions.
friend void VLog(log_level, const char *, va_list);
friend void LogNext(log_level, const char *, ...);
template <class T>
friend void DoLogStruct(log_level, const ::std::string &, const T &);
+ template <class T>
+ friend void DoLogMatrix(log_level, const ::std::string &, const T &);
LogImplementation *next_;
};
@@ -262,11 +287,26 @@
const ::std::function<size_t(char *)> &serialize,
LogMessage *message);
+// Fills in all the parts of the message according to the given inputs (with
+// type kMatrix).
+void FillInMessageMatrix(log_level level,
+ const ::std::string &message_string, uint32_t type_id,
+ int rows, int cols, const void *data,
+ LogMessage *message);
+
// Fills in *message according to the given inputs (with type kString).
// Used for implementing LogImplementation::DoLog.
void FillInMessage(log_level level, const char *format, va_list ap,
LogMessage *message);
+static inline void FillInMessageVarargs(log_level level, LogMessage *message,
+ const char *format, ...) {
+ va_list ap;
+ va_start(ap, format);
+ FillInMessage(level, format, ap, message);
+ va_end(ap);
+}
+
// Prints message to output.
void PrintMessage(FILE *output, const LogMessage &message);
diff --git a/aos/common/logging/logging_interface.cc b/aos/common/logging/logging_interface.cc
index b4244fb..ae8cc01 100644
--- a/aos/common/logging/logging_interface.cc
+++ b/aos/common/logging/logging_interface.cc
@@ -68,7 +68,10 @@
int levels) {
internal::RunWithCurrentImplementation(
levels, [&](LogImplementation * implementation) {
- implementation->DoLog(level, format, ap);
+ va_list ap1;
+ va_copy(ap1, ap);
+ implementation->DoLog(level, format, ap1);
+ va_end(ap1);
if (level == FATAL) {
VDie(format, ap);
diff --git a/aos/common/logging/matrix_logging-tmpl.h b/aos/common/logging/matrix_logging-tmpl.h
new file mode 100644
index 0000000..bfc3ce5
--- /dev/null
+++ b/aos/common/logging/matrix_logging-tmpl.h
@@ -0,0 +1,20 @@
+#include "aos/common/logging/logging_impl.h"
+
+#include <functional>
+
+#include "aos/queue_primitives.h"
+
+namespace aos {
+namespace logging {
+
+template <class T>
+void DoLogMatrix(log_level level, const ::std::string &message,
+ const T &matrix) {
+ static_assert(!T::IsRowMajor, "we only handle column-major storage");
+ LogImplementation::DoLogMatrix(level, message, TypeID<typename T::Scalar>::id,
+ matrix.rows(), matrix.cols(), matrix.data(),
+ 1);
+}
+
+} // namespace logging
+} // namespace aos
diff --git a/aos/common/logging/matrix_logging.cc b/aos/common/logging/matrix_logging.cc
new file mode 100644
index 0000000..6173074
--- /dev/null
+++ b/aos/common/logging/matrix_logging.cc
@@ -0,0 +1,35 @@
+#include "aos/common/logging/matrix_logging.h"
+
+#include "aos/common/queue_types.h"
+
+namespace aos {
+namespace logging {
+
+void LogImplementation::DoLogMatrix(log_level level,
+ const ::std::string &message,
+ uint32_t type_id, int rows, int cols,
+ const void *data, int levels) {
+ internal::RunWithCurrentImplementation(
+ levels, [&](LogImplementation * implementation) {
+ implementation->LogMatrix(level, message, type_id, rows, cols, data);
+ });
+
+ if (level == FATAL) {
+ char serialized[1024];
+ if (static_cast<size_t>(rows * cols * MessageType::Sizeof(type_id)) >
+ sizeof(serialized)) {
+ Die("LOG(FATAL) matrix too big to serialize");
+ }
+ SerializeMatrix(type_id, serialized, data, rows, cols);
+ char printed[LOG_MESSAGE_LEN];
+ size_t printed_bytes = sizeof(printed);
+ if (!PrintMatrix(printed, &printed_bytes, serialized, type_id, rows, cols)) {
+ Die("LOG(FATAL) PrintMatrix call failed");
+ }
+ Die("%.*s: %.*s\n", static_cast<int>(message.size()), message.data(),
+ static_cast<int>(printed_bytes), printed);
+ }
+}
+
+} // namespace logging
+} // namespace aos
diff --git a/aos/common/logging/matrix_logging.h b/aos/common/logging/matrix_logging.h
new file mode 100644
index 0000000..6efaa06
--- /dev/null
+++ b/aos/common/logging/matrix_logging.h
@@ -0,0 +1,36 @@
+#ifndef AOS_COMMON_LOGGING_MATRIX_LOGGING_H_
+#define AOS_COMMON_LOGGING_MATRIX_LOGGING_H_
+
+#include <string>
+
+#include "Eigen/Dense"
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/die.h"
+
+namespace aos {
+namespace logging {
+
+// Logs the contents of a matrix and a constant string.
+// matrix must be an instance of an Eigen matrix (or something similar).
+#define LOG_MATRIX(level, message, matrix) \
+ do { \
+ static const ::std::string kAosLoggingMessage( \
+ LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": " message); \
+ ::aos::logging::DoLogMatrix(level, kAosLoggingMessage, matrix); \
+ /* so that GCC knows that it won't return */ \
+ if (level == FATAL) { \
+ ::aos::Die("DoLogStruct(FATAL) fell through!!!!!\n"); \
+ } \
+ } while (false)
+
+template <class T>
+void DoLogMatrix(log_level level, const ::std::string &message,
+ const T &matrix);
+
+} // namespace logging
+} // namespace aos
+
+#include "aos/common/logging/matrix_logging-tmpl.h"
+
+#endif // AOS_COMMON_LOGGING_MATRIX_LOGGING_H_
diff --git a/aos/common/logging/queue_logging.cc b/aos/common/logging/queue_logging.cc
index eefb4ac..ce96c29 100644
--- a/aos/common/logging/queue_logging.cc
+++ b/aos/common/logging/queue_logging.cc
@@ -1,6 +1,6 @@
-#include "aos/common/logging/logging_impl.h"
+#include "aos/common/logging/queue_logging.h"
-#include "aos/common/die.h"
+#include "aos/common/logging/logging_impl.h"
#include "aos/common/queue_types.h"
namespace aos {
@@ -13,22 +13,22 @@
internal::RunWithCurrentImplementation(
levels, [&](LogImplementation * implementation) {
implementation->LogStruct(level, message, size, type, serialize);
-
- if (level == FATAL) {
- char serialized[1024];
- if (size > sizeof(serialize)) {
- Die("LOG(FATAL) structure too big to serialize");
- }
- size_t used = serialize(serialized);
- char printed[LOG_MESSAGE_LEN];
- size_t printed_bytes = sizeof(printed);
- if (!PrintMessage(printed, &printed_bytes, serialized, &used, *type)) {
- Die("LOG(FATAL) PrintMessage call failed");
- }
- Die("%.*s: %.*s\n", static_cast<int>(message.size()), message.data(),
- static_cast<int>(printed_bytes), printed);
- }
});
+
+ if (level == FATAL) {
+ char serialized[1024];
+ if (size > sizeof(serialized)) {
+ Die("LOG(FATAL) structure too big to serialize");
+ }
+ size_t used = serialize(serialized);
+ char printed[LOG_MESSAGE_LEN];
+ size_t printed_bytes = sizeof(printed);
+ if (!PrintMessage(printed, &printed_bytes, serialized, &used, *type)) {
+ Die("LOG(FATAL) PrintMessage call failed");
+ }
+ Die("%.*s: %.*s\n", static_cast<int>(message.size()), message.data(),
+ static_cast<int>(printed_bytes), printed);
+ }
}
} // namespace logging
diff --git a/aos/common/logging/queue_logging.h b/aos/common/logging/queue_logging.h
index 191d4c7..ff0167a 100644
--- a/aos/common/logging/queue_logging.h
+++ b/aos/common/logging/queue_logging.h
@@ -7,10 +7,13 @@
#include <string>
#include "aos/common/logging/logging.h"
+#include "aos/common/die.h"
namespace aos {
namespace logging {
+// Logs the contents of a structure (or Queue message) and a constant string.
+// structure must be an instance of one of the generated queue types.
#define LOG_STRUCT(level, message, structure) \
do { \
static const ::std::string kAosLoggingMessage( \
@@ -18,9 +21,7 @@
::aos::logging::DoLogStruct(level, kAosLoggingMessage, structure); \
/* so that GCC knows that it won't return */ \
if (level == FATAL) { \
- fprintf(stderr, "DoLogStruct(FATAL) fell through!!!!!\n"); \
- printf("see stderr\n"); \
- abort(); \
+ ::aos::Die("DoLogStruct(FATAL) fell through!!!!!\n"); \
} \
} while (false)
diff --git a/aos/common/messages/QueueHolder.h b/aos/common/messages/QueueHolder.h
deleted file mode 100644
index c40a630..0000000
--- a/aos/common/messages/QueueHolder.h
+++ /dev/null
@@ -1,176 +0,0 @@
-#ifndef __AOS_MESSAGES_QUEUE_HOLDER_H_
-#define __AOS_MESSAGES_QUEUE_HOLDER_H_
-
-#include <stddef.h>
-#include <string.h>
-
-#include <algorithm>
-
-#include "aos/common/control_loop/Timing.h"
-#include "aos/common/byteorder.h"
-#include "aos/common/time.h"
-#include "aos/common/type_traits.h"
-#include "aos/common/logging/logging.h"
-#ifndef __VXWORKS__
-#include "aos/linux_code/ipc_lib/queue.h"
-#endif
-
-namespace aos {
-
-// Specializations of TypeOperator and QueueBuilder that are actually
-// implemented are created in the generated code for all message value types.
-// These specializations have the same functions declared in the actual types.
-
-// Defines a way for types to be manipulated.
-template<typename T> class TypeOperator {
- public:
- // Sets all fields to their default constructor.
- static void Zero(T &t_);
- // Returns the size of buffer NToH and HToN use.
- static size_t Size();
- // Converts everything from network to host byte order.
- // input must have Size() bytes available in it.
- static void NToH(char *input, T &t_);
- // Converts everything from host to network byte order and puts it into output.
- // output must have Size() bytes available in it.
- static void HToN(const T &t_, char *output);
- // Creates a string with the names and values of all the fields.
- // The return value might will be to a static buffer.
- static const char *Print(const T &t_);
-};
-
-template<typename T> class QueueHolder;
-
-// An easy way to set values for queue messages.
-// Each specialization has chainable setter functions for building a message
-// of type T to put into a queue (like QueueBuilder<T> &field(int value);).
-template<class T> class QueueBuilder {
- public:
- QueueBuilder(QueueHolder<T> &holder);
- bool Send();
-};
-
-// Internal class to make implementing identical behavior with structs that go
-// into queues easier. Also a central location for the documentation.
-//
-// When compiled for the cRIO, everything except Clear does nothing (and Get
-// turns into just a Clear) which means that the internal T instance is the only one.
-// Also, the internal instance becomes static.
-//
-// To look at the current message, you Get a copy and then View the result.
-// To make changes, you modify the message that you can View (after possibly
-// Clearing it) and then you Send it. You can also just Clear the message, put
-// data into it, and then Send it. A way to modify the local message is using
-// the Builder function.
-// Note that there is no way to avoid potentially overwriting other changes between
-// when you Get one and when you Send it (mainly applicable to 1-length queues).
-//
-// T must be POD and have a "timespec set_time;" field.
-//
-// This first class doesn't have the builder; QueueHolder does.
-#define aos_check_rv __attribute__((warn_unused_result))
-template<typename T> class QueueHolderNoBuilder {
-#ifndef __VXWORKS__
- Queue *const queue_;
- static_assert(shm_ok<T>::value, "T must be able to"
- " go through shared memory and memcpy");
- T t_;
-#else
- static T t_;
-#endif
- public:
-#ifndef __VXWORKS__
- explicit QueueHolderNoBuilder(Queue *queue) : queue_(queue) {}
-#else
- QueueHolderNoBuilder() {}
-#endif
- // Gets the current value and stores it in View().
- // check_time is whether or not to check to see if the last time a value was Sent
- // was too long ago (returns false if it was)
- // Returns true if View() is now the current value.
- // IMPORTANT: If this function returns false, the contents of View() are
- // either the same as they were before or (if check_time is true) the current
- // message. That is why it creates compile-time warnings if the return value
- // is not checked.
- bool Get(bool check_time) aos_check_rv;
- // Looks at the current value. Starts out undefined.
- // If the last Get call returned false, then this the contents of the
- // return value are undefined.
-#ifdef __VXWORKS__
- static
-#endif
- inline T &View() { return t_; }
- // Clears (calls the default constructor of) all the fields of the current
- // Goal.
- void Clear() { TypeOperator<T>::Zero(t_); }
- // Sends the current value. Does not affect the current value.
- // Returns whether or not the Send succeeded.
- bool Send();
- // Returns a string containing the values of all the fields in the current
- // value.
- // The return value is valid until Print is called again. The class owns the
- // memory.
- const char *Print() const { return TypeOperator<T>::Print(t_); }
-};
-
-template<typename T>
-bool QueueHolderNoBuilder<T>::Get(bool check_time) {
-#ifdef __VXWORKS__
- (void)check_time;
- return true;
-#else
- const T *msg = static_cast<const T *>(aos_queue_read_msg(queue_,
- PEEK | NON_BLOCK));
- if (msg == NULL) {
- return false;
- }
- static_assert(sizeof(t_) == sizeof(*msg), "something is wrong here");
- memcpy(&t_, msg, sizeof(t_));
- aos_queue_free_msg(queue_, msg);
- if (check_time && !((time::Time::Now() - time::Time(t_.set_time)) > time::Time::InMS(45))) {
- LOG(WARNING, "too long since last Set msg={%s}\n", Print());
- return false;
- } else {
- return true;
- }
-#endif
-}
-template<typename T>
-bool QueueHolderNoBuilder<T>::Send() {
-#ifndef __VXWORKS__
- T *msg = static_cast<T *>(aos_queue_get_msg(queue_));
- if (msg == NULL) {
- return false;
- }
- static_assert(sizeof(*msg) == sizeof(t_), "something is wrong here");
- memcpy(msg, &t_, sizeof(t_));
- msg->set_time = ::aos::time::Time::Now().ToTimespec();
-
- return aos_queue_write_msg_free(queue_, msg, OVERRIDE) == 0;
-#else
- return true;
-#endif
-}
-#ifdef __VXWORKS__
-template<typename T> T QueueHolderNoBuilder<T>::t_;
-#endif
-template<typename T> class QueueHolder : public QueueHolderNoBuilder<T> {
- QueueBuilder<T> builder_;
- public:
-#ifndef __VXWORKS__
- explicit QueueHolder(Queue *queue) : QueueHolderNoBuilder<T>(queue),
- builder_(*this) {}
-#else
- QueueHolder() : builder_(*this) {}
-#endif
- // Clears the current Goal and returns an object that allows setting various
- // fields with chained method calls and then calling Send() on it.
- QueueBuilder<T> &Builder() {
- QueueHolderNoBuilder<T>::Clear();
- return builder_;
- }
-};
-
-} // namespace aos
-
-#endif
diff --git a/aos/common/messages/QueueHolder.swg b/aos/common/messages/QueueHolder.swg
deleted file mode 100644
index 816e297..0000000
--- a/aos/common/messages/QueueHolder.swg
+++ /dev/null
@@ -1,9 +0,0 @@
-namespace aos {
-
-template<class T> class QueueBuilder {
- public:
- bool Send();
-};
-
-} // namespace aos
-
diff --git a/aos/common/messages/RobotState.q b/aos/common/messages/RobotState.q
deleted file mode 100644
index 32baa05..0000000
--- a/aos/common/messages/RobotState.q
+++ /dev/null
@@ -1,14 +0,0 @@
-package aos;
-
-message RobotState {
- bool enabled;
- bool autonomous;
- uint16_t team_id;
-};
-
-// The robot_state Queue is checked by all control loops to make sure that the
-// joystick code hasn't died.
-// It also provides information about whether or not the robot is in autonomous
-// mode and what the team_id is.
-
-queue RobotState robot_state;
diff --git a/aos/common/messages/messages.gyp b/aos/common/messages/messages.gyp
index e134a95..0b7805b 100644
--- a/aos/common/messages/messages.gyp
+++ b/aos/common/messages/messages.gyp
@@ -1,37 +1,10 @@
{
'targets': [
{
- 'target_name': 'QueueHolder',
+ 'target_name': 'robot_state',
'type': 'static_library',
'sources': [
- # 'QueueHolder.h'
- ],
- 'dependencies': [
- '<(AOS)/common/common.gyp:timing',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/build/aos.gyp:logging',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/common.gyp:timing',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/build/aos.gyp:logging',
- ],
- 'conditions': [
- ['OS!="crio"', {
- 'dependencies': [
- '<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:queue',
- ],
- 'export_dependent_settings': [
- '<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:queue',
- ],
- }],
- ],
- },
- {
- 'target_name': 'aos_queues',
- 'type': 'static_library',
- 'sources': [
- 'RobotState.q',
+ 'robot_state.q',
],
'variables': {
'header_path': 'aos/common/messages',
@@ -44,21 +17,5 @@
],
'includes': ['../../build/queues.gypi'],
},
- {
- 'target_name': 'queues_so',
- 'type': 'shared_library',
- 'sources': [
- 'RobotState.q',
- ],
- 'variables': {
- 'header_path': 'aos/common/messages',
- },
- 'direct_dependent_settings': {
- 'variables': {
- 'jni_libs': ['queues_so'],
- },
- },
- 'includes': ['../../build/queues.gypi'],
- },
],
}
diff --git a/aos/common/messages/robot_state.q b/aos/common/messages/robot_state.q
new file mode 100644
index 0000000..696bf2c
--- /dev/null
+++ b/aos/common/messages/robot_state.q
@@ -0,0 +1,20 @@
+package aos;
+
+message RobotState {
+ bool enabled;
+ bool autonomous;
+ uint16_t team_id;
+ // If this is true, then this message isn't actually from the control
+ // system and so should not be trusted as evidence that the button inputs
+ // etc are actually real and should be acted on.
+ // However, most things should ignore this so that sending fake messages is
+ // useful for testing.
+ bool fake;
+};
+
+// The robot_state Queue is checked by all control loops to make sure that the
+// joystick code hasn't died.
+// It also provides information about whether or not the robot is in autonomous
+// mode and what the team_id is.
+
+queue RobotState robot_state;
diff --git a/aos/common/queue.h b/aos/common/queue.h
index 65bfa95..6c33822 100644
--- a/aos/common/queue.h
+++ b/aos/common/queue.h
@@ -214,8 +214,6 @@
// Fetches the next message from the queue.
// Returns true if there was a new message available and we successfully
// fetched it. This removes the message from the queue for all readers.
- // TODO(aschuh): Fix this to use a different way of fetching messages so other
- // readers can also FetchNext.
bool FetchNext();
bool FetchNextBlocking();
@@ -287,6 +285,7 @@
T *MakeRawMessage();
// Pointer to the queue that this object fetches from.
RawQueue *queue_;
+ int index_ = 0;
#endif
// Scoped pointer holding the latest message or NULL.
ScopedMessagePtr<const T> queue_msg_;
diff --git a/aos/common/queue_types.cc b/aos/common/queue_types.cc
index bcf3e94..93cf135 100644
--- a/aos/common/queue_types.cc
+++ b/aos/common/queue_types.cc
@@ -15,18 +15,18 @@
ssize_t MessageType::Serialize(char *buffer, size_t max_bytes) const {
char *const buffer_start = buffer;
- size_t fields_size = 0;
+ uint16_t fields_size = 0;
for (int i = 0; i < number_fields; ++i) {
fields_size += sizeof(fields[i]->type);
- fields_size += sizeof(size_t);
+ fields_size += sizeof(uint16_t);
fields_size += fields[i]->name.size();
}
- if (max_bytes < sizeof(id) + sizeof(super_size) + sizeof(size_t) +
+ if (max_bytes < sizeof(id) + sizeof(super_size) + sizeof(uint16_t) +
sizeof(number_fields) + name.size() + fields_size) {
return -1;
}
- size_t length;
+ uint16_t length;
to_network(&super_size, buffer);
buffer += sizeof(super_size);
@@ -37,7 +37,7 @@
buffer += sizeof(length);
to_network(&number_fields, buffer);
buffer += sizeof(number_fields);
- memcpy(buffer, name.data(), name.size());
+ memcpy(buffer, name.data(), length);
buffer += name.size();
for (int i = 0; i < number_fields; ++i) {
@@ -46,7 +46,7 @@
length = fields[i]->name.size();
to_network(&length, buffer);
buffer += sizeof(length);
- memcpy(buffer, fields[i]->name.data(), fields[i]->name.size());
+ memcpy(buffer, fields[i]->name.data(), length);
buffer += length;
}
@@ -54,7 +54,7 @@
}
MessageType *MessageType::Deserialize(const char *buffer, size_t *bytes) {
- size_t name_length;
+ uint16_t name_length;
decltype(MessageType::super_size) super_size;
decltype(MessageType::id) id;
decltype(MessageType::number_fields) number_fields;
@@ -86,7 +86,7 @@
buffer += name_length;
for (int i = 0; i < number_fields; ++i) {
- size_t field_name_length;
+ uint16_t field_name_length;
if (*bytes < sizeof(fields[i]->type) + sizeof(field_name_length)) {
return nullptr;
}
@@ -164,6 +164,95 @@
return true;
}
+bool PrintMatrix(char *output, size_t *output_bytes, const void *input,
+ uint32_t type_id, int rows, int cols) {
+ CHECK(MessageType::IsPrimitive(type_id));
+ const size_t element_size = MessageType::Sizeof(type_id);
+
+ if (*output_bytes < 1) return false;
+ *output_bytes -= 1;
+ *(output++) = '[';
+
+ bool first_row = true;
+ for (int row = 0; row < rows; ++row) {
+ if (first_row) {
+ first_row = false;
+ } else {
+ if (*output_bytes < 2) return false;
+ *output_bytes -= 2;
+ *(output++) = ',';
+ *(output++) = ' ';
+ }
+
+ if (*output_bytes < 1) return false;
+ *output_bytes -= 1;
+ *(output++) = '[';
+
+ bool first_col = true;
+ for (int col = 0; col < cols; ++col) {
+ if (first_col) {
+ first_col = false;
+ } else {
+ if (*output_bytes < 2) return false;
+ *output_bytes -= 2;
+ *(output++) = ',';
+ *(output++) = ' ';
+ }
+
+ const size_t output_bytes_before = *output_bytes;
+ size_t input_bytes = element_size;
+ if (!PrintField(output, output_bytes,
+ static_cast<const char *>(input) +
+ (row + col * rows) * element_size,
+ &input_bytes, type_id)) {
+ return false;
+ }
+ CHECK_EQ(0u, input_bytes);
+ // Update the output pointer, ignoring the trailing '\0' that
+ // the subcall put on.
+ output += output_bytes_before - *output_bytes - 1;
+ *output_bytes += 1;
+ }
+
+ if (*output_bytes < 1) return false;
+ *output_bytes -= 1;
+ *(output++) = ']';
+ }
+ if (*output_bytes < 2) return false;
+ *output_bytes -= 2;
+ *(output++) = ']';
+ *(output++) = '\0';
+ return true;
+}
+
+void SerializeMatrix(int type_id, void *output_void, const void *input_void,
+ int rows, int cols) {
+ char *const output = static_cast<char *>(output_void);
+ const char *const input = static_cast<const char *>(input_void);
+
+ CHECK(MessageType::IsPrimitive(type_id));
+ const size_t element_size = MessageType::Sizeof(type_id);
+
+ for (int i = 0; i < rows * cols; ++i) {
+ switch(element_size) {
+ case 1:
+ to_network<1>(&input[i * element_size], &output[i * element_size]);
+ break;
+ case 2:
+ to_network<2>(&input[i * element_size], &output[i * element_size]);
+ break;
+ case 4:
+ to_network<4>(&input[i * element_size], &output[i * element_size]);
+ break;
+ case 8:
+ to_network<8>(&input[i * element_size], &output[i * element_size]);
+ break;
+ default:
+ LOG(FATAL, "illegal primitive type size %zu\n", element_size);
+ }
+ }
+}
+
namespace type_cache {
namespace {
diff --git a/aos/common/queue_types.h b/aos/common/queue_types.h
index 55d26b4..b95c946 100644
--- a/aos/common/queue_types.h
+++ b/aos/common/queue_types.h
@@ -9,6 +9,7 @@
#include <string>
#include "aos/common/macros.h"
+#include "aos/common/byteorder.h"
namespace aos {
@@ -52,19 +53,27 @@
return (type_id & 0x2000) != 0;
}
+ static unsigned int Sizeof(uint32_t type_id) {
+ if (IsPrimitive(type_id)) {
+ return (type_id & 0xFFFF) - 0x2000;
+ } else {
+ return type_id & 0xFFFF;
+ }
+ }
+
// How many (serialized) bytes the superclass takes up.
- size_t super_size;
+ uint16_t super_size;
// The type ID for this.
uint32_t id;
::std::string name;
- int number_fields;
+ uint16_t number_fields;
const Field **fields;
private:
// Internal constructor for Deserialize to use.
- MessageType(size_t super_size, uint32_t id, ::std::string &&name,
- int number_fields, Field **fields)
+ MessageType(uint16_t super_size, uint32_t id, ::std::string &&name,
+ uint16_t number_fields, Field **fields)
: super_size(super_size),
id(id),
name(name),
@@ -101,6 +110,14 @@
bool PrintMessage(char *output, size_t *output_bytes, const void *input,
size_t *input_bytes, const MessageType &type)
__attribute__((warn_unused_result));
+// Calls PrintField to print out a matrix of values.
+bool PrintMatrix(char *output, size_t *output_bytes, const void *input,
+ uint32_t type, int rows, int cols);
+
+// "Serializes" a matrix (basically just converts to network byte order). The
+// result can be passed to PrintMatrix.
+void SerializeMatrix(int type_id, void *output_void, const void *input_void,
+ int rows, int cols);
// Implements a cache of types which generally works per-process but can (when
// instructed) put a type in shared memory which other processes will
diff --git a/aos/common/queue_types_test.cc b/aos/common/queue_types_test.cc
index 72eae94..ac7e683 100644
--- a/aos/common/queue_types_test.cc
+++ b/aos/common/queue_types_test.cc
@@ -6,6 +6,7 @@
#include "aos/common/test_queue.q.h"
#include "aos/common/byteorder.h"
+#include "aos/queue_primitives.h"
using ::aos::common::testing::Structure;
using ::aos::common::testing::MessageWithStructure;
@@ -174,5 +175,19 @@
EXPECT_EQ(kTestStructure1String.size() + 1, sizeof(output) - output_bytes);
}
+TEST_F(PrintMessageTest, Matrix) {
+ static const uint16_t kTestMatrix[] = {971, 254, 1768, 8971, 9971, 973};
+ uint16_t test_matrix[sizeof(kTestMatrix) / sizeof(kTestMatrix[0])];
+ SerializeMatrix(queue_primitive_types::uint16_t_p, test_matrix, kTestMatrix,
+ 3, 2);
+ static const ::std::string kOutput =
+ "[[971, 8971], [254, 9971], [1768, 973]]";
+ output_bytes = sizeof(output);
+ ASSERT_TRUE(PrintMatrix(output, &output_bytes, test_matrix,
+ queue_primitive_types::uint16_t_p, 3, 2));
+ EXPECT_EQ(kOutput, ::std::string(output));
+ EXPECT_EQ(kOutput.size() + 1, sizeof(output) - output_bytes);
+}
+
} // namespace testing
} // namespace aos
diff --git a/aos/common/util/log_interval.h b/aos/common/util/log_interval.h
index 188a16c..7c8a329 100644
--- a/aos/common/util/log_interval.h
+++ b/aos/common/util/log_interval.h
@@ -34,7 +34,7 @@
}
bool ShouldLog() {
const ::aos::time::Time now = ::aos::time::Time::Now();
- const bool r = (now - last_done_) >= interval_;
+ const bool r = (now - last_done_) >= interval_ && count_ > 0;
if (r) {
last_done_ = now;
}
@@ -55,7 +55,8 @@
};
// This one is even easier to use. It always logs with a message "%s %d
-// times\n".
+// times\n". Call LOG_INTERVAL wherever it should log and make sure Print gets
+// called often (ie not after a conditional return)
class SimpleLogInterval {
public:
SimpleLogInterval(const ::aos::time::Time &interval, log_level level,
@@ -63,13 +64,19 @@
: interval_(interval), level_(level), message_(message) {}
#define LOG_INTERVAL(simple_log) \
- simple_log.Hit(LOG_SOURCENAME ": " STRINGIFY(__LINE__))
- void Hit(const char *context) {
+ simple_log.WantToLog(LOG_SOURCENAME ": " STRINGIFY(__LINE__))
+ void WantToLog(const char *context) {
+ context_ = context;
interval_.WantToLog();
+ }
+
+ void Print() {
if (interval_.ShouldLog()) {
- log_do(level_, "%s: %.*s %d times over %f sec\n", context,
+ CHECK_NOTNULL(context_);
+ log_do(level_, "%s: %.*s %d times over %f sec\n", context_,
static_cast<int>(message_.size()), message_.data(),
interval_.Count(), interval_.interval().ToSeconds());
+ context_ = NULL;
}
}
@@ -77,6 +84,7 @@
LogInterval interval_;
const log_level level_;
const ::std::string message_;
+ const char *context_ = NULL;
};
} // namespace util
diff --git a/aos/linux_code/camera/Buffers.h b/aos/linux_code/camera/Buffers.h
index aedd79f..c73b3d8 100644
--- a/aos/linux_code/camera/Buffers.h
+++ b/aos/linux_code/camera/Buffers.h
@@ -8,7 +8,7 @@
#include "aos/linux_code/ipc_lib/queue.h"
#include "aos/common/type_traits.h"
-#include "aos/atom_code/ipc_lib/unique_message_ptr.h"
+#include "aos/linux_code/ipc_lib/unique_message_ptr.h"
namespace aos {
namespace camera {
diff --git a/aos/linux_code/configuration.cc b/aos/linux_code/configuration.cc
index e5e5583..6b1cdf6 100644
--- a/aos/linux_code/configuration.cc
+++ b/aos/linux_code/configuration.cc
@@ -78,7 +78,7 @@
r, size, errno, strerror(errno));
}
if (ret < size) {
- void *last_slash = memrchr(r, '/', size);
+ void *last_slash = memrchr(r, '/', ret);
if (last_slash == NULL) {
r[ret] = '\0';
LOG(FATAL, "couldn't find a '/' in \"%s\"\n", r);
diff --git a/aos/linux_code/ipc_lib/ipc_lib.gyp b/aos/linux_code/ipc_lib/ipc_lib.gyp
index 08e8df8..fe8b2e0 100644
--- a/aos/linux_code/ipc_lib/ipc_lib.gyp
+++ b/aos/linux_code/ipc_lib/ipc_lib.gyp
@@ -51,7 +51,7 @@
'target_name': 'raw_queue_test',
'type': 'executable',
'sources': [
- 'queue_test.cc',
+ 'raw_queue_test.cc',
],
'dependencies': [
'<(EXTERNALS):gtest',
diff --git a/aos/linux_code/ipc_lib/queue.cc b/aos/linux_code/ipc_lib/queue.cc
index 8a86b7f..211882f 100644
--- a/aos/linux_code/ipc_lib/queue.cc
+++ b/aos/linux_code/ipc_lib/queue.cc
@@ -6,6 +6,7 @@
#include <assert.h>
#include <memory>
+#include <algorithm>
#include "aos/common/logging/logging.h"
#include "aos/common/type_traits.h"
@@ -21,6 +22,7 @@
const bool kWriteDebug = false;
const bool kRefDebug = false;
const bool kFetchDebug = false;
+const bool kReadIndexDebug = false;
// The number of extra messages the pool associated with each queue will be able
// to hold (for readers who are slow about freeing them or who leak one when
@@ -35,9 +37,14 @@
const int RawQueue::kBlock;
const int RawQueue::kOverride;
+// This is what gets stuck in before each queue message in memory. It is always
+// allocated aligned to 8 bytes and its size has to maintain that alignment for
+// the message that follows immediately.
struct RawQueue::MessageHeader {
+ // This gets incremented and decremented with atomic instructions without any
+ // locks held.
int ref_count;
- int index; // in pool_
+ MessageHeader *next;
// Gets the message header immediately preceding msg.
static MessageHeader *Get(const void *msg) {
return reinterpret_cast<MessageHeader *>(__builtin_assume_aligned(
@@ -50,31 +57,52 @@
memcpy(other, this, sizeof(*other));
memcpy(this, &temp, sizeof(*this));
}
+#if __SIZEOF_POINTER__ == 8
+ char padding[16 - sizeof(next) - sizeof(ref_count)];
+#elif __SIZEOF_POINTER__ == 4
+ // No padding needed to get 8 byte total size.
+#else
+#error Unknown pointer size.
+#endif
};
-static_assert(shm_ok<RawQueue::MessageHeader>::value,
- "the whole point is to stick it in shared memory");
struct RawQueue::ReadData {
bool writable_start;
};
-// TODO(brians) maybe do this with atomic integer instructions so it doesn't
-// have to lock/unlock pool_lock_
void RawQueue::DecrementMessageReferenceCount(const void *msg) {
- MutexLocker locker(&pool_lock_);
MessageHeader *header = MessageHeader::Get(msg);
- --header->ref_count;
- assert(header->ref_count >= 0);
+ __atomic_sub_fetch(&header->ref_count, 1, __ATOMIC_RELAXED);
if (kRefDebug) {
- printf("ref_dec_count: %p count=%d\n", msg, header->ref_count);
+ printf("%p ref dec count: %p count=%d\n", this, msg, header->ref_count);
}
+
+ // The only way it should ever be 0 is if we were the last one to decrement,
+ // in which case nobody else should have it around to re-increment it or
+ // anything in the middle, so this is safe to do not atomically with the
+ // decrement.
if (header->ref_count == 0) {
DoFreeMessage(msg);
+ } else {
+ assert(header->ref_count > 0);
+ }
+}
+
+void RawQueue::IncrementMessageReferenceCount(const void *msg) const {
+ MessageHeader *const header = MessageHeader::Get(msg);
+ __atomic_add_fetch(&header->ref_count, 1, __ATOMIC_RELAXED);
+ if (kRefDebug) {
+ printf("%p ref inc count: %p\n", this, msg);
}
}
RawQueue::RawQueue(const char *name, size_t length, int hash, int queue_length)
: readable_(&data_lock_), writable_(&data_lock_) {
+ static_assert(shm_ok<RawQueue::MessageHeader>::value,
+ "the whole point is to stick it in shared memory");
+ static_assert((sizeof(RawQueue::MessageHeader) % 8) == 0,
+ "need to revalidate size/alignent assumptions");
+
const size_t name_size = strlen(name) + 1;
char *temp = static_cast<char *>(shm_malloc(name_size));
memcpy(temp, name, name_size);
@@ -100,12 +128,16 @@
data_end_ = 0;
messages_ = 0;
- mem_length_ = queue_length + kExtraMessages;
- pool_length_ = 0;
- messages_used_ = 0;
msg_length_ = length + sizeof(MessageHeader);
- pool_ = static_cast<MessageHeader **>(
- shm_malloc(sizeof(MessageHeader *) * mem_length_));
+
+ MessageHeader *previous = nullptr;
+ for (int i = 0; i < queue_length + kExtraMessages; ++i) {
+ MessageHeader *const message =
+ static_cast<MessageHeader *>(shm_malloc(msg_length_));
+ free_messages_ = message;
+ message->next = previous;
+ previous = message;
+ }
if (kFetchDebug) {
printf("made queue %s\n", name);
@@ -168,17 +200,9 @@
void RawQueue::DoFreeMessage(const void *msg) {
MessageHeader *header = MessageHeader::Get(msg);
- if (pool_[header->index] != header) { // if something's messed up
- fprintf(stderr, "queue: something is very very wrong with queue %p."
- " pool_(=%p)[header->index(=%d)] != header(=%p)\n",
- this, pool_, header->index, header);
- printf("queue: see stderr\n");
- abort();
- }
if (kRefDebug) {
- printf("ref free: %p\n", msg);
+ printf("%p ref free->%p: %p\n", this, recycle_, msg);
}
- --messages_used_;
if (recycle_ != NULL) {
void *const new_msg = recycle_->GetMessage();
@@ -186,21 +210,7 @@
fprintf(stderr, "queue: couldn't get a message"
" for recycle queue %p\n", recycle_);
} else {
- // Take a message from recycle_ and switch its
- // header with the one being freed, which effectively
- // switches which queue each message belongs to.
- MessageHeader *const new_header = MessageHeader::Get(new_msg);
- // Also switch the messages between the pools.
- pool_[header->index] = new_header;
- {
- MutexLocker locker(&recycle_->pool_lock_);
- recycle_->pool_[new_header->index] = header;
- // Swap the information in both headers.
- header->Swap(new_header);
- // Don't unlock the other pool until all of its messages are valid.
- }
- // use the header for new_msg which is now for this pool
- header = new_header;
+ IncrementMessageReferenceCount(msg);
if (!recycle_->WriteMessage(const_cast<void *>(msg), kOverride)) {
fprintf(stderr, "queue: %p->WriteMessage(%p, kOverride) failed."
" aborting\n", recycle_, msg);
@@ -208,23 +218,19 @@
abort();
}
msg = new_msg;
+ header = MessageHeader::Get(new_msg);
}
}
- // Where the one we're freeing was.
- int index = header->index;
- header->index = -1;
- if (index != messages_used_) { // if we're not freeing the one on the end
- // Put the last one where the one we're freeing was.
- header = pool_[index] = pool_[messages_used_];
- // Put the one we're freeing at the end.
- pool_[messages_used_] = MessageHeader::Get(msg);
- // Update the former last one's index.
- header->index = index;
+ header->next = __atomic_load_n(&free_messages_, __ATOMIC_RELAXED);
+ while (
+ !__atomic_compare_exchange_n(&free_messages_, &header->next, header,
+ true, __ATOMIC_RELEASE, __ATOMIC_RELAXED)) {
}
}
bool RawQueue::WriteMessage(void *msg, int options) {
+ // TODO(brians): Test this function.
if (kWriteDebug) {
printf("queue: %p->WriteMessage(%p, %x)\n", this, msg, options);
}
@@ -250,6 +256,7 @@
if (kWriteDebug) {
printf("queue: not blocking on %p. returning false\n", this);
}
+ DecrementMessageReferenceCount(msg);
return false;
} else if (options & kOverride) {
if (kWriteDebug) {
@@ -319,11 +326,12 @@
}
}
if (kReadDebug) {
- printf("queue: %p->read start=%d end=%d\n", this, data_start_, data_end_);
+ printf("queue: %p->read(%p) start=%d end=%d\n", this, index, data_start_,
+ data_end_);
}
return true;
}
-void *RawQueue::ReadPeek(int options, int start) {
+void *RawQueue::ReadPeek(int options, int start) const {
void *ret;
if (options & kFromEnd) {
int pos = data_end_ - 1;
@@ -335,19 +343,17 @@
}
ret = data_[pos];
} else {
+ assert(start != -1);
if (kReadDebug) {
printf("queue: %p reading from line %d: %d\n", this, __LINE__, start);
}
ret = data_[start];
}
- MessageHeader *const header = MessageHeader::Get(ret);
- ++header->ref_count;
- if (kRefDebug) {
- printf("ref inc count: %p\n", ret);
- }
+ IncrementMessageReferenceCount(ret);
return ret;
}
const void *RawQueue::ReadMessage(int options) {
+ // TODO(brians): Test this function.
if (kReadDebug) {
printf("queue: %p->ReadMessage(%x)\n", this, options);
}
@@ -390,7 +396,6 @@
printf("queue: %p reading from d2: %d\n", this, data_start_);
}
msg = data_[data_start_];
- // TODO(brians): Doesn't this need to increment the ref count?
data_start_ = (data_start_ + 1) % data_length_;
}
}
@@ -419,20 +424,46 @@
// TODO(parker): Handle integer wrap on the index.
- // How many unread messages we have.
- const int offset = messages_ - *index;
// Where we're going to start reading.
- int my_start = data_end_ - offset;
- if (my_start < 0) { // If we want to read off the end of the buffer.
- // Unwrap it.
- my_start += data_length_;
+ int my_start;
+
+ if (options & kFromEnd) {
+ my_start = -1;
+ } else {
+ const int unread_messages = messages_ - *index;
+ assert(unread_messages > 0);
+ int current_messages = data_end_ - data_start_;
+ if (current_messages < 0) current_messages += data_length_;
+ if (kReadIndexDebug) {
+ printf("queue: %p start=%d end=%d current=%d\n",
+ this, data_start_, data_end_, current_messages);
+ }
+ assert(current_messages > 0);
+ // If we're behind the available messages.
+ if (unread_messages > current_messages) {
+ // Catch index up to the last available message.
+ *index = messages_ - current_messages;
+ // And that's the one we're going to read.
+ my_start = data_start_;
+ if (kReadIndexDebug) {
+ printf("queue: %p jumping ahead to message %d (have %d) (at %d)\n",
+ this, *index, messages_, data_start_);
+ }
+ } else {
+ // Just start reading at the first available message that we haven't yet
+ // read.
+ my_start = data_end_ - unread_messages;
+ if (kReadIndexDebug) {
+ printf("queue: %p original read from %d\n", this, my_start);
+ }
+ if (data_start_ < data_end_) {
+ assert(my_start >= data_start_);
+ } else {
+ if (my_start < 0) my_start += data_length_;
+ }
+ }
}
- if (offset >= data_length_) { // If we're behind the available messages.
- // Catch index up to the last available message.
- *index += data_start_ - my_start;
- // And that's the one we're going to read.
- my_start = data_start_;
- }
+
if (options & kPeek) {
msg = ReadPeek(options, my_start);
} else {
@@ -441,6 +472,9 @@
printf("queue: %p start of c1\n", this);
}
int pos = data_end_ - 1;
+ if (kReadIndexDebug) {
+ printf("queue: %p end pos start %d\n", this, pos);
+ }
if (pos < 0) { // If it wrapped.
pos = data_length_ - 1; // Unwrap it.
}
@@ -453,40 +487,53 @@
if (kReadDebug) {
printf("queue: %p reading from d1: %d\n", this, my_start);
}
+ // This assert checks that we're either within both endpoints (duh) or
+ // not between them (if the queue is wrapped around).
+ assert((my_start >= data_start_ && my_start < data_end_) ||
+ ((my_start >= data_start_) == (my_start > data_end_)));
+ // More sanity checking.
+ assert((my_start >= 0) && (my_start < data_length_));
msg = data_[my_start];
++(*index);
}
- MessageHeader *const header = MessageHeader::Get(msg);
- ++header->ref_count;
- if (kRefDebug) {
- printf("ref_inc_count: %p\n", msg);
- }
+ IncrementMessageReferenceCount(msg);
}
ReadCommonEnd(&read_data);
return msg;
}
void *RawQueue::GetMessage() {
- MutexLocker locker(&pool_lock_);
- MessageHeader *header;
- if (pool_length_ - messages_used_ > 0) {
- header = pool_[messages_used_];
- } else {
- if (pool_length_ >= mem_length_) {
+ // TODO(brians): Test this function.
+
+ MessageHeader *header = __atomic_load_n(&free_messages_, __ATOMIC_RELAXED);
+ do {
+ if (__builtin_expect(header == nullptr, 0)) {
LOG(FATAL, "overused pool of queue %p\n", this);
}
- header = pool_[pool_length_] =
- static_cast<MessageHeader *>(shm_malloc(msg_length_));
- ++pool_length_;
- }
- void *msg = reinterpret_cast<uint8_t *>(header) + sizeof(MessageHeader);
+ } while (
+ !__atomic_compare_exchange_n(&free_messages_, &header, header->next, true,
+ __ATOMIC_ACQ_REL, __ATOMIC_RELAXED));
+ void *msg = reinterpret_cast<uint8_t *>(header + 1);
+ // It might be uninitialized, 0 from a previous use, or 1 from previously
+ // getting recycled.
header->ref_count = 1;
+ static_assert(
+ __atomic_always_lock_free(sizeof(header->ref_count), &header->ref_count),
+ "we access this using not specifically atomic loads and stores");
if (kRefDebug) {
printf("%p ref alloc: %p\n", this, msg);
}
- header->index = messages_used_;
- ++messages_used_;
return msg;
}
+int RawQueue::FreeMessages() const {
+ int r = 0;
+ MessageHeader *header = free_messages_;
+ while (header != nullptr) {
+ ++r;
+ header = header->next;
+ }
+ return r;
+}
+
} // namespace aos
diff --git a/aos/linux_code/ipc_lib/queue.h b/aos/linux_code/ipc_lib/queue.h
index a58b65e..33ed9f4 100644
--- a/aos/linux_code/ipc_lib/queue.h
+++ b/aos/linux_code/ipc_lib/queue.h
@@ -55,20 +55,22 @@
// Constants for passing to options arguments.
// The non-conflicting ones can be combined with bitwise-or.
- // Causes the returned message to be left in the queue.
+ // Doesn't update the currently read index (the read messages in the queue or
+ // the index). This means the returned message (and any others skipped with
+ // kFromEnd) will be left in the queue.
// For reading only.
static const int kPeek = 0x0001;
// Reads the last message in the queue instead of just the next one.
// NOTE: This removes all of the messages until the last one from the queue
- // (which means that nobody else will read them). However, PEEK means to not
- // remove any from the queue, including the ones that are skipped.
+ // (which means that nobody else will read them).
// For reading only.
static const int kFromEnd = 0x0002;
// Causes reads to return NULL and writes to fail instead of waiting.
// For reading and writing.
static const int kNonBlock = 0x0004;
// Causes things to block.
- // IMPORTANT: Has a value of 0 so that it is the default. This has to stay.
+ // IMPORTANT: Has a value of 0 so that it is the default. This has to stay
+ // this way.
// For reading and writing.
static const int kBlock = 0x0000;
// Causes writes to overwrite the oldest message in the queue instead of
@@ -79,7 +81,7 @@
// Writes a message into the queue.
// This function takes ownership of msg.
// NOTE: msg must point to a valid message from this queue
- // Returns truen on success.
+ // Returns true on success.
bool WriteMessage(void *msg, int options);
// Reads a message out of the queue.
@@ -94,7 +96,7 @@
// same message twice with the same index argument. However, it may not
// return some messages that pass through the queue.
// *index should start as 0. index does not have to be in shared memory, but
- // it can be
+ // it can be.
const void *ReadMessageIndex(int options, int *index);
// Retrieves ("allocates") a message that can then be written to the queue.
@@ -111,6 +113,10 @@
if (msg != NULL) DecrementMessageReferenceCount(msg);
}
+ // UNSAFE! Returns the number of free messages we have. Only safe to use when
+ // only 1 task is using this object (ie in tests).
+ int FreeMessages() const;
+
private:
struct MessageHeader;
struct ReadData;
@@ -139,28 +145,28 @@
int messages_; // that have passed through
void **data_; // array of messages (with headers)
- Mutex pool_lock_;
size_t msg_length_; // sizeof(each message) including the header
- int mem_length_; // the max number of messages that will ever be allocated
- int messages_used_;
- int pool_length_; // the number of allocated messages
- MessageHeader **pool_; // array of pointers to messages
+ // A pointer to the first in the linked list of free messages.
+ MessageHeader *free_messages_;
// Actually frees the given message.
void DoFreeMessage(const void *msg);
// Calls DoFreeMessage if appropriate.
void DecrementMessageReferenceCount(const void *msg);
+ // Only does the actual incrementing of the reference count.
+ void IncrementMessageReferenceCount(const void *msg) const;
- // Should be called with data_lock_ locked.
+ // Must be called with data_lock_ locked.
// *read_data will be initialized.
// Returns with a readable message in data_ or false.
bool ReadCommonStart(int options, int *index, ReadData *read_data);
// Deals with setting/unsetting readable_ and writable_.
- // Should be called after data_lock_ has been unlocked.
+ // Must be called after data_lock_ has been unlocked.
// read_data should be the same thing that was passed in to ReadCommonStart.
void ReadCommonEnd(ReadData *read_data);
// Handles reading with kPeek.
- void *ReadPeek(int options, int start);
+ // start can be -1 if options has kFromEnd set.
+ void *ReadPeek(int options, int start) const;
// Gets called by Fetch when necessary (with placement new).
RawQueue(const char *name, size_t length, int hash, int queue_length);
diff --git a/aos/linux_code/ipc_lib/queue_test.cc b/aos/linux_code/ipc_lib/queue_test.cc
deleted file mode 100644
index 0a982cc..0000000
--- a/aos/linux_code/ipc_lib/queue_test.cc
+++ /dev/null
@@ -1,428 +0,0 @@
-#include "aos/common/queue.h"
-
-#include <unistd.h>
-#include <sys/mman.h>
-#include <inttypes.h>
-
-#include <ostream>
-#include <memory>
-#include <map>
-
-#include "gtest/gtest.h"
-
-#include "aos/linux_code/ipc_lib/core_lib.h"
-#include "aos/common/type_traits.h"
-#include "aos/common/queue_testutils.h"
-#include "aos/common/time.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/die.h"
-
-using ::testing::AssertionResult;
-using ::testing::AssertionSuccess;
-using ::testing::AssertionFailure;
-using ::aos::common::testing::GlobalCoreInstance;
-
-namespace aos {
-namespace testing {
-
-class QueueTest : public ::testing::Test {
- protected:
- static const size_t kFailureSize = 400;
- static char *fatal_failure;
- private:
- enum class ResultType : uint8_t {
- NotCalled,
- Called,
- Returned,
- };
- const std::string ResultTypeString(volatile const ResultType &result) {
- switch (result) {
- case ResultType::Returned:
- return "Returned";
- case ResultType::Called:
- return "Called";
- case ResultType::NotCalled:
- return "NotCalled";
- default:
- return std::string("unknown(" + static_cast<uint8_t>(result)) + ")";
- }
- }
- static_assert(aos::shm_ok<ResultType>::value,
- "this will get put in shared memory");
- template<typename T>
- struct FunctionToCall {
- FunctionToCall() : result(ResultType::NotCalled) {
- started.Lock();
- }
-
- volatile ResultType result;
- bool expected;
- void (*function)(T*, char*);
- T *arg;
- volatile char failure[kFailureSize];
- Mutex started;
- };
- template<typename T>
- static void Hangs_(FunctionToCall<T> *const to_call) {
- to_call->started.Unlock();
- to_call->result = ResultType::Called;
- to_call->function(to_call->arg, const_cast<char *>(to_call->failure));
- to_call->result = ResultType::Returned;
- }
-
- // How long until a function is considered to have hung.
- static constexpr time::Time kHangTime = time::Time::InSeconds(0.035);
- // How long to sleep after forking (for debugging).
- static constexpr time::Time kForkSleep = time::Time::InSeconds(0);
-
- // Represents a process that has been forked off. The destructor kills the
- // process and wait(2)s for it.
- class ForkedProcess {
- public:
- ForkedProcess(pid_t pid, mutex *lock) : pid_(pid), lock_(lock) {};
- ~ForkedProcess() {
- if (kill(pid_, SIGINT) == -1) {
- if (errno == ESRCH) {
- printf("process %jd was already dead\n", static_cast<intmax_t>(pid_));
- } else {
- fprintf(stderr, "kill(SIGKILL, %jd) failed with %d: %s\n",
- static_cast<intmax_t>(pid_), errno, strerror(errno));
- }
- return;
- }
- const pid_t ret = wait(NULL);
- if (ret == -1) {
- LOG(WARNING, "wait(NULL) failed."
- " child %jd might still be alive\n",
- static_cast<intmax_t>(pid_));
- } else if (ret == 0) {
- LOG(WARNING, "child %jd wasn't waitable. it might still be alive\n",
- static_cast<intmax_t>(pid_));
- } else if (ret != pid_) {
- LOG(WARNING, "child %d is now confirmed dead"
- ", but child %jd might still be alive\n",
- ret, static_cast<intmax_t>(pid_));
- }
- }
-
- enum class JoinResult {
- Finished, Hung, Error
- };
- JoinResult Join(time::Time timeout = kHangTime) {
- timespec lock_timeout = (kForkSleep + timeout).ToTimespec();
- switch (mutex_lock_timeout(lock_, &lock_timeout)) {
- case 2:
- return JoinResult::Hung;
- case 0:
- return JoinResult::Finished;
- default:
- return JoinResult::Error;
- }
- }
-
- private:
- const pid_t pid_;
- mutex *const lock_;
- } __attribute__((unused));
-
- // State for HangsFork and HangsCheck.
- typedef uint8_t ChildID;
- static void ReapExitHandler() {
- for (auto it = children_.begin(); it != children_.end(); ++it) {
- delete it->second;
- }
- }
- static std::map<ChildID, ForkedProcess *> children_;
- std::map<ChildID, FunctionToCall<void> *> to_calls_;
-
- void SetUp() override {
- ::testing::Test::SetUp();
-
- SetDieTestMode(true);
-
- fatal_failure = static_cast<char *>(shm_malloc(sizeof(fatal_failure)));
- static bool registered = false;
- if (!registered) {
- atexit(ReapExitHandler);
- registered = true;
- }
- }
-
- protected:
- // function gets called with arg in a forked process.
- // Leaks shared memory.
- template<typename T> __attribute__((warn_unused_result))
- std::unique_ptr<ForkedProcess> ForkExecute(void (*function)(T*), T *arg) {
- mutex *lock = static_cast<mutex *>(shm_malloc_aligned(
- sizeof(*lock), sizeof(int)));
- assert(mutex_lock(lock) == 0);
- const pid_t pid = fork();
- switch (pid) {
- case 0: // child
- if (kForkSleep != time::Time(0, 0)) {
- LOG(INFO, "pid %jd sleeping for %ds%dns\n",
- static_cast<intmax_t>(getpid()),
- kForkSleep.sec(), kForkSleep.nsec());
- time::SleepFor(kForkSleep);
- }
- ::aos::common::testing::PreventExit();
- function(arg);
- mutex_unlock(lock);
- exit(EXIT_SUCCESS);
- case -1: // parent failure
- LOG(ERROR, "fork() failed with %d: %s\n", errno, strerror(errno));
- return std::unique_ptr<ForkedProcess>();
- default: // parent
- return std::unique_ptr<ForkedProcess>(new ForkedProcess(pid, lock));
- }
- }
-
- // Checks whether or not the given function hangs.
- // expected is whether to return success or failure if the function hangs
- // NOTE: There are other reasons for it to return a failure than the function
- // doing the wrong thing.
- // Leaks shared memory.
- template<typename T>
- AssertionResult Hangs(void (*function)(T*, char*), T *arg, bool expected) {
- AssertionResult fork_result(HangsFork<T>(function, arg, expected, 0));
- if (!fork_result) {
- return fork_result;
- }
- return HangsCheck(0);
- }
- // Starts the first part of Hangs.
- // Use HangsCheck to get the result.
- // Returns whether the fork succeeded or not, NOT whether or not the hang
- // check succeeded.
- template<typename T>
- AssertionResult HangsFork(void (*function)(T*, char *), T *arg,
- bool expected, ChildID id) {
- static_assert(aos::shm_ok<FunctionToCall<T>>::value,
- "this is going into shared memory");
- FunctionToCall<T> *const to_call =
- static_cast<FunctionToCall<T> *>(
- shm_malloc_aligned(sizeof(*to_call), alignof(FunctionToCall<T>)));
- new (to_call) FunctionToCall<T>();
- to_call->function = function;
- to_call->arg = arg;
- to_call->expected = expected;
- to_call->failure[0] = '\0';
- static_cast<char *>(fatal_failure)[0] = '\0';
- children_[id] = ForkExecute(Hangs_, to_call).release();
- if (!children_[id]) return AssertionFailure() << "ForkExecute failed";
- to_calls_[id] = reinterpret_cast<FunctionToCall<void> *>(to_call);
- to_call->started.Lock();
- return AssertionSuccess();
- }
- // Checks whether or not a function hung like it was supposed to.
- // Use HangsFork first.
- // NOTE: calls to HangsFork and HangsCheck with the same id argument will
- // correspond, but they do not nest. Also, id 0 is used by Hangs.
- // Return value is the same as Hangs.
- AssertionResult HangsCheck(ChildID id) {
- std::unique_ptr<ForkedProcess> child(children_[id]);
- children_.erase(id);
- const ForkedProcess::JoinResult result = child->Join();
- if (to_calls_[id]->failure[0] != '\0') {
- return AssertionFailure() << "function says: "
- << const_cast<char *>(to_calls_[id]->failure);
- }
- if (result == ForkedProcess::JoinResult::Finished) {
- return !to_calls_[id]->expected ? AssertionSuccess() : (AssertionFailure()
- << "something happened and the the test only got to "
- << ResultTypeString(to_calls_[id]->result));
- } else {
- if (to_calls_[id]->result == ResultType::Called) {
- return to_calls_[id]->expected ? AssertionSuccess() :
- AssertionFailure();
- } else if (result == ForkedProcess::JoinResult::Error) {
- return AssertionFailure() << "error joining child";
- } else {
- abort();
- return AssertionFailure() << "something weird happened";
- }
- }
- }
-#define EXPECT_HANGS(function, arg) \
- EXPECT_HANGS_COND(function, arg, true, EXPECT_TRUE)
-#define EXPECT_RETURNS(function, arg) \
- EXPECT_HANGS_COND(function, arg, false, EXPECT_TRUE)
-#define EXPECT_RETURNS_FAILS(function, arg) \
- EXPECT_HANGS_COND(function, arg, false, EXPECT_FALSE)
-#define EXPECT_HANGS_COND(function, arg, hangs, cond) do { \
- cond(Hangs(function, arg, hangs)); \
- if (fatal_failure[0] != '\0') { \
- FAIL() << fatal_failure; \
- } \
-} while (false)
-
- struct TestMessage {
- // Some contents because we don't really want to test empty messages.
- int16_t data;
- };
- struct MessageArgs {
- RawQueue *const queue;
- int flags;
- int16_t data; // -1 means NULL expected
- };
- static void WriteTestMessage(MessageArgs *args, char *failure) {
- TestMessage *msg = static_cast<TestMessage *>(args->queue->GetMessage());
- if (msg == NULL) {
- snprintf(fatal_failure, kFailureSize,
- "couldn't get_msg from %p", args->queue);
- return;
- }
- msg->data = args->data;
- if (!args->queue->WriteMessage(msg, args->flags)) {
- snprintf(failure, kFailureSize, "write_msg_free(%p, %p, %d) failed",
- args->queue, msg, args->flags);
- }
- }
- static void ReadTestMessage(MessageArgs *args, char *failure) {
- const TestMessage *msg = static_cast<const TestMessage *>(
- args->queue->ReadMessage(args->flags));
- if (msg == NULL) {
- if (args->data != -1) {
- snprintf(failure, kFailureSize,
- "expected data of %" PRId16 " but got NULL message",
- args->data);
- }
- } else {
- if (args->data != msg->data) {
- snprintf(failure, kFailureSize,
- "expected data of %" PRId16 " but got %" PRId16 " instead",
- args->data, msg->data);
- }
- args->queue->FreeMessage(msg);
- }
- }
-
- private:
- GlobalCoreInstance my_core;
-};
-char *QueueTest::fatal_failure;
-std::map<QueueTest::ChildID, QueueTest::ForkedProcess *> QueueTest::children_;
-constexpr time::Time QueueTest::kHangTime;
-constexpr time::Time QueueTest::kForkSleep;
-
-TEST_F(QueueTest, Reading) {
- RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
- MessageArgs args{queue, 0, -1};
-
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kNonBlock | RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = 0;
- EXPECT_HANGS(ReadTestMessage, &args);
- args.flags = RawQueue::kPeek;
- EXPECT_HANGS(ReadTestMessage, &args);
- args.data = 254;
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kPeek | RawQueue::kNonBlock;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = 0;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = 0;
- args.data = -1;
- EXPECT_HANGS(ReadTestMessage, &args);
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = 0;
- args.data = 971;
- EXPECT_RETURNS_FAILS(ReadTestMessage, &args);
-}
-TEST_F(QueueTest, Writing) {
- RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
- MessageArgs args{queue, 0, 973};
-
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = RawQueue::kBlock;
- EXPECT_HANGS(WriteTestMessage, &args);
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
- args.flags = RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.data = 971;
- args.flags = RawQueue::kOverride;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = RawQueue::kOverride;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = 0;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kNonBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = 0;
- EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = RawQueue::kOverride;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = 0;
- EXPECT_RETURNS(ReadTestMessage, &args);
-}
-
-TEST_F(QueueTest, MultiRead) {
- RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
- MessageArgs args{queue, 0, 1323};
-
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = RawQueue::kBlock;
- ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 1));
- ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 2));
- EXPECT_TRUE(HangsCheck(1) != HangsCheck(2));
- // TODO(brians) finish this
-}
-
-TEST_F(QueueTest, Recycle) {
- // TODO(brians) basic test of recycle queue
- // include all of the ways a message can get into the recycle queue
- RawQueue *recycle_queue = reinterpret_cast<RawQueue *>(23);
- RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage),
- 1, 2, 2, 2, &recycle_queue);
- ASSERT_NE(reinterpret_cast<RawQueue *>(23), recycle_queue);
- MessageArgs args{queue, 0, 973}, recycle{recycle_queue, 0, 973};
-
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(WriteTestMessage, &args);
- EXPECT_HANGS(ReadTestMessage, &recycle);
- args.data = 254;
- EXPECT_RETURNS(WriteTestMessage, &args);
- EXPECT_HANGS(ReadTestMessage, &recycle);
- args.data = 971;
- args.flags = RawQueue::kOverride;
- EXPECT_RETURNS(WriteTestMessage, &args);
- recycle.flags = RawQueue::kBlock;
- EXPECT_RETURNS(ReadTestMessage, &recycle);
-
- EXPECT_HANGS(ReadTestMessage, &recycle);
-
- TestMessage *msg = static_cast<TestMessage *>(queue->GetMessage());
- ASSERT_TRUE(msg != NULL);
- msg->data = 341;
- queue->FreeMessage(msg);
- recycle.data = 341;
- EXPECT_RETURNS(ReadTestMessage, &recycle);
-
- EXPECT_HANGS(ReadTestMessage, &recycle);
-
- args.data = 254;
- args.flags = RawQueue::kPeek;
- EXPECT_RETURNS(ReadTestMessage, &args);
- recycle.flags = RawQueue::kBlock;
- EXPECT_HANGS(ReadTestMessage, &recycle);
- args.flags = RawQueue::kBlock;
- EXPECT_RETURNS(ReadTestMessage, &args);
- recycle.data = 254;
- EXPECT_RETURNS(ReadTestMessage, &recycle);
-}
-
-} // namespace testing
-} // namespace aos
diff --git a/aos/linux_code/ipc_lib/raw_queue_test.cc b/aos/linux_code/ipc_lib/raw_queue_test.cc
new file mode 100644
index 0000000..e91655a
--- /dev/null
+++ b/aos/linux_code/ipc_lib/raw_queue_test.cc
@@ -0,0 +1,881 @@
+#include "aos/common/queue.h"
+
+#include <unistd.h>
+#include <sys/mman.h>
+#include <inttypes.h>
+
+#include <ostream>
+#include <memory>
+#include <map>
+
+#include "gtest/gtest.h"
+
+#include "aos/linux_code/ipc_lib/core_lib.h"
+#include "aos/common/type_traits.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/time.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/die.h"
+
+using ::testing::AssertionResult;
+using ::testing::AssertionSuccess;
+using ::testing::AssertionFailure;
+using ::aos::common::testing::GlobalCoreInstance;
+
+namespace aos {
+namespace testing {
+
+// The same constant from queue.cc. This will have to be updated if that one is.
+const int kExtraMessages = 20;
+
+class RawQueueTest : public ::testing::Test {
+ protected:
+ static const size_t kFailureSize = 400;
+ static char *fatal_failure;
+ private:
+ enum class ResultType : uint8_t {
+ NotCalled,
+ Called,
+ Returned,
+ };
+ const std::string ResultTypeString(volatile const ResultType &result) {
+ switch (result) {
+ case ResultType::Returned:
+ return "Returned";
+ case ResultType::Called:
+ return "Called";
+ case ResultType::NotCalled:
+ return "NotCalled";
+ default:
+ return std::string("unknown(" + static_cast<uint8_t>(result)) + ")";
+ }
+ }
+ static_assert(aos::shm_ok<ResultType>::value,
+ "this will get put in shared memory");
+ template<typename T>
+ struct FunctionToCall {
+ FunctionToCall() : result(ResultType::NotCalled) {
+ started.Lock();
+ }
+
+ volatile ResultType result;
+ bool expected;
+ void (*function)(T*, char*);
+ T *arg;
+ volatile char failure[kFailureSize];
+ Mutex started;
+ };
+ template<typename T>
+ static void Hangs_(FunctionToCall<T> *const to_call) {
+ to_call->started.Unlock();
+ to_call->result = ResultType::Called;
+ to_call->function(to_call->arg, const_cast<char *>(to_call->failure));
+ to_call->result = ResultType::Returned;
+ }
+
+ // How long until a function is considered to have hung.
+ static constexpr time::Time kHangTime = time::Time::InSeconds(0.035);
+ // How long to sleep after forking (for debugging).
+ static constexpr time::Time kForkSleep = time::Time::InSeconds(0);
+
+ // Represents a process that has been forked off. The destructor kills the
+ // process and wait(2)s for it.
+ class ForkedProcess {
+ public:
+ ForkedProcess(pid_t pid, mutex *lock) : pid_(pid), lock_(lock) {};
+ ~ForkedProcess() {
+ if (kill(pid_, SIGINT) == -1) {
+ if (errno == ESRCH) {
+ printf("process %jd was already dead\n", static_cast<intmax_t>(pid_));
+ } else {
+ fprintf(stderr, "kill(SIGKILL, %jd) failed with %d: %s\n",
+ static_cast<intmax_t>(pid_), errno, strerror(errno));
+ }
+ return;
+ }
+ const pid_t ret = wait(NULL);
+ if (ret == -1) {
+ LOG(WARNING, "wait(NULL) failed."
+ " child %jd might still be alive\n",
+ static_cast<intmax_t>(pid_));
+ } else if (ret == 0) {
+ LOG(WARNING, "child %jd wasn't waitable. it might still be alive\n",
+ static_cast<intmax_t>(pid_));
+ } else if (ret != pid_) {
+ LOG(WARNING, "child %d is now confirmed dead"
+ ", but child %jd might still be alive\n",
+ ret, static_cast<intmax_t>(pid_));
+ }
+ }
+
+ enum class JoinResult {
+ Finished, Hung, Error
+ };
+ JoinResult Join(time::Time timeout = kHangTime) {
+ timespec lock_timeout = (kForkSleep + timeout).ToTimespec();
+ switch (mutex_lock_timeout(lock_, &lock_timeout)) {
+ case 2:
+ return JoinResult::Hung;
+ case 0:
+ return JoinResult::Finished;
+ default:
+ return JoinResult::Error;
+ }
+ }
+
+ private:
+ const pid_t pid_;
+ mutex *const lock_;
+ } __attribute__((unused));
+
+ // State for HangsFork and HangsCheck.
+ typedef uint8_t ChildID;
+ static void ReapExitHandler() {
+ for (auto it = children_.begin(); it != children_.end(); ++it) {
+ delete it->second;
+ }
+ }
+ static std::map<ChildID, ForkedProcess *> children_;
+ std::map<ChildID, FunctionToCall<void> *> to_calls_;
+
+ void SetUp() override {
+ ::testing::Test::SetUp();
+
+ SetDieTestMode(true);
+
+ fatal_failure = static_cast<char *>(shm_malloc(sizeof(fatal_failure)));
+ static bool registered = false;
+ if (!registered) {
+ atexit(ReapExitHandler);
+ registered = true;
+ }
+ }
+
+ protected:
+ // function gets called with arg in a forked process.
+ // Leaks shared memory.
+ template<typename T> __attribute__((warn_unused_result))
+ std::unique_ptr<ForkedProcess> ForkExecute(void (*function)(T*), T *arg) {
+ mutex *lock = static_cast<mutex *>(shm_malloc_aligned(
+ sizeof(*lock), sizeof(int)));
+ assert(mutex_lock(lock) == 0);
+ const pid_t pid = fork();
+ switch (pid) {
+ case 0: // child
+ if (kForkSleep != time::Time(0, 0)) {
+ LOG(INFO, "pid %jd sleeping for %ds%dns\n",
+ static_cast<intmax_t>(getpid()),
+ kForkSleep.sec(), kForkSleep.nsec());
+ time::SleepFor(kForkSleep);
+ }
+ ::aos::common::testing::PreventExit();
+ function(arg);
+ mutex_unlock(lock);
+ exit(EXIT_SUCCESS);
+ case -1: // parent failure
+ LOG(ERROR, "fork() failed with %d: %s\n", errno, strerror(errno));
+ return std::unique_ptr<ForkedProcess>();
+ default: // parent
+ return std::unique_ptr<ForkedProcess>(new ForkedProcess(pid, lock));
+ }
+ }
+
+ // Checks whether or not the given function hangs.
+ // expected is whether to return success or failure if the function hangs
+ // NOTE: There are other reasons for it to return a failure than the function
+ // doing the wrong thing.
+ // Leaks shared memory.
+ template<typename T>
+ AssertionResult Hangs(void (*function)(T*, char*), T *arg, bool expected) {
+ AssertionResult fork_result(HangsFork<T>(function, arg, expected, 0));
+ if (!fork_result) {
+ return fork_result;
+ }
+ return HangsCheck(0);
+ }
+ // Starts the first part of Hangs.
+ // Use HangsCheck to get the result.
+ // Returns whether the fork succeeded or not, NOT whether or not the hang
+ // check succeeded.
+ template<typename T>
+ AssertionResult HangsFork(void (*function)(T*, char *), T *arg,
+ bool expected, ChildID id) {
+ static_assert(aos::shm_ok<FunctionToCall<T>>::value,
+ "this is going into shared memory");
+ FunctionToCall<T> *const to_call =
+ static_cast<FunctionToCall<T> *>(
+ shm_malloc_aligned(sizeof(*to_call), alignof(FunctionToCall<T>)));
+ new (to_call) FunctionToCall<T>();
+ to_call->function = function;
+ to_call->arg = arg;
+ to_call->expected = expected;
+ to_call->failure[0] = '\0';
+ static_cast<char *>(fatal_failure)[0] = '\0';
+ children_[id] = ForkExecute(Hangs_, to_call).release();
+ if (!children_[id]) return AssertionFailure() << "ForkExecute failed";
+ to_calls_[id] = reinterpret_cast<FunctionToCall<void> *>(to_call);
+ to_call->started.Lock();
+ return AssertionSuccess();
+ }
+ // Checks whether or not a function hung like it was supposed to.
+ // Use HangsFork first.
+ // NOTE: calls to HangsFork and HangsCheck with the same id argument will
+ // correspond, but they do not nest. Also, id 0 is used by Hangs.
+ // Return value is the same as Hangs.
+ AssertionResult HangsCheck(ChildID id) {
+ std::unique_ptr<ForkedProcess> child(children_[id]);
+ children_.erase(id);
+ const ForkedProcess::JoinResult result = child->Join();
+ if (to_calls_[id]->failure[0] != '\0') {
+ return AssertionFailure() << "function says: "
+ << const_cast<char *>(to_calls_[id]->failure);
+ }
+ if (result == ForkedProcess::JoinResult::Finished) {
+ return !to_calls_[id]->expected ? AssertionSuccess() : (AssertionFailure()
+ << "something happened and the the test only got to "
+ << ResultTypeString(to_calls_[id]->result));
+ } else {
+ if (to_calls_[id]->result == ResultType::Called) {
+ return to_calls_[id]->expected ? AssertionSuccess() :
+ AssertionFailure();
+ } else if (result == ForkedProcess::JoinResult::Error) {
+ return AssertionFailure() << "error joining child";
+ } else {
+ abort();
+ return AssertionFailure() << "something weird happened";
+ }
+ }
+ }
+#define EXPECT_HANGS(function, arg) \
+ EXPECT_HANGS_COND(function, arg, true, EXPECT_TRUE)
+#define EXPECT_RETURNS(function, arg) \
+ EXPECT_HANGS_COND(function, arg, false, EXPECT_TRUE)
+#define EXPECT_RETURNS_FAILS(function, arg) \
+ EXPECT_HANGS_COND(function, arg, false, EXPECT_FALSE)
+#define EXPECT_HANGS_COND(function, arg, hangs, cond) do { \
+ cond(Hangs(function, arg, hangs)); \
+ if (fatal_failure[0] != '\0') { \
+ FAIL() << fatal_failure; \
+ } \
+} while (false)
+
+ struct TestMessage {
+ // Some contents because we don't really want to test empty messages.
+ int16_t data;
+ };
+ struct MessageArgs {
+ RawQueue *const queue;
+ int flags;
+ int16_t data; // -1 means NULL expected
+ };
+ static void WriteTestMessage(MessageArgs *args, char *failure) {
+ TestMessage *msg = static_cast<TestMessage *>(args->queue->GetMessage());
+ if (msg == NULL) {
+ snprintf(fatal_failure, kFailureSize,
+ "couldn't get_msg from %p", args->queue);
+ return;
+ }
+ msg->data = args->data;
+ if (!args->queue->WriteMessage(msg, args->flags)) {
+ snprintf(failure, kFailureSize, "write_msg_free(%p, %p, %d) failed",
+ args->queue, msg, args->flags);
+ }
+ }
+ static void ReadTestMessage(MessageArgs *args, char *failure) {
+ const TestMessage *msg = static_cast<const TestMessage *>(
+ args->queue->ReadMessage(args->flags));
+ if (msg == NULL) {
+ if (args->data != -1) {
+ snprintf(failure, kFailureSize,
+ "expected data of %" PRId16 " but got NULL message",
+ args->data);
+ }
+ } else {
+ if (args->data != msg->data) {
+ snprintf(failure, kFailureSize,
+ "expected data of %" PRId16 " but got %" PRId16 " instead",
+ args->data, msg->data);
+ }
+ args->queue->FreeMessage(msg);
+ }
+ }
+
+ void PushMessage(RawQueue *queue, uint16_t data) {
+ TestMessage *message = static_cast<TestMessage *>(queue->GetMessage());
+ message->data = data;
+ ASSERT_TRUE(queue->WriteMessage(message, RawQueue::kOverride));
+ }
+
+ private:
+ GlobalCoreInstance my_core;
+};
+char *RawQueueTest::fatal_failure;
+std::map<RawQueueTest::ChildID, RawQueueTest::ForkedProcess *>
+ RawQueueTest::children_;
+constexpr time::Time RawQueueTest::kHangTime;
+constexpr time::Time RawQueueTest::kForkSleep;
+
+TEST_F(RawQueueTest, Reading) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
+ MessageArgs args{queue, 0, -1};
+
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kNonBlock | RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ EXPECT_HANGS(ReadTestMessage, &args);
+ args.flags = RawQueue::kPeek;
+ EXPECT_HANGS(ReadTestMessage, &args);
+ args.data = 254;
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kPeek | RawQueue::kNonBlock;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ args.data = -1;
+ EXPECT_HANGS(ReadTestMessage, &args);
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ args.data = 971;
+ EXPECT_RETURNS_FAILS(ReadTestMessage, &args);
+}
+TEST_F(RawQueueTest, Writing) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
+ MessageArgs args{queue, 0, 973};
+
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = RawQueue::kBlock;
+ EXPECT_HANGS(WriteTestMessage, &args);
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
+ args.flags = RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.data = 971;
+ args.flags = RawQueue::kOverride;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = RawQueue::kOverride;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kNonBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = RawQueue::kOverride;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+}
+
+TEST_F(RawQueueTest, MultiRead) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
+ MessageArgs args{queue, 0, 1323};
+
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = RawQueue::kBlock;
+ ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 1));
+ ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 2));
+ EXPECT_TRUE(HangsCheck(1) != HangsCheck(2));
+ // TODO(brians) finish this
+}
+
+// There used to be a bug where reading first without an index and then with an
+// index would crash. This test makes sure that's fixed.
+TEST_F(RawQueueTest, ReadIndexAndNot) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+
+ // Write a message, read it (with ReadMessage), and then write another
+ // message (before freeing the read one so the queue allocates a distinct
+ // message to use for it).
+ TestMessage *msg = static_cast<TestMessage *>(queue->GetMessage());
+ ASSERT_NE(nullptr, msg);
+ ASSERT_TRUE(queue->WriteMessage(msg, 0));
+ const void *read_msg = queue->ReadMessage(0);
+ EXPECT_NE(nullptr, read_msg);
+ msg = static_cast<TestMessage *>(queue->GetMessage());
+ queue->FreeMessage(read_msg);
+ ASSERT_NE(nullptr, msg);
+ ASSERT_TRUE(queue->WriteMessage(msg, 0));
+
+ int index = 0;
+ const void *second_read_msg = queue->ReadMessageIndex(0, &index);
+ EXPECT_NE(nullptr, second_read_msg);
+ EXPECT_NE(read_msg, second_read_msg)
+ << "We already took that message out of the queue.";
+}
+
+TEST_F(RawQueueTest, Recycle) {
+ // TODO(brians) basic test of recycle queue
+ // include all of the ways a message can get into the recycle queue
+ RawQueue *recycle_queue = reinterpret_cast<RawQueue *>(23);
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage),
+ 1, 2, 2, 2, &recycle_queue);
+ ASSERT_NE(reinterpret_cast<RawQueue *>(23), recycle_queue);
+ MessageArgs args{queue, 0, 973}, recycle{recycle_queue, 0, 973};
+
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+ args.data = 254;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+ args.data = 971;
+ args.flags = RawQueue::kOverride;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ recycle.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(ReadTestMessage, &recycle);
+
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+
+ TestMessage *msg = static_cast<TestMessage *>(queue->GetMessage());
+ ASSERT_TRUE(msg != NULL);
+ msg->data = 341;
+ queue->FreeMessage(msg);
+ recycle.data = 341;
+ EXPECT_RETURNS(ReadTestMessage, &recycle);
+
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+
+ args.data = 254;
+ args.flags = RawQueue::kPeek;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ recycle.flags = RawQueue::kBlock;
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+ args.flags = RawQueue::kBlock;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ recycle.data = 254;
+ EXPECT_RETURNS(ReadTestMessage, &recycle);
+}
+
+// Makes sure that when a message doesn't get written with kNonBlock it does get
+// freed.
+TEST_F(RawQueueTest, NonBlockFailFree) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
+
+ void *message1 = queue->GetMessage();
+ void *message2 = queue->GetMessage();
+ ASSERT_TRUE(queue->WriteMessage(message1, RawQueue::kNonBlock));
+ ASSERT_FALSE(queue->WriteMessage(message2, RawQueue::kNonBlock));
+ EXPECT_EQ(message2, queue->GetMessage());
+}
+
+TEST_F(RawQueueTest, ReadIndexNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ EXPECT_EQ(0, kExtraMessages + 2 - queue->FreeMessages());
+ PushMessage(queue, 971);
+ EXPECT_EQ(1, kExtraMessages + 2 - queue->FreeMessages());
+
+ int index = 0;
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(971, message->data);
+ EXPECT_EQ(1, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ PushMessage(queue, 1768);
+ EXPECT_EQ(2, kExtraMessages + 2 - queue->FreeMessages());
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ PushMessage(queue, 254);
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ EXPECT_EQ(2, kExtraMessages + 2 - queue->FreeMessages());
+}
+
+TEST_F(RawQueueTest, ReadIndexNotBehind) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ EXPECT_EQ(0, kExtraMessages + 2 - queue->FreeMessages());
+ PushMessage(queue, 971);
+ EXPECT_EQ(1, kExtraMessages + 2 - queue->FreeMessages());
+ PushMessage(queue, 1768);
+ EXPECT_EQ(2, kExtraMessages + 2 - queue->FreeMessages());
+
+ int index = 0;
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(971, message->data);
+ EXPECT_EQ(1, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexLittleBehindNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexMoreBehind) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1768, message->data);
+ EXPECT_EQ(2, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexMoreBehindNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexLotBehind) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ {
+ const void *message1, *message2;
+ message1 = queue->ReadMessage(RawQueue::kNonBlock);
+ ASSERT_NE(nullptr, message1);
+ PushMessage(queue, 254);
+ message2 = queue->ReadMessage(RawQueue::kNonBlock);
+ ASSERT_NE(nullptr, message2);
+ PushMessage(queue, 973);
+ EXPECT_EQ(4, kExtraMessages + 2 - queue->FreeMessages());
+ queue->FreeMessage(message1);
+ EXPECT_EQ(3, kExtraMessages + 2 - queue->FreeMessages());
+ queue->FreeMessage(message2);
+ EXPECT_EQ(2, kExtraMessages + 2 - queue->FreeMessages());
+ }
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(254, message->data);
+ EXPECT_EQ(3, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexLotBehindNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 973);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexEvenMoreBehind) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 973);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 1114);
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(973, message->data);
+ EXPECT_EQ(4, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1114, message->data);
+ EXPECT_EQ(5, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1114, message->data);
+ EXPECT_EQ(5, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, ReadIndexEvenMoreBehindNotFull) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const TestMessage *message, *peek_message;
+
+ PushMessage(queue, 971);
+ PushMessage(queue, 1768);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 254);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 973);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+ PushMessage(queue, 1114);
+ ASSERT_NE(nullptr, queue->ReadMessage(RawQueue::kNonBlock));
+
+ int index = 0;
+
+ peek_message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock | RawQueue::kPeek, &index));
+ message = static_cast<const TestMessage *>(
+ queue->ReadMessageIndex(RawQueue::kNonBlock, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1114, message->data);
+ EXPECT_EQ(5, index);
+ queue->FreeMessage(message);
+ queue->FreeMessage(peek_message);
+
+ index = 0;
+ peek_message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kPeek | RawQueue::kFromEnd, &index));
+ message = static_cast<const TestMessage *>(queue->ReadMessageIndex(
+ RawQueue::kNonBlock | RawQueue::kFromEnd, &index));
+ ASSERT_NE(nullptr, message);
+ EXPECT_EQ(message, peek_message);
+ EXPECT_EQ(1114, message->data);
+ EXPECT_EQ(5, index);
+ queue->FreeMessage(message);
+}
+
+TEST_F(RawQueueTest, MessageReferenceCounts) {
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 2);
+ const void *message1, *message2;
+
+ EXPECT_EQ(0, kExtraMessages + 2 - queue->FreeMessages());
+ message1 = queue->GetMessage();
+ EXPECT_NE(nullptr, message1);
+ EXPECT_EQ(1, kExtraMessages + 2 - queue->FreeMessages());
+ message2 = queue->GetMessage();
+ EXPECT_NE(nullptr, message2);
+ EXPECT_EQ(2, kExtraMessages + 2 - queue->FreeMessages());
+ queue->FreeMessage(message1);
+ EXPECT_EQ(1, kExtraMessages + 2 - queue->FreeMessages());
+ queue->FreeMessage(message2);
+ EXPECT_EQ(0, kExtraMessages + 2 - queue->FreeMessages());
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/linux_code/ipc_lib/shared_mem.c b/aos/linux_code/ipc_lib/shared_mem.c
index 069fe19..4071ca2 100644
--- a/aos/linux_code/ipc_lib/shared_mem.c
+++ b/aos/linux_code/ipc_lib/shared_mem.c
@@ -7,6 +7,7 @@
#include <unistd.h>
#include <sys/types.h>
#include <errno.h>
+#include <stdlib.h>
#include "aos/linux_code/ipc_lib/core_lib.h"
@@ -35,32 +36,45 @@
struct aos_core *global_core = NULL;
+// TODO(brians): madvise(2) it to put this shm in core dumps.
int aos_core_create_shared_mem(enum aos_core_create to_create) {
static struct aos_core global_core_data;
global_core = &global_core_data;
+
+ {
+ char *shm_name = getenv("AOS_SHM_NAME");
+ if (shm_name == NULL) {
+ global_core->shm_name = AOS_SHM_NAME;
+ } else {
+ global_core->shm_name = shm_name;
+ }
+ }
+
int shm;
before:
if (to_create == create) {
- printf("shared_mem: creating\n");
- shm = shm_open(AOS_SHM_NAME, O_RDWR | O_CREAT | O_EXCL, 0666);
+ printf("shared_mem: creating %s\n", global_core->shm_name);
+ shm = shm_open(global_core->shm_name, O_RDWR | O_CREAT | O_EXCL, 0666);
global_core->owner = 1;
if (shm == -1 && errno == EEXIST) {
- printf("shared_mem: going to shm_unlink(" AOS_SHM_NAME ")\n");
- if (shm_unlink(AOS_SHM_NAME) == -1) {
- fprintf(stderr, "shared_mem: shm_unlink(" AOS_SHM_NAME ") failed with of %d: %s\n", errno, strerror(errno));
+ printf("shared_mem: going to shm_unlink(%s)\n", global_core->shm_name);
+ if (shm_unlink(global_core->shm_name) == -1) {
+ fprintf(stderr, "shared_mem: shm_unlink(%s) failed with of %d: %s\n",
+ global_core->shm_name, errno, strerror(errno));
} else {
goto before;
}
}
} else {
printf("shared_mem: not creating\n");
- shm = shm_open(AOS_SHM_NAME, O_RDWR, 0);
+ shm = shm_open(global_core->shm_name, O_RDWR, 0);
global_core->owner = 0;
}
if (shm == -1) {
fprintf(stderr, "shared_mem:"
- " shm_open(" AOS_SHM_NAME ", O_RDWR [| O_CREAT | O_EXCL, 0|0666)"
- " failed with %d: %s\n", errno, strerror(errno));
+ " shm_open(%s, O_RDWR [| O_CREAT | O_EXCL, 0|0666)"
+ " failed with %d: %s\n",
+ global_core->shm_name, errno, strerror(errno));
return -1;
}
if (global_core->owner) {
@@ -90,13 +104,17 @@
(void *)SHM_START, shm_address);
return -1;
}
- return aos_core_use_address_as_shared_mem(shm_address, SIZEOFSHMSEG);
+ int r = aos_core_use_address_as_shared_mem(shm_address, SIZEOFSHMSEG);
+ fprintf(stderr, "shared_mem: end of create_shared_mem owner=%d r=%d\n",
+ global_core->owner, r);
+ return r;
}
int aos_core_use_address_as_shared_mem(void *address, size_t size) {
global_core->mem_struct = address;
global_core->size = size;
- global_core->shared_mem = (uint8_t *)address + sizeof(*global_core->mem_struct);
+ global_core->shared_mem =
+ (uint8_t *)address + sizeof(*global_core->mem_struct);
if (global_core->owner) {
global_core->mem_struct->msg_alloc = (uint8_t *)address + global_core->size;
init_shared_mem_core(global_core->mem_struct);
@@ -109,8 +127,6 @@
return -1;
}
}
- fprintf(stderr, "shared_mem: end of create_shared_mem owner=%d\n",
- global_core->owner);
return 0;
}
@@ -122,9 +138,9 @@
return -1;
}
if (global_core->owner) {
- if (shm_unlink(AOS_SHM_NAME)) {
- fprintf(stderr, "shared_mem: shm_unlink(" AOS_SHM_NAME ") failed with %d: %s\n",
- errno, strerror(errno));
+ if (shm_unlink(global_core->shm_name)) {
+ fprintf(stderr, "shared_mem: shm_unlink(%s) failed with %d: %s\n",
+ global_core->shm_name, errno, strerror(errno));
return -1;
}
}
diff --git a/aos/linux_code/ipc_lib/shared_mem.h b/aos/linux_code/ipc_lib/shared_mem.h
index 4d5e88e..edab4d0 100644
--- a/aos/linux_code/ipc_lib/shared_mem.h
+++ b/aos/linux_code/ipc_lib/shared_mem.h
@@ -52,6 +52,8 @@
// How large the chunk of shared memory is.
ptrdiff_t size;
aos_shm_core *mem_struct;
+ // For the owner to store the name of the file to unlink when closing.
+ const char *shm_name;
};
void init_shared_mem_core(aos_shm_core *shm_core);
diff --git a/aos/atom_code/ipc_lib/unique_message_ptr.h b/aos/linux_code/ipc_lib/unique_message_ptr.h
similarity index 100%
rename from aos/atom_code/ipc_lib/unique_message_ptr.h
rename to aos/linux_code/ipc_lib/unique_message_ptr.h
diff --git a/aos/linux_code/logging/binary_log_file.cc b/aos/linux_code/logging/binary_log_file.cc
index 8c8c8b4..896422c 100644
--- a/aos/linux_code/logging/binary_log_file.cc
+++ b/aos/linux_code/logging/binary_log_file.cc
@@ -6,6 +6,8 @@
#include <sys/mman.h>
#include <sys/stat.h>
#include <unistd.h>
+#include <signal.h>
+#include <setjmp.h>
namespace aos {
namespace logging {
@@ -110,12 +112,21 @@
MAP_SHARED, fd_, offset_));
if (current_ == MAP_FAILED) {
LOG(FATAL,
- "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)"
- " failed with %d: %s. aborting\n",
+ "mmap(NULL, %zd, PROT_READ [ | PROT_WRITE], MAP_SHARED, %d, %jd)"
+ " failed with %d: %s\n",
kPageSize, fd_, static_cast<intmax_t>(offset_), errno,
strerror(errno));
}
+ if (madvise(current_, kPageSize, MADV_SEQUENTIAL | MADV_WILLNEED) == -1) {
+ LOG(WARNING, "madvise(%p, %zd, MADV_SEQUENTIAL | MADV_WILLNEED)"
+ " failed with %d: %s\n",
+ current_, kPageSize, errno, strerror(errno));
+ }
offset_ += kPageSize;
+
+ if (!writable_) {
+ CheckCurrentPageReadable();
+ }
}
void LogFileAccessor::Unmap(void *location) {
@@ -126,6 +137,59 @@
is_last_page_ = 0;
}
+// This mess is because the only not completely hackish way to do this is to set
+// up a handler for SIGBUS/SIGSEGV that siglongjmps out to avoid either the
+// instruction being repeated infinitely (and more signals being delivered) or
+// (with SA_RESETHAND) the signal killing the process.
+namespace {
+
+void *volatile fault_address;
+sigjmp_buf jump_context;
+
+void CheckCurrentPageReadableHandler(int /*signal*/, siginfo_t *info, void *) {
+ fault_address = info->si_addr;
+
+ siglongjmp(jump_context, 1);
+}
+
+} // namespace
+void LogFileAccessor::CheckCurrentPageReadable() {
+ if (sigsetjmp(jump_context, 1) == 0) {
+ struct sigaction action;
+ action.sa_sigaction = CheckCurrentPageReadableHandler;
+ sigemptyset(&action.sa_mask);
+ action.sa_flags = SA_RESETHAND | SA_SIGINFO;
+ struct sigaction previous_bus, previous_segv;
+ if (sigaction(SIGBUS, &action, &previous_bus) == -1) {
+ LOG(FATAL, "sigaction(SIGBUS(=%d), %p, %p) failed with %d: %s\n",
+ SIGBUS, &action, &previous_bus, errno, strerror(errno));
+ }
+ if (sigaction(SIGSEGV, &action, &previous_segv) == -1) {
+ LOG(FATAL, "sigaction(SIGSEGV(=%d), %p, %p) failed with %d: %s\n",
+ SIGSEGV, &action, &previous_segv, errno, strerror(errno));
+ }
+
+ char __attribute__((unused)) c = current_[0];
+
+ if (sigaction(SIGBUS, &previous_bus, NULL) == -1) {
+ LOG(FATAL, "sigaction(SIGBUS(=%d), %p, NULL) failed with %d: %s\n",
+ SIGBUS, &previous_bus, errno, strerror(errno));
+ }
+ if (sigaction(SIGSEGV, &previous_segv, NULL) == -1) {
+ LOG(FATAL, "sigaction(SIGSEGV(=%d), %p, NULL) failed with %d: %s\n",
+ SIGSEGV, &previous_segv, errno, strerror(errno));
+ }
+ } else {
+ if (fault_address == current_) {
+ LOG(FATAL, "could not read 1 byte at offset 0x%jx into log file\n",
+ static_cast<uintmax_t>(offset_));
+ } else {
+ LOG(FATAL, "faulted at %p, not %p like we were (maybe) supposed to\n",
+ fault_address, current_);
+ }
+ }
+}
+
} // namespace linux_code
} // namespace logging
} // namespace aos
diff --git a/aos/linux_code/logging/binary_log_file.h b/aos/linux_code/logging/binary_log_file.h
index 0f8c3fb..f25482c 100644
--- a/aos/linux_code/logging/binary_log_file.h
+++ b/aos/linux_code/logging/binary_log_file.h
@@ -39,6 +39,7 @@
kString,
kStructType,
kStruct,
+ kMatrix,
};
// Gets futex_set once this one has been written
@@ -118,6 +119,9 @@
void MapNextPage();
void Unmap(void *location);
+ // Tries reading from the current page to see if it fails because the file
+ // isn't big enough.
+ void CheckCurrentPageReadable();
// Advances position to the next (aligned) location.
void AlignPosition() {
diff --git a/aos/linux_code/logging/binary_log_writer.cc b/aos/linux_code/logging/binary_log_writer.cc
index 67cb166..7b9b6da 100644
--- a/aos/linux_code/logging/binary_log_writer.cc
+++ b/aos/linux_code/logging/binary_log_writer.cc
@@ -8,6 +8,7 @@
#include <sys/types.h>
#include <pwd.h>
#include <fcntl.h>
+#include <dirent.h>
#include <map>
#include <unordered_set>
@@ -17,6 +18,7 @@
#include "aos/linux_code/init.h"
#include "aos/linux_code/configuration.h"
#include "aos/common/queue_types.h"
+#include "aos/common/die.h"
namespace aos {
namespace logging {
@@ -58,6 +60,47 @@
written_type_ids.insert(type_id);
}
+void AllocateLogName(char **filename, const char *directory) {
+ int fileindex = 0;
+ DIR *d;
+ if ((d = opendir(directory))) {
+ int index = 0;
+ struct dirent *dir;
+ while ((dir = readdir(d)) != NULL) {
+ if (sscanf(dir->d_name, "aos_log-%d", &index) == 1) {
+ if (index >= fileindex) {
+ fileindex = index + 1;
+ }
+ }
+ }
+ closedir(d);
+ } else {
+ aos::Die("could not open directory %s because of %d (%s).\n", directory,
+ errno, strerror(errno));
+ }
+
+ char previous[512];
+ ::std::string path = ::std::string(directory) + "/aos_log-current";
+ ssize_t len = ::readlink(path.c_str(), previous, sizeof(previous));
+ if (len != -1) {
+ previous[len] = '\0';
+ } else {
+ previous[0] = '\0';
+ LOG(INFO, "Could not find aos_log-current\n");
+ printf("Could not find aos_log-current\n");
+ }
+ if (asprintf(filename, "%s/aos_log-%03d", directory, fileindex) == -1) {
+ aos::Die("couldn't create final name because of %d (%s)\n",
+ errno, strerror(errno));
+ }
+ LOG(INFO, "Created log file (aos_log-%d) in directory (%s). Previous file "
+ "was (%s).\n",
+ fileindex, directory, previous);
+ printf("Created log file (aos_log-%d) in directory (%s). Previous file was "
+ "(%s).\n",
+ fileindex, directory, previous);
+}
+
int BinaryLogReaderMain() {
InitNRT();
@@ -67,15 +110,8 @@
}
LOG(INFO, "logging to folder '%s'\n", folder);
- const time_t t = time(NULL);
char *tmp;
- if (asprintf(&tmp, "%s/aos_log-%jd", folder, static_cast<uintmax_t>(t)) ==
- -1) {
- fprintf(stderr,
- "BinaryLogReader: couldn't create final name because of %d (%s)."
- " exiting\n", errno, strerror(errno));
- return EXIT_FAILURE;
- }
+ AllocateLogName(&tmp, folder);
char *tmp2;
if (asprintf(&tmp2, "%s/aos_log-current", folder) == -1) {
fprintf(stderr,
@@ -118,14 +154,19 @@
const LogMessage *const msg = ReadNext();
if (msg == NULL) continue;
- size_t output_length =
+ const size_t raw_output_length =
sizeof(LogFileMessageHeader) + msg->name_length + msg->message_length;
+ size_t output_length = raw_output_length;
if (msg->type == LogMessage::Type::kStruct) {
output_length += sizeof(msg->structure.type_id) + sizeof(uint32_t) +
msg->structure.string_length;
CheckTypeWritten(msg->structure.type_id, writer);
+ } else if (msg->type == LogMessage::Type::kMatrix) {
+ output_length +=
+ sizeof(msg->matrix.type) + sizeof(uint32_t) + sizeof(uint16_t) +
+ sizeof(uint16_t) + msg->matrix.string_length;
}
- LogFileMessageHeader *const output = writer.GetWritePosition(output_length);;
+ LogFileMessageHeader *const output = writer.GetWritePosition(output_length);
char *output_strings = reinterpret_cast<char *>(output) + sizeof(*output);
output->name_size = msg->name_length;
output->message_size = msg->message_length;
@@ -142,10 +183,11 @@
msg->message_length);
output->type = LogFileMessageHeader::MessageType::kString;
break;
- case LogMessage::Type::kStruct:
+ case LogMessage::Type::kStruct: {
char *position = output_strings + msg->name_length;
- memcpy(position, &msg->structure.type_id, sizeof(msg->structure.type_id));
+ memcpy(position, &msg->structure.type_id,
+ sizeof(msg->structure.type_id));
position += sizeof(msg->structure.type_id);
output->message_size += sizeof(msg->structure.type_id);
@@ -161,7 +203,39 @@
msg->message_length);
output->type = LogFileMessageHeader::MessageType::kStruct;
- break;
+ } break;
+ case LogMessage::Type::kMatrix: {
+ char *position = output_strings + msg->name_length;
+
+ memcpy(position, &msg->matrix.type, sizeof(msg->matrix.type));
+ position += sizeof(msg->matrix.type);
+ output->message_size += sizeof(msg->matrix.type);
+
+ uint32_t length = msg->matrix.string_length;
+ memcpy(position, &length, sizeof(length));
+ position += sizeof(length);
+ output->message_size += sizeof(length);
+
+ uint16_t rows = msg->matrix.rows, cols = msg->matrix.cols;
+ memcpy(position, &rows, sizeof(rows));
+ position += sizeof(rows);
+ memcpy(position, &cols, sizeof(cols));
+ position += sizeof(cols);
+ output->message_size += sizeof(rows) + sizeof(cols);
+ CHECK_EQ(msg->message_length,
+ MessageType::Sizeof(msg->matrix.type) * rows * cols);
+
+ memcpy(position, msg->matrix.data, msg->message_length + length);
+ output->message_size += length;
+
+ output->type = LogFileMessageHeader::MessageType::kMatrix;
+ } break;
+ }
+
+ if (output->message_size - msg->message_length !=
+ output_length - raw_output_length) {
+ LOG(FATAL, "%zu != %zu\n", output->message_size - msg->message_length,
+ output_length - raw_output_length);
}
futex_set(&output->marker);
diff --git a/aos/linux_code/logging/linux_logging.cc b/aos/linux_code/logging/linux_logging.cc
index 676e3b7..aed8697 100644
--- a/aos/linux_code/logging/linux_logging.cc
+++ b/aos/linux_code/logging/linux_logging.cc
@@ -8,33 +8,35 @@
#include <errno.h>
#include <unistd.h>
#include <limits.h>
+#include <inttypes.h>
#include <algorithm>
#include "aos/common/die.h"
#include "aos/common/logging/logging_impl.h"
#include "aos/linux_code/ipc_lib/queue.h"
+#include "aos/common/time.h"
namespace aos {
namespace logging {
-namespace {
-
-RawQueue *queue;
-
-} // namespace
namespace linux_code {
namespace {
-class LinuxQueueLogImplementation : public LogImplementation {
- LogMessage *GetMessageOrDie() {
- LogMessage *message = static_cast<LogMessage *>(queue->GetMessage());
- if (message == NULL) {
- LOG(FATAL, "%p->GetMessage() failed\n", queue);
- } else {
- return message;
- }
- }
+RawQueue *queue = NULL;
+int dropped_messages = 0;
+int32_t dropped_start_seconds, dropped_start_nseconds;
+
+LogMessage *GetMessageOrDie() {
+ LogMessage *message = static_cast<LogMessage *>(queue->GetMessage());
+ if (message == NULL) {
+ LOG(FATAL, "%p->GetMessage() failed\n", queue);
+ } else {
+ return message;
+ }
+}
+
+class LinuxQueueLogImplementation : public LogImplementation {
virtual void DoLog(log_level level, const char *format, va_list ap) override {
LogMessage *message = GetMessageOrDie();
internal::FillInMessage(level, format, ap, message);
@@ -50,6 +52,15 @@
serialize, message);
Write(message);
}
+
+ virtual void LogMatrix(log_level level, const ::std::string &message_string,
+ uint32_t type_id, int rows, int cols, const void *data)
+ override {
+ LogMessage *message = GetMessageOrDie();
+ internal::FillInMessageMatrix(level, message_string, type_id, rows, cols,
+ data, message);
+ Write(message);
+ }
};
} // namespace
@@ -57,7 +68,7 @@
void Register() {
Init();
- queue = RawQueue::Fetch("LoggingQueue", sizeof(LogMessage), 1323, 1500);
+ queue = RawQueue::Fetch("LoggingQueue", sizeof(LogMessage), 1323, 80000);
if (queue == NULL) {
Die("logging: couldn't fetch queue\n");
}
@@ -91,8 +102,30 @@
}
void Write(LogMessage *msg) {
- if (!queue->WriteMessage(msg, RawQueue::kOverride)) {
- LOG(FATAL, "writing failed");
+ if (__builtin_expect(dropped_messages > 0, 0)) {
+ LogMessage *dropped_message = GetMessageOrDie();
+ internal::FillInMessageVarargs(
+ ERROR, dropped_message,
+ "%d logs starting at %" PRId32 ".%" PRId32 " dropped\n",
+ dropped_messages, dropped_start_seconds, dropped_start_nseconds);
+ if (queue->WriteMessage(dropped_message, RawQueue::kNonBlock)) {
+ dropped_messages = 0;
+ internal::PrintMessage(stderr, *dropped_message);
+ } else {
+ // Don't even bother trying to write this message because it's not likely
+ // to work and it would be confusing to have one log in the middle of a
+ // string of failures get through.
+ ++dropped_messages;
+ queue->FreeMessage(msg);
+ return;
+ }
+ }
+ if (!queue->WriteMessage(msg, RawQueue::kNonBlock)) {
+ if (dropped_messages == 0) {
+ dropped_start_seconds = msg->seconds;
+ dropped_start_nseconds = msg->nseconds;
+ }
+ ++dropped_messages;
}
}
diff --git a/aos/linux_code/logging/linux_logging.h b/aos/linux_code/logging/linux_logging.h
index 928a480..3dc8763 100644
--- a/aos/linux_code/logging/linux_logging.h
+++ b/aos/linux_code/logging/linux_logging.h
@@ -10,7 +10,7 @@
// Calls AddImplementation to register the usual linux logging implementation
// which sends the messages through a queue. This implementation relies on
// another process(es) to read the log messages that it puts into the queue.
-// It gets called by aos::Init*.
+// This function is usually called by aos::Init*.
void Register();
// Fairly simple wrappers around the raw queue calls.
diff --git a/aos/linux_code/logging/log_displayer.cc b/aos/linux_code/logging/log_displayer.cc
index 98021b5..1cc5f43 100644
--- a/aos/linux_code/logging/log_displayer.cc
+++ b/aos/linux_code/logging/log_displayer.cc
@@ -227,7 +227,39 @@
log_message.type = ::aos::logging::LogMessage::Type::kStruct;
break;
}
- case LogFileMessageHeader::MessageType::kString:
+ case LogFileMessageHeader::MessageType::kMatrix: {
+ const char *position =
+ reinterpret_cast<const char *>(msg + 1) + msg->name_size;
+ memcpy(&log_message.matrix.type, position,
+ sizeof(log_message.matrix.type));
+ position += sizeof(log_message.matrix.type);
+
+ uint32_t length;
+ memcpy(&length, position, sizeof(length));
+ log_message.matrix.string_length = length;
+ position += sizeof(length);
+
+ uint16_t rows;
+ memcpy(&rows, position, sizeof(rows));
+ log_message.matrix.rows = rows;
+ position += sizeof(rows);
+ uint16_t cols;
+ memcpy(&cols, position, sizeof(cols));
+ log_message.matrix.cols = cols;
+ position += sizeof(cols);
+
+ log_message.message_length -=
+ sizeof(log_message.matrix.type) + sizeof(uint32_t) +
+ sizeof(uint16_t) + sizeof(uint16_t) + length;
+ CHECK_EQ(
+ log_message.message_length,
+ ::aos::MessageType::Sizeof(log_message.matrix.type) * rows * cols);
+ memcpy(log_message.matrix.data, position,
+ log_message.message_length + length);
+
+ log_message.type = ::aos::logging::LogMessage::Type::kMatrix;
+ break;
+ } case LogFileMessageHeader::MessageType::kString:
memcpy(
log_message.message,
reinterpret_cast<const char *>(msg) + sizeof(*msg) + msg->name_size,
diff --git a/aos/linux_code/logging/logging.gyp b/aos/linux_code/logging/logging.gyp
index 32df43d..52f8b96 100644
--- a/aos/linux_code/logging/logging.gyp
+++ b/aos/linux_code/logging/logging.gyp
@@ -11,6 +11,7 @@
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/linux_code/linux_code.gyp:init',
'<(AOS)/linux_code/linux_code.gyp:configuration',
+ '<(AOS)/common/common.gyp:die',
'binary_log_file',
'<(AOS)/common/common.gyp:queue_types',
],
diff --git a/aos/linux_code/queue-tmpl.h b/aos/linux_code/queue-tmpl.h
index 84eb02c..15029986 100644
--- a/aos/linux_code/queue-tmpl.h
+++ b/aos/linux_code/queue-tmpl.h
@@ -177,16 +177,14 @@
queue_ = NULL;
queue_msg_.set_queue(NULL);
}
+ index_ = 0;
}
template <class T>
bool Queue<T>::FetchNext() {
Init();
- // TODO(aschuh): Use RawQueue::ReadMessageIndex so that multiple readers
- // reading don't randomly get only part of the messages.
- // Document here the tradoffs that are part of each method.
const T *msg = static_cast<const T *>(
- queue_->ReadMessage(RawQueue::kNonBlock));
+ queue_->ReadMessageIndex(RawQueue::kNonBlock, &index_));
// Only update the internal pointer if we got a new message.
if (msg != NULL) {
queue_msg_.reset(msg);
@@ -197,7 +195,7 @@
template <class T>
bool Queue<T>::FetchNextBlocking() {
Init();
- const T *msg = static_cast<const T *>(queue_->ReadMessage(RawQueue::kBlock));
+ const T *msg = static_cast<const T *>(queue_->ReadMessageIndex(RawQueue::kBlock, &index_));
queue_msg_.reset(msg);
assert (msg != NULL);
return true;
@@ -206,8 +204,8 @@
template <class T>
bool Queue<T>::FetchLatest() {
Init();
- const T *msg = static_cast<const T *>(queue_->ReadMessage(
- RawQueue::kFromEnd | RawQueue::kNonBlock | RawQueue::kPeek));
+ const T *msg = static_cast<const T *>(queue_->ReadMessageIndex(
+ RawQueue::kFromEnd | RawQueue::kNonBlock, &index_));
// Only update the internal pointer if we got a new message.
if (msg != NULL && msg != queue_msg_.get()) {
queue_msg_.reset(msg);
diff --git a/aos/linux_code/starter/starter.sh b/aos/linux_code/starter/starter.sh
index 9d7cc13..360937a 100755
--- a/aos/linux_code/starter/starter.sh
+++ b/aos/linux_code/starter/starter.sh
@@ -19,4 +19,4 @@
#DUMPCAP_STDOUT_FILE=$(date "/home/driver/tmp/robot_logs/stdout_dumpcap.%F_%H-%M-%S")
#chrt -o 0 bash -c "dumpcap -i eth0 -w ${DUMPCAP_LOG_FILE} -f 'not port 8080 and not net 10.9.71.13' > ${DUMPCAP_STDOUT_FILE}" &
-chrt -o 0 bash -c 'while true; do /home/driver/robot_code/bin/netconsole /home/driver/tmp/robot_logs/netconsole-`date +%s`; done' &
+chrt -o 0 bash -c 'while true; do /home/driver/robot_code/bin/netconsole /home/driver/tmp/robot_logs/netconsole-`date +%s`; sleep 1; done' &
diff --git a/aos/prime/input/input.gyp b/aos/prime/input/input.gyp
index 8aba065..4779ddc 100644
--- a/aos/prime/input/input.gyp
+++ b/aos/prime/input/input.gyp
@@ -8,10 +8,11 @@
],
'dependencies': [
'<(AOS)/common/input/input.gyp:driver_station_data',
- '<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
'<(AOS)/common/network/network.gyp:socket',
'<(EXTERNALS):WPILib-NetworkRobotValues',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'<(AOS)/common/input/input.gyp:driver_station_data',
diff --git a/aos/prime/input/joystick_input.cc b/aos/prime/input/joystick_input.cc
index e53a180..a2ba21b 100644
--- a/aos/prime/input/joystick_input.cc
+++ b/aos/prime/input/joystick_input.cc
@@ -6,43 +6,52 @@
#include "aos/common/network_port.h"
#include "aos/common/network/ReceiveSocket.h"
-#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/messages/robot_state.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
namespace aos {
namespace input {
void JoystickInput::Run() {
ReceiveSocket sock(NetworkPort::kDS);
+ // If true, this code won't try to read anything from the network and instead
+ // feed all 0s to the joystick code.
+ // The RobotState messages will be marked as fake so anything that outputs
+ // values won't while this is enabled.
+ static const bool kFakeJoysticks = false;
NetworkRobotJoysticks joysticks;
char buffer[sizeof(joysticks) + ::buffers::kOverhead];
driver_station::Data data;
while (true) {
- int received = sock.Receive(buffer, sizeof(buffer));
- if (received == -1) {
- LOG(WARNING, "socket receive failed with %d: %s\n",
- errno, strerror(errno));
- continue;
+ if (kFakeJoysticks) {
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.02));
+ memset(&joysticks, 0, sizeof(joysticks));
+ } else {
+ int received = sock.Receive(buffer, sizeof(buffer));
+ if (received == -1) {
+ LOG(WARNING, "socket receive failed with %d: %s\n",
+ errno, strerror(errno));
+ continue;
+ }
+
+ if (!joysticks.DeserializeFrom(buffer, received)) {
+ LOG(WARNING, "deserializing data from %d bytes failed\n", received);
+ continue;
+ }
}
- if (!joysticks.DeserializeFrom(buffer, received)) {
- LOG(WARNING, "deserializing data from %d bytes failed\n", received);
- continue;
- }
+ auto new_state = robot_state.MakeMessage();
+ new_state->enabled = joysticks.control.enabled();
+ new_state->autonomous = joysticks.control.autonomous();
+ new_state->team_id = joysticks.team_number;
+ new_state->fake = kFakeJoysticks;
+ LOG_STRUCT(DEBUG, "sending", *new_state);
- if (!robot_state.MakeWithBuilder()
- .enabled(joysticks.control.enabled())
- .autonomous(joysticks.control.autonomous())
- .team_id(joysticks.team_number)
- .Send()) {
- LOG(WARNING, "sending robot_state failed\n");
- } else {
- LOG(DEBUG, "sent robot_state{%s, %s, %hu}\n",
- joysticks.control.enabled() ? "enabled" : "disabled",
- joysticks.control.autonomous() ? "auto" : "not auto",
- joysticks.team_number);
+ if (!new_state.Send()) {
+ LOG(WARNING, "sending robot_state failed\n");
}
data.Update(joysticks);
diff --git a/aos/prime/output/motor_output.cc b/aos/prime/output/motor_output.cc
index a532183..54add2d 100644
--- a/aos/prime/output/motor_output.cc
+++ b/aos/prime/output/motor_output.cc
@@ -5,6 +5,7 @@
#include "aos/common/control_loop/Timing.h"
#include "aos/common/logging/logging.h"
#include "aos/common/network_port.h"
+#include "aos/common/messages/robot_state.q.h"
namespace aos {
@@ -36,6 +37,7 @@
MotorOutput::MotorOutput()
: socket_(NetworkPort::kMotors, ::aos::NetworkAddress::kCRIO) {
+ values_.solenoid_values = 0;
}
void MotorOutput::Run() {
@@ -50,7 +52,12 @@
values_.pressure_switch_channel = 0;
values_.compressor_channel = 0;
values_.solenoid_module = -1;
- values_.solenoid_values = 0;
+
+ ::aos::robot_state.FetchLatest();
+ if (!::aos::robot_state.get() || ::aos::robot_state->fake) {
+ LOG(DEBUG, "fake robot state -> not outputting\n");
+ continue;
+ }
RunIteration();
diff --git a/aos/prime/output/output.gyp b/aos/prime/output/output.gyp
index b1e4b79..67ad7af 100644
--- a/aos/prime/output/output.gyp
+++ b/aos/prime/output/output.gyp
@@ -11,6 +11,7 @@
'<(AOS)/common/common.gyp:timing',
'<(EXTERNALS):WPILib-NetworkRobotValues',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
],
'export_dependent_settings': [
'<(AOS)/common/network/network.gyp:socket',
diff --git a/bbb_cape/src/bbb/bbb.gyp b/bbb_cape/src/bbb/bbb.gyp
index 8efe553..1da4732 100644
--- a/bbb_cape/src/bbb/bbb.gyp
+++ b/bbb_cape/src/bbb/bbb.gyp
@@ -11,6 +11,16 @@
},
'targets': [
{
+ 'target_name': 'led',
+ 'type': 'static_library',
+ 'sources': [
+ 'led.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ {
'target_name': 'crc',
'type': 'static_library',
'sources': [
diff --git a/bbb_cape/src/bbb/gpios.cc b/bbb_cape/src/bbb/gpios.cc
index 3584d06..a5a891a 100644
--- a/bbb_cape/src/bbb/gpios.cc
+++ b/bbb_cape/src/bbb/gpios.cc
@@ -12,7 +12,7 @@
GpioPin::GpioPin(int bank, int pin, bool input, bool initial_value)
: bank_(bank), pin_(pin), kernel_pin_(bank * 32 + pin) {
// Export the pin.
- FILE *export_handle = fopen("/sys/class/gpio/export", "a");
+ FILE *export_handle = fopen("/sys/class/gpio/export", "w");
if (export_handle == NULL) {
LOG(WARNING,
"Could not open file to export pin (%d,%d) because of %d: %s.\n",
@@ -35,7 +35,7 @@
FILE *direction_handle = fopen(direction_path, "w");
if (direction_handle == NULL) {
- LOG(FATAL, "fopen(%s, \"w+\") failed with %d: %s\n",
+ LOG(FATAL, "fopen(%s, \"w\") failed with %d: %s\n",
direction_path, errno, strerror(errno));
}
@@ -54,7 +54,7 @@
"/sys/class/gpio/gpio%d/value", kernel_pin_);
value_handle_ = fopen(value_path, "w+");
if (value_handle_ == NULL) {
- LOG(FATAL, "fopen(%s, \"rw\") failed with %d: %s\n",
+ LOG(FATAL, "fopen(%s, \"w+\") failed with %d: %s\n",
value_path, errno, strerror(errno));
}
}
diff --git a/bbb_cape/src/bbb/gpios.h b/bbb_cape/src/bbb/gpios.h
index 1471b43..0ba3323 100644
--- a/bbb_cape/src/bbb/gpios.h
+++ b/bbb_cape/src/bbb/gpios.h
@@ -1,5 +1,5 @@
-#ifndef BBB_CAPE_SRC_BBB_CAPE_CONTROL_H_
-#define BBB_CAPE_SRC_BBB_CAPE_CONTROL_H_
+#ifndef BBB_CAPE_SRC_BBB_GPIOS_H_
+#define BBB_CAPE_SRC_BBB_GPIOS_H_
#include <stdio.h>
@@ -31,4 +31,4 @@
} // namespace bbb
-#endif
+#endif // BBB_CAPE_SRC_BBB_GPIOS_H_
diff --git a/bbb_cape/src/bbb/led.cc b/bbb_cape/src/bbb/led.cc
new file mode 100644
index 0000000..5dc5e71
--- /dev/null
+++ b/bbb_cape/src/bbb/led.cc
@@ -0,0 +1,60 @@
+#include "bbb_cape/src/bbb/led.h"
+
+#include "aos/common/logging/logging.h"
+
+#include <string.h>
+#include <errno.h>
+
+#define DIRECTORY "/sys/class/leds/beaglebone:green:usr%d/"
+
+namespace bbb {
+
+LED::LED(int number) : number_(number) {
+ char trigger_path[64];
+ snprintf(trigger_path, sizeof(trigger_path), DIRECTORY "trigger", number_);
+ FILE *trigger_handle = fopen(trigger_path, "w");
+ if (trigger_handle == nullptr) {
+ LOG(FATAL, "couldn't open trigger file for LED %d because of %d: %s\n",
+ number_, errno, strerror(errno));
+ }
+ if (fputs("none", trigger_handle) < 0) {
+ LOG(FATAL,
+ "writing 'none' to file %p (trigger for LED %d) failed with %d: %s\n",
+ trigger_handle, number_, errno, strerror(errno));
+ }
+ if (fclose(trigger_handle) == -1) {
+ LOG(WARNING, "fclose(%p) failed with %d: %s\n",
+ trigger_handle, errno, strerror(errno));
+ }
+
+ char brightness_path[64];
+ snprintf(brightness_path, sizeof(brightness_path),
+ DIRECTORY "brightness", number_);
+
+ brightness_handle_ = fopen(brightness_path, "w");
+ if (brightness_handle_ == nullptr) {
+ LOG(FATAL, "fopen('%s', 'w') failed with %d: %s\n",
+ brightness_path, errno, strerror(errno));
+ }
+}
+
+LED::~LED() {
+ if (fclose(brightness_handle_) == -1) {
+ LOG(WARNING, "fclose(%p) failed with %d: %s\n",
+ brightness_handle_, errno, strerror(errno));
+ }
+}
+
+void LED::Set(bool on) {
+ rewind(brightness_handle_);
+ if (fputs(on ? "255" : "0", brightness_handle_) == EOF) {
+ LOG(FATAL, "fputs(255|0, %p) for LED %d failed with %d: %s\n",
+ brightness_handle_, number_, errno, strerror(errno));
+ }
+ if (fflush(brightness_handle_) == EOF) {
+ LOG(FATAL, "fflush(%p) for LED %d failed with %d: %s\n",
+ brightness_handle_, number_, errno, strerror(errno));
+ }
+}
+
+} // namespace bbb
diff --git a/bbb_cape/src/bbb/led.h b/bbb_cape/src/bbb/led.h
new file mode 100644
index 0000000..19976e6
--- /dev/null
+++ b/bbb_cape/src/bbb/led.h
@@ -0,0 +1,27 @@
+#ifndef BBB_CAPE_SRC_BBB_LED_H_
+#define BBB_CAPE_SRC_BBB_LED_H_
+
+#include <stdio.h>
+
+#include "aos/common/macros.h"
+
+namespace bbb {
+
+// Allows easily controlling one of the LEDs.
+class LED {
+ public:
+ LED(int number);
+ ~LED();
+
+ void Set(bool on);
+
+ private:
+ FILE *brightness_handle_ = nullptr;
+ const int number_;
+
+ DISALLOW_COPY_AND_ASSIGN(LED);
+};
+
+} // namespace bbb
+
+#endif // BBB_CAPE_SRC_BBB_LED_H_
diff --git a/bbb_cape/src/bbb/packet_finder.cc b/bbb_cape/src/bbb/packet_finder.cc
index 5decec8..15a8e62 100644
--- a/bbb_cape/src/bbb/packet_finder.cc
+++ b/bbb_cape/src/bbb/packet_finder.cc
@@ -110,6 +110,9 @@
uint32_t unstuffed = cows_unstuff(
reinterpret_cast<uint32_t *>(buf_), packet_size_,
reinterpret_cast<uint32_t *>(unstuffed_data_), packet_size_ - 4);
+ invalid_packet_.Print();
+ bad_checksum_.Print();
+
if (unstuffed == 0) {
if (kDebugLogs) LOG(INFO, "invalid\n");
LOG_INTERVAL(invalid_packet_);
diff --git a/bbb_cape/src/bbb/sensor_reader.cc b/bbb_cape/src/bbb/sensor_reader.cc
index fd4b79d..c0e9604 100644
--- a/bbb_cape/src/bbb/sensor_reader.cc
+++ b/bbb_cape/src/bbb/sensor_reader.cc
@@ -42,6 +42,8 @@
::aos::time::Time::InSeconds(5);
while (true) {
+ receive_failed_.Print();
+
::aos::time::Time next_timeout = last_received_time_ + kResetTimeout;
if (next_timeout <= ::aos::time::Time::Now()) {
LOG(WARNING, "Too long since good packet received. Resetting.\n");
diff --git a/bbb_cape/src/bbb/uart_reader.cc b/bbb_cape/src/bbb/uart_reader.cc
index 34b907b..9abb284 100644
--- a/bbb_cape/src/bbb/uart_reader.cc
+++ b/bbb_cape/src/bbb/uart_reader.cc
@@ -84,9 +84,6 @@
LOG(FATAL, "aos_uart_reader_set_tty_options(%d) failed with %d: %s\n",
fd_, errno, strerror(errno));
}
-
- FD_ZERO(&fd_set_);
- FD_SET(fd_, &fd_set_);
}
UartReader::~UartReader() {
@@ -99,7 +96,10 @@
::aos::time::Time timeout = timeout_time - ::aos::time::Time::Now();
if (timeout < ::aos::time::Time(0, 0)) return -2;
struct timeval timeout_timeval = timeout.ToTimeval();
- switch (select(fd_ + 1, &fd_set_, NULL, NULL, &timeout_timeval)) {
+ fd_set fds;
+ FD_ZERO(&fds);
+ FD_SET(fd_, &fds);
+ switch (select(fd_ + 1, &fds, NULL, NULL, &timeout_timeval)) {
case 0:
return -2;
case -1:
diff --git a/bbb_cape/src/bbb/uart_reader.h b/bbb_cape/src/bbb/uart_reader.h
index a6cf34c..a8edc6e 100644
--- a/bbb_cape/src/bbb/uart_reader.h
+++ b/bbb_cape/src/bbb/uart_reader.h
@@ -23,8 +23,6 @@
private:
const int fd_;
- // Gets initialized to only contain fd_.
- fd_set fd_set_;
DISALLOW_COPY_AND_ASSIGN(UartReader);
};
diff --git a/bbb_cape/src/cape/analog.c b/bbb_cape/src/cape/analog.c
index 855098c..297afd6 100644
--- a/bbb_cape/src/cape/analog.c
+++ b/bbb_cape/src/cape/analog.c
@@ -2,8 +2,6 @@
#include <string.h>
-#include <STM32F2XX.h>
-
#include "cape/util.h"
#include "cape/led.h"
@@ -20,27 +18,13 @@
#define NUM_CHANNELS 8
+// This file handles reading values from the MCP3008-I/SL ADC.
+
uint16_t analog_readings[NUM_CHANNELS] __attribute__((aligned(8)));
static volatile int current_channel;
static volatile int partial_reading;
static volatile int frame;
-
-static void start_read(int channel) {
- // This needs to wait 13 cycles between enabling the CSEL pin and starting to
- // send data.
- // (100ns+8ns)*120MHz = 12.96
-
- // Clear the CSEL pin to select it.
- for (int i = 0; i < 9; ++i) gpio_off(CSEL_GPIO, CSEL_NUM);
- current_channel = channel;
- partial_reading = 0;
- frame = 0;
- SPI->DR = 1; // start bit
- uint16_t data = (1 << 15) /* not differential */ |
- (channel << 12);
- while (!(SPI->SR & SPI_SR_TXE));
- SPI->DR = data;
-}
+static volatile int analog_errors;
void SPI_IRQHandler(void) {
uint32_t status = SPI->SR;
@@ -50,15 +34,15 @@
frame = 1;
partial_reading = value;
} else {
+ frame = 2;
// Masking off the high bits is important because there's nothing driving
// the MISO line during the time the MCU receives them.
- analog_readings[current_channel] = (partial_reading << 16 | value) & 0x3FF;
+ analog_readings[current_channel] =
+ (partial_reading << 16 | value) & 0x3FF;
for (int i = 0; i < 100; ++i) gpio_off(CSEL_GPIO, CSEL_NUM);
gpio_on(CSEL_GPIO, CSEL_NUM);
- TIM->CR1 = TIM_CR1_OPM;
- TIM->EGR = TIM_EGR_UG;
- TIM->CR1 |= TIM_CR1_CEN;
+ current_channel = (current_channel + 1) % NUM_CHANNELS;
}
}
}
@@ -66,7 +50,27 @@
void TIM_IRQHandler(void) {
TIM->SR = ~TIM_SR_CC1IF;
- start_read((current_channel + 1) % NUM_CHANNELS);
+ if (frame != 2) {
+ // We're not done with the previous reading yet, so we're going to reset and
+ // try again.
+ // 270ns*120MHz = 32.4
+ for (int i = 0; i < 33; ++i) gpio_on(CSEL_GPIO, CSEL_NUM);
+ ++analog_errors;
+ }
+
+ // This needs to wait 13 cycles between enabling the CSEL pin and starting to
+ // send data.
+ // (100ns+8ns)*120MHz = 12.96
+
+ // Clear the CSEL pin to select it.
+ for (int i = 0; i < 9; ++i) gpio_off(CSEL_GPIO, CSEL_NUM);
+ partial_reading = 0;
+ frame = 0;
+ SPI->DR = 1; // start bit
+ uint16_t data = (1 << 15) /* not differential */ |
+ (current_channel << 12);
+ while (!(SPI->SR & SPI_SR_TXE));
+ SPI->DR = data;
}
void analog_init(void) {
@@ -87,7 +91,7 @@
NVIC_SetPriority(TIM_IRQn, 6);
NVIC_EnableIRQ(TIM_IRQn);
- TIM->CR1 = TIM_CR1_OPM;
+ TIM->CR1 = 0;
TIM->DIER = TIM_DIER_CC1IE;
TIM->CCMR1 = 0;
// Make each tick take 1500ns.
@@ -104,5 +108,17 @@
SPI->CR2 = SPI_CR2_RXNEIE;
SPI->CR1 |= SPI_CR1_SPE; // enable it
- start_read(0);
+ current_channel = 0;
+ analog_errors = 0;
+
+ TIM->EGR = TIM_EGR_UG;
+ TIM->CR1 |= TIM_CR1_CEN;
+}
+
+int analog_get_errors(void) {
+ NVIC_DisableIRQ(TIM_IRQn);
+ int r = analog_errors;
+ analog_errors = 0;
+ NVIC_EnableIRQ(TIM_IRQn);
+ return r;
}
diff --git a/bbb_cape/src/cape/analog.h b/bbb_cape/src/cape/analog.h
index 50038d5..cd9ce82 100644
--- a/bbb_cape/src/cape/analog.h
+++ b/bbb_cape/src/cape/analog.h
@@ -3,6 +3,8 @@
#include <stdint.h>
+#include <STM32F2XX.h>
+
// Starts up constantly reading analog values and storing them in an array to
// be retrieved by analog_get.
void analog_init(void);
@@ -14,4 +16,9 @@
return analog_readings[num];
}
+// Returns the number of errors since last called.
+// Must be called from something with priority equal to or lower than our
+// timer's IRQ.
+int analog_get_errors(void);
+
#endif // CAPE_ANALOG_H_
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index debef38..d60a0e9 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -38,6 +38,8 @@
// contents of flash for the main code (aka what's in the .hex file).
uint32_t flash_checksum;
+ uint8_t analog_errors;
+
struct {
// If the current gyro_angle has been not updated because of a bad
// reading from the sensor.
@@ -86,10 +88,12 @@
int32_t shooter_position, pusher_distal_posedge_position,
pusher_proximal_posedge_position;
- uint16_t left_drive_hall;
- uint16_t right_drive_hall;
+ uint16_t low_left_drive_hall;
+ uint16_t high_left_drive_hall;
+ uint16_t low_right_drive_hall;
+ uint16_t high_right_drive_hall;
- uint16_t battery_voltage;
+ uint16_t battery_voltage_high, battery_voltage_low;
HallEffectEdges pusher_distal, pusher_proximal;
diff --git a/bbb_cape/src/cape/fill_packet.c b/bbb_cape/src/cape/fill_packet.c
index 29ea095..8d1f72a 100644
--- a/bbb_cape/src/cape/fill_packet.c
+++ b/bbb_cape/src/cape/fill_packet.c
@@ -40,6 +40,7 @@
packet->uninitialized_gyro = !gyro_output.initialized;
packet->zeroing_gyro = !gyro_output.zeroed;
packet->bad_gyro = gyro_output.gyro_bad;
+ packet->analog_errors = analog_get_errors();
robot_fill_packet(packet);
//counter_update_u64_u16(×tamp, TIMESTAMP_TIM->CNT);
@@ -86,6 +87,8 @@
led_set(LED_ERR, 0);
gyro_init();
+ robot_init();
+
uart_common_configure(750000);
uart_dma_configure(DATA_STRUCT_SEND_SIZE, buffer1, buffer2);
}
diff --git a/bbb_cape/src/cape/robot_comp.c b/bbb_cape/src/cape/robot_comp.c
index 57e3d66..cab3e28 100644
--- a/bbb_cape/src/cape/robot_comp.c
+++ b/bbb_cape/src/cape/robot_comp.c
@@ -93,21 +93,20 @@
void TIM1_TRG_COM_TIM11_IRQHandler(void) {
TIM11->SR = ~TIM_SR_CC1IF;
- TIM11->CR1 = 0;
- ultrasonic_pulse_length = TIM11->CCR1;
- ultrasonic_pulse_length = 1;
- TIM11->CNT = 0;
+ if (digital_read(6)) {
+ TIM11->EGR = TIM_EGR_UG;
+ } else {
+ ultrasonic_pulse_length = TIM11->CCR1;
+ }
}
void robot_init(void) {
gpio_setup_alt(GPIOB, 9, 3);
RCC->APB2ENR |= RCC_APB2ENR_TIM11EN;
TIM11->CR1 = TIM_CR1_URS;
- TIM11->SMCR = 5 << 4 /* TI1 */ |
- 6 << 0 /* start on rising edge */;
TIM11->DIER = TIM_DIER_CC1IE;
TIM11->CCMR1 = TIM_CCMR1_CC1S_0 /* input pin 1 -> timer input 1 */;
- TIM11->CCER = TIM_CCER_CC1P /* go on falling edge */ | TIM_CCER_CC1E;
+ TIM11->CCER = TIM_CCER_CC1P | TIM_CCER_CC1NP | TIM_CCER_CC1E;
TIM11->EGR = TIM_EGR_UG;
TIM11->PSC = 1200 - 1; // 100kHZ timer
TIM11->CR1 |= TIM_CR1_CEN;
@@ -118,8 +117,13 @@
void robot_fill_packet(struct DataStruct *packet) {
packet->main.left_drive = encoder_read(6);
packet->main.right_drive = encoder_read(5);
- packet->main.left_drive_hall = analog_get(7);
- packet->main.right_drive_hall = analog_get(0);
+ packet->main.low_left_drive_hall = analog_get(7);
+ packet->main.low_right_drive_hall = analog_get(0);
+ packet->main.high_left_drive_hall = analog_get(6);
+ packet->main.high_right_drive_hall = analog_get(1);
+
+ packet->main.battery_voltage_high = analog_get(5);
+ packet->main.battery_voltage_low = analog_get(3);
packet->main.ultrasonic_pulse_length = ultrasonic_pulse_length;
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
new file mode 100644
index 0000000..fe17676
--- /dev/null
+++ b/frc971/actions/action.h
@@ -0,0 +1,104 @@
+#ifndef FRC971_ACTIONS_ACTION_H_
+#define FRC971_ACTIONS_ACTION_H_
+
+#include <stdio.h>
+
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/network/team_number.h"
+
+#include "frc971/constants.h"
+
+namespace frc971 {
+namespace actions {
+
+template <class T> class ActionBase {
+ public:
+ ActionBase(T* acq) : action_q_(acq) {}
+
+ virtual void RunAction() = 0;
+
+ // runs action while enabled
+ void Run() {
+ LOG(DEBUG, "Waiting for input to start\n");
+ action_q_->goal.FetchLatest();
+ while (!action_q_->goal.get()) {
+ action_q_->goal.FetchNextBlocking();
+ }
+
+ if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ while (true) {
+ while (!action_q_->goal->run) {
+ LOG(INFO, "Waiting for an action request.\n");
+ action_q_->goal.FetchNextBlocking();
+ if (!action_q_->goal->run) {
+ if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ }
+ }
+ LOG(INFO, "Starting action\n");
+ if (!action_q_->status.MakeWithBuilder().running(true).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ RunAction();
+ LOG(INFO, "Done with action\n");
+ if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ while (action_q_->goal->run) {
+ LOG(INFO, "Waiting for the action to be stopped.\n");
+ action_q_->goal.FetchNextBlocking();
+ }
+ }
+ }
+
+ // will run until the done condition is met.
+ // will return false if successful and true if the action was canceled or
+ // failed.
+ // done condition are defined as functions that return true when done and have
+ // some sort of blocking statement(FetchNextBlocking) to throttle spin rate.
+ bool WaitUntil(::std::function<bool(void)> done_condition) {
+ while (!done_condition()) {
+ if (ShouldCancel() || abort_) {
+ // Clear abort bit as we have just aborted.
+ abort_ = false;
+ return true;
+ }
+ }
+ if (ShouldCancel() || abort_) {
+ // Clear abort bit as we have just aborted.
+ abort_ = false;
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ // Returns true if the action should be canceled.
+ bool ShouldCancel() {
+ action_q_->goal.FetchLatest();
+ bool ans = !action_q_->goal->run;
+ if (ans) {
+ LOG(INFO, "Time to stop action\n");
+ }
+ return ans;
+ }
+
+ protected:
+
+ // boolean to stop on fail
+ bool abort_ = false;
+
+ // queue for this action
+ T* action_q_;
+};
+
+} // namespace actions
+} // namespace frc971
+
+#endif // FRC971_ACTIONS_ACTION_H_
diff --git a/frc971/actions/action_client.h b/frc971/actions/action_client.h
new file mode 100644
index 0000000..2a7d65f
--- /dev/null
+++ b/frc971/actions/action_client.h
@@ -0,0 +1,117 @@
+#ifndef FRC971_ACTIONS_ACTION_CLIENT_H_
+#define FRC971_ACTIONS_ACTION_CLIENT_H_
+
+#include <type_traits>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/queue.h"
+
+namespace frc971 {
+
+class Action {
+ public:
+ // Cancels the action.
+ void Cancel() { DoCancel(); }
+ // Returns true if the action is currently running.
+ bool Running() { return DoRunning(); }
+ // Starts the action.
+ void Start() { DoStart(); }
+
+ // Waits until the action has finished.
+ void WaitUntilDone() { DoWaitUntilDone(); }
+
+ virtual ~Action() {}
+
+ private:
+ virtual void DoCancel() = 0;
+ virtual bool DoRunning() = 0;
+ virtual void DoStart() = 0;
+ virtual void DoWaitUntilDone() = 0;
+};
+
+// Templated subclass to hold the type information.
+template <typename T>
+class TypedAction : public Action {
+ public:
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+
+ TypedAction(T *queue_group)
+ : queue_group_(queue_group),
+ goal_(queue_group_->goal.MakeMessage()),
+ has_started_(false) {}
+
+ // Returns the current goal that will be sent when the action is sent.
+ GoalType *GetGoal() { return goal_.get(); }
+
+ virtual ~TypedAction() {
+ LOG(INFO, "Calling destructor\n");
+ DoCancel();
+ }
+
+ private:
+ // Cancels the action.
+ virtual void DoCancel() {
+ LOG(INFO, "Canceling action on queue %s\n", queue_group_->goal.name());
+ queue_group_->goal.MakeWithBuilder().run(false).Send();
+ }
+
+ // Returns true if the action is running or we don't have an initial response
+ // back from it to signal whether or not it is running.
+ virtual bool DoRunning() {
+ if (has_started_) {
+ queue_group_->status.FetchLatest();
+ } else if (queue_group_->status.FetchLatest()) {
+ if (queue_group_->status->running) {
+ // Wait until it reports that it is running to start.
+ has_started_ = true;
+ }
+ }
+ return !has_started_ ||
+ (queue_group_->status.get() && queue_group_->status->running);
+ }
+
+ // Returns true if the action is running or we don't have an initial response
+ // back from it to signal whether or not it is running.
+ virtual void DoWaitUntilDone() {
+ queue_group_->status.FetchLatest();
+ while (true) {
+ if (has_started_) {
+ queue_group_->status.FetchNextBlocking();
+ } else {
+ queue_group_->status.FetchNextBlocking();
+ if (queue_group_->status->running) {
+ // Wait until it reports that it is running to start.
+ has_started_ = true;
+ }
+ }
+ if (has_started_ &&
+ (queue_group_->status.get() && !queue_group_->status->running)) {
+ return;
+ }
+ }
+ }
+
+ // Starts the action if a goal has been created.
+ virtual void DoStart() {
+ if (goal_) {
+ goal_->run = true;
+ goal_.Send();
+ has_started_ = false;
+ LOG(INFO, "Starting action\n");
+ } else {
+ has_started_ = true;
+ }
+ }
+
+ T *queue_group_;
+ ::aos::ScopedMessagePtr<GoalType> goal_;
+ // Track if we have seen a response to the start message.
+ // If we haven't, we are considered running regardless.
+ bool has_started_;
+};
+
+} // namespace frc971
+
+#endif // FRC971_ACTIONS_ACTION_CLIENT_H_
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
new file mode 100644
index 0000000..7cfb7a5
--- /dev/null
+++ b/frc971/actions/actions.gyp
@@ -0,0 +1,232 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'action_client',
+ 'type': 'static_library',
+ 'sources': [
+ #'action_client.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'shoot_action_queue',
+ 'type': 'static_library',
+ 'sources': ['shoot_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'drivetrain_action_queue',
+ 'type': 'static_library',
+ 'sources': ['drivetrain_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'selfcatch_action_queue',
+ 'type': 'static_library',
+ 'sources': ['selfcatch_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'catch_action_queue',
+ 'type': 'static_library',
+ 'sources': ['catch_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'shoot_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shoot_action.cc',
+ ],
+ 'dependencies': [
+ 'shoot_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ 'action_client',
+ 'action',
+ ],
+ 'export_dependent_settings': [
+ 'action',
+ 'shoot_action_queue',
+ 'action_client',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain_action.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ 'action_client',
+ 'action',
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ ],
+ 'export_dependent_settings': [
+ 'action',
+ 'drivetrain_action_queue',
+ 'action_client',
+ ],
+ },
+ {
+ 'target_name': 'action',
+ 'type': 'static_library',
+ 'sources': [
+ #'action.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ },
+ {
+ 'target_name': 'selfcatch_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'selfcatch_action.cc',
+ ],
+ 'dependencies': [
+ 'selfcatch_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ ],
+ },
+ {
+ 'target_name': 'catch_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'catch_action.cc',
+ ],
+ 'dependencies': [
+ 'catch_action_queue',
+ 'action',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ ],
+ },
+ {
+ 'target_name': 'shoot_action',
+ 'type': 'executable',
+ 'sources': [
+ 'shoot_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'shoot_action_queue',
+ 'shoot_action_lib',
+ 'action',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'drivetrain_action_queue',
+ 'drivetrain_action_lib',
+ 'action',
+ ],
+ },
+ {
+ 'target_name': 'selfcatch_action',
+ 'type': 'executable',
+ 'sources': [
+ 'selfcatch_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'selfcatch_action_queue',
+ 'selfcatch_action_lib',
+ 'action',
+ ],
+ },
+ {
+ 'target_name': 'catch_action',
+ 'type': 'executable',
+ 'sources': [
+ 'catch_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'catch_action_queue',
+ 'catch_action_lib',
+ 'action',
+ ],
+ },
+ ],
+}
diff --git a/frc971/actions/catch_action.cc b/frc971/actions/catch_action.cc
new file mode 100644
index 0000000..1fddd30
--- /dev/null
+++ b/frc971/actions/catch_action.cc
@@ -0,0 +1,154 @@
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/actions/catch_action.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/queues/other_sensors.q.h"
+
+namespace frc971 {
+namespace actions {
+
+CatchAction::CatchAction(actions::CatchActionGroup* s)
+ : actions::ActionBase<actions::CatchActionGroup>(s) {}
+
+void CatchAction::RunAction() {
+ control_loops::claw_queue_group.goal.FetchLatest();
+ if (control_loops::claw_queue_group.goal.get() == nullptr) {
+ LOG(WARNING, "no claw goal\n");
+ return;
+ }
+
+ // Set claw angle.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(action_q_->goal->catch_angle - kCatchSeparation / 2.0)
+ .separation_angle(kCatchSeparation)
+ .intake(kCatchIntake)
+ .centering(kCatchCentering)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ LOG(INFO, "Waiting for the claw to be ready\n");
+
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&CatchAction::DoneSetupCatch, this))) return;
+ LOG(INFO, "Waiting for the sonar\n");
+
+ close_count_ = 0;
+
+ // wait for the sonar to trigger
+ if (WaitUntil(::std::bind(&CatchAction::DoneFoundSonar, this))) return;
+
+ LOG(INFO, "Closing the claw\n");
+
+ // close the claw
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(action_q_->goal->catch_angle)
+ .separation_angle(0.0)
+ .intake(kCatchIntake)
+ .centering(kCatchCentering)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ // claw now closed
+ if (WaitUntil(::std::bind(&CatchAction::DoneClawWithBall, this))) return;
+
+ for (int i = 0; i < 5; ++i) {
+ aos::time::SleepFor(aos::time::Time::InSeconds(0.05));
+ if (ShouldCancel()) return;
+ }
+}
+
+
+/*bool CatchAction::DoneBallIn() {
+ if (!sensors::othersensors.FetchLatest()) {
+ sensors::othersensors.FetchNextBlocking();
+ }
+ if (sensors::othersensors->travis_hall_effect_distance > 0.005) {
+ LOG(INFO, "Ball in at %.2f.\n",
+ sensors::othersensors->travis_hall_effect_distance);
+ return true;
+ }
+ return false;
+}*/
+
+bool CatchAction::DoneClawWithBall() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+
+ bool ans =
+ control_loops::claw_queue_group.status->zeroed &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
+ 1.0) &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) < 0.10) &&
+ (::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle) <
+ 0.4);
+
+ if (!ans) {
+ LOG(INFO,
+ "Claw is ready %d zeroed %d bottom_velocity %f bottom %f sep %f\n", ans,
+ control_loops::claw_queue_group.status->zeroed,
+ ::std::abs(control_loops::claw_queue_group.status->bottom_velocity),
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle),
+ ::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle));
+ }
+ return ans;
+}
+
+bool CatchAction::DoneFoundSonar() {
+ if (!sensors::other_sensors.FetchLatest()) {
+ sensors::other_sensors.FetchNextBlocking();
+ }
+ LOG(DEBUG, "Sonar at %.2f.\n", sensors::other_sensors->sonar_distance);
+ if (sensors::other_sensors->sonar_distance > 0.1 &&
+ sensors::other_sensors->sonar_distance < kSonarTriggerDist) {
+ ++close_count_;
+ } else {
+ close_count_ = 0;
+ }
+ if (close_count_ > 50) {
+ return true;
+ }
+ return false;
+}
+
+bool CatchAction::DoneSetupCatch() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+
+ // Make sure that the shooter and claw has reached the necessary state.
+ // Check the current positions of the various mechanisms to make sure that we
+ // avoid race conditions where we send it a new goal but it still thinks that
+ // it has the old goal and thinks that it is already done.
+ bool claw_angle_correct =
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) < 0.15;
+ bool open_enough =
+ control_loops::claw_queue_group.status->separation > kCatchMinSeparation;
+
+ if (claw_angle_correct && open_enough) {
+ LOG(INFO, "Claw ready for catching.\n");
+ return true;
+ }
+
+ return false;
+}
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/catch_action.h b/frc971/actions/catch_action.h
new file mode 100644
index 0000000..56c5f42
--- /dev/null
+++ b/frc971/actions/catch_action.h
@@ -0,0 +1,42 @@
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/action.h"
+#include "aos/common/time.h"
+
+namespace frc971 {
+namespace actions {
+
+class CatchAction : public ActionBase<CatchActionGroup> {
+ public:
+
+ explicit CatchAction(CatchActionGroup* s);
+
+ // Actually executes the action of moving the claw into position and closing
+ // it.
+ virtual void RunAction();
+
+ static constexpr double kCatchSeparation = 0.8;
+ static constexpr double kCatchMinSeparation = 0.65;
+ static constexpr double kCatchIntake = 12.0;
+ static constexpr double kSonarTriggerDist = 0.725;
+ static constexpr double kCatchCentering = 12.0;
+ static constexpr double kFinishAngle = 0.2;
+
+ protected:
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupCatch();
+ // sonar is in valid range to close
+ bool DoneFoundSonar();
+ // Claw reports it is done
+ bool DoneClawWithBall();
+ // hall effect reports the ball is in
+ bool DoneBallIn();
+
+ private:
+ int close_count_;
+};
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/catch_action.q b/frc971/actions/catch_action.q
new file mode 100644
index 0000000..33b21f0
--- /dev/null
+++ b/frc971/actions/catch_action.q
@@ -0,0 +1,19 @@
+package frc971.actions;
+
+queue_group CatchActionGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run;
+ double catch_angle;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group CatchActionGroup catch_action;
diff --git a/frc971/actions/catch_action_main.cc b/frc971/actions/catch_action_main.cc
new file mode 100644
index 0000000..88fa48b
--- /dev/null
+++ b/frc971/actions/catch_action_main.cc
@@ -0,0 +1,20 @@
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/catch_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actions::CatchAction action_catch(
+ &::frc971::actions::catch_action);
+ action_catch.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
new file mode 100644
index 0000000..1ca6219
--- /dev/null
+++ b/frc971/actions/drivetrain_action.cc
@@ -0,0 +1,65 @@
+#include <functional>
+#include <numeric>
+
+#include <Eigen/Dense>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+#include "frc971/actions/drivetrain_action.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+namespace frc971 {
+namespace actions {
+
+DrivetrainAction::DrivetrainAction(actions::DrivetrainActionQueueGroup* s)
+ : actions::ActionBase<actions::DrivetrainActionQueueGroup>(s) {}
+
+void DrivetrainAction::RunAction() {
+ const double yoffset = action_q_->goal->y_offset;
+ LOG(INFO, "Going to move %f\n", yoffset);
+
+ // Measured conversion to get the distance right.
+ ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
+ ::Eigen::Matrix<double, 2, 1> profile_goal_state;
+ const double goal_velocity = 0.0;
+ const double epsilon = 0.01;
+
+ profile.set_maximum_acceleration(2.2);
+ profile.set_maximum_velocity(action_q_->goal->maximum_velocity);
+
+ while (true) {
+ // wait until next 10ms tick
+ ::aos::time::PhasedLoop10MS(5000);
+ profile_goal_state = profile.Update(yoffset, goal_velocity);
+
+ if (::std::abs(profile_goal_state(0, 0) - yoffset) < epsilon) break;
+ if (ShouldCancel()) return;
+
+ LOG(DEBUG, "Driving left to %f, right to %f\n",
+ profile_goal_state(0, 0) + action_q_->goal->left_initial_position,
+ profile_goal_state(0, 0) + action_q_->goal->right_initial_position);
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .highgear(false)
+ .left_goal(profile_goal_state(0, 0) + action_q_->goal->left_initial_position)
+ .right_goal(profile_goal_state(0, 0) + action_q_->goal->right_initial_position)
+ .left_velocity_goal(profile_goal_state(1, 0))
+ .right_velocity_goal(profile_goal_state(1, 0))
+ .Send();
+ }
+ LOG(INFO, "Done moving\n");
+}
+
+::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+MakeDrivetrainAction() {
+ return ::std::unique_ptr<
+ TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>(
+ new TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>(
+ &::frc971::actions::drivetrain_action));
+}
+
+} // namespace actions
+} // namespace frc971
diff --git a/frc971/actions/drivetrain_action.h b/frc971/actions/drivetrain_action.h
new file mode 100644
index 0000000..b164e22
--- /dev/null
+++ b/frc971/actions/drivetrain_action.h
@@ -0,0 +1,22 @@
+#include <memory>
+
+#include "frc971/actions/drivetrain_action.q.h"
+#include "frc971/actions/action.h"
+#include "frc971/actions/action_client.h"
+
+namespace frc971 {
+namespace actions {
+
+class DrivetrainAction : public ActionBase<actions::DrivetrainActionQueueGroup> {
+ public:
+ explicit DrivetrainAction(actions::DrivetrainActionQueueGroup* s);
+
+ virtual void RunAction();
+};
+
+// Makes a new DrivetrainAction action.
+::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+MakeDrivetrainAction();
+
+} // namespace actions
+} // namespace frc971
diff --git a/frc971/actions/drivetrain_action.q b/frc971/actions/drivetrain_action.q
new file mode 100644
index 0000000..cde518c
--- /dev/null
+++ b/frc971/actions/drivetrain_action.q
@@ -0,0 +1,22 @@
+package frc971.actions;
+
+queue_group DrivetrainActionQueueGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run;
+ double left_initial_position;
+ double right_initial_position;
+ double y_offset;
+ double maximum_velocity;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/frc971/actions/drivetrain_action_main.cc b/frc971/actions/drivetrain_action_main.cc
new file mode 100644
index 0000000..0251e25
--- /dev/null
+++ b/frc971/actions/drivetrain_action_main.cc
@@ -0,0 +1,19 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actions/drivetrain_action.q.h"
+#include "frc971/actions/drivetrain_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actions::DrivetrainAction drivetrain(&::frc971::actions::drivetrain_action);
+ drivetrain.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/actions/selfcatch_action.cc b/frc971/actions/selfcatch_action.cc
new file mode 100644
index 0000000..eef0a05
--- /dev/null
+++ b/frc971/actions/selfcatch_action.cc
@@ -0,0 +1,244 @@
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/constants.h"
+
+#include "frc971/actions/selfcatch_action.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/queues/other_sensors.q.h"
+
+namespace frc971 {
+namespace actions {
+
+SelfCatchAction::SelfCatchAction(actions::SelfCatchActionGroup* s)
+ : actions::ActionBase<actions::SelfCatchActionGroup>(s) {}
+
+double SelfCatchAction::SpeedToAngleOffset(double speed) {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ // scale speed to a [0.0-1.0] on something close to the max
+ return (speed / values.drivetrain_max_speed) *
+ SelfCatchAction::kSpeedOffsetRadians;
+}
+
+void SelfCatchAction::RunAction() {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+
+ // Set shot power to established constant
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
+ kShotPower)
+ .shot_requested(false).unload_requested(false).load_requested(false)
+ .Send()) {
+ LOG(ERROR, "Failed to send the shoot action\n");
+ return;
+ }
+
+ // Set claw angle to account for velocity
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ 0.0 +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneSetupShot, this))) return;
+
+ // Open up the claw in preparation for shooting.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ 0.0 +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(values.shooter_action.claw_separation_goal)
+ .intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for the claw to open up a little before we shoot
+ if (WaitUntil(::std::bind(&SelfCatchAction::DonePreShotOpen, this))) return;
+
+ // Make sure that we have the latest shooter status.
+ control_loops::shooter_queue_group.status.FetchLatest();
+ // Get the number of shots fired up to this point. This should not be updated
+ // again for another few cycles.
+ previous_shots_ = control_loops::shooter_queue_group.status->shots;
+ // Shoot!
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
+ kShotPower)
+ .shot_requested(true).unload_requested(false).load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+
+ // wait for record of shot having been fired
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneShot, this))) return;
+
+ // Set claw angle to account for velocity note this is negative
+ // since we will be catching from behind
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ 0.0 -
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(kCatchSeperation).intake(kCatchIntake)
+ .centering(kCatchCentering).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for the sonar to trigger
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneFoundSonar, this))) return;
+
+ // close the claw
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ kFinishAngle)
+ .separation_angle(0.0).intake(kCatchIntake).centering(kCatchCentering)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // claw now closed
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneClawWithBall, this))) return;
+ // ball is fully in
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneBallIn, this))) return;
+
+ // head to a finshed pose
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ kFinishAngle)
+ .separation_angle(0.0).intake(0.0).centering(0.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // thats it
+ if (WaitUntil(::std::bind(&SelfCatchAction::DoneClawWithBall, this))) return;
+
+ // done with action
+ return;
+}
+
+
+bool SelfCatchAction::DoneBallIn() {
+ if (!sensors::other_sensors.FetchLatest()) {
+ sensors::other_sensors.FetchNextBlocking();
+ }
+ if (sensors::other_sensors->plunger_hall_effect_distance > 0.005) {
+ LOG(INFO, "Ball in at %.2f.\n",
+ sensors::other_sensors->plunger_hall_effect_distance);
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneClawWithBall() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ // Make sure that both the shooter and claw have reached the necessary
+ // states.
+ if (control_loops::claw_queue_group.status->done_with_ball) {
+ LOG(INFO, "Claw at goal.\n");
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneFoundSonar() {
+ if (!sensors::other_sensors.FetchLatest()) {
+ sensors::other_sensors.FetchNextBlocking();
+ }
+ if (sensors::other_sensors->sonar_distance > 0.3 &&
+ sensors::other_sensors->sonar_distance < kSonarTriggerDist) {
+ LOG(INFO, "Hit Sonar at %.2f.\n", sensors::other_sensors->sonar_distance);
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneSetupShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ if (!control_loops::shooter_queue_group.goal.FetchLatest()) {
+ LOG(ERROR, "Failed to fetch shooter goal.\n");
+ }
+ if (!control_loops::claw_queue_group.goal.FetchLatest()) {
+ LOG(ERROR, "Failed to fetch claw goal.\n");
+ }
+ // Make sure that both the shooter and claw have reached the necessary
+ // states.
+ // Check the current positions of the various mechanisms to make sure that we
+ // avoid race conditions where we send it a new goal but it still thinks that
+ // it has the old goal and thinks that it is already done.
+ bool shooter_power_correct =
+ ::std::abs(control_loops::shooter_queue_group.status->hard_stop_power -
+ control_loops::shooter_queue_group.goal->shot_power) <
+ 0.005;
+
+ bool claw_angle_correct =
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) <
+ 0.005;
+
+ if (control_loops::shooter_queue_group.status->ready &&
+ control_loops::claw_queue_group.status->done_with_ball &&
+ shooter_power_correct && claw_angle_correct) {
+ LOG(INFO, "Claw and Shooter ready for shooting.\n");
+ // TODO(james): Get realer numbers for shooter_action.
+ return true;
+ }
+
+ // update the claw position to track velocity
+ // TODO(ben): the claw may never reach the goal if the velocity is
+ // continually changing, we will need testing to see
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ selfcatch_action.goal->shot_angle +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ abort_ = true;
+ return true;
+ } else {
+ LOG(INFO, "Updating claw angle for velocity offset(%.4f).\n",
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed));
+ }
+ return false;
+}
+
+bool SelfCatchAction::DonePreShotOpen() {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+
+ if (control_loops::claw_queue_group.status->separation >
+ values.shooter_action.claw_shooting_separation) {
+ LOG(INFO, "Opened up enough to shoot.\n");
+ return true;
+ }
+ return false;
+}
+
+bool SelfCatchAction::DoneShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (control_loops::shooter_queue_group.status->shots > previous_shots_) {
+ LOG(INFO, "Shot succeeded!\n");
+ return true;
+ }
+ return false;
+}
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/selfcatch_action.h b/frc971/actions/selfcatch_action.h
new file mode 100644
index 0000000..c57d24a
--- /dev/null
+++ b/frc971/actions/selfcatch_action.h
@@ -0,0 +1,48 @@
+#include "frc971/actions/selfcatch_action.q.h"
+#include "frc971/actions/action.h"
+#include "aos/common/time.h"
+
+namespace frc971 {
+namespace actions {
+
+class SelfCatchAction : public ActionBase<SelfCatchActionGroup> {
+ public:
+
+ explicit SelfCatchAction(SelfCatchActionGroup* s);
+
+ // Actually execute the action of moving the claw and shooter into position
+ // and actually firing them.
+ virtual void RunAction();
+
+ // calc an offset to our requested shot based on robot speed
+ double SpeedToAngleOffset(double speed);
+
+ static constexpr double kSpeedOffsetRadians = 0.2;
+ static constexpr double kShotPower = 100.0;
+ static constexpr double kCatchSeperation = 1.0;
+ static constexpr double kCatchIntake = 12.0;
+ static constexpr double kSonarTriggerDist = 0.8;
+ static constexpr double kCatchCentering = 12.0;
+ static constexpr double kFinishAngle = 0.2;
+
+ protected:
+ // completed shot
+ bool DoneShot();
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupShot();
+ // sonar is in valid range to close
+ bool DoneFoundSonar();
+ // Claw reports it is done
+ bool DoneClawWithBall();
+ // hall effect reports the ball is in
+ bool DoneBallIn();
+
+ // to track when shot is complete
+ int previous_shots_;
+};
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/selfcatch_action.q b/frc971/actions/selfcatch_action.q
new file mode 100644
index 0000000..9ba21ad
--- /dev/null
+++ b/frc971/actions/selfcatch_action.q
@@ -0,0 +1,19 @@
+package frc971.actions;
+
+queue_group SelfCatchActionGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run;
+ double shot_angle;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group SelfCatchActionGroup selfcatch_action;
diff --git a/frc971/actions/selfcatch_action_main.cc b/frc971/actions/selfcatch_action_main.cc
new file mode 100644
index 0000000..53600ac
--- /dev/null
+++ b/frc971/actions/selfcatch_action_main.cc
@@ -0,0 +1,22 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actions/selfcatch_action.q.h"
+#include "frc971/actions/selfcatch_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actions::SelfCatchAction selfcatch(
+ &::frc971::actions::selfcatch_action);
+ selfcatch.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
new file mode 100644
index 0000000..b1e27b4
--- /dev/null
+++ b/frc971/actions/shoot_action.cc
@@ -0,0 +1,191 @@
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/actions/shoot_action.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+namespace frc971 {
+namespace actions {
+
+ShootAction::ShootAction(actions::ShootActionQueueGroup* s)
+ : actions::ActionBase<actions::ShootActionQueueGroup>(s) {}
+
+double ShootAction::SpeedToAngleOffset(double speed) {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ // scale speed to a [0.0-1.0] on something close to the max
+ return (speed / values.drivetrain_max_speed) * ShootAction::kOffsetRadians;
+}
+void ShootAction::RunAction() {
+ InnerRunAction();
+
+ // Now do our 'finally' block and make sure that we aren't requesting shots
+ // continually.
+ control_loops::shooter_queue_group.goal.FetchLatest();
+ if (control_loops::shooter_queue_group.goal.get() == nullptr) {
+ return;
+ }
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(control_loops::shooter_queue_group.goal->shot_power)
+ .shot_requested(false)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+}
+
+void ShootAction::InnerRunAction() {
+ LOG(INFO, "Shooting at the original angle and power.\n");
+
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&ShootAction::DoneSetupShot, this))) return;
+
+ // Turn the intake off.
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
+ .separation_angle(
+ control_loops::claw_queue_group.goal->separation_angle)
+ .intake(0.0)
+ .centering(0.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // Make sure that we have the latest shooter status.
+ control_loops::shooter_queue_group.status.FetchLatest();
+ // Get the number of shots fired up to this point. This should not be updated
+ // again for another few cycles.
+ previous_shots_ = control_loops::shooter_queue_group.status->shots;
+ // Shoot!
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(control_loops::shooter_queue_group.goal->shot_power)
+ .shot_requested(true)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+
+ // wait for record of shot having been fired
+ if (WaitUntil(::std::bind(&ShootAction::DoneShot, this))) return;
+
+ // Turn the intake off.
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
+ .separation_angle(
+ control_loops::claw_queue_group.goal->separation_angle)
+ .intake(0.0)
+ .centering(0.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+}
+
+bool ClawIsReady() {
+ if (!control_loops::claw_queue_group.goal.FetchLatest()) {
+ control_loops::claw_queue_group.goal.FetchLatest();
+ }
+
+ bool ans =
+ control_loops::claw_queue_group.status->zeroed &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
+ 0.5) &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) < 0.10) &&
+ (::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle) <
+ 0.4);
+ if (!ans) {
+ LOG(INFO,
+ "Claw is ready %d zeroed %d bottom_velocity %f bottom %f sep %f\n", ans,
+ control_loops::claw_queue_group.status->zeroed,
+ ::std::abs(control_loops::claw_queue_group.status->bottom_velocity),
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle),
+ ::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle));
+ }
+ return ans;
+}
+
+bool ShooterIsReady() {
+ control_loops::shooter_queue_group.goal.FetchLatest();
+ control_loops::shooter_queue_group.status.FetchLatest();
+ if (control_loops::shooter_queue_group.status->ready) {
+ LOG(INFO, "Power error is %f - %f -> %f, ready %d\n",
+ control_loops::shooter_queue_group.status->hard_stop_power,
+ control_loops::shooter_queue_group.goal->shot_power,
+ ::std::abs(control_loops::shooter_queue_group.status->hard_stop_power -
+ control_loops::shooter_queue_group.goal->shot_power),
+ control_loops::shooter_queue_group.status->ready);
+ }
+ return (::std::abs(
+ control_loops::shooter_queue_group.status->hard_stop_power -
+ control_loops::shooter_queue_group.goal->shot_power) < 1.0) &&
+ control_loops::shooter_queue_group.status->ready;
+}
+
+bool ShootAction::DoneSetupShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ // Make sure that both the shooter and claw have reached the necessary
+ // states.
+ if (ShooterIsReady() && ClawIsReady()) {
+ LOG(INFO, "Claw and Shooter ready for shooting.\n");
+ return true;
+ }
+
+ return false;
+}
+
+bool ShootAction::DonePreShotOpen() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ if (control_loops::claw_queue_group.status->separation >
+ kClawShootingSeparation) {
+ LOG(INFO, "Opened up enough to shoot.\n");
+ return true;
+ }
+ return false;
+}
+
+bool ShootAction::DoneShot() {
+ if (!control_loops::shooter_queue_group.status.FetchLatest()) {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ }
+ if (control_loops::shooter_queue_group.status->shots > previous_shots_) {
+ LOG(INFO, "Shot succeeded!\n");
+ return true;
+ }
+ return false;
+}
+
+::std::unique_ptr<TypedAction< ::frc971::actions::ShootActionQueueGroup>>
+MakeShootAction() {
+ return ::std::unique_ptr<
+ TypedAction< ::frc971::actions::ShootActionQueueGroup>>(
+ new TypedAction< ::frc971::actions::ShootActionQueueGroup>(
+ &::frc971::actions::shoot_action));
+}
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/shoot_action.h b/frc971/actions/shoot_action.h
new file mode 100644
index 0000000..6d29ca7
--- /dev/null
+++ b/frc971/actions/shoot_action.h
@@ -0,0 +1,44 @@
+#include <memory>
+
+#include "frc971/actions/shoot_action.q.h"
+#include "frc971/actions/action.h"
+#include "frc971/actions/action_client.h"
+
+namespace frc971 {
+namespace actions {
+
+class ShootAction : public ActionBase<actions::ShootActionQueueGroup> {
+ public:
+
+ explicit ShootAction(actions::ShootActionQueueGroup* s);
+
+ // Actually execute the action of moving the claw and shooter into position
+ // and actually firing them.
+ virtual void RunAction();
+ void InnerRunAction();
+
+ // calc an offset to our requested shot based on robot speed
+ double SpeedToAngleOffset(double speed);
+
+ static constexpr double kOffsetRadians = 0.4;
+ static constexpr double kClawShootingSeparation = 0.10;
+ static constexpr double kClawShootingSeparationGoal = 0.10;
+
+ protected:
+ // completed shot
+ bool DoneShot();
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupShot();
+
+ // to track when shot is complete
+ int previous_shots_;
+};
+
+// Makes a new ShootAction action.
+::std::unique_ptr<TypedAction< ::frc971::actions::ShootActionQueueGroup>>
+MakeShootAction();
+
+} // namespace actions
+} // namespace frc971
diff --git a/frc971/actions/shoot_action.q b/frc971/actions/shoot_action.q
new file mode 100644
index 0000000..d080071
--- /dev/null
+++ b/frc971/actions/shoot_action.q
@@ -0,0 +1,21 @@
+package frc971.actions;
+
+queue_group ShootActionQueueGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run; // Shot power in joules.
+ double shot_power;
+ // Claw angle when shooting.
+ double shot_angle;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group ShootActionQueueGroup shoot_action;
diff --git a/frc971/actions/shoot_action_main.cc b/frc971/actions/shoot_action_main.cc
new file mode 100644
index 0000000..18df9aa
--- /dev/null
+++ b/frc971/actions/shoot_action_main.cc
@@ -0,0 +1,19 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actions/shoot_action.q.h"
+#include "frc971/actions/shoot_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actions::ShootAction shoot(&::frc971::actions::shoot_action);
+ shoot.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index ea2a74b..107b380 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -1,4 +1,6 @@
-#include "stdio.h"
+#include <stdio.h>
+
+#include <memory>
#include "aos/common/control_loop/Timing.h"
#include "aos/common/time.h"
@@ -9,12 +11,19 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/shoot_action.h"
+#include "frc971/actions/drivetrain_action.h"
using ::aos::time::Time;
namespace frc971 {
namespace autonomous {
+namespace time = ::aos::time;
+
static double left_initial_position, right_initial_position;
bool ShouldExitAuto() {
@@ -48,42 +57,6 @@
.Send();
}
-void SetDriveGoal(double yoffset, double maximum_velocity = 1.5) {
- LOG(INFO, "Going to move %f\n", yoffset);
-
- // Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
- ::Eigen::Matrix<double, 2, 1> driveTrainState;
- const double goal_velocity = 0.0;
- const double epsilon = 0.01;
-
- profile.set_maximum_acceleration(2.0);
- profile.set_maximum_velocity(maximum_velocity);
-
- while (true) {
- ::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
- driveTrainState = profile.Update(yoffset, goal_velocity);
-
- if (::std::abs(driveTrainState(0, 0) - yoffset) < epsilon) break;
- if (ShouldExitAuto()) return;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- driveTrainState(0, 0) + left_initial_position,
- driveTrainState(0, 0) + right_initial_position);
- control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(false)
- .left_goal(driveTrainState(0, 0) + left_initial_position)
- .right_goal(driveTrainState(0, 0) + right_initial_position)
- .left_velocity_goal(driveTrainState(1, 0))
- .right_velocity_goal(driveTrainState(1, 0))
- .Send();
- }
- left_initial_position += yoffset;
- right_initial_position += yoffset;
- LOG(INFO, "Done moving\n");
-}
-
void DriveSpin(double radians) {
LOG(INFO, "going to spin %f\n", radians);
@@ -123,13 +96,85 @@
LOG(INFO, "Done moving\n");
}
-void HandleAuto() {
- LOG(INFO, "Handling auto mode\n");
+void PositionClawVertically(double intake_power = 0.0, double centering_power = 0.0) {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.0)
+ .separation_angle(0.0)
+ .intake(intake_power)
+ .centering(centering_power)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
- ResetDrivetrain();
+void PositionClawBackIntake() {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(-2.273474)
+ .separation_angle(0.0)
+ .intake(12.0)
+ .centering(12.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
- if (ShouldExitAuto()) return;
-
+void PositionClawForShot() {
+ // Turn the claw on, keep it straight up until the ball has been grabbed.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.86)
+ .separation_angle(0.10)
+ .intake(4.0)
+ .centering(1.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ }
+}
+
+void SetShotPower(double power) {
+ LOG(INFO, "Setting shot power to %f\n", power);
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(power)
+ .shot_requested(false)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ }
+}
+
+void WaitUntilDoneOrCanceled(Action *action) {
+ while (true) {
+ // Poll the running bit and auto done bits.
+ ::aos::time::PhasedLoop10MS(5000);
+ if (!action->Running() || ShouldExitAuto()) {
+ return;
+ }
+ }
+}
+
+void Shoot() {
+ // Shoot.
+ auto shoot_action = actions::MakeShootAction();
+ shoot_action->Start();
+ WaitUntilDoneOrCanceled(shoot_action.get());
+}
+
+::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+SetDriveGoal(double distance, double maximum_velocity = 1.7) {
+ LOG(INFO, "Driving to %f\n", distance);
+ auto drivetrain_action = actions::MakeDrivetrainAction();
+ drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
+ drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
+ drivetrain_action->GetGoal()->y_offset = distance;
+ drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
+ drivetrain_action->Start();
+ // Uncomment to make relative again.
+ left_initial_position += distance;
+ right_initial_position += distance;
+ return ::std::move(drivetrain_action);
+}
+
+void InitializeEncoders() {
control_loops::drivetrain.position.FetchLatest();
while (!control_loops::drivetrain.position.get()) {
LOG(WARNING, "No previous drivetrain position packet, trying to fetch again\n");
@@ -140,7 +185,107 @@
right_initial_position =
control_loops::drivetrain.position->right_encoder;
- StopDrivetrain();
+}
+
+void WaitUntilClawDone() {
+ while (true) {
+ // Poll the running bit and auto done bits.
+ ::aos::time::PhasedLoop10MS(5000);
+ control_loops::claw_queue_group.status.FetchLatest();
+ control_loops::claw_queue_group.goal.FetchLatest();
+ if (ShouldExitAuto()) {
+ return;
+ }
+ if (control_loops::claw_queue_group.status.get() == nullptr ||
+ control_loops::claw_queue_group.goal.get() == nullptr) {
+ continue;
+ }
+ bool ans =
+ control_loops::claw_queue_group.status->zeroed &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom_velocity) <
+ 1.0) &&
+ (::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) <
+ 0.10) &&
+ (::std::abs(control_loops::claw_queue_group.status->separation -
+ control_loops::claw_queue_group.goal->separation_angle) <
+ 0.4);
+ if (ans) {
+ return;
+ }
+ }
+}
+
+void HandleAuto() {
+ // The front of the robot is 1.854 meters from the wall
+ const double kShootDistance = 3.15;
+ const double kPickupDistance = 0.5;
+ LOG(INFO, "Handling auto mode\n");
+ ResetDrivetrain();
+
+ if (ShouldExitAuto()) return;
+ InitializeEncoders();
+
+ // Turn the claw on, keep it straight up until the ball has been grabbed.
+ LOG(INFO, "Claw going up\n");
+ PositionClawVertically(12.0, 4.0);
+ SetShotPower(115.0);
+
+ // Wait for the ball to enter the claw.
+ time::SleepFor(time::Time::InSeconds(0.25));
+ if (ShouldExitAuto()) return;
+ LOG(INFO, "Readying claw for shot\n");
+
+ {
+ if (ShouldExitAuto()) return;
+ // Drive to the goal.
+ auto drivetrain_action = SetDriveGoal(-kShootDistance);
+ time::SleepFor(time::Time::InSeconds(0.75));
+ PositionClawForShot();
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+ LOG(INFO, "Shooting\n");
+
+ // Shoot.
+ Shoot();
+ time::SleepFor(time::Time::InSeconds(0.1));
+
+ {
+ if (ShouldExitAuto()) return;
+ // Intake the new ball.
+ LOG(INFO, "Claw ready for intake\n");
+ PositionClawBackIntake();
+ auto drivetrain_action = SetDriveGoal(kShootDistance + kPickupDistance);
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ LOG(INFO, "Wait for the claw.\n");
+ WaitUntilClawDone();
+ if (ShouldExitAuto()) return;
+ }
+
+ // Drive back.
+ {
+ LOG(INFO, "Driving back\n");
+ auto drivetrain_action = SetDriveGoal(-(kShootDistance + kPickupDistance));
+ time::SleepFor(time::Time::InSeconds(0.7));
+ if (ShouldExitAuto()) return;
+ PositionClawForShot();
+ LOG(INFO, "Waiting until drivetrain is finished\n");
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ WaitUntilClawDone();
+ if (ShouldExitAuto()) return;
+ }
+
+ LOG(INFO, "Shooting\n");
+ // Shoot
+ Shoot();
+ if (ShouldExitAuto()) return;
+
+ // Get ready to zero when we come back up.
+ PositionClawVertically(0.0, 0.0);
}
} // namespace autonomous
diff --git a/frc971/autonomous/auto_main.cc b/frc971/autonomous/auto_main.cc
index e8914c6..36494bc 100644
--- a/frc971/autonomous/auto_main.cc
+++ b/frc971/autonomous/auto_main.cc
@@ -12,6 +12,7 @@
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init();
+ LOG(INFO, "Auto main started\n");
::frc971::autonomous::autonomous.FetchLatest();
while (!::frc971::autonomous::autonomous.get()) {
::frc971::autonomous::autonomous.FetchNextBlocking();
@@ -24,9 +25,13 @@
LOG(INFO, "Got another auto packet\n");
}
LOG(INFO, "Starting auto mode\n");
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
::frc971::autonomous::HandleAuto();
- LOG(INFO, "Auto mode exited, waiting for it to finish.\n");
+
+ ::aos::time::Time elapsed_time = ::aos::time::Time::Now() - start_time;
+ LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
+ elapsed_time.ToSeconds());
while (::frc971::autonomous::autonomous->run_auto) {
::frc971::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 91b1b2f..980425f 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -25,11 +25,16 @@
'auto_queue',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/actions/actions.gyp:action_client',
+ '<(DEPTH)/frc971/actions/actions.gyp:shoot_action_lib',
+ '<(DEPTH)/frc971/actions/actions.gyp:drivetrain_action_lib',
],
'export_dependent_settings': [
'<(AOS)/common/common.gyp:controls',
diff --git a/frc971/constants.cc b/frc971/constants.cc
index dd47975..9db46fd 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -9,9 +9,7 @@
#include "aos/common/network/team_number.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -21,29 +19,28 @@
namespace constants {
namespace {
+const uint16_t kCompTeamNumber = 971;
+const uint16_t kPracticeTeamNumber = 9971;
+
const double kCompDrivetrainEncoderRatio =
- (15.0 / 50.0) /*output reduction*/ * (36.0 / 24.0) /*encoder gears*/;
-const double kCompLowGearRatio = 14.0 / 60.0 * 15.0 / 50.0;
-const double kCompHighGearRatio = 30.0 / 44.0 * 15.0 / 50.0;
+ (18.0 / 50.0) /*output reduction*/ * (56.0 / 30.0) /*encoder gears*/;
+const double kCompLowGearRatio = 18.0 / 60.0 * 18.0 / 50.0;
+const double kCompHighGearRatio = 28.0 / 50.0 * 18.0 / 50.0;
-const double kPracticeDrivetrainEncoderRatio =
- (17.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/;
-const double kPracticeLowGearRatio = 16.0 / 60.0 * 19.0 / 50.0;
-const double kPracticeHighGearRatio = 28.0 / 48.0 * 19.0 / 50.0;
+const double kPracticeDrivetrainEncoderRatio = kCompDrivetrainEncoderRatio;
+const double kPracticeLowGearRatio = kCompLowGearRatio;
+const double kPracticeHighGearRatio = kCompHighGearRatio;
-const ShifterHallEffect kCompLeftDriveShifter{0.83, 2.32, 1.2, 1.0};
-const ShifterHallEffect kCompRightDriveShifter{0.865, 2.375, 1.2, 1.0};
+const ShifterHallEffect kCompRightDriveShifter{555, 657, 660, 560, 0.2, 0.7};
+const ShifterHallEffect kCompLeftDriveShifter{555, 660, 644, 552, 0.2, 0.7};
-const ShifterHallEffect kPracticeLeftDriveShifter{5, 0, 0.60,
- 0.47};
-const ShifterHallEffect kPracticeRightDriveShifter{5, 0, 0.62,
- 0.55};
-const double shooter_zeroing_off_speed = 0.0;
+const ShifterHallEffect kPracticeRightDriveShifter{550, 640, 635, 550, 0.2, 0.7};
+const ShifterHallEffect kPracticeLeftDriveShifter{540, 620, 640, 550, 0.2, 0.7};
+
const double shooter_zeroing_speed = 0.05;
+const double shooter_unload_speed = 0.08;
-const Values *DoGetValues() {
- uint16_t team = ::aos::network::GetTeamNumber();
- LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+const Values *DoGetValuesForTeam(uint16_t team) {
switch (team) {
case 1: // for tests
return new Values{
@@ -52,15 +49,15 @@
kCompHighGearRatio,
kCompLeftDriveShifter,
kCompRightDriveShifter,
- true,
- control_loops::MakeVClutchDrivetrainLoop,
- control_loops::MakeClutchDrivetrainLoop,
+ false,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
// ShooterLimits
// TODO(ben): make these real numbers
{-0.00127, 0.298196, -0.001524, 0.305054, 0.0149098,
{-0.001778, 0.000762, 0, 0}, {-0.001778, 0.009906, 0, 0}, {0.006096, 0.026416, 0, 0},
- shooter_zeroing_off_speed,
- shooter_zeroing_speed
+ shooter_zeroing_speed,
+ shooter_unload_speed
},
{0.5,
0.1,
@@ -74,7 +71,10 @@
0.01, // claw_unimportant_epsilon
0.9, // start_fine_tune_pos
4.0,
- }
+ },
+ {0.07, 0.15}, // shooter_action
+ 0.02, // drivetrain done delta
+ 5.0 // drivetrain max speed
};
break;
case kCompTeamNumber:
@@ -85,28 +85,39 @@
kCompLeftDriveShifter,
kCompRightDriveShifter,
true,
- control_loops::MakeVClutchDrivetrainLoop,
- control_loops::MakeClutchDrivetrainLoop,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
// ShooterLimits
- // TODO(ben): make these real numbers
- {-0.00127, 0.298196, -0.001524, 0.305054, 0.0149098,
- {-0.001778, 0.000762, 0, 0}, {-0.001778, 0.009906, 0, 0}, {0.006096, 0.026416, 0, 0},
- shooter_zeroing_off_speed,
- shooter_zeroing_speed
+ {-0.001041, 0.296019, -0.001488, 0.302717, 0.0149098,
+ {-0.002, 0.000446, -0.002, 0.000446},
+ {-0.002, 0.009078, -0.002, 0.009078},
+ {0.003870, 0.026194, 0.003869, 0.026343},
+ shooter_zeroing_speed,
+ shooter_unload_speed
},
- {0.5,
- 0.1,
- 0.1,
- 0.0,
- 1.57,
- 0,
- 0,
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
- {0.0, 2.05, 0.02, 2.02, {-0.1, 0.05, 0, 0}, {1.0, 1.1, 0, 0}, {2.0, 2.1, 0, 0}},
- 0.01, // claw_unimportant_epsilon
- 0.9, // start_fine_tune_pos
- 4.0,
- }
+ {0.800000,
+ 0.400000,
+ 0.000000,
+ -1.220821,
+ 1.822142,
+ -0.849484,
+ 1.42309,
+ {-3.328397, 2.091668, -3.166136, 1.95,
+ {-3.4, -3.092051, -3.4, -3.093414},
+ {-0.249073, -0.035452, -0.251800, -0.033179},
+ {1.889410, 2.2, 1.883956, 2.2}},
+ {-2.453460, 3.082960, -2.453460, 3.082960,
+ {-2.6, -2.185752, -2.6, -2.184843},
+ {-0.280434, -0.049087, -0.277707, -0.047724},
+ {2.892065, 3.2, 2.888429, 3.2}},
+ 0.040000, // claw_unimportant_epsilon
+ -0.400000, // start_fine_tune_pos
+ 4.000000,
+ },
+ //TODO(james): Get realer numbers for shooter_action.
+ {0.07, 0.15}, // shooter_action
+ 0.02, // drivetrain done delta
+ 5.0 // drivetrain max speed
};
break;
case kPracticeTeamNumber:
@@ -117,30 +128,39 @@
kPracticeLeftDriveShifter,
kPracticeRightDriveShifter,
false,
- control_loops::MakeVDogDrivetrainLoop,
- control_loops::MakeDogDrivetrainLoop,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
// ShooterLimits
- // TODO(ben): make these real numbers
{-0.001042, 0.294084, -0.001935, 0.303460, 0.0138401,
{-0.002, 0.000446, -0.002, 0.000446},
{-0.002, 0.009078, -0.002, 0.009078},
{0.003869, 0.026194, 0.003869, 0.026194},
- shooter_zeroing_off_speed,
- shooter_zeroing_speed
+ shooter_zeroing_speed,
+ shooter_unload_speed
},
- {0.400000,
- 0.200000,
- 0.000000,
- -0.762218,
- 0.912207,
- -0.362218,
- 0.512207,
- {-1.682379, 1.043334, -1.282379, 0.643334, {-1.7, -1.544662, -1.7, -1.547616}, {-0.130218, -0.019771, -0.132036, -0.018862}, {0.935842, 1.1, 0.932660, 1.1}},
- {-1.225821, 1.553752, -0.825821, 1.153752, {-1.3, -1.088331, -1.3, -1.088331}, {-0.134536, -0.018408, -0.136127, -0.019771}, {1.447396, 1.6, 1.443987, 1.6}},
- 0.020000, // claw_unimportant_epsilon
- -0.200000, // start_fine_tune_pos
+ {0.400000 * 2.0,
+ 0.200000 * 2.0,
+ 0.000000 * 2.0,
+ -0.762218 * 2.0,
+ 0.912207 * 2.0,
+ -0.849484,
+ 1.42308,
+ {-3.364758, 2.086668, -3.166136, 1.95,
+ {-1.7 * 2.0, -1.544662 * 2.0, -1.7 * 2.0, -1.547616 * 2.0},
+ {-0.130218 * 2.0, -0.019771 * 2.0, -0.132036 * 2.0, -0.018862 * 2.0},
+ {0.935842 * 2.0, 1.1 * 2.0, 0.932660 * 2.0, 1.1 * 2.0}},
+ {-2.451642, 3.107504, -2.273474, 2.750,
+ {-1.3 * 2.0, -1.088331 * 2.0, -1.3 * 2.0, -1.088331 * 2.0},
+ {-0.134536 * 2.0, -0.018408 * 2.0, -0.136127 * 2.0, -0.019771 * 2.0},
+ {1.447396 * 2.0, 1.6 * 2.0, 1.443987 * 2.0, 1.6 * 2.0}},
+ 0.020000 * 2.0, // claw_unimportant_epsilon
+ -0.200000 * 2.0, // start_fine_tune_pos
4.000000,
- }
+ },
+ //TODO(james): Get realer numbers for shooter_action.
+ {0.07, 0.15}, // shooter_action
+ 0.02, // drivetrain done delta
+ 5.0 // drivetrain max speed
};
break;
default:
@@ -148,6 +168,12 @@
}
}
+const Values *DoGetValues() {
+ uint16_t team = ::aos::network::GetTeamNumber();
+ LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+ return DoGetValuesForTeam(team);
+}
+
} // namespace
const Values &GetValues() {
@@ -155,5 +181,9 @@
return *once.Get();
}
+const Values &GetValuesForTeam(uint16_t team_number) {
+ return *(DoGetValuesForTeam(team_number));
+}
+
} // namespace constants
} // namespace frc971
diff --git a/frc971/constants.h b/frc971/constants.h
index f696e8b..3a37aa7 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -11,16 +11,15 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
-const uint16_t kCompTeamNumber = 8971;
-const uint16_t kPracticeTeamNumber = 971;
-
// Contains the voltages for an analog hall effect sensor on a shifter.
struct ShifterHallEffect {
// The numbers to use for scaling raw voltages to 0-1.
- double high, low;
+ // Low is near 0.0, high is near 1.0
+ double low_gear_middle, low_gear_low;
+ double high_gear_high, high_gear_middle;
// The numbers for when the dog is clear of each gear.
- double clear_high, clear_low;
+ double clear_low, clear_high;
};
// This structure contains current values for all of the things that change.
@@ -59,8 +58,8 @@
AnglePair plunger_back;
AnglePair pusher_distal;
AnglePair pusher_proximal;
- double zeroing_off_speed;
double zeroing_speed;
+ double unload_speed;
};
Shooter shooter;
@@ -99,10 +98,26 @@
double max_zeroing_voltage;
};
Claws claw;
+
+ // Has all the constants for the ShootAction class.
+ struct ShooterAction {
+ // Minimum separation required between the claws in order to be able to
+ // shoot.
+ double claw_shooting_separation;
+
+ // Goal to send to the claw when opening it up in preparation for shooting;
+ // should be larger than claw_shooting_separation so that we can shoot
+ // promptly.
+ double claw_separation_goal;
+ };
+ ShooterAction shooter_action;
+ double drivetrain_done_distance;
+ double drivetrain_max_speed;
};
// Creates (once) a Values instance and returns a reference to it.
const Values &GetValues();
+const Values &GetValuesForTeam(uint16_t team_number);
} // namespace constants
} // namespace frc971
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index b804399..58b2ca0 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -4,6 +4,7 @@
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
@@ -34,7 +35,8 @@
// correct side of the zero and go zero.
// Valid region plan.
-// Difference between the arms has a range, and the values of each arm has a range.
+// Difference between the arms has a range, and the values of each arm has a
+// range.
// If a claw runs up against a static limit, don't let the goal change outside
// the limit.
// If a claw runs up against a movable limit, move both claws outwards to get
@@ -170,9 +172,9 @@
void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition &claw) {
set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- front_.Reset();
- calibration_.Reset();
- back_.Reset();
+ front_.Reset(claw.front);
+ calibration_.Reset(claw.calibration);
+ back_.Reset(claw.back);
// close up the min and max edge positions as they are no longer valid and
// will be expanded in future iterations
min_hall_effect_on_angle_ = claw.position;
@@ -211,7 +213,8 @@
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
- : aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
+ : aos::control_loops::ControlLoop<control_loops::ClawGroup, true, true,
+ false>(my_claw),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -226,17 +229,54 @@
const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
+bool ZeroedStateFeedbackLoop::SawFilteredPosedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (posedge_filter_ == nullptr && this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() && !sensorB.posedge_count_changed() &&
+ this_sensor.value() && !this_sensor.last_value()) {
+ posedge_filter_ = &this_sensor;
+ } else if (posedge_filter_ == &this_sensor &&
+ !this_sensor.posedge_count_changed() &&
+ !sensorA.posedge_count_changed() &&
+ !sensorB.posedge_count_changed() && this_sensor.value()) {
+ posedge_filter_ = nullptr;
+ return true;
+ } else if (posedge_filter_ == &this_sensor) {
+ posedge_filter_ = nullptr;
+ }
+ return false;
+}
+
+bool ZeroedStateFeedbackLoop::SawFilteredNegedge(
+ const HallEffectTracker &this_sensor, const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB) {
+ if (negedge_filter_ == nullptr && this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() && !sensorB.negedge_count_changed() &&
+ !this_sensor.value() && this_sensor.last_value()) {
+ negedge_filter_ = &this_sensor;
+ } else if (negedge_filter_ == &this_sensor &&
+ !this_sensor.negedge_count_changed() &&
+ !sensorA.negedge_count_changed() &&
+ !sensorB.negedge_count_changed() && !this_sensor.value()) {
+ negedge_filter_ = nullptr;
+ return true;
+ } else if (negedge_filter_ == &this_sensor) {
+ negedge_filter_ = nullptr;
+ }
+ return false;
+}
+
bool ZeroedStateFeedbackLoop::DoGetPositionOfEdge(
const constants::Values::Claws::AnglePair &angles, double *edge_encoder,
- double *edge_angle, const HallEffectTracker &sensor,
+ double *edge_angle, const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA, const HallEffectTracker &sensorB,
const char *hall_effect_name) {
bool found_edge = false;
- if (sensor.posedge_count_changed()) {
+ if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
- // we oddly got two of the same edge.
- *edge_angle = last_edge_value_;
- found_edge = true;
+ LOG(WARNING, "%s: Uncertain which side, rejecting posedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
@@ -251,14 +291,14 @@
name_, hall_effect_name, *edge_angle, posedge_value_,
average_last_encoder);
}
- }
- *edge_encoder = posedge_value_;
- found_edge = true;
- }
- if (sensor.negedge_count_changed()) {
- if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
- *edge_angle = last_edge_value_;
+ *edge_encoder = posedge_value_;
found_edge = true;
+ }
+ }
+
+ if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
+ if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
+ LOG(WARNING, "%s: Uncertain which side, rejecting negedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
@@ -274,12 +314,8 @@
average_last_encoder);
}
*edge_encoder = negedge_value_;
+ found_edge = true;
}
- found_edge = true;
- }
-
- if (found_edge) {
- last_edge_value_ = *edge_angle;
}
return found_edge;
@@ -288,23 +324,16 @@
bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
const constants::Values::Claws::Claw &claw_values, double *edge_encoder,
double *edge_angle) {
- // TODO(austin): Validate that the hall effect edge makes sense.
- // We must now be on the side of the edge that we expect to be, and the
- // encoder must have been on either side of the edge before and after.
-
- // TODO(austin): Compute the last off range min and max and compare the edge
- // value to the middle of the range. This will be quite a bit more reliable.
-
- if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle,
- front_, "front")) {
+ if (DoGetPositionOfEdge(claw_values.front, edge_encoder, edge_angle, front_,
+ calibration_, back_, "front")) {
return true;
}
if (DoGetPositionOfEdge(claw_values.calibration, edge_encoder, edge_angle,
- calibration_, "calibration")) {
+ calibration_, front_, back_, "calibration")) {
return true;
}
- if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle,
- back_, "back")) {
+ if (DoGetPositionOfEdge(claw_values.back, edge_encoder, edge_angle, back_,
+ calibration_, front_, "back")) {
return true;
}
return false;
@@ -358,14 +387,12 @@
const double separation = *top_goal - *bottom_goal;
if (separation > values.claw.claw_max_separation) {
- LOG(DEBUG, "Greater than\n");
const double dsep = (separation - values.claw.claw_max_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
LOG(DEBUG, "Goals now bottom: %f, top: %f\n", *bottom_goal, *top_goal);
}
if (separation < values.claw.claw_min_separation) {
- LOG(DEBUG, "Less than\n");
const double dsep = (separation - values.claw.claw_min_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
@@ -396,7 +423,8 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (::aos::robot_state->autonomous &&
+ (((::aos::robot_state.get() == NULL) ? true
+ : ::aos::robot_state->autonomous) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -411,7 +439,7 @@
void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
- ::aos::control_loops::Status *status) {
+ control_loops::ClawGroup::Status *status) {
constexpr double dt = 0.01;
// Disable the motors now so that all early returns will return with the
@@ -420,6 +448,15 @@
output->top_claw_voltage = 0;
output->bottom_claw_voltage = 0;
output->intake_voltage = 0;
+ output->tusk_voltage = 0;
+ }
+
+ if (goal) {
+ if (::std::isnan(goal->bottom_angle) ||
+ ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
+ ::std::isnan(goal->centering)) {
+ return;
+ }
}
if (reset()) {
@@ -427,10 +464,6 @@
bottom_claw_.Reset(position->bottom);
}
- if (::aos::robot_state.get() == nullptr) {
- return;
- }
-
const frc971::constants::Values &values = constants::GetValues();
if (position) {
@@ -454,25 +487,33 @@
initial_separation_ =
top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
- LOG(DEBUG, "Claw position is (top: %f bottom: %f\n",
- top_claw_.absolute_position(), bottom_claw_.absolute_position());
+ LOG_STRUCT(DEBUG, "absolute position",
+ ClawPositionToLog(top_claw_.absolute_position(),
+ bottom_claw_.absolute_position()));
}
- const bool autonomous = ::aos::robot_state->autonomous;
- const bool enabled = ::aos::robot_state->enabled;
+ bool autonomous, enabled;
+ if (::aos::robot_state.get() == nullptr) {
+ autonomous = true;
+ enabled = false;
+ } else {
+ autonomous = ::aos::robot_state->autonomous;
+ enabled = ::aos::robot_state->enabled;
+ }
double bottom_claw_velocity_ = 0.0;
double top_claw_velocity_ = 0.0;
- if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
- bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (autonomous &&
- ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- top_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
- (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- bottom_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION)))) {
+ if (goal != NULL &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
+ bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
+ (autonomous &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
// Ready to use the claw.
// Limit the goals here.
bottom_claw_goal_ = goal->bottom_angle;
@@ -483,9 +524,9 @@
mode_ = READY;
} else if (top_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
bottom_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
// Time to fine tune the zero.
// Limit the goals here.
if (!enabled) {
@@ -527,23 +568,22 @@
mode_ = PREP_FINE_TUNE_BOTTOM;
}
- if (bottom_claw_.calibration().value()) {
- if (bottom_claw_.calibration().posedge_count_changed() &&
- position) {
- // do calibration
- bottom_claw_.SetCalibration(
- position->bottom.posedge_value,
- values.claw.lower_claw.calibration.lower_angle);
- bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calibrated so we are done fine tuning bottom
- doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the bottom correctly!\n");
- } else {
- doing_calibration_fine_tune_ = false;
- bottom_claw_goal_ = values.claw.start_fine_tune_pos;
- top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- mode_ = PREP_FINE_TUNE_BOTTOM;
- }
+ if (position && bottom_claw_.SawFilteredPosedge(
+ bottom_claw_.calibration(), bottom_claw_.front(),
+ bottom_claw_.back())) {
+ // do calibration
+ bottom_claw_.SetCalibration(
+ position->bottom.posedge_value,
+ values.claw.lower_claw.calibration.lower_angle);
+ bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning bottom
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the bottom correctly!\n");
+ } else if (bottom_claw_.calibration().last_value()) {
+ doing_calibration_fine_tune_ = false;
+ bottom_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ mode_ = PREP_FINE_TUNE_BOTTOM;
} else {
LOG(DEBUG, "Fine tuning\n");
}
@@ -583,23 +623,23 @@
LOG(DEBUG, "Found a limit, starting over.\n");
mode_ = PREP_FINE_TUNE_TOP;
}
- if (top_claw_.calibration().value()) {
- if (top_claw_.calibration().posedge_count_changed() &&
- position) {
- // do calibration
- top_claw_.SetCalibration(
- position->top.posedge_value,
- values.claw.upper_claw.calibration.lower_angle);
- top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
- // calibrated so we are done fine tuning top
- doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the top correctly!\n");
- } else {
- doing_calibration_fine_tune_ = false;
- top_claw_goal_ = values.claw.start_fine_tune_pos;
- top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- mode_ = PREP_FINE_TUNE_TOP;
- }
+
+ if (position &&
+ top_claw_.SawFilteredPosedge(top_claw_.calibration(),
+ top_claw_.front(), top_claw_.back())) {
+ // do calibration
+ top_claw_.SetCalibration(
+ position->top.posedge_value,
+ values.claw.upper_claw.calibration.lower_angle);
+ top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
+ // calibrated so we are done fine tuning top
+ doing_calibration_fine_tune_ = false;
+ LOG(DEBUG, "Calibrated the top correctly!\n");
+ } else if (top_claw_.calibration().last_value()) {
+ doing_calibration_fine_tune_ = false;
+ top_claw_goal_ = values.claw.start_fine_tune_pos;
+ top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
+ mode_ = PREP_FINE_TUNE_TOP;
}
}
// now set the bottom claw to track
@@ -609,8 +649,8 @@
doing_calibration_fine_tune_ = false;
if (!was_enabled_ && enabled) {
if (position) {
- top_claw_goal_ = position->top.position;
- bottom_claw_goal_ = position->bottom.position;
+ top_claw_goal_ = position->top.position + top_claw_.offset();
+ bottom_claw_goal_ = position->bottom.position + bottom_claw_.offset();
initial_separation_ =
position->top.position - position->bottom.position;
} else {
@@ -644,18 +684,26 @@
}
}
- if (enabled) {
- top_claw_.SetCalibrationOnEdge(
- values.claw.upper_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
- bottom_claw_.SetCalibrationOnEdge(
- values.claw.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
- } else {
- // TODO(austin): Only calibrate on the predetermined edge.
- // We might be able to just ignore this since the backlash is soooo low. :)
- top_claw_.SetCalibrationOnEdge(
- values.claw.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
- bottom_claw_.SetCalibrationOnEdge(
- values.claw.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ if (position) {
+ if (enabled) {
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
+ } else {
+ // TODO(austin): Only calibrate on the predetermined edge.
+ // We might be able to just ignore this since the backlash is soooo
+ // low.
+ // :)
+ top_claw_.SetCalibrationOnEdge(
+ values.claw.upper_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ bottom_claw_.SetCalibrationOnEdge(
+ values.claw.lower_claw,
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ }
}
mode_ = UNKNOWN_LOCATION;
}
@@ -672,8 +720,8 @@
if (position != nullptr) {
separation = position->top.position - position->bottom.position;
}
- LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
- claw_.R(1, 0), separation);
+ LOG_STRUCT(DEBUG, "actual goal",
+ ClawGoalToLog(claw_.R(0, 0), claw_.R(1, 0), separation));
// Only cap power when one of the halves of the claw is moving slowly and
// could wind up.
@@ -709,8 +757,9 @@
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
- LOG(DEBUG, "Uncapped is %f, max is %f, difference is %f\n",
+ LOG(DEBUG, "Moving the goal by %f to prevent windup."
+ " Uncapped is %f, max is %f, difference is %f\n",
+ dx,
claw_.uncapped_average_voltage(), values.claw.max_zeroing_voltage,
(claw_.uncapped_average_voltage() -
values.claw.max_zeroing_voltage));
@@ -735,8 +784,19 @@
}
if (output) {
+ if (goal) {
+ //setup the intake
+ output->intake_voltage =
+ (goal->intake > 12.0) ? 12 : (goal->intake < -12.0) ? -12.0
+ : goal->intake;
+ output->tusk_voltage = goal->centering;
+ output->tusk_voltage =
+ (goal->centering > 12.0) ? 12 : (goal->centering < -12.0)
+ ? -12.0
+ : goal->centering;
+ }
output->top_claw_voltage = claw_.U(1, 0);
- output->bottom_claw_voltage = claw_.U(0, 0);
+ output->bottom_claw_voltage = claw_.U(0, 0);
if (output->top_claw_voltage > kMaxVoltage) {
output->top_claw_voltage = kMaxVoltage;
@@ -750,10 +810,41 @@
output->bottom_claw_voltage = -kMaxVoltage;
}
}
- status->done = false;
- was_enabled_ = ::aos::robot_state->enabled;
+ status->bottom = bottom_absolute_position();
+ status->separation = top_absolute_position() - bottom_absolute_position();
+ status->bottom_velocity = claw_.X_hat(2, 0);
+ status->separation_velocity = claw_.X_hat(3, 0);
+
+ if (goal) {
+ bool bottom_done =
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
+ bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
+ bool separation_done =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.020;
+ bool separation_done_with_ball =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.06;
+ status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
+ status->done_with_ball =
+ is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ } else {
+ status->done = status->done_with_ball = false;
+ }
+
+ status->zeroed = is_ready();
+ status->zeroed_for_auto =
+ (top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+
+ was_enabled_ = enabled;
}
} // namespace control_loops
} // namespace frc971
+
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index f71bf6b..bdce229 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -31,6 +31,7 @@
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'claw_loop',
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 48650b4..dab1dca 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -75,10 +75,6 @@
void Reset(const HalfClawPosition &claw);
- bool ready() {
- return front_.ready() && calibration_.ready() && back_.ready();
- }
-
double absolute_position() const { return encoder() + offset(); }
const HallEffectTracker &front() const { return front_; }
@@ -107,6 +103,14 @@
bool GetPositionOfEdge(const constants::Values::Claws::Claw &claw,
double *edge_encoder, double *edge_angle);
+ bool SawFilteredPosedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
+ bool SawFilteredNegedge(const HallEffectTracker &this_sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB);
+
#undef COUNT_SETTER_GETTER
protected:
@@ -121,7 +125,6 @@
JointZeroingState zeroing_state_;
double posedge_value_;
double negedge_value_;
- double last_edge_value_;
double min_hall_effect_on_angle_;
double max_hall_effect_on_angle_;
double min_hall_effect_off_angle_;
@@ -132,11 +135,16 @@
double last_off_encoder_;
bool any_triggered_last_;
+ const HallEffectTracker* posedge_filter_ = nullptr;
+ const HallEffectTracker* negedge_filter_ = nullptr;
+
private:
// Does the edges of 1 sensor for GetPositionOfEdge.
bool DoGetPositionOfEdge(const constants::Values::Claws::AnglePair &angles,
double *edge_encoder, double *edge_angle,
const HallEffectTracker &sensor,
+ const HallEffectTracker &sensorA,
+ const HallEffectTracker &sensorB,
const char *hall_effect_name);
};
@@ -164,8 +172,8 @@
JointZeroingState zeroing_state);
};
-class ClawMotor
- : public aos::control_loops::ControlLoop<control_loops::ClawGroup> {
+class ClawMotor : public aos::control_loops::ControlLoop<
+ control_loops::ClawGroup, true, true, false> {
public:
explicit ClawMotor(control_loops::ClawGroup *my_claw =
&control_loops::claw_queue_group);
@@ -201,7 +209,7 @@
virtual void RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
- ::aos::control_loops::Status *status);
+ control_loops::ClawGroup::Status *status);
double top_absolute_position() const {
return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 333f83c..116a182 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -31,7 +31,10 @@
double bottom_angle;
// How much higher the top claw is.
double separation_angle;
- bool intake;
+ // top claw intake roller
+ double intake;
+ // bottom claw tusk centering
+ double centering;
};
message Position {
@@ -48,10 +51,38 @@
double tusk_voltage;
};
+ message Status {
+ // True if zeroed enough for the current period (autonomous or teleop).
+ bool zeroed;
+ // True if zeroed as much as we will force during autonomous.
+ bool zeroed_for_auto;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ bool done;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ // seperation allowance much wider as a ball may be included
+ bool done_with_ball;
+ // Dump the values of the state matrix.
+ double bottom;
+ double bottom_velocity;
+ double separation;
+ double separation_velocity;
+ };
+
queue Goal goal;
queue Position position;
queue Output output;
- queue aos.control_loops.Status status;
+ queue Status status;
};
queue_group ClawGroup claw_queue_group;
+
+struct ClawPositionToLog {
+ double top;
+ double bottom;
+};
+
+struct ClawGoalToLog {
+ double bottom_pos;
+ double bottom_vel;
+ double separation;
+};
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index e35cd7f..8704ad0 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -231,6 +231,7 @@
v.claw.claw_max_separation);
EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
v.claw.claw_min_separation);
+ ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
}
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -283,6 +284,7 @@
.reader_pid(254)
.cape_resets(5)
.Send();
+ ::aos::time::Time::EnableMockTime(::aos::time::Time::InSeconds(0.0));
}
void SendDSPacket(bool enabled) {
@@ -309,9 +311,23 @@
virtual ~ClawTest() {
::aos::robot_state.Clear();
::bbb::sensor_generation.Clear();
+ ::aos::time::Time::DisableMockTime();
}
};
+TEST_F(ClawTest, HandlesNAN) {
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(::std::nan(""))
+ .separation_angle(::std::nan(""))
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+}
+
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ClawTest, ZerosCorrectly) {
claw_queue_group.goal.MakeWithBuilder()
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index babbb04..106491d 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -25,9 +25,9 @@
StateFeedbackController<4, 2, 2> MakeClawController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.42518438347, 4.71027737605e-16, -1.42518438347, 1.04485564063, 30.6346010502, 1.00485917356e-14, -30.6346010502, 2.04727497147;
+ L << 1.48518438347, 2.30607329869e-16, -1.48518438347, 1.10485564063, 34.6171964667, 5.33831435952e-15, -34.6171964667, 3.52560374486;
Eigen::Matrix<double, 2, 4> K;
- K << 50.0, 0.0, 1.0, 0.0, 23.5668757858, 300.0, -0.88836718554, 1.1;
+ K << 104.272994613, 0.0, 1.72618753001, 0.0, 49.1477742369, 129.930293084, -0.546087759204, 0.551235956004;
return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b54949d..c8b943c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -11,15 +11,16 @@
#include "aos/controls/polytope.h"
#include "aos/common/commonmath.h"
#include "aos/common/logging/queue_logging.h"
+#include "aos/common/logging/matrix_logging.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/constants.h"
-using frc971::sensors::gyro;
+using frc971::sensors::gyro_reading;
namespace frc971 {
namespace control_loops {
@@ -65,8 +66,36 @@
SetRawPosition(left, right);
}
+ void SetExternalMotors(double left_voltage, double right_voltage) {
+ loop_->U << left_voltage, right_voltage;
+ }
+
void Update(bool stop_motors) {
- loop_->Update(stop_motors);
+ if (_control_loop_driving) {
+ loop_->Update(stop_motors);
+ } else {
+ if (stop_motors) {
+ loop_->U.setZero();
+ loop_->U_uncapped.setZero();
+ }
+ loop_->UpdateObserver();
+ }
+ ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
+ LOG_MATRIX(DEBUG, "E", E);
+ }
+
+ double GetEstimatedRobotSpeed() {
+ // lets just call the average of left and right velocities close enough
+ return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
+ }
+
+ double GetEstimatedLeftEncoder() {
+ // lets just call the average of left and right velocities close enough
+ return loop_->X_hat(0, 0);
+ }
+
+ double GetEstimatedRightEncoder() {
+ return loop_->X_hat(2, 0);
}
void SendMotors(Drivetrain::Output *output) {
@@ -75,10 +104,6 @@
output->right_voltage = loop_->U(1, 0);
}
}
- void PrintMotors() const {
- ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
- LOG(DEBUG, "E[0, 0]: %f E[1, 0] %f E[2, 0] %f E[3, 0] %f\n", E(0, 0), E(1, 0), E(2, 0), E(3, 0));
- }
private:
::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> loop_;
@@ -137,8 +162,6 @@
/*[*/ 12 /*]]*/).finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(
constants::GetValues().make_v_drivetrain_loop())),
- left_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
- right_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
ttrust_(1.1),
wheel_(0.0),
throttle_(0.0),
@@ -154,18 +177,24 @@
}
static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
- static double MotorSpeed(double shifter_position, double velocity) {
+ static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
+ double shifter_position, double velocity) {
// TODO(austin): G_high, G_low and kWheelRadius
- if (shifter_position > 0.57) {
+ const double avg_hall_effect =
+ (hall_effect.clear_high + hall_effect.clear_low) / 2.0;
+
+ if (shifter_position > avg_hall_effect) {
return velocity / constants::GetValues().high_gear_ratio / kWheelRadius;
} else {
return velocity / constants::GetValues().low_gear_ratio / kWheelRadius;
}
}
- Gear ComputeGear(double velocity, Gear current) {
- const double low_omega = MotorSpeed(0, ::std::abs(velocity));
- const double high_omega = MotorSpeed(1.0, ::std::abs(velocity));
+ Gear ComputeGear(const constants::ShifterHallEffect &hall_effect,
+ double velocity, Gear current) {
+ const double low_omega = MotorSpeed(hall_effect, 0.0, ::std::abs(velocity));
+ const double high_omega =
+ MotorSpeed(hall_effect, 1.0, ::std::abs(velocity));
double high_torque = ((12.0 - high_omega / Kv) * Kt / kR);
double low_torque = ((12.0 - low_omega / Kv) * Kt / kR);
@@ -200,6 +229,7 @@
// TODO(austin): Fix the upshift logic to include states.
Gear requested_gear;
if (false) {
+ const auto &values = constants::GetValues();
const double current_left_velocity =
(position_.left_encoder - last_position_.left_encoder) /
position_time_delta_;
@@ -207,8 +237,10 @@
(position_.right_encoder - last_position_.right_encoder) /
position_time_delta_;
- Gear left_requested = ComputeGear(current_left_velocity, left_gear_);
- Gear right_requested = ComputeGear(current_right_velocity, right_gear_);
+ Gear left_requested =
+ ComputeGear(values.left_drive, current_left_velocity, left_gear_);
+ Gear right_requested =
+ ComputeGear(values.right_drive, current_right_velocity, right_gear_);
requested_gear =
(left_requested == HIGH || right_requested == HIGH) ? HIGH : LOW;
} else {
@@ -252,6 +284,7 @@
}
}
void SetPosition(const Drivetrain::Position *position) {
+ const auto &values = constants::GetValues();
if (position == NULL) {
++stale_count_;
} else {
@@ -264,9 +297,14 @@
if (position) {
GearLogging gear_logging;
// Switch to the correct controller.
- // TODO(austin): Un-hard code 0.57
- if (position->left_shifter_position < 0.57) {
- if (position->right_shifter_position < 0.57 || right_gear_ == LOW) {
+ const double left_middle_shifter_position =
+ (values.left_drive.clear_high + values.left_drive.clear_low) / 2.0;
+ const double right_middle_shifter_position =
+ (values.right_drive.clear_high + values.right_drive.clear_low) / 2.0;
+
+ if (position->left_shifter_position < left_middle_shifter_position) {
+ if (position->right_shifter_position < right_middle_shifter_position ||
+ right_gear_ == LOW) {
gear_logging.left_loop_high = false;
gear_logging.right_loop_high = false;
loop_->set_controller_index(gear_logging.controller_index = 0);
@@ -276,7 +314,8 @@
loop_->set_controller_index(gear_logging.controller_index = 1);
}
} else {
- if (position->right_shifter_position < 0.57 || left_gear_ == LOW) {
+ if (position->right_shifter_position < right_middle_shifter_position ||
+ left_gear_ == LOW) {
gear_logging.left_loop_high = true;
gear_logging.right_loop_high = false;
loop_->set_controller_index(gear_logging.controller_index = 2);
@@ -288,16 +327,16 @@
}
// TODO(austin): Constants.
- if (position->left_shifter_position > 0.9 && left_gear_ == SHIFTING_UP) {
+ if (position->left_shifter_position > values.left_drive.clear_high && left_gear_ == SHIFTING_UP) {
left_gear_ = HIGH;
}
- if (position->left_shifter_position < 0.1 && left_gear_ == SHIFTING_DOWN) {
+ if (position->left_shifter_position < values.left_drive.clear_low && left_gear_ == SHIFTING_DOWN) {
left_gear_ = LOW;
}
- if (position->right_shifter_position > 0.9 && right_gear_ == SHIFTING_UP) {
+ if (position->right_shifter_position > values.right_drive.clear_high && right_gear_ == SHIFTING_UP) {
right_gear_ = HIGH;
}
- if (position->right_shifter_position < 0.1 && right_gear_ == SHIFTING_DOWN) {
+ if (position->right_shifter_position < values.right_drive.clear_low && right_gear_ == SHIFTING_DOWN) {
right_gear_ = LOW;
}
@@ -355,6 +394,7 @@
}
void Update() {
+ const auto &values = constants::GetValues();
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
++counter_;
@@ -365,9 +405,11 @@
(position_.right_encoder - last_position_.right_encoder) /
position_time_delta_;
const double left_motor_speed =
- MotorSpeed(position_.left_shifter_position, current_left_velocity);
+ MotorSpeed(values.left_drive, position_.left_shifter_position,
+ current_left_velocity);
const double right_motor_speed =
- MotorSpeed(position_.right_shifter_position, current_right_velocity);
+ MotorSpeed(values.right_drive, position_.right_shifter_position,
+ current_right_velocity);
{
CIMLogging logging;
@@ -376,7 +418,6 @@
// shift.
if (IsInGear(left_gear_)) {
logging.left_in_gear = true;
- left_cim_->X_hat(0, 0) = left_motor_speed;
} else {
logging.left_in_gear = false;
}
@@ -384,7 +425,6 @@
logging.left_velocity = current_left_velocity;
if (IsInGear(right_gear_)) {
logging.right_in_gear = true;
- right_cim_->X_hat(0, 0) = right_motor_speed;
} else {
logging.right_in_gear = false;
}
@@ -451,23 +491,19 @@
} else {
// Any motor is not in gear. Speed match.
::Eigen::Matrix<double, 1, 1> R_left;
- R_left(0, 0) = left_motor_speed;
- const double wiggle =
- (static_cast<double>((counter_ % 4) / 2) - 0.5) * 3.5;
-
- loop_->U(0, 0) =
- ::aos::Clip((R_left / Kv)(0, 0) + wiggle, -position_.battery_voltage,
- position_.battery_voltage);
- right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat +
- right_cim_->B() * loop_->U(0, 0);
-
::Eigen::Matrix<double, 1, 1> R_right;
+ R_left(0, 0) = left_motor_speed;
R_right(0, 0) = right_motor_speed;
- loop_->U(1, 0) =
- ::aos::Clip((R_right / Kv)(0, 0) + wiggle, -position_.battery_voltage,
- position_.battery_voltage);
- right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat +
- right_cim_->B() * loop_->U(1, 0);
+
+ const double wiggle =
+ (static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
+
+ loop_->U(0, 0) = ::aos::Clip(
+ (R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+ loop_->U(1, 0) = ::aos::Clip(
+ (R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
loop_->U *= 12.0 / position_.battery_voltage;
}
}
@@ -485,8 +521,6 @@
const ::aos::controls::HPolytope<2> U_Poly_;
::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
- ::std::unique_ptr<StateFeedbackLoop<1, 1, 1>> left_cim_;
- ::std::unique_ptr<StateFeedbackLoop<1, 1, 1>> right_cim_;
const double ttrust_;
double wheel_;
@@ -516,7 +550,7 @@
void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
const Drivetrain::Position *position,
Drivetrain::Output *output,
- Drivetrain::Status * /*status*/) {
+ Drivetrain::Status * status) {
// TODO(aschuh): These should be members of the class.
static DrivetrainMotorsSS dt_closedloop;
static PolyDrivetrain dt_openloop;
@@ -526,6 +560,7 @@
LOG_INTERVAL(no_position_);
bad_pos = true;
}
+ no_position_.Print();
double wheel = goal->steering;
double throttle = goal->throttle;
@@ -541,22 +576,43 @@
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
- if (gyro.FetchLatest()) {
- LOG_STRUCT(DEBUG, "using", *gyro);
- dt_closedloop.SetPosition(left_encoder, right_encoder, gyro->angle,
- control_loop_driving);
+ if (gyro_reading.FetchLatest()) {
+ LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+ dt_closedloop.SetPosition(left_encoder, right_encoder,
+ gyro_reading->angle, control_loop_driving);
} else {
dt_closedloop.SetRawPosition(left_encoder, right_encoder);
}
}
dt_openloop.SetPosition(position);
- dt_closedloop.Update(output == NULL);
dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
+
if (control_loop_driving) {
+ dt_closedloop.Update(output == NULL);
dt_closedloop.SendMotors(output);
} else {
dt_openloop.SendMotors(output);
+ if (output) {
+ dt_closedloop.SetExternalMotors(output->left_voltage,
+ output->right_voltage);
+ }
+ dt_closedloop.Update(output == NULL);
+ }
+
+ // set the output status of the controll loop state
+ if (status) {
+ bool done = false;
+ if (goal) {
+ done = ((::std::abs(goal->left_goal -
+ dt_closedloop.GetEstimatedLeftEncoder()) <
+ constants::GetValues().drivetrain_done_distance) &&
+ (::std::abs(goal->right_goal -
+ dt_closedloop.GetEstimatedRightEncoder()) <
+ constants::GetValues().drivetrain_done_distance));
+ }
+ status->is_done = done;
+ status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 5941f63..bf73d9f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -22,9 +22,7 @@
'type': 'static_library',
'sources': [
'polydrivetrain_dog_motor_plant.cc',
- 'polydrivetrain_clutch_motor_plant.cc',
'drivetrain_dog_motor_plant.cc',
- 'drivetrain_clutch_motor_plant.cc',
],
'dependencies': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
@@ -50,6 +48,7 @@
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/util/util.gyp:log_interval',
'<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/logging/logging.gyp:matrix_logging',
],
'export_dependent_settings': [
'<(DEPTH)/aos/build/externals.gyp:libcdd',
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 443282c..50d9dbf 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -45,12 +45,13 @@
message Output {
float left_voltage;
float right_voltage;
- bool left_high;
- bool right_high;
+ bool left_high;
+ bool right_high;
};
message Status {
bool is_done;
+ double robot_speed;
};
queue Goal goal;
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
deleted file mode 100644
index b3aa088..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00876671940282, 0.0, 0.000204905465153, 0.0, 0.764245148008, 0.0, 0.0373841350548, 0.0, 0.000204905465153, 1.0, 0.00876671940282, 0.0, 0.0373841350548, 0.0, 0.764245148008;
- Eigen::Matrix<double, 4, 2> B;
- B << 0.000157874070659, -2.62302512161e-05, 0.0301793267864, -0.00478559834045, -2.62302512161e-05, 0.000157874070659, -0.00478559834045, 0.0301793267864;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<4, 2, 2> MakeClutchDrivetrainController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.60424514801, 0.0373841350548, 53.4463554671, 4.58647914599, 0.0373841350548, 1.60424514801, 4.58647914599, 53.4463554671;
- Eigen::Matrix<double, 2, 4> K;
- K << 292.330461448, 10.4890095334, -85.5980253252, -0.517234397951, -58.0206391358, -1.5636023242, 153.384904309, 5.5616531565;
- return StateFeedbackController<4, 2, 2>(L, K, MakeClutchDrivetrainPlantCoefficients());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeClutchDrivetrainPlantCoefficients());
- return StateFeedbackPlant<4, 2, 2>(plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop() {
- ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeClutchDrivetrainController());
- return StateFeedbackLoop<4, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
deleted file mode 100644
index e9444e6..0000000
--- a/frc971/control_loops/drivetrain/drivetrain_clutch_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeClutchDrivetrainPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeClutchDrivetrainController();
-
-StateFeedbackPlant<4, 2, 2> MakeClutchDrivetrainPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeClutchDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index 7822056..231eefb 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -7,11 +7,11 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients() {
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00923787174605, 0.0, 0.000131162317098, 0.0, 0.851672729447, 0.0, 0.0248457251406, 0.0, 0.000131162317098, 1.0, 0.00923787174605, 0.0, 0.0248457251406, 0.0, 0.851672729447;
+ A << 1.0, 0.00860955515291, 0.0, 0.000228184998733, 0.0, 0.735841675858, 0.0, 0.0410810558113, 0.0, 0.000228184998733, 1.0, 0.00860955515291, 0.0, 0.0410810558113, 0.0, 0.735841675858;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000126364935405, -2.17474127771e-05, 0.0245934537462, -0.00411955394149, -2.17474127771e-05, 0.000126364935405, -0.00411955394149, 0.0245934537462;
+ B << 0.000272244648044, -4.46778919705e-05, 0.0517213538779, -0.00804353916233, -4.46778919705e-05, 0.000272244648044, -0.00804353916233, 0.0517213538779;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -23,23 +23,23 @@
return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<4, 2, 2> MakeDogDrivetrainController() {
+StateFeedbackController<4, 2, 2> MakeDrivetrainController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.69167272945, 0.0248457251406, 64.4706646869, 3.2355304474, 0.0248457251406, 1.69167272945, 3.2355304474, 64.4706646869;
+ L << 1.57584167586, 0.0410810558113, 50.0130674801, 4.93325200717, 0.0410810558113, 1.57584167586, 4.93325200717, 50.0130674801;
Eigen::Matrix<double, 2, 4> K;
- K << 248.918529922, 14.4460993245, 41.6953764051, 3.43594323497, 41.6953764051, 3.43594323497, 248.918529922, 14.4460993245;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDogDrivetrainPlantCoefficients());
+ K << 128.210620632, 6.93828382074, 5.11036686771, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
+ return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
}
-StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant() {
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDogDrivetrainPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainPlantCoefficients());
return StateFeedbackPlant<4, 2, 2>(plants);
}
-StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop() {
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDogDrivetrainController());
+ controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainController());
return StateFeedbackLoop<4, 2, 2>(controllers);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
index ba3d584..3829e9a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -6,13 +6,13 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients();
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients();
-StateFeedbackController<4, 2, 2> MakeDogDrivetrainController();
+StateFeedbackController<4, 2, 2> MakeDrivetrainController();
-StateFeedbackPlant<4, 2, 2> MakeDogDrivetrainPlant();
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
-StateFeedbackLoop<4, 2, 2> MakeDogDrivetrainLoop();
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 73f8525..0827fec 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -13,7 +13,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/other_sensors.q.h"
using ::aos::time::Time;
@@ -50,7 +50,7 @@
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation()
: drivetrain_plant_(
- new StateFeedbackPlant<4, 2, 2>(MakeDogDrivetrainPlant())),
+ new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
my_drivetrain_loop_(".frc971.control_loops.drivetrain",
0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
".frc971.control_loops.drivetrain.position",
@@ -134,7 +134,7 @@
.reader_pid(254)
.cape_resets(5)
.Send();
- ::frc971::sensors::gyro.Clear();
+ ::frc971::sensors::gyro_reading.Clear();
SendDSPacket(true);
}
@@ -158,7 +158,7 @@
virtual ~DrivetrainTest() {
::aos::robot_state.Clear();
- ::frc971::sensors::gyro.Clear();
+ ::frc971::sensors::gyro_reading.Clear();
::bbb::sensor_generation.Clear();
}
};
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
deleted file mode 100644
index 82962f0..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.cc
+++ /dev/null
@@ -1,125 +0,0 @@
-#include "frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.764245148008, 0.0373841350548, 0.0373841350548, 0.764245148008;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0301793267864, -0.00478559834045, -0.00478559834045, 0.0301793267864;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.763446428918, 0.00494258902788, 0.042202491067, 0.968991856576;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0302815719967, -0.00184882243178, -0.00540240320973, 0.011598890947;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.968991856576, 0.042202491067, 0.00494258902788, 0.763446428918;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.011598890947, -0.00540240320973, -0.00184882243178, 0.0302815719967;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.968881997557, 0.00555499847336, 0.00555499847336, 0.968881997557;
- Eigen::Matrix<double, 2, 2> B;
- B << 0.0116399847578, -0.0020779000091, -0.0020779000091, 0.0116399847578;
- Eigen::Matrix<double, 2, 2> C;
- C << 1.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0.0, 0.0, 0.0, 0.0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.744245148008, 0.0373841350548, 0.0373841350548, 0.744245148008;
- Eigen::Matrix<double, 2, 2> K;
- K << 5.78417881324, 2.15594244513, 2.15594244513, 5.78417881324;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.742469928763, 0.0421768815418, 0.0421768815418, 0.949968356732;
- Eigen::Matrix<double, 2, 2> K;
- K << 5.78418649682, 2.16715237139, 6.33258809821, 32.8220766317;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.954934950673, 0.00591596315544, 0.00591596315544, 0.737503334821;
- Eigen::Matrix<double, 2, 2> K;
- K << 32.8220766317, 6.33258809821, 2.16715237139, 5.78418649682;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
-}
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController() {
- Eigen::Matrix<double, 2, 2> L;
- L << 0.948881997557, 0.00555499847336, 0.00555499847336, 0.948881997557;
- Eigen::Matrix<double, 2, 2> K;
- K << 32.8220767657, 6.33643373411, 6.33643373411, 32.8220767657;
- return StateFeedbackController<2, 2, 2>(L, K, MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<2, 2, 2>(plants);
-}
-
-StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop() {
- ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeClutchVelocityDrivetrainHighHighController());
- return StateFeedbackLoop<2, 2, 2>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
deleted file mode 100644
index 85c87c1..0000000
--- a/frc971/control_loops/drivetrain/polydrivetrain_clutch_motor_plant.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainLowHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainLowHighController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighLowPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighLowController();
-
-StateFeedbackPlantCoefficients<2, 2, 2> MakeClutchVelocityDrivetrainHighHighPlantCoefficients();
-
-StateFeedbackController<2, 2, 2> MakeClutchVelocityDrivetrainHighHighController();
-
-StateFeedbackPlant<2, 2, 2> MakeVClutchDrivetrainPlant();
-
-StateFeedbackLoop<2, 2, 2> MakeVClutchDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CLUTCH_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index b3d4277..b31cf85 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -7,11 +7,11 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.851672729447, 0.0248457251406, 0.0248457251406, 0.851672729447;
+ A << 0.735841675858, 0.0410810558113, 0.0410810558113, 0.735841675858;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0245934537462, -0.00411955394149, -0.00411955394149, 0.0245934537462;
+ B << 0.0517213538779, -0.00804353916233, -0.00804353916233, 0.0517213538779;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -23,11 +23,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.851389310398, 0.00553670185935, 0.0264939835067, 0.967000817219;
+ A << 0.735048848179, 0.0131811893199, 0.045929121897, 0.915703853642;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0246404461385, -0.00200815724925, -0.00439284398274, 0.0119687766843;
+ B << 0.0518765869984, -0.00481755802263, -0.00899277497558, 0.0308091755839;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -39,11 +39,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.967000817219, 0.0264939835067, 0.00553670185935, 0.851389310398;
+ A << 0.915703853642, 0.045929121897, 0.0131811893199, 0.735048848179;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0119687766843, -0.00439284398274, -0.00200815724925, 0.0246404461385;
+ B << 0.0308091755839, -0.00899277497558, -0.00481755802263, 0.0518765869984;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -55,11 +55,11 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.966936300149, 0.00589655754287, 0.00589655754287, 0.966936300149;
+ A << 0.915439806567, 0.0146814193986, 0.0146814193986, 0.915439806567;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0119921769728, -0.00213867661221, -0.00213867661221, 0.0119921769728;
+ B << 0.0309056814511, -0.00536587314624, -0.00536587314624, 0.0309056814511;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -71,53 +71,53 @@
return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.831672729447, 0.0248457251406, 0.0248457251406, 0.831672729447;
+ L << 0.715841675858, 0.0410810558113, 0.0410810558113, 0.715841675858;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7028500331, 2.80305051463, 2.80305051463, 10.7028500331;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowLowPlantCoefficients());
+ K << 2.81809403994, 1.23253744933, 1.23253744933, 2.81809403994;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.831852326508, 0.0264837489415, 0.0264837489415, 0.946537801108;
+ L << 0.715885457343, 0.0459077351335, 0.0459077351335, 0.894867244478;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7028511964, 2.80768406175, 6.14180888507, 31.6936764099;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowHighPlantCoefficients());
+ K << 2.81810038978, 1.23928174475, 2.31332592354, 10.6088017388;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.951097545753, 0.0063707209266, 0.0063707209266, 0.827292581863;
+ L << 0.902328849033, 0.014581304798, 0.014581304798, 0.708423852788;
Eigen::Matrix<double, 2, 2> K;
- K << 31.6936764099, 6.14180888507, 2.80768406175, 10.7028511964;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighLowPlantCoefficients());
+ K << 10.6088017388, 2.31332592354, 1.23928174475, 2.81810038978;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController() {
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.946936300149, 0.00589655754287, 0.00589655754287, 0.946936300149;
+ L << 0.895439806567, 0.0146814193986, 0.0146814193986, 0.895439806567;
Eigen::Matrix<double, 2, 2> K;
- K << 31.6936764663, 6.14392885659, 6.14392885659, 31.6936764663;
- return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+ K << 10.6088022944, 2.31694961514, 2.31694961514, 10.6088022944;
+ return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
-StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant() {
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeDogVelocityDrivetrainHighHighPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
return StateFeedbackPlant<2, 2, 2>(plants);
}
-StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop() {
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeDogVelocityDrivetrainHighHighController());
+ controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
+ controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
+ controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
+ controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
return StateFeedbackLoop<2, 2, 2>(controllers);
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
index 613bff4..27aa4dd 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
@@ -6,25 +6,25 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowLowPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowLowController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainLowHighPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighLowPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController();
-StateFeedbackPlantCoefficients<2, 2, 2> MakeDogVelocityDrivetrainHighHighPlantCoefficients();
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients();
-StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController();
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController();
-StateFeedbackPlant<2, 2, 2> MakeVDogDrivetrainPlant();
+StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant();
-StateFeedbackLoop<2, 2, 2> MakeVDogDrivetrainLoop();
+StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop();
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index 7e6617c..b63a34b 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -20,47 +20,42 @@
bool negedge_count_changed() const { return negedges_.count_changed(); }
bool value() const { return value_; }
+ bool last_value() const { return last_value_; }
void Update(const HallEffectStruct &position) {
+ last_value_ = value_;
value_ = position.current;
posedges_.update(position.posedge_count);
negedges_.update(position.negedge_count);
}
- void Reset() {
- posedges_.Reset();
- negedges_.Reset();
+ void Reset(const HallEffectStruct &position) {
+ posedges_.Reset(position.posedge_count);
+ negedges_.Reset(position.negedge_count);
+ value_ = position.current;
+ last_value_ = position.current;
}
- bool ready() { return posedges_.ready() && negedges_.ready(); }
-
private:
class {
public:
void update(int32_t count) {
- if (first_) {
- count_ = count;
- LOG(DEBUG, "First time through the hall effect, resetting\n");
- }
previous_count_ = count_;
count_ = count;
- first_ = false;
}
- void Reset() { first_ = true; }
+ void Reset(int32_t count) { count_ = count; }
- bool count_changed() const { return !first_ && previous_count_ != count_; }
+ bool count_changed() const { return previous_count_ != count_; }
int32_t count() const { return count_; }
- bool ready() { return !first_; }
-
private:
int32_t count_ = 0;
int32_t previous_count_ = 0;
- bool first_ = true;
} posedges_, negedges_;
bool value_ = false;
+ bool last_value_ = false;
};
} // namespace frc971
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index aacf31e..ca69a2b 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -23,6 +23,7 @@
# measured from CAD
self.J_top = 0.3
self.J_bottom = 0.9
+
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
@@ -144,8 +145,8 @@
print "eigenvalues"
print numpy.linalg.eig(F)[0]
- self.rpl = .05
- self.ipl = 0.008
+ self.rpl = .02
+ self.ipl = 0.004
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl,
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 90faf9f..a103c79 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -1,8 +1,21 @@
import controls
import numpy
+class Constant(object):
+ def __init__ (self, name, formatt, value):
+ self.name = name
+ self.formatt = formatt
+ self.value = value
+ self.formatToType = {}
+ self.formatToType['%f'] = "double";
+ self.formatToType['%d'] = "int";
+ def __str__ (self):
+ return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+ (self.formatToType[self.formatt], self.name, self.value)
+
+
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None):
+ def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
"""Constructs a control loop writer.
Args:
@@ -24,6 +37,16 @@
self._namespace_end = '\n'.join(
['} // namespace %s' % name for name in reversed(self._namespaces)])
+
+ self._constant_list = []
+
+ def AddConstant(self, constant):
+ """Adds a constant to write.
+
+ Args:
+ constant: Constant, the constant to add to the header.
+ """
+ self._constant_list.append(constant)
def _TopDirectory(self):
return self._namespaces[0]
@@ -74,6 +97,10 @@
fd.write('\n')
fd.write(self._namespace_start)
+
+ for const in self._constant_list:
+ fd.write(str(const))
+
fd.write('\n\n')
for loop in self._loops:
fd.write(loop.DumpPlantHeader())
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 001fd1e..3d6e441 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
import control_loop
+import controls
import numpy
import sys
from matplotlib import pylab
@@ -89,7 +90,7 @@
self.Gr = self.G_high
# Control loop time step
self.dt = 0.01
-
+
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
self.msp = 1.0 / self.m + self.rb * self.rb / self.J
@@ -118,13 +119,29 @@
self.D = numpy.matrix([[0, 0],
[0, 0]])
+ #print "THE NUMBER I WANT" + str(numpy.linalg.inv(self.A_continuous) * -self.B_continuous * numpy.matrix([[12.0], [12.0]]))
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
# Poles from last year.
self.hp = 0.65
self.lp = 0.83
- self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+ self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
+ print self.K
+ q_pos = 0.07
+ q_vel = 1.0
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ print self.A
+ print self.B
+ print self.K
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.hlp = 0.07
self.llp = 0.09
@@ -200,6 +217,7 @@
#pylab.show()
# Write the generated constants out to a file.
+ print "Output one"
drivetrain = Drivetrain()
if len(argv) != 5:
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 280db16..f2dfdbe 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -396,10 +396,10 @@
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
- "VDogDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high])
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 89f682a..69f2599 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -30,6 +30,9 @@
self.Kt = self.stall_torque / self.stall_current
# Spring constant for the springs, N/m
self.Ks = 2800.0
+ # Maximum extension distance (Distance from the 0 force point on the
+ # spring to the latch position.)
+ self.max_extension = 0.32385
# Gear ratio multiplied by radius of final sprocket.
self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
@@ -235,7 +238,13 @@
sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter, shooter])
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter,
+ shooter])
+
+ loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
+ sprung_shooter.max_extension))
+ loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
+ sprung_shooter.Ks))
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 4789e1d..4a3e6e4 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -5,8 +5,8 @@
#include <algorithm>
#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
@@ -27,17 +27,17 @@
// against last cycle's voltage.
if (X_hat(2, 0) > last_voltage_ + 4.0) {
voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
- LOG(INFO, "Capping due to runawway\n");
+ LOG(DEBUG, "Capping due to runaway\n");
} else if (X_hat(2, 0) < last_voltage_ - 4.0) {
voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
- LOG(INFO, "Capping due to runawway\n");
+ LOG(DEBUG, "Capping due to runaway\n");
}
voltage_ = std::min(max_voltage_, voltage_);
voltage_ = std::max(-max_voltage_, voltage_);
U(0, 0) = voltage_ - old_voltage;
- LOG(INFO, "X_hat is %f, applied is %f\n", X_hat(2, 0), voltage_);
+ LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
last_voltage_ = voltage_;
capped_goal_ = false;
@@ -56,7 +56,7 @@
R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
if (controller_index() == 0) {
@@ -69,7 +69,7 @@
R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
} else {
capped_goal_ = false;
}
@@ -85,13 +85,10 @@
void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
double known_position) {
- LOG(INFO, "Setting calibration such that %f -> %f\n", encoder_val,
- known_position);
- LOG(INFO, "Position was %f\n", absolute_position());
+ double old_position = absolute_position();
double previous_offset = offset_;
offset_ = known_position - encoder_val;
double doffset = offset_ - previous_offset;
- LOG(INFO, "Changing offset from %f to %f\n", previous_offset, offset_);
X_hat(0, 0) += doffset;
// Offset our measurements because the offset is baked into them.
Y_(0, 0) += doffset;
@@ -100,7 +97,10 @@
if (controller_index() == 0) {
R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
}
- LOG(INFO, "Validation: position is %f\n", absolute_position());
+ LOG_STRUCT(
+ DEBUG, "sensor edge (fake?)",
+ ShooterChangeCalibration(encoder_val, known_position, old_position,
+ absolute_position(), previous_offset, offset_));
}
ShooterMotor::ShooterMotor(control_loops::ShooterGroup *my_shooter)
@@ -111,18 +111,48 @@
load_timeout_(0, 0),
shooter_brake_set_time_(0, 0),
unload_timeout_(0, 0),
- prepare_fire_end_time_(0, 0),
shot_end_time_(0, 0),
- cycles_not_moved_(0) {}
+ cycles_not_moved_(0),
+ shot_count_(0),
+ zeroed_(false),
+ distal_posedge_validation_cycles_left_(0),
+ proximal_posedge_validation_cycles_left_(0),
+ last_distal_current_(true),
+ last_proximal_current_(true) {}
double ShooterMotor::PowerToPosition(double power) {
- // LOG(WARNING, "power to position not correctly implemented\n");
const frc971::constants::Values &values = constants::GetValues();
- double new_pos = ::std::min(::std::max(power, values.shooter.lower_limit),
+ double maxpower = 0.5 * kSpringConstant *
+ (kMaxExtension * kMaxExtension -
+ (kMaxExtension - values.shooter.upper_limit) *
+ (kMaxExtension - values.shooter.upper_limit));
+ if (power < 0) {
+ LOG_STRUCT(WARNING, "negative power", PowerAdjustment(power, 0));
+ power = 0;
+ } else if (power > maxpower) {
+ LOG_STRUCT(WARNING, "power too high", PowerAdjustment(power, maxpower));
+ power = maxpower;
+ }
+
+ double mp = kMaxExtension * kMaxExtension - (power + power) / kSpringConstant;
+ double new_pos = 0.10;
+ if (mp < 0) {
+ LOG(ERROR,
+ "Power calculation has negative number before square root (%f).\n", mp);
+ } else {
+ new_pos = kMaxExtension - ::std::sqrt(mp);
+ }
+
+ new_pos = ::std::min(::std::max(new_pos, values.shooter.lower_limit),
values.shooter.upper_limit);
return new_pos;
}
+double ShooterMotor::PositionToPower(double position) {
+ double power = kSpringConstant * position * (kMaxExtension - position / 2.0);
+ return power;
+}
+
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
const control_loops::ShooterGroup::Goal *goal,
@@ -131,15 +161,23 @@
control_loops::ShooterGroup::Status *status) {
constexpr double dt = 0.01;
+ if (::std::isnan(goal->shot_power)) {
+ state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because got a shot power of NAN.\n");
+ }
+
// we must always have these or we have issues.
if (goal == NULL || status == NULL) {
if (output) output->voltage = 0;
LOG(ERROR, "Thought I would just check for null and die.\n");
return;
}
+ status->ready = false;
if (reset()) {
state_ = STATE_INITIALIZE;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
}
if (position) {
shooter_.CorrectPosition(position->position);
@@ -154,11 +192,8 @@
// Don't even let the control loops run.
bool shooter_loop_disable = false;
- // Adds voltage to take up slack in gears before shot.
- bool apply_some_voltage = false;
-
-
- const bool disabled = !::aos::robot_state->enabled;
+ const bool disabled =
+ !::aos::robot_state.get() || !::aos::robot_state->enabled;
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -174,7 +209,7 @@
shooter_.set_controller_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(1);
+ shooter_.set_controller_index(0);
}
if (shooter_.controller_index() != last_controller_index) {
shooter_.RecalculatePowerGoal();
@@ -200,13 +235,18 @@
values.shooter.upper_limit);
}
- state_ = STATE_REQUEST_LOAD;
-
// Go to the current position.
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
// If the plunger is all the way back, we want to be latched.
latch_piston_ = position->plunger;
brake_piston_ = false;
+ if (position->latch == latch_piston_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ shooter_loop_disable = true;
+ LOG(DEBUG,
+ "Not moving on until the latch has moved to avoid a crash\n");
+ }
} else {
// If we can't start yet because we don't know where we are, set the
// latch and brake to their defaults.
@@ -216,7 +256,9 @@
break;
case STATE_REQUEST_LOAD:
if (position) {
- if (position->pusher_distal.current) {
+ zeroed_ = false;
+ if (position->pusher_distal.current ||
+ position->pusher_proximal.current) {
// We started on the sensor, back up until we are found.
// If the plunger is all the way back and not latched, it won't be
// there for long.
@@ -259,12 +301,13 @@
shooter_.set_max_voltage(4.0);
if (position) {
- if (!position->pusher_distal.current) {
+ if (!position->pusher_distal.current &&
+ !position->pusher_proximal.current) {
Load();
}
+ latch_piston_ = position->plunger;
}
- latch_piston_ = false;
brake_piston_ = false;
break;
case STATE_LOAD:
@@ -281,18 +324,48 @@
shooter_.SetGoalPosition(0.0, 0.0);
if (position) {
+ // TODO(austin): Validate that this is the right edge.
// If we see a posedge on any of the hall effects,
if (position->pusher_proximal.posedge_count !=
- last_proximal_posedge_count_) {
- LOG(DEBUG, "Setting calibration using proximal sensor\n");
- shooter_.SetCalibration(position->pusher_proximal.posedge_value,
- values.shooter.pusher_proximal.upper_angle);
+ last_proximal_posedge_count_ &&
+ !last_proximal_current_) {
+ proximal_posedge_validation_cycles_left_ = 2;
}
+ if (proximal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_proximal.current) {
+ --proximal_posedge_validation_cycles_left_;
+ if (proximal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_proximal.posedge_value,
+ values.shooter.pusher_proximal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using proximal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ proximal_posedge_validation_cycles_left_ = 0;
+ }
+ }
+
if (position->pusher_distal.posedge_count !=
- last_distal_posedge_count_) {
- LOG(DEBUG, "Setting calibration using distal sensor\n");
- shooter_.SetCalibration(position->pusher_distal.posedge_value,
- values.shooter.pusher_distal.upper_angle);
+ last_distal_posedge_count_ &&
+ !last_distal_current_) {
+ distal_posedge_validation_cycles_left_ = 2;
+ }
+ if (distal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_distal.current) {
+ --distal_posedge_validation_cycles_left_;
+ if (distal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_distal.posedge_value,
+ values.shooter.pusher_distal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using distal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ distal_posedge_validation_cycles_left_ = 0;
+ }
}
// Latch if the plunger is far enough back to trigger the hall effect.
@@ -303,7 +376,11 @@
// way back as well.
if (position->plunger && position->latch &&
position->pusher_distal.current) {
- state_ = STATE_PREPARE_SHOT;
+ if (!zeroed_) {
+ state_ = STATE_REQUEST_LOAD;
+ } else {
+ state_ = STATE_PREPARE_SHOT;
+ }
} else if (position->plunger &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
@@ -317,6 +394,7 @@
if (!position->pusher_distal.current ||
!position->pusher_proximal.current) {
state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to load.\n");
}
}
} else if (goal->unload_requested) {
@@ -326,7 +404,8 @@
break;
case STATE_LOADING_PROBLEM:
if (disabled) {
- Load();
+ state_ = STATE_REQUEST_LOAD;
+ break;
}
// We got to the goal, but the latch hasn't registered as down. It might
// be stuck, or on it's way but not there yet.
@@ -378,15 +457,8 @@
LOG(DEBUG, "In ready\n");
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
- if (::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) > 0.002) {
- // TODO(austin): Add a state to release the brake.
-
- // TODO(austin): Do we want to set the brake here or after shooting?
- // Depends on air usage.
- LOG(DEBUG, "Preparing shot again.\n");
- state_ = STATE_PREPARE_SHOT;
- } else if (Time::Now() > shooter_brake_set_time_) {
+ if (Time::Now() > shooter_brake_set_time_) {
+ status->ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
shooter_loop_disable = true;
@@ -394,13 +466,23 @@
if (goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
- apply_some_voltage = true;
- state_ = STATE_PREPARE_FIRE;
+ shot_end_time_ = Time::Now() + kShotEndTimeout;
+ firing_starting_position_ = shooter_.absolute_position();
+ state_ = STATE_FIRE;
}
- } else {
- LOG(DEBUG, "Nothing %d %d\n", goal->shot_requested, !disabled);
}
+ if (state_ == STATE_READY &&
+ ::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) > 0.002) {
+ // TODO(austin): Add a state to release the brake.
+
+ // TODO(austin): Do we want to set the brake here or after shooting?
+ // Depends on air usage.
+ status->ready = false;
+ LOG(DEBUG, "Preparing shot again.\n");
+ state_ = STATE_PREPARE_SHOT;
+ }
+
shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
latch_piston_ = true;
@@ -411,29 +493,6 @@
}
break;
- case STATE_PREPARE_FIRE:
- // Apply a bit of voltage to bias the gears for a little bit of time, and
- // then fire.
- shooter_loop_disable = true;
- if (disabled) {
- // If we are disabled, reset the backlash bias timer.
- prepare_fire_end_time_ = Time::Now() + kPrepareFireEndTime;
- break;
- }
- if (Time::Now() > prepare_fire_end_time_) {
- cycles_not_moved_ = 0;
- firing_starting_position_ = shooter_.absolute_position();
- shot_end_time_ = Time::Now() + kShotEndTimeout;
- state_ = STATE_FIRE;
- latch_piston_ = false;
- } else {
- apply_some_voltage = true;
- latch_piston_ = true;
- }
-
- brake_piston_ = true;
- break;
-
case STATE_FIRE:
if (disabled) {
if (position) {
@@ -461,6 +520,7 @@
cycles_not_moved_ > 3) ||
Time::Now() > shot_end_time_) {
state_ = STATE_REQUEST_LOAD;
+ ++shot_count_;
}
latch_piston_ = false;
brake_piston_ = true;
@@ -500,6 +560,7 @@
// We have been stuck trying to unload for way too long, give up and
// turn everything off.
state_ = STATE_ESTOP;
+ LOG(ERROR, "Estopping because took too long to unload.\n");
}
brake_piston_ = false;
@@ -510,7 +571,7 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
- shooter_.set_max_voltage(5.0);
+ shooter_.set_max_voltage(6.0);
// Slowly move back until we hit the upper limit.
// If we were at the limit last cycle, we are done unloading.
@@ -526,8 +587,8 @@
shooter_.SetGoalPosition(
::std::min(
values.shooter.upper_limit,
- shooter_.goal_position() + values.shooter.zeroing_speed * dt),
- values.shooter.zeroing_speed);
+ shooter_.goal_position() + values.shooter.unload_speed * dt),
+ values.shooter.unload_speed);
}
latch_piston_ = false;
@@ -537,7 +598,7 @@
if (goal->load_requested) {
state_ = STATE_REQUEST_LOAD;
}
- // If we are ready to load again,
+ // If we are ready to load again,
shooter_loop_disable = true;
latch_piston_ = false;
@@ -545,6 +606,7 @@
break;
case STATE_ESTOP:
+ LOG(WARNING, "estopped\n");
// Totally lost, go to a safe state.
shooter_loop_disable = true;
latch_piston_ = true;
@@ -552,13 +614,10 @@
break;
}
- if (apply_some_voltage) {
- shooter_.Update(true);
- shooter_.ZeroPower();
- if (output) output->voltage = 2.0;
- } else if (!shooter_loop_disable) {
- LOG(DEBUG, "Running the loop, goal is %f, position is %f\n",
- shooter_.goal_position(), shooter_.absolute_position());
+ if (!shooter_loop_disable) {
+ LOG_STRUCT(DEBUG, "running the loop",
+ ShooterStatusToLog(shooter_.goal_position(),
+ shooter_.absolute_position()));
if (!cap_goal) {
shooter_.set_max_voltage(12.0);
}
@@ -573,25 +632,38 @@
if (output) output->voltage = 0.0;
}
+ status->hard_stop_power = PositionToPower(shooter_.absolute_position());
+
if (output) {
output->latch_piston = latch_piston_;
output->brake_piston = brake_piston_;
}
- status->done = ::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) < 0.004;
+ if (position) {
+ LOG_STRUCT(DEBUG, "internal state",
+ ShooterStateToLog(
+ shooter_.absolute_position(), shooter_.absolute_velocity(),
+ state_, position->latch, position->pusher_proximal.current,
+ position->pusher_distal.current, position->plunger,
+ brake_piston_, latch_piston_));
- if (position) {
last_position_ = *position;
- LOG(DEBUG,
- "pos > absolute: %f velocity: %f state= %d\n",
- shooter_.absolute_position(), shooter_.absolute_velocity(),
- state_);
- }
- if (position) {
+
last_distal_posedge_count_ = position->pusher_distal.posedge_count;
last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
+ last_distal_current_ = position->pusher_distal.current;
+ last_proximal_current_ = position->pusher_proximal.current;
}
+
+ status->shots = shot_count_;
+}
+
+void ShooterMotor::ZeroOutputs() {
+ queue_group()->output.MakeWithBuilder()
+ .voltage(0)
+ .latch_piston(latch_piston_)
+ .brake_piston(brake_piston_)
+ .Send();
}
} // namespace control_loops
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index efcc0e7..10e0f4e 100755
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -30,6 +30,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'shooter_loop',
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 195894b..1ab224b 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -66,7 +66,6 @@
void CorrectPosition(double position) {
Eigen::Matrix<double, 1, 1> Y;
Y << position + offset_ - kPositionOffset;
- LOG(DEBUG, "Setting position to %f\n", position);
Correct(Y);
}
@@ -97,7 +96,7 @@
private:
// The offset between what is '0' (0 rate on the spring) and the 0 (all the
// way cocked).
- constexpr static double kPositionOffset = 0.305054 + 0.0254;
+ constexpr static double kPositionOffset = kMaxExtension;
// The accumulated voltage to apply to the motor.
double voltage_;
double last_voltage_;
@@ -108,10 +107,11 @@
};
const Time kUnloadTimeout = Time::InSeconds(10);
-const Time kLoadTimeout = Time::InSeconds(10);
-const Time kLoadProblemEndTimeout = Time::InSeconds(0.5);
+const Time kLoadTimeout = Time::InSeconds(2);
+const Time kLoadProblemEndTimeout = Time::InSeconds(1.0);
const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
-const Time kShotEndTimeout = Time::InSeconds(1.0);
+// Time to wait after releasing the latch piston before winching back again.
+const Time kShotEndTimeout = Time::InSeconds(0.2);
const Time kPrepareFireEndTime = Time::InMS(40);
class ShooterMotor
@@ -124,6 +124,7 @@
bool capped_goal() const { return shooter_.capped_goal(); }
double PowerToPosition(double power);
+ double PositionToPower(double position);
typedef enum {
STATE_INITIALIZE = 0,
@@ -133,7 +134,6 @@
STATE_LOADING_PROBLEM = 4,
STATE_PREPARE_SHOT = 5,
STATE_READY = 6,
- STATE_PREPARE_FIRE = 7,
STATE_FIRE = 8,
STATE_UNLOAD = 9,
STATE_UNLOAD_MOVE = 10,
@@ -150,6 +150,9 @@
ShooterGroup::Output *output, ShooterGroup::Status *status);
private:
+ // We have to override this to keep the pistons in the correct positions.
+ virtual void ZeroOutputs();
+
// Friend the test classes for acces to the internal state.
friend class testing::ShooterTest_UnloadWindupPositive_Test;
friend class testing::ShooterTest_UnloadWindupNegative_Test;
@@ -180,14 +183,10 @@
// wait for brake to set
Time shooter_brake_set_time_;
-
+
// The timeout for unloading.
Time unload_timeout_;
- // we are attempting to take up some of the backlash
- // in the gears before the plunger hits
- Time prepare_fire_end_time_;
-
// time that shot must have completed
Time shot_end_time_;
@@ -201,6 +200,12 @@
bool brake_piston_;
int32_t last_distal_posedge_count_;
int32_t last_proximal_posedge_count_;
+ uint32_t shot_count_;
+ bool zeroed_;
+ int distal_posedge_validation_cycles_left_;
+ int proximal_posedge_validation_cycles_left_;
+ bool last_distal_current_;
+ bool last_proximal_current_;
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 6310320..2a42172 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -41,10 +41,14 @@
// Whether it's ready to shoot right now.
bool ready;
// Whether the plunger is in and out of the way of grabbing a ball.
+ // TODO(ben): Populate these!
bool cocked;
// How many times we've shot.
int32_t shots;
bool done;
+ // What we think the current position of the hard stop on the shooter is, in
+ // shot power (Joules).
+ double hard_stop_power;
};
queue Goal goal;
@@ -54,3 +58,43 @@
};
queue_group ShooterGroup shooter_queue_group;
+
+struct ShooterStateToLog {
+ double absolute_position;
+ double absolute_velocity;
+ uint32_t state;
+ bool latch_sensor;
+ bool proximal;
+ bool distal;
+ bool plunger;
+ bool brake;
+ bool latch_piston;
+};
+
+struct ShooterVoltageToLog {
+ double X_hat;
+ double applied;
+};
+
+struct ShooterMovingGoal {
+ double dx;
+};
+
+struct ShooterChangeCalibration {
+ double encoder;
+ double real_position;
+ double old_position;
+ double new_position;
+ double old_offset;
+ double new_offset;
+};
+
+struct ShooterStatusToLog {
+ double goal;
+ double position;
+};
+
+struct PowerAdjustment {
+ double requested_power;
+ double actual_power;
+};
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 8aa4c27..669e147 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -50,7 +50,7 @@
// The difference between the position with 0 at the back, and the position
// with 0 measured where the spring has 0 force.
- constexpr static double kPositionOffset = 0.305054 + 0.0254;
+ constexpr static double kPositionOffset = kMaxExtension;
void Reinitialize(double initial_position) {
LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
@@ -83,7 +83,8 @@
// (encoder, hall effect).
void SetPhysicalSensors(control_loops::ShooterGroup::Position *position) {
const frc971::constants::Values &values = constants::GetValues();
- position->position = GetPosition();
+
+ position->position = GetPosition();
LOG(DEBUG, "Physical shooter at {%f}\n", GetAbsolutePosition());
@@ -134,12 +135,27 @@
}
}
- // Sends out the position queue messages.
void SendPositionMessage() {
+ // the first bool is false
+ SendPositionMessage(false, false, false, false);
+ }
+
+ // Sends out the position queue messages.
+ // if the first bool is false then this is
+ // just the default state, otherwise will force
+ // it into a state using the passed values
+ void SendPositionMessage(bool use_passed, bool plunger_in,
+ bool latch_in, bool brake_in) {
const frc971::constants::Values &values = constants::GetValues();
::aos::ScopedMessagePtr<control_loops::ShooterGroup::Position> position =
shooter_queue_group_.position.MakeMessage();
+ if (use_passed) {
+ plunger_latched_ = latch_in && plunger_in;
+ latch_piston_state_ = plunger_latched_;
+ brake_piston_state_ = brake_in;
+ }
+
SetPhysicalSensors(position.get());
position->latch = latch_piston_state_;
@@ -170,10 +186,6 @@
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << ": The test doesn't support that.";
latch_delay_count_ = -6;
- if (GetAbsolutePosition() > 0.01) {
- EXPECT_GE(last_voltage_, 1)
- << ": Must preload the gearbox when firing.";
- }
}
if (shooter_queue_group_.output->brake_piston && !brake_piston_state_ &&
@@ -206,6 +218,7 @@
shooter_plant_->D() * shooter_plant_->U;
} else {
shooter_plant_->U << last_voltage_;
+ //shooter_plant_->U << shooter_queue_group_.output->voltage;
shooter_plant_->Update();
}
LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
@@ -272,6 +285,7 @@
};
class ShooterTest : public ::testing::Test {
+
protected:
::aos::common::testing::GlobalCoreInstance my_core;
@@ -333,21 +347,39 @@
};
TEST_F(ShooterTest, PowerConversion) {
- // test a couple of values return the right thing
- EXPECT_EQ(0.021, shooter_motor_.PowerToPosition(0.021));
- EXPECT_EQ(0.175, shooter_motor_.PowerToPosition(0.175));
-
const frc971::constants::Values &values = constants::GetValues();
+ // test a couple of values return the right thing
+ EXPECT_NEAR(0.254001, shooter_motor_.PowerToPosition(140.0), 0.00001);
+ EXPECT_NEAR(0.00058, shooter_motor_.PowerToPosition(0.53), 0.00001);
+ EXPECT_NEAR(0.095251238129837101, shooter_motor_.PowerToPosition(73.67),
+ 0.00001);
+
// value too large should get max
- EXPECT_EQ(values.shooter.upper_limit,
- shooter_motor_.PowerToPosition(505050.99));
+ EXPECT_NEAR(values.shooter.upper_limit,
+ shooter_motor_.PowerToPosition(505050.99), 0.00001);
// negative values should zero
- EXPECT_EQ(values.shooter.lower_limit, shooter_motor_.PowerToPosition(-123.4));
+ EXPECT_NEAR(0, shooter_motor_.PowerToPosition(-123.4), 0.00001);
+}
+
+// Test that PowerToPosition and PositionToPower are inverses of each other.
+// Note that PowerToPosition will cap position whereas PositionToPower will not
+// cap power.
+TEST_F(ShooterTest, InversePowerConversion) {
+ // Test a few values.
+ double power = 140.0;
+ double position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = .53;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = 71.971;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -356,13 +388,15 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.021).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 120; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -370,21 +404,21 @@
SendDSPacket(true);
}
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
+ shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_power(35.0)
+ .shot_requested(true)
+ .Send();
- bool hit_preparefire = false;
bool hit_fire = false;
for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
- hit_preparefire = true;
- }
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_power(17.0)
.shot_requested(false)
.Send();
}
@@ -393,15 +427,16 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, FireLong) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -411,16 +446,12 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
- bool hit_preparefire = false;
bool hit_fire = false;
for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
- hit_preparefire = true;
- }
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
shooter_queue_group_.goal.MakeWithBuilder()
@@ -432,16 +463,30 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
}
+// Verifies that it doesn't try to go out too far if you give it a ridicilous
+// power.
+TEST_F(ShooterTest, LoadTooFar) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(500.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_LT(
+ shooter_motor_plant_.GetAbsolutePosition(),
+ constants::GetValuesForTeam(971).shooter.upper_limit);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, MoveGoal) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -449,7 +494,7 @@
SendDSPacket(true);
}
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.11).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(14.0).Send();
for (int i = 0; i < 100; ++i) {
shooter_motor_plant_.SendPositionMessage();
@@ -459,14 +504,16 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Unload) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -476,7 +523,9 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -484,13 +533,13 @@
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
- shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ shooter_motor_plant_.GetAbsolutePosition(), 0.015);
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupNegative) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.20).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -502,7 +551,9 @@
int kicked_delay = 20;
int capped_goal_count = 0;
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
@@ -512,15 +563,15 @@
shooter_motor_.shooter_.R(0, 0) -= 100;
}
}
- if (shooter_motor_.capped_goal()) {
- ++capped_goal_count;
- }
+ if (shooter_motor_.capped_goal() && kicked_delay < 0) {
+ ++capped_goal_count;
+ }
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
- shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ shooter_motor_plant_.GetAbsolutePosition(), 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
EXPECT_LE(1, capped_goal_count);
EXPECT_GE(3, capped_goal_count);
@@ -528,7 +579,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupPositive) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.20).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -540,7 +591,9 @@
int kicked_delay = 20;
int capped_goal_count = 0;
- for (int i = 0; i < 400; ++i) {
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
@@ -550,15 +603,15 @@
shooter_motor_.shooter_.R(0, 0) += 0.1;
}
}
- if (shooter_motor_.capped_goal()) {
- ++capped_goal_count;
- }
+ if (shooter_motor_.capped_goal() && kicked_delay < 0) {
+ ++capped_goal_count;
+ }
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
- shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ shooter_motor_plant_.GetAbsolutePosition(), 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
EXPECT_LE(1, capped_goal_count);
EXPECT_GE(3, capped_goal_count);
@@ -571,7 +624,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnDistal) {
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -580,7 +633,9 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -589,7 +644,7 @@
TEST_F(ShooterTest, StartsOnProximal) {
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 300; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -598,17 +653,59 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
+class ShooterZeroingTest : public ShooterTest,
+ public ::testing::WithParamInterface<
+ ::std::tr1::tuple<bool, bool, bool, double>> {};
+
+TEST_P(ShooterZeroingTest, AllDisparateStartingZero) {
+ bool latch = ::std::tr1::get<0>(GetParam());
+ bool brake = ::std::tr1::get<1>(GetParam());
+ bool plunger_back = ::std::tr1::get<2>(GetParam());
+ double start_pos = ::std::tr1::get<3>(GetParam());
+ // flag to initialize test
+ //printf("@@@@ l= %d b= %d p= %d s= %.3f\n",
+ // latch, brake, plunger_back, start_pos);
+ bool initialized = false;
+ Reinitialize(start_pos);
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(120.0).Send();
+ for (int i = 0; i < 200; ++i) {
+ shooter_motor_plant_.SendPositionMessage(!initialized, plunger_back, latch, brake);
+ initialized = true;
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
+ ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+INSTANTIATE_TEST_CASE_P(
+ ShooterZeroingTest, ShooterZeroingTest,
+ ::testing::Combine(
+ ::testing::Bool(), ::testing::Bool(), ::testing::Bool(),
+ ::testing::Values(
+ 0.05, constants::GetValuesForTeam(971).shooter.upper_limit - 0.05,
+ HallEffectMiddle(constants::GetValuesForTeam(971)
+ .shooter.pusher_proximal),
+ HallEffectMiddle(constants::GetValuesForTeam(971)
+ .shooter.pusher_distal),
+ constants::GetValuesForTeam(971).shooter.latch_max_safe_position -
+ 0.001)));
+
// TODO(austin): Slip the encoder somewhere.
// TODO(austin): Test all the timeouts...
-// TODO(austin): Test starting latched and with the plunger back.
-// TODO(austin): Verify that we zeroed if we started latched and all the way back.
-
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
old mode 100755
new mode 100644
index 968fd04..606395a
--- a/frc971/control_loops/shooter/shooter_motor_plant.h
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -5,6 +5,10 @@
namespace frc971 {
namespace control_loops {
+static constexpr double kMaxExtension = 0.323850;
+
+static constexpr double kSpringConstant = 2800.000000;
+
StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients();
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
old mode 100755
new mode 100644
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0ef0df5..f289e81 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -323,14 +323,17 @@
//::std::cout << "Predict xhat before " << X_hat;
//::std::cout << "Measurement error is " << Y_ - C() * X_hat;
//X_hat = A() * X_hat + B() * U;
+ //::std::cout << "Predict xhat after " << X_hat;
+ UpdateObserver();
+ }
+
+ void UpdateObserver() {
if (new_y_) {
- LOG(INFO, "Got Y. R is (%f, %f, %f)\n", R(0, 0), R(1, 0), R(2, 0));
X_hat = (A() - L() * C()) * X_hat + L() * Y_ + B() * U;
new_y_ = false;
} else {
X_hat = A() * X_hat + B() * U;
}
- //::std::cout << "Predict xhat after " << X_hat;
}
// Sets the current controller to be index and verifies that it isn't out of
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 195195e..c91a8ac 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -10,12 +10,19 @@
'<(AOS)/prime/input/input.gyp:joystick_input',
'<(AOS)/linux_code/linux_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:log_interval',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/actions/actions.gyp:shoot_action_queue',
+ '<(DEPTH)/frc971/actions/actions.gyp:action_client',
+ '<(DEPTH)/frc971/actions/actions.gyp:catch_action_queue',
+ '<(DEPTH)/frc971/actions/actions.gyp:shoot_action_lib',
],
},
{
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index d751a43..7b97bbd 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -7,15 +7,22 @@
#include "aos/prime/input/joystick_input.h"
#include "aos/common/input/driver_station_data.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/time.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/constants.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/actions/shoot_action.q.h"
+#include "frc971/actions/action_client.h"
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/shoot_action.h"
using ::frc971::control_loops::drivetrain;
-using ::frc971::sensors::gyro;
+using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -30,114 +37,440 @@
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kClawClosed(3, 7);
-const ButtonLocation kClawOpen(3, 9);
-const ButtonLocation kFire(3, 11);
-const ButtonLocation kUnload(3, 12);
+
+const ButtonLocation kCatch(3, 10);
+
+const ButtonLocation kFire(3, 9);
+const ButtonLocation kUnload(2, 11);
+const ButtonLocation kReload(2, 6);
+
+const ButtonLocation kRollersOut(3, 8);
+const ButtonLocation kRollersIn(3, 3);
+
+const ButtonLocation kTuck(3, 4);
+const ButtonLocation kIntakePosition(3, 5);
+const ButtonLocation kIntakeOpenPosition(3, 11);
+const JoystickAxis kFlipRobot(3, 3);
+
+const ButtonLocation kLongShot(3, 7);
+const ButtonLocation kMediumShot(3, 6);
+const ButtonLocation kShortShot(3, 2);
+const ButtonLocation kTrussShot(3, 1);
+
+const JoystickAxis kAdjustClawGoal(3, 2);
+const JoystickAxis kAdjustClawSeparation(3, 1);
+
+struct ClawGoal {
+ double angle;
+ double separation;
+};
+
+struct ShotGoal {
+ ClawGoal claw;
+ double shot_power;
+ double velocity_compensation;
+ double intake_power;
+};
+
+const double kIntakePower = 4.0;
+// TODO(brians): This wants to be -0.04 on the comp bot. Make them both the
+// same.
+const double kGrabSeparation = 0;
+const double kShootSeparation = 0.11 + kGrabSeparation;
+
+const ClawGoal kTuckGoal = {-2.273474, -0.749484};
+const ClawGoal kIntakeGoal = {-2.273474, kGrabSeparation};
+const ClawGoal kIntakeOpenGoal = {-2.0, 1.2};
+
+// TODO(austin): Tune these by hand...
+const ClawGoal kFlippedTuckGoal = {2.733474, -0.75};
+const ClawGoal kFlippedIntakeGoal = {2.0, kGrabSeparation};
+const ClawGoal kFlippedIntakeOpenGoal = {0.95, 1.0};
+
+//const ShotGoal kLongShotGoal = {
+ //{-M_PI / 2.0 + 0.46, kShootSeparation}, 120, false, kIntakePower};
+const ShotGoal kLongShotGoal = {
+ {-1.04, kShootSeparation}, 140, 0.04, kIntakePower};
+const ShotGoal kMediumShotGoal = {
+ {-0.90, kShootSeparation}, 105, 0.2, kIntakePower};
+const ShotGoal kShortShotGoal = {
+ {-0.670, kShootSeparation}, 71.0, 0, kIntakePower};
+const ShotGoal kTrussShotGoal = {
+ {-0.05, kShootSeparation}, 61.0, 0, kIntakePower};
+
+const ShotGoal kFlippedLongShotGoal = {
+ {0.97, kShootSeparation}, 140, 0.08, kIntakePower};
+const ShotGoal kFlippedMediumShotGoal = {
+ {0.80, kShootSeparation}, 105, 0.2, kIntakePower};
+const ShotGoal kFlippedShortShotGoal = {
+ {0.57, kShootSeparation}, 80.0, 0, kIntakePower};
+
+// Makes a new ShootAction action.
+::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>
+MakeCatchAction() {
+ return ::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>(
+ new TypedAction< ::frc971::actions::CatchActionGroup>(
+ &::frc971::actions::catch_action));
+}
+
+// A queue which queues Actions and cancels them.
+class ActionQueue {
+ public:
+ // Queues up an action for sending.
+ void QueueAction(::std::unique_ptr<Action> action) {
+ if (current_action_) {
+ LOG(INFO, "Queueing action, canceling prior\n");
+ current_action_->Cancel();
+ next_action_ = ::std::move(action);
+ } else {
+ LOG(INFO, "Queueing action\n");
+ current_action_ = ::std::move(action);
+ current_action_->Start();
+ }
+ }
+
+ // Cancels the current action, and runs the next one when the current one has
+ // finished.
+ void CancelCurrentAction() {
+ LOG(INFO, "Canceling current action\n");
+ if (current_action_) {
+ current_action_->Cancel();
+ }
+ }
+
+ // Cancels all running actions.
+ void CancelAllActions() {
+ LOG(DEBUG, "Cancelling all actions\n");
+ if (current_action_) {
+ current_action_->Cancel();
+ }
+ next_action_.reset();
+ }
+
+ // Runs the next action when the current one is finished running.
+ void Tick() {
+ if (current_action_) {
+ if (!current_action_->Running()) {
+ LOG(INFO, "Action is done.\n");
+ current_action_ = ::std::move(next_action_);
+ if (current_action_) {
+ LOG(INFO, "Running next action\n");
+ current_action_->Start();
+ }
+ }
+ }
+ }
+
+ // Returns true if any action is running or could be running.
+ // For a one cycle faster response, call Tick before running this.
+ bool Running() { return (bool)current_action_; }
+
+ private:
+ ::std::unique_ptr<Action> current_action_;
+ ::std::unique_ptr<Action> next_action_;
+};
+
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() : closed_(true) {}
+ Reader()
+ : is_high_gear_(false),
+ shot_power_(80.0),
+ goal_angle_(0.0),
+ separation_angle_(kGrabSeparation),
+ velocity_compensation_(0.0),
+ intake_power_(0.0),
+ was_running_(false) {}
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
- static bool is_high_gear = false;
-
if (data.GetControlBit(ControlBit::kAutonomous)) {
if (data.PosEdge(ControlBit::kEnabled)){
LOG(INFO, "Starting auto mode\n");
::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(true).Send();
+ .run_auto(true)
+ .Send();
} else if (data.NegEdge(ControlBit::kEnabled)) {
LOG(INFO, "Stopping auto mode\n");
::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(false).Send();
+ .run_auto(false)
+ .Send();
}
- } else { // teleop
- bool is_control_loop_driving = false;
- double left_goal = 0.0;
- double right_goal = 0.0;
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
- const double kThrottleGain = 1.0 / 2.5;
- if (false && (data.IsPressed(kDriveControlLoopEnable1) ||
- data.IsPressed(kDriveControlLoopEnable2))) {
- static double distance = 0.0;
- static double angle = 0.0;
- static double filtered_goal_distance = 0.0;
- if (data.PosEdge(kDriveControlLoopEnable1) ||
- data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
- distance = (drivetrain.position->left_encoder +
- drivetrain.position->right_encoder) / 2.0
- - throttle * kThrottleGain / 2.0;
- angle = gyro->angle;
- filtered_goal_distance = distance;
- }
+ } else {
+ HandleTeleop(data);
+ }
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ bool is_control_loop_driving = false;
+ double left_goal = 0.0;
+ double right_goal = 0.0;
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+ const double kThrottleGain = 1.0 / 2.5;
+ if (false && (data.IsPressed(kDriveControlLoopEnable1) ||
+ data.IsPressed(kDriveControlLoopEnable2))) {
+ // TODO(austin): Static sucks!
+ static double distance = 0.0;
+ static double angle = 0.0;
+ static double filtered_goal_distance = 0.0;
+ if (data.PosEdge(kDriveControlLoopEnable1) ||
+ data.PosEdge(kDriveControlLoopEnable2)) {
+ if (drivetrain.position.FetchLatest() && gyro_reading.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.position->right_encoder) /
+ 2.0 -
+ throttle * kThrottleGain / 2.0;
+ angle = gyro_reading->angle;
+ filtered_goal_distance = distance;
}
- is_control_loop_driving = true;
-
- //const double gyro_angle = Gyro.View().angle;
- const double goal_theta = angle - wheel * 0.27;
- const double goal_distance = distance + throttle * kThrottleGain;
- const double robot_width = 22.0 / 100.0 * 2.54;
- const double kMaxVelocity = 0.6;
- if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
- filtered_goal_distance += kMaxVelocity * 0.02;
- } else if (goal_distance < -kMaxVelocity * 0.02 +
- filtered_goal_distance) {
- filtered_goal_distance -= kMaxVelocity * 0.02;
- } else {
- filtered_goal_distance = goal_distance;
- }
- left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
- right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
- is_high_gear = false;
-
- LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
}
- if (!drivetrain.goal.MakeWithBuilder()
- .steering(wheel)
- .throttle(throttle)
- .highgear(is_high_gear).quickturn(data.IsPressed(kQuickTurn))
- .control_loop_driving(is_control_loop_driving)
- .left_goal(left_goal).right_goal(right_goal).Send()) {
- LOG(WARNING, "sending stick values failed\n");
+ is_control_loop_driving = true;
+
+ // const double gyro_angle = Gyro.View().angle;
+ const double goal_theta = angle - wheel * 0.27;
+ const double goal_distance = distance + throttle * kThrottleGain;
+ const double robot_width = 22.0 / 100.0 * 2.54;
+ const double kMaxVelocity = 0.6;
+ if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance += kMaxVelocity * 0.02;
+ } else if (goal_distance <
+ -kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance -= kMaxVelocity * 0.02;
+ } else {
+ filtered_goal_distance = goal_distance;
+ }
+ left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
+ right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
+ is_high_gear_ = false;
+
+ LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
+ }
+ if (!drivetrain.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear_)
+ .quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal)
+ .right_goal(right_goal)
+ .Send()) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+ if (data.PosEdge(kShiftHigh)) {
+ is_high_gear_ = false;
+ }
+ if (data.PosEdge(kShiftLow)) {
+ is_high_gear_ = true;
+ }
+ }
+
+ void SetGoal(ClawGoal goal) {
+ goal_angle_ = goal.angle;
+ separation_angle_ = goal.separation;
+ velocity_compensation_ = 0.0;
+ intake_power_ = 0.0;
+ }
+
+ void SetGoal(ShotGoal goal) {
+ goal_angle_ = goal.claw.angle;
+ separation_angle_ = goal.claw.separation;
+ shot_power_ = goal.shot_power;
+ velocity_compensation_ = goal.velocity_compensation;
+ intake_power_ = goal.intake_power;
+ }
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ HandleDrivetrain(data);
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+ if (data.IsPressed(kRollersIn) || data.IsPressed(kRollersOut)) {
+ intake_power_ = 0.0;
+ separation_angle_ = kGrabSeparation;
+ }
+
+ static const double kAdjustClawGoalDeadband = 0.08;
+ double claw_goal_adjust = data.GetAxis(kAdjustClawGoal);
+ if (::std::abs(claw_goal_adjust) < kAdjustClawGoalDeadband) {
+ claw_goal_adjust = 0;
+ } else {
+ claw_goal_adjust = (claw_goal_adjust -
+ ((claw_goal_adjust < 0) ? -kAdjustClawGoalDeadband
+ : kAdjustClawGoalDeadband)) *
+ 0.035;
+ }
+ double claw_separation_adjust = data.GetAxis(kAdjustClawSeparation);
+ if (::std::abs(claw_separation_adjust) < kAdjustClawGoalDeadband) {
+ claw_separation_adjust = 0;
+ } else {
+ claw_separation_adjust =
+ (claw_separation_adjust -
+ ((claw_separation_adjust < 0) ? -kAdjustClawGoalDeadband
+ : kAdjustClawGoalDeadband)) *
+ -0.035;
+ }
+
+ if (data.GetAxis(kFlipRobot) > 0.5) {
+ claw_goal_adjust += claw_separation_adjust;
+ claw_goal_adjust *= -1;
+
+ if (data.IsPressed(kIntakeOpenPosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedIntakeOpenGoal);
+ } else if (data.IsPressed(kIntakePosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedIntakeGoal);
+ } else if (data.IsPressed(kTuck)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedTuckGoal);
+ } else if (data.PosEdge(kLongShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedLongShotGoal);
+ } else if (data.PosEdge(kMediumShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedMediumShotGoal);
+ } else if (data.PosEdge(kShortShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlippedShortShotGoal);
+ } else if (data.PosEdge(kTrussShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTrussShotGoal);
+ }
+ } else {
+ if (data.IsPressed(kIntakeOpenPosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kIntakeOpenGoal);
+ } else if (data.IsPressed(kIntakePosition)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kIntakeGoal);
+ } else if (data.IsPressed(kTuck)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTuckGoal);
+ } else if (data.PosEdge(kLongShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kLongShotGoal);
+ } else if (data.PosEdge(kMediumShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kMediumShotGoal);
+ } else if (data.PosEdge(kShortShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kShortShotGoal);
+ } else if (data.PosEdge(kTrussShot)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kTrussShotGoal);
+ }
+ }
+
+ /*
+ if (data.PosEdge(kCatch)) {
+ auto catch_action = MakeCatchAction();
+ catch_action->GetGoal()->catch_angle = goal_angle_;
+ action_queue_.QueueAction(::std::move(catch_action));
+ }
+ */
+
+ if (data.PosEdge(kFire)) {
+ action_queue_.QueueAction(actions::MakeShootAction());
+ }
+
+ action_queue_.Tick();
+ if (data.IsPressed(kUnload) || data.IsPressed(kReload)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ intake_power_ = 0.0;
+ velocity_compensation_ = 0.0;
+ }
+
+ // Send out the claw and shooter goals if no actions are running.
+ if (!action_queue_.Running()) {
+ goal_angle_ += claw_goal_adjust;
+ separation_angle_ += claw_separation_adjust;
+
+ // If the action just ended, turn the intake off and stop velocity
+ // compensating.
+ if (was_running_) {
+ intake_power_ = 0.0;
+ velocity_compensation_ = 0.0;
}
- if (data.PosEdge(kShiftHigh)) {
- is_high_gear = false;
+ control_loops::drivetrain.status.FetchLatest();
+ double goal_angle = goal_angle_;
+ if (control_loops::drivetrain.status.get()) {
+ goal_angle +=
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed);
+ } else {
+ LOG_INTERVAL(no_drivetrain_status_);
}
- if (data.PosEdge(kShiftLow)) {
- is_high_gear = true;
- }
- if (data.PosEdge(kClawClosed)) {
- closed_ = true;
- }
- if (data.PosEdge(kClawOpen)) {
- closed_ = false;
+ double separation_angle = separation_angle_;
+
+ if (data.IsPressed(kCatch)) {
+ const double kCatchSeparation = 1.0;
+ goal_angle -= kCatchSeparation / 2.0;
+ separation_angle = kCatchSeparation;
}
- double separation_angle = closed_ ? 0.0 : 0.5;
- double goal_angle = closed_ ? 0.0 : -0.2;
+ bool intaking =
+ data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition) ||
+ data.IsPressed(kIntakeOpenPosition) || data.IsPressed(kCatch);
if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
.bottom_angle(goal_angle)
.separation_angle(separation_angle)
- .intake(false)
+ .intake(intaking ? 12.0
+ : (data.IsPressed(kRollersOut) ? -12.0
+ : intake_power_))
+ .centering(intaking ? 12.0 : 0.0)
.Send()) {
LOG(WARNING, "sending claw goal failed\n");
}
+
if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
- .shot_power(0.25)
+ .shot_power(shot_power_)
.shot_requested(data.IsPressed(kFire))
.unload_requested(data.IsPressed(kUnload))
+ .load_requested(data.IsPressed(kReload))
.Send()) {
LOG(WARNING, "sending shooter goal failed\n");
}
}
+ was_running_ = action_queue_.Running();
}
-
+
+ double SpeedToAngleOffset(double speed) {
+ const frc971::constants::Values &values = frc971::constants::GetValues();
+ // scale speed to a [0.0-1.0] on something close to the max
+ // TODO(austin): Change the scale factor for different shots.
+ return (speed / values.drivetrain_max_speed) * velocity_compensation_;
+ }
+
private:
- bool closed_;
+ bool is_high_gear_;
+ double shot_power_;
+ double goal_angle_;
+ double separation_angle_;
+ double velocity_compensation_;
+ double intake_power_;
+ bool was_running_;
+
+ ActionQueue action_queue_;
+
+ ::aos::util::SimpleLogInterval no_drivetrain_status_ =
+ ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ "no drivetrain status");
};
} // namespace joysticks
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 8c4172d..7648db8 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -9,7 +9,7 @@
#include "bbb/sensor_reader.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/constants.h"
#include "frc971/queues/to_log.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
@@ -20,7 +20,8 @@
#endif
using ::frc971::control_loops::drivetrain;
-using ::frc971::sensors::gyro;
+using ::frc971::sensors::other_sensors;
+using ::frc971::sensors::gyro_reading;
using ::aos::util::WrappingCounter;
namespace frc971 {
@@ -47,29 +48,47 @@
}
// Translates values from the ADC into voltage.
-// TODO(brian): Fix this.
+// TODO(brian): Tune this to the actual hardware.
double adc_translate(uint16_t in) {
static const double kVcc = 5;
static const double kR1 = 5, kR2 = 6.65;
static const uint16_t kMaximumValue = 0x3FF;
- return (kVcc *
- (static_cast<double>(in) / static_cast<double>(kMaximumValue)) -
- kVcc * kR1) /
- kR2;
+ const double raw =
+ (kVcc * static_cast<double>(in) / static_cast<double>(kMaximumValue));
+ return (raw * (kR1 + kR2) - (kVcc / 2) * kR2) / kR1;
}
double gyro_translate(int64_t in) {
return in / 16.0 / 1000.0 / (180.0 / M_PI);
}
-double battery_translate(uint16_t in) {
- static const double kDividerBig = 98.9, kDividerSmall = 17.8;
- return adc_translate(in) * kDividerBig / kDividerSmall;
+double battery_translate(uint16_t in_high, uint16_t in_low) {
+ const double high = adc_translate(in_high), low = adc_translate(in_low);
+ static const double kDividerBig = 5.55, kDividerSmall = 2.66;
+ static const double kSensorVcc = 5.0;
+ return (high - low) * (kDividerBig + kDividerSmall) / kDividerSmall +
+ kDividerBig / kDividerSmall * kSensorVcc;
}
-double hall_translate(const constants::ShifterHallEffect &k, uint16_t in) {
- const double voltage = adc_translate(in);
- return (voltage - k.low) / (k.high - k.low);
+double sonar_translate(uint32_t in) {
+ return static_cast<double>(in) / 1000.0 * 2.0;
+}
+
+double hall_translate(const constants::ShifterHallEffect &k, uint16_t in_low,
+ uint16_t in_high) {
+ const double low_ratio =
+ 0.5 * (in_low - static_cast<double>(k.low_gear_low)) /
+ static_cast<double>(k.low_gear_middle - k.low_gear_low);
+ const double high_ratio =
+ 0.5 + 0.5 * (in_high - static_cast<double>(k.high_gear_middle)) /
+ static_cast<double>(k.high_gear_high - k.high_gear_middle);
+
+ // Return low when we are below 1/2, and high when we are above 1/2.
+ if (low_ratio + high_ratio < 1.0) {
+ return low_ratio;
+ } else {
+ return high_ratio;
+ }
}
double claw_translate(int32_t in) {
@@ -77,7 +96,8 @@
/ (256.0 /*cpr*/ * 4.0 /*quad*/)
/ (18.0 / 48.0 /*encoder gears*/)
/ (12.0 / 60.0 /*chain reduction*/)
- * (M_PI / 180.0);
+ * (M_PI / 180.0)
+ * 2.0 /*TODO(austin): Debug this, encoders read too little*/;
}
double shooter_translate(int32_t in) {
@@ -119,9 +139,12 @@
State *state) {
::frc971::logging_structs::CapeReading reading_to_log(
cape_timestamp.sec(), cape_timestamp.nsec(),
- data->main.ultrasonic_pulse_length);
+ sizeof(*data), sonar_translate(data->main.ultrasonic_pulse_length),
+ data->main.low_left_drive_hall, data->main.high_left_drive_hall,
+ data->main.low_right_drive_hall, data->main.high_right_drive_hall);
LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
bool bad_gyro;
+ // TODO(brians): Switch to LogInterval for these things.
if (data->uninitialized_gyro) {
LOG(DEBUG, "uninitialized gyro\n");
bad_gyro = true;
@@ -131,7 +154,6 @@
} else if (data->bad_gyro) {
LOG(ERROR, "bad gyro\n");
bad_gyro = true;
- gyro.MakeWithBuilder().angle(0).Send();
} else if (data->old_gyro_reading) {
LOG(WARNING, "old/bad gyro reading\n");
bad_gyro = true;
@@ -140,19 +162,30 @@
}
if (!bad_gyro) {
- gyro.MakeWithBuilder()
+ gyro_reading.MakeWithBuilder()
.angle(gyro_translate(data->gyro_angle))
.Send();
}
+ if (data->analog_errors != 0) {
+ LOG(WARNING, "%" PRIu8 " analog errors\n", data->analog_errors);
+ }
+
+ other_sensors.MakeWithBuilder()
+ .sonar_distance(sonar_translate(data->main.ultrasonic_pulse_length))
+ .Send();
+
drivetrain.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(data->main.right_drive))
.left_encoder(-drivetrain_translate(data->main.left_drive))
.left_shifter_position(hall_translate(constants::GetValues().left_drive,
- data->main.left_drive_hall))
- .right_shifter_position(hall_translate(
- constants::GetValues().right_drive, data->main.right_drive_hall))
- .battery_voltage(battery_translate(data->main.battery_voltage))
+ data->main.low_left_drive_hall,
+ data->main.high_left_drive_hall))
+ .right_shifter_position(hall_translate(constants::GetValues().right_drive,
+ data->main.low_right_drive_hall,
+ data->main.high_right_drive_hall))
+ .battery_voltage(battery_translate(data->main.battery_voltage_high,
+ data->main.battery_voltage_low))
.Send();
{
diff --git a/frc971/output/led_setter.cc b/frc971/output/led_setter.cc
new file mode 100644
index 0000000..a69183d
--- /dev/null
+++ b/frc971/output/led_setter.cc
@@ -0,0 +1,19 @@
+#include "aos/common/logging/logging.h"
+#include "aos/linux_code/init.h"
+
+#include "bbb/led.h"
+
+#include "frc971/control_loops/claw/claw.q.h"
+
+using ::frc971::control_loops::claw_queue_group;
+
+int main() {
+ ::aos::InitNRT();
+
+ ::bbb::LED claw_zeroed(3);
+
+ while (true) {
+ CHECK(claw_queue_group.status.FetchNextBlocking());
+ claw_zeroed.Set(claw_queue_group.status->zeroed_for_auto);
+ }
+}
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index e17d6bc..dca221b 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -26,6 +26,12 @@
static constexpr ::aos::time::Time kOldLogInterval =
::aos::time::Time::InSeconds(0.5);
+ double Cap(double value, double max) {
+ if (value > max) return max;
+ if (value < -max) return -max;
+ return value;
+ }
+
virtual void RunIteration() {
values_.digital_module = 0;
values_.pressure_switch_channel = 1;
@@ -45,6 +51,7 @@
DisablePWMOutput(8);
LOG_INTERVAL(drivetrain_old_);
}
+ drivetrain_old_.Print();
}
{
@@ -61,6 +68,7 @@
SetSolenoid(5, false); // engage the brake
LOG_INTERVAL(shooter_old_);
}
+ shooter_old_.Print();
}
{
@@ -83,6 +91,7 @@
DisablePWMOutput(5);
LOG_INTERVAL(claw_old_);
}
+ claw_old_.Print();
}
}
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 6faca12..bc2d604 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -1,6 +1,19 @@
{
'targets': [
{
+ 'target_name': 'led_setter',
+ 'type': 'executable',
+ 'sources': [
+ 'led_setter.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:led',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ ],
+ },
+ {
'target_name': 'CameraServer',
'type': 'executable',
'sources': [
@@ -24,7 +37,7 @@
},
{
'target_name': 'motor_writer',
- 'type': '<(aos_target)',
+ 'type': 'executable',
'sources': [
'motor_writer.cc'
],
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 2b5d600..c4db795 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -13,6 +13,10 @@
'../control_loops/shooter/shooter.gyp:shooter',
'../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../autonomous/autonomous.gyp:auto',
+ '../actions/actions.gyp:shoot_action',
+ '../actions/actions.gyp:selfcatch_action',
+ '../actions/actions.gyp:catch_action',
+ '../actions/actions.gyp:drivetrain_action',
'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',
'../input/input.gyp:sensor_receiver',
@@ -21,12 +25,13 @@
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:packet_finder_test',
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:cows_test',
'<(DEPTH)/bbb_cape/src/flasher/flasher.gyp:stm32_flasher',
+ '../output/output.gyp:led_setter',
],
'copies': [
{
'destination': '<(rsync_dir)',
'files': [
- 'scripts/start_list.txt',
+ 'start_list.txt',
],
},
],
diff --git a/frc971/prime/scripts/start_list.txt b/frc971/prime/start_list.txt
similarity index 67%
rename from frc971/prime/scripts/start_list.txt
rename to frc971/prime/start_list.txt
index 179c122..94bf6bb 100644
--- a/frc971/prime/scripts/start_list.txt
+++ b/frc971/prime/start_list.txt
@@ -6,3 +6,6 @@
sensor_receiver
claw
shooter
+shoot_action
+drivetrain_action
+catch_action
diff --git a/frc971/queues/gyro_angle.q b/frc971/queues/gyro_angle.q
deleted file mode 100644
index bcf3ac4..0000000
--- a/frc971/queues/gyro_angle.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package frc971.sensors;
-
-message Gyro {
- double angle;
-};
-
-queue Gyro gyro;
diff --git a/frc971/queues/other_sensors.q b/frc971/queues/other_sensors.q
new file mode 100644
index 0000000..7181ce9
--- /dev/null
+++ b/frc971/queues/other_sensors.q
@@ -0,0 +1,12 @@
+package frc971.sensors;
+
+message OtherSensors {
+ double sonar_distance;
+ double plunger_hall_effect_distance;
+};
+queue OtherSensors other_sensors;
+
+message GyroReading {
+ double angle;
+};
+queue GyroReading gyro_reading;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index ac53a70..8434437 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -1,7 +1,7 @@
{
'variables': {
'queue_files': [
- 'gyro_angle.q',
+ 'other_sensors.q',
'to_log.q',
]
},
diff --git a/frc971/queues/to_log.q b/frc971/queues/to_log.q
index 24e2402..ded9b64 100644
--- a/frc971/queues/to_log.q
+++ b/frc971/queues/to_log.q
@@ -3,5 +3,11 @@
struct CapeReading {
uint32_t sec;
uint32_t nsec;
- uint32_t sonar;
+ uint64_t struct_size;
+ double sonar;
+
+ uint16_t left_low;
+ uint16_t left_high;
+ uint16_t right_low;
+ uint16_t right_high;
};