merged in joes build changes
diff --git a/aos/build/queues/compiler.rb b/aos/build/queues/compiler.rb
index 28e206b..ffee5d3 100644
--- a/aos/build/queues/compiler.rb
+++ b/aos/build/queues/compiler.rb
@@ -1,12 +1,13 @@
+$LOAD_PATH.unshift(".")
["tokenizer.rb","q_file.rb","queue_group.rb","queue.rb","namespaces.rb",
-"interface.rb","errors.rb"].each do |name|
+"interface.rb","errors.rb", "q_struct.rb"].each do |name|
require File.dirname(__FILE__) + "/objects/" + name
end
["standard_types.rb","auto_gen.rb","file_pair_types.rb",
"dep_file_pair.rb","swig.rb"].each do |name|
require File.dirname(__FILE__) + "/cpp_pretty_print/" + name
end
-["q_file.rb","message_dec.rb","queue_dec.rb"].each do |name|
+["q_file.rb","message_dec.rb","queue_dec.rb", "q_struct.rb"].each do |name|
require File.dirname(__FILE__) + "/output/" + name
end
require "fileutils"
diff --git a/aos/build/queues/objects/namespaces.rb b/aos/build/queues/objects/namespaces.rb
index b799d36..ea3aa80 100644
--- a/aos/build/queues/objects/namespaces.rb
+++ b/aos/build/queues/objects/namespaces.rb
@@ -43,7 +43,7 @@
end
class BoundSituation < LocalSituation
def initialize(locals,bind_to)
- @globals = globals
+ @globals = locals.globals
@local = bind_to
end
end
@@ -150,6 +150,7 @@
end
def test_lookup(namespace)
@names.each do |name|
+ return nil if(!namespace.respond_to?(:[]))
namespace = namespace[name]
return nil if(!namespace)
end
diff --git a/aos/build/queues/objects/q_file.rb b/aos/build/queues/objects/q_file.rb
index f683dda..beaf8f0 100644
--- a/aos/build/queues/objects/q_file.rb
+++ b/aos/build/queues/objects/q_file.rb
@@ -134,6 +134,8 @@
suite << QueueStmt.parse(tokens)
when "interface"
suite << InterfaceStmt.parse(tokens)
+ when "struct"
+ suite << StructStmt.parse(tokens)
else
tokens.qError(<<ERROR_MSG)
expected a "package","import","queue","queue_group", or "message" statement rather
diff --git a/aos/build/queues/objects/q_struct.rb b/aos/build/queues/objects/q_struct.rb
new file mode 100644
index 0000000..cd47fe6
--- /dev/null
+++ b/aos/build/queues/objects/q_struct.rb
@@ -0,0 +1,37 @@
+
+class StructStmt
+ def initialize(name,suite)
+ @name = name
+ @suite = suite
+ end
+ def q_eval(locals)
+ group = Target::StructDec.new(@name)
+ locals.register(group)
+ @suite.each do |stmt|
+ stmt.q_eval(locals.bind(group))
+ end
+ return group
+ end
+ def self.parse(tokens)
+ name = tokens.expect(:tWord).data
+ values = []
+ tokens.expect(:tOpenB)
+ while(tokens.peak != :tCloseB)
+ values << MessageElementStmt.parse(tokens)
+ end
+ names = {}
+ values.each do |val|
+ if(names[val.name])
+ raise QSyntaxError.new(<<ERROR_MSG)
+Hey! duplicate name #{val.name.inspect} in your message declaration statement (message #{name}).
+\tI found them at: #{names[val.name].q_stack_name()} and #{val.q_stack_name()}.
+\tWot. Wot.
+ERROR_MSG
+ end
+ names[val.name] = val
+ end
+ tokens.expect(:tCloseB)
+ tokens.expect(:tSemi)
+ self.new(name,values)
+ end
+end
diff --git a/aos/build/queues/objects/queue.rb b/aos/build/queues/objects/queue.rb
index e993eb9..1199cc2 100644
--- a/aos/build/queues/objects/queue.rb
+++ b/aos/build/queues/objects/queue.rb
@@ -1,12 +1,13 @@
class MessageElementStmt < QStmt
attr_accessor :name
- def initialize(type,name,length = nil) #lengths are for arrays
- @type = type
+ def initialize(type_name,name,length = nil) #lengths are for arrays
+ @type_name = type_name
+ @type = type_name.to_s
@name = name
@length = length
end
CommonMistakes = {"short" => "int16_t","int" => "int32_t","long" => "int64_t"}
- def check_type_error()
+ def check_type_error(locals)
if(!(Sizes[@type] || (@length != nil && @type == "char")) )
if(correction = CommonMistakes[@type])
raise QError.new(<<ERROR_MSG)
@@ -23,11 +24,13 @@
\tWot. Wot.
ERROR_MSG
else
+ @is_struct_type = true
+ return if(lookup_type(locals))
raise QError.new(<<ERROR_MSG)
Hey! you have a \"#{@type}\" in your message statement.
\tThat is not in the list of supported types.
\there is the list of supported types:
-\tint{8,16,32,64}_t,uint{8,16,32,64}_t,bool,float,double#{len_comment}
+\tint{8,16,32,64}_t,uint{8,16,32,64}_t,bool,float,double
\tWot. Wot.
ERROR_MSG
end
@@ -72,21 +75,30 @@
\tWot. Wot.
ERROR_MSG
end
+ def lookup_type(locals)
+ return @type_name.lookup(locals)
+ end
def q_eval(locals)
- check_type_error()
- if(@length == nil)
- member = Target::MessageElement.new(@type,@name)
+ check_type_error(locals)
+ if(@is_struct_type)
+ tval = lookup_type(locals)
+ member = Target::MessageStructElement.new(tval, name)
else
- member = Target::MessageArrayElement.new(@type,@name,@length)
+ if(@length == nil)
+ member = Target::MessageElement.new(@type,@name)
+ else
+ member = Target::MessageArrayElement.new(@type,@name,@length)
+ end
+ member.size = size()
+ member.zero = Zero[@type] || "0";
+ member.printformat = toPrintFormat()
end
- member.size = size()
- member.zero = Zero[@type] || "0";
- member.printformat = toPrintFormat()
locals.local.add_member(member)
end
def self.parse(tokens)
line = tokens.pos
- type = tokens.expect(:tWord).data
+ #type = tokens.expect(:tWord).data
+ type_name = QualifiedName.parse(tokens)
len = nil
if(tokens.peak == :tOpenB)
tokens.expect(:tOpenB)
@@ -95,7 +107,7 @@
end
name = tokens.expect(:tWord).data
tokens.expect(:tSemi)
- return self.new(type,name,len).set_line(line)
+ return self.new(type_name,name,len).set_line(line)
end
end
class MessageStmt < QStmt
diff --git a/aos/build/queues/output/message_dec.rb b/aos/build/queues/output/message_dec.rb
index 3b89149..579ce0c 100644
--- a/aos/build/queues/output/message_dec.rb
+++ b/aos/build/queues/output/message_dec.rb
@@ -34,17 +34,13 @@
@members.each do |elem|
format += ", "
format += elem.toPrintFormat()
- if (elem.type == 'bool')
- args.push("#{elem.name} ? 'T' : 'f'")
- else
- args.push(elem.name)
- end
+ elem.fetchPrintArgs(args)
end
- format += "\""
- member_func.suite << "size_t super_size = ::aos::Message::Print(buffer, length)"
- member_func.suite << "buffer += super_size"
- member_func.suite << "length -= super_size"
- member_func.suite << "return super_size + snprintf(buffer, length, " + ([format] + args).join(", ") + ")";
+ format += "\""
+ member_func.suite << "size_t super_size = ::aos::Message::Print(buffer, length)"
+ member_func.suite << "buffer += super_size"
+ member_func.suite << "length -= super_size"
+ member_func.suite << "return super_size + snprintf(buffer, length, " + ([format] + args).join(", ") + ")";
end
def create_Serialize(type_class,cpp_tree)
member_func = CPP::MemberFunc.new(type_class,"size_t","Serialize")
@@ -221,26 +217,33 @@
def create_usage(cpp_tree)
"#{@type} #{@name}"
end
- def toNetwork(offset,suite)
+ def toNetwork(offset,suite, parent = "")
offset = (offset == 0) ? "" : "#{offset} + "
suite << f_call = CPP::FuncCall.build("to_network",
- "&#{@name}",
+ "&#{parent}#{@name}",
"&buffer[#{offset}::aos::Message::Size()]")
f_call.args.dont_wrap = true
end
- def toHost(offset,suite)
+ def toHost(offset,suite, parent = "")
offset = (offset == 0) ? "" : "#{offset} + "
suite << f_call = CPP::FuncCall.build("to_host",
"&buffer[#{offset}::aos::Message::Size()]",
- "&#{@name}")
+ "&#{parent}#{@name}")
f_call.args.dont_wrap = true
end
def set_message_builder(suite)
suite << "msg_ptr_->#{@name} = #{@name}"
end
- def zeroCall(suite)
- suite << CPP::Assign.new(@name,@zero)
+ def zeroCall(suite, parent = "")
+ suite << CPP::Assign.new(parent + @name,@zero)
+ end
+ def fetchPrintArgs(args, parent = "")
+ if (self.type == 'bool')
+ args.push("#{parent}#{self.name} ? 'T' : 'f'")
+ else
+ args.push("#{parent}#{self.name}")
+ end
end
end
class Target::MessageArrayElement < Target::Node
diff --git a/aos/build/queues/output/q_file.rb b/aos/build/queues/output/q_file.rb
index 5e016c0..1ef47a4 100644
--- a/aos/build/queues/output/q_file.rb
+++ b/aos/build/queues/output/q_file.rb
@@ -8,6 +8,13 @@
end
class Target::Node
attr_accessor :created_by
+ def get_name()
+ if(@parent)
+ return "#{@parent.get_name}.#{@name}"
+ else
+ return "#{@name}"
+ end
+ end
end
class Target::QFile < Target::Node
def initialize() #needs to know repo_path,
diff --git a/aos/build/queues/output/q_struct.rb b/aos/build/queues/output/q_struct.rb
new file mode 100644
index 0000000..6ce1794
--- /dev/null
+++ b/aos/build/queues/output/q_struct.rb
@@ -0,0 +1,101 @@
+class Target::StructDec < Target::Node
+ attr_accessor :name,:loc,:parent, :extern
+ def initialize(name)
+ @name = name
+ @members = []
+ end
+ def add_member(member)
+ @members << member
+ end
+ def create(cpp_tree)
+ return self if(@extern)
+ orig_namespace = namespace = cpp_tree.get(@loc)
+ name = ""
+ if(namespace.class < Types::Type) #is nested
+ name = namespace.name + "_" + name
+ namespace = namespace.space
+ end
+ type_class = namespace.add_struct(name + @name)
+
+ @members.each do |elem|
+ type_class.add_member(elem.create_usage(cpp_tree))
+ end
+ return type_class
+ end
+ def size()
+ return @size if(@size)
+ @size = 0
+ @members.each do |elem|
+ @size += elem.size
+ end
+ return @size
+ end
+ def getPrintFormat()
+ return "{" + @members.collect { |elem| elem.toPrintFormat() }.join(", ") + "}"
+ end
+ def fetchPrintArgs(args, parent = "")
+ @members.each do |elem|
+ elem.fetchPrintArgs(args, parent)
+ end
+ end
+ def toHost(offset, suite, parent)
+ @members.each do |elem|
+ elem.toHost(offset, suite, parent)
+ offset += elem.size()
+ end
+ end
+ def toNetwork(offset, suite, parent)
+ @members.each do |elem|
+ elem.toNetwork(offset, suite, parent)
+ offset += elem.size()
+ end
+ end
+ def zeroCall(suite, parent)
+ @members.each do |elem|
+ elem.zeroCall(suite, parent)
+ end
+ end
+end
+class Target::MessageStructElement < Target::Node
+ attr_accessor :name,:loc
+ def initialize(type,name)
+ @type, @name = type, name
+ end
+ def type()
+ return @type.get_name
+ end
+ def type_name(cpp_tree)
+ type = cpp_tree.get(@type)
+ if(@type.loc == @loc) #use relative name
+ return type.name
+ else #use full name
+ return @type.loc.to_cpp_id(type.name)
+ end
+ end
+ def size()
+ return @type.size()
+ end
+ def toPrintFormat()
+ @type.getPrintFormat()
+ end
+ def create_usage(cpp_tree)
+ return "#{type_name(cpp_tree)} #{@name}"
+ end
+ def fetchPrintArgs(args, parent = "")
+ @type.fetchPrintArgs(args, parent + "#{@name}.")
+ end
+ def toNetwork(offset,suite, parent = "")
+ @type.toNetwork(offset, suite, parent + "#{@name}.")
+ end
+ def toHost(offset,suite, parent = "")
+ @type.toHost(offset, suite, parent + "#{@name}.")
+ end
+ def set_message_builder(suite)
+ suite << "msg_ptr_->#{@name} = #{@name}"
+ end
+
+ def zeroCall(suite, parent = "")
+ @type.zeroCall(suite, parent + "#{@name}.")
+ end
+
+end
diff --git a/aos/build/queues/q_specs/nested_structs.q b/aos/build/queues/q_specs/nested_structs.q
new file mode 100644
index 0000000..7162620
--- /dev/null
+++ b/aos/build/queues/q_specs/nested_structs.q
@@ -0,0 +1,12 @@
+package aos.test;
+
+import "q_specs/struct_test.q";
+
+
+message Position {
+ int32_t a;
+ int32_t[2] b;
+ .aos.test2.ClawHalfPosition top;
+ .aos.test2.ClawHalfPosition bottom;
+ bool c;
+};
diff --git a/aos/build/queues/q_specs/struct_queue_group.q b/aos/build/queues/q_specs/struct_queue_group.q
new file mode 100644
index 0000000..2bc5046
--- /dev/null
+++ b/aos/build/queues/q_specs/struct_queue_group.q
@@ -0,0 +1,20 @@
+package aos.test;
+
+struct Claw {
+ int32_t a;
+ int32_t b;
+};
+
+ message HPosition {
+ Claw top;
+ };
+
+// All angles here are 0 horizontal, positive up.
+queue_group ClawGroup {
+
+ message Position {
+ Claw top;
+ };
+ queue Position kjks;
+};
+
diff --git a/aos/build/queues/q_specs/struct_test.q b/aos/build/queues/q_specs/struct_test.q
new file mode 100644
index 0000000..afb22ac
--- /dev/null
+++ b/aos/build/queues/q_specs/struct_test.q
@@ -0,0 +1,12 @@
+package aos.test2;
+
+struct SubStruct {
+ int32_t g;
+};
+
+struct ClawHalfPosition {
+ double position;
+ bool hall_effect;
+ SubStruct clampy;
+};
+
diff --git a/aos/config/configure-fitpc.txt b/aos/config/configure-fitpc.txt
deleted file mode 100644
index cb19d7a..0000000
--- a/aos/config/configure-fitpc.txt
+++ /dev/null
@@ -1,58 +0,0 @@
-This file contains notes on setting up a new fit-pc image.
-Doing it right requires knowing what you're doing as a Debian sysadmin, so the
-main goal of this file is to avoid forgetting anything.
-
-Brian last updated this on 2013-08-28 with Debian Wheezy.
-
-1. Install Debian stable.
- It will need firmware during the installation.
- I partitioned it with (in order) a 0.5G ext2 /boot, a 2G swap, a 3.0G xfs
- /, and then an xfs /home on the rest of the SSD.
- Create 1 user named "driver".
- Select only "SSH Server" and
- "Basic System Utilities"(or something like that) in tasksel.
-2. Install aos.conf and starter.
- I just changed aos.conf to give driver permissions instead of the group.
-3. Make exim4 start faster.
- `dpkg-reconfigure exim4-config` and select the option for making minimal
- DNS queries (it also says some junk about modems).
-4. Configure the network.
- Edit /etc/network/interfaces to give it the right static IP address.
- Set up eth1 like the default eth0 in case the NIC gets assigned that (see
- #8 below). That shouldn't be a problem any more, but it's probably good
- to be on the safe side because it can be a pain to find a screen to fix
- it otherwise.
-5. Install stuff.
- firmware-linux-nonfree is always a good one to get.
- Remember to add
- <http://robotics.mvla.net/files/frc971/packages/frc971.list>.
- Besides the custom linux-image package, you'll figure everything else out
- as you start trying to run stuff.
-6. Make SSH logins faster.
- Add the line "UseDNS no" to /etc/ssh/sshd_config.
-7. Make it so that the programming team can log in without passwords.
- Everybody will have to run `ssh-copy-id -i ~/.ssh/id_rsa.pub fitpc` (see
- <http://www.debian-administration.org/articles/152> for details).
-8. Make udev to stop being annoying and naming NICs eth1.
- udev want to remember the ethernet NIC from each device and name the one
- in a new fitpc eth1, which breaks stuff. If that happens, removing
- /etc/udev/rules.d/70-persistent-net.rules will make it autogenerate a
- new one and fix it.
- To prevent this problem from happening in the first place, follow the
- directions in 10-net-eth0.rules.
-9. Download the code!
- In order for it to actually run, you'll have to
- `mkdir -p ~driver/tmp/robot_logs`.
-50. Clone the image to the rest of the disks.
- I've had good luck with `dd if=/dev/sdx of=/dev/sdx bs=16384` before, but
- that didn't work this time...
- Using gparted's copy/paste feature for each partition worked this time.
- A bug in the gparted version I used meant that it wouldn't copy an XFS
- partition to a smaller destination, so I had to manually mount both of
- them and `xfsdump -J - /path/to/src | xfsrestore -J - /path/to/dest`.
- Doing it that way changes the UUIDs on the XFS partitions.
- To deal with that, I edited the UUIDs in the /etc/fstab of the clone.
- I also had to manually install grub on the clone, which meant mounting
- the clone's /boot, `grub-install --boot-directory=bla /dev/sdx`, and
- then using sed to switch the UUIDs in the clone's
- /boot/grub/grub.cfg.
diff --git a/aos/config/configure-prime.txt b/aos/config/configure-prime.txt
new file mode 100644
index 0000000..87b2818
--- /dev/null
+++ b/aos/config/configure-prime.txt
@@ -0,0 +1,82 @@
+This file contains notes on setting up a new BBB image.
+Doing it right requires knowing what you're doing as a Debian sysadmin, so the
+main goal of this file is to avoid forgetting anything.
+
+Daniel last updated this on 2014-01-20 with Debian Wheezy.
+
+1. Install Debian stable.
+ Follow the instructions here:
+ http://blogs.bu.edu/mhirsch/2013/11/install-debian-7-to-emmc-internal-flash-drive-of-beaglebone-black/
+ Create a "driver" user.
+2. Install aos.conf and starter.
+ I just changed aos.conf to give driver permissions instead of the group.
+3. Install and configure exim4.
+ TODO (daniel): We might not need this.
+ `apt-get install exim4-config`
+ `dpkg-reconfigure exim4-config` and select the option for making minimal
+ DNS queries (it also says some junk about modems).
+4. Configure the network.
+ Edit /etc/network/interfaces to give it the right static IP address.
+ Set up eth1 like the default eth0 in case the NIC gets assigned that (see
+ #8 below). That shouldn't be a problem any more, but it's probably good
+ to be on the safe side because it can be a pain to find a screen to fix
+ it otherwise.
+5. Install stuff.
+ This includes the realtime kernel.
+ Note that at the time of this writing, you can't apt-get install the
+ realtime kernel packages directly on the beaglebone.
+ You must download them on your computer, copy them to the beaglebone,
+ and install them using dpkg -i.
+ <http://robotics.mvla.net/files/frc971/packages/>.
+ After you do this, you will still need to modify the zImage
+ symlink to point to the right kernel.
+ `rm /boot/zImage`
+ `ln -s /boot/vmlinuz-3.8.13.9-rt20+ /boot/zImage`
+ After you reboot, you should be running the rt kernel.
+ (Check it with `uname -r`.)
+ Besides the realtime kernel packages, you'll figure everything else out
+ as you start trying to run stuff.
+6. Make SSH logins faster.
+ Add the line "UseDNS no" to /etc/ssh/sshd_config.
+7. Make it so that the programming team can log in without passwords.
+ Everybody will have to run `ssh-copy-id -i ~/.ssh/id_rsa.pub BBB` (see
+ <http://www.debian-administration.org/articles/152> for details).
+8. Make udev stop being annoying and naming NICs eth1.
+ udev wants to remember the ethernet NIC from each device and name the one
+ in a new BBB eth1, which breaks stuff. If that happens, removing
+ /etc/udev/rules.d/70-persistent-net.rules will make it autogenerate a
+ new one and fix it.
+ To prevent this problem from happening in the first place, follow the
+ directions in 10-net-eth0.rules.
+9. Set up /etc/fstab sanely.
+ Open /etc/fstab on the bbb and remove the last two lines, which the
+ comments indicate mount both partitions on the uSD card.
+ Because the uSD card shows up as /dev/mmcblk0 and pushes the internal emmc
+ to /dev/mmcblk1 when it's plugged in, using those names for the two of them
+ doesn't work very well. Instead, we use /dev/disk/by-path.
+ Add "/dev/disk/by-path/platform-mmc.14-part1 /boot/uboot msdos defaults 0 2"
+ to mount the uboot partition.
+ Also add
+ "/dev/disk/by-path/platform-mmc.5-part1 /home/driver/tmp/robot_logs ext4 defaults,noatime,data=writeback 0 0"
+ to mount the uSD card in the right place for the logs to go to it.
+10. Set up logging.
+ Fairly straightforward here. We want it to log to the uSD card, so:
+ `mkdir ~/tmp`
+ `mkdir /media/driver/sdcard/robot_logs`
+ `ln -s /media/driver/sdcard/robot_logs ~/tmp/robot_logs`
+11. Set the correct date.
+ `date` to check if date is correct.
+ `date -s <date string>` to set it if it isn't.
+12. Make it export UART1 on boot.
+ Add the following to /boot/uboot/uenv.txt:
+ "optargs=capemgr.enable_partno=BB-UART1"
+13. Fix the locale setup for SSHing in.
+ `dpkg-reconfigure locales`, leave it with "en_US.UTF-8" only being
+ enabled, and then select "None" instead of that for the default in
+ the second screen.
+30. Download the code!
+50. Clone the image to the rest of the BBBs.
+ Boot up from a uSD card.
+ `dd if=/dev/mmcblk1 | gzip -c > BBB.img.gz`
+ You can then copy this image to your computer.
+ Use this image to flash other uSD cards and/or BBBs.
diff --git a/aos/linux_code/configuration.cc b/aos/linux_code/configuration.cc
index a8cc4ae..e5e5583 100644
--- a/aos/linux_code/configuration.cc
+++ b/aos/linux_code/configuration.cc
@@ -20,7 +20,8 @@
// Including the terminating '\0'.
const size_t kMaxAddrLength = 18;
-// TODO(brians): Don't hard-code this.
+// TODO(brians): This shouldn't be necesary for running tests. Provide a way to
+// set the IP address when running tests from the test.
const char *const kLinuxNetInterface = "eth0";
const in_addr *DoGetOwnIPAddress() {
static const char *kOverrideVariable = "FRC971_IP_OVERRIDE";
@@ -46,8 +47,8 @@
// but it does do a very nice job of making sure that addrs gets freed.
unique_c_ptr<ifaddrs, freeifaddrs> addrs_deleter(addrs);
- for (; addrs != NULL; addrs = addrs->ifa_next) {
- if (addrs->ifa_addr->sa_family == AF_INET) {
+ for (; addrs != nullptr; addrs = addrs->ifa_next) {
+ if (addrs->ifa_addr != nullptr && addrs->ifa_addr->sa_family == AF_INET) {
if (strcmp(kLinuxNetInterface, addrs->ifa_name) == 0) {
static const in_addr r =
reinterpret_cast<sockaddr_in *>(__builtin_assume_aligned(
diff --git a/aos/linux_code/starter/starter.sh b/aos/linux_code/starter/starter.sh
index 33906c3..62338af 100755
--- a/aos/linux_code/starter/starter.sh
+++ b/aos/linux_code/starter/starter.sh
@@ -17,4 +17,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; /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`; done' &
diff --git a/bbb_cape/src/bbb/cape_manager.cc b/bbb_cape/src/bbb/cape_manager.cc
index d9b1e28..923021b 100644
--- a/bbb_cape/src/bbb/cape_manager.cc
+++ b/bbb_cape/src/bbb/cape_manager.cc
@@ -22,14 +22,14 @@
}
void CapeManager::DoReset(bool bootloader) {
- static constexpr ::aos::time::Time kTimeout =
+ static constexpr ::aos::time::Time kWaitTime =
::aos::time::Time::InSeconds(0.1);
reset_.Set(false);
- ::aos::time::SleepFor(kTimeout);
- custom_bootloader_.Set(bootloader);
- ::aos::time::SleepFor(kTimeout);
+ ::aos::time::SleepFor(kWaitTime);
+ custom_bootloader_.Set(!bootloader);
+ ::aos::time::SleepFor(kWaitTime);
reset_.Set(true);
- ::aos::time::SleepFor(kTimeout);
+ ::aos::time::SleepFor(kWaitTime);
}
} // namespace bbb
diff --git a/bbb_cape/src/bbb/gpios.cc b/bbb_cape/src/bbb/gpios.cc
index 0aca731..3584d06 100644
--- a/bbb_cape/src/bbb/gpios.cc
+++ b/bbb_cape/src/bbb/gpios.cc
@@ -48,6 +48,15 @@
LOG(WARNING, "fclose(%p) failed with %d: %s\n", direction_handle, errno,
strerror(errno));
}
+
+ char value_path[64];
+ snprintf(value_path, sizeof(value_path),
+ "/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",
+ value_path, errno, strerror(errno));
+ }
}
GpioPin::~GpioPin() {
diff --git a/bbb_cape/src/bbb/gpo.cc b/bbb_cape/src/bbb/gpo.cc
index f02ded3..d17abe7 100644
--- a/bbb_cape/src/bbb/gpo.cc
+++ b/bbb_cape/src/bbb/gpo.cc
@@ -3,6 +3,7 @@
#include <stdio.h>
#include <errno.h>
#include <string.h>
+#include <unistd.h>
#include "aos/common/logging/logging.h"
@@ -12,11 +13,13 @@
: GpioPin(bank, pin, false, initial_value) {}
void Gpo::Set(bool high) {
- rewind(value_handle_);
- if (fputc(high ? '1' : '0', value_handle_) < 0) {
+ // TODO(brians): Figure out why this breaks it.
+ //rewind(value_handle_);
+ if (fputc(high ? '1' : '0', value_handle_) == EOF) {
LOG(FATAL, "fputc(%c, %p) for pin (%d,%d) failed with %d: %s\n",
high ? '1': '0', value_handle_, bank_, pin_, errno, strerror(errno));
}
+ sync();
}
} // namespace bbb
diff --git a/bbb_cape/src/bbb/hex_byte_reader.cc b/bbb_cape/src/bbb/hex_byte_reader.cc
index 968e771..7c6a28f 100644
--- a/bbb_cape/src/bbb/hex_byte_reader.cc
+++ b/bbb_cape/src/bbb/hex_byte_reader.cc
@@ -24,6 +24,7 @@
HexByteReader::HexByteReader(const ::std::string &filename)
: parser_status_(kParser.init()) {
+ LOG(DEBUG, "reading hex file %s\n", filename.c_str());
if (parser_status_ == NULL) {
LOG(FATAL, "%s parser failed to initialize.\n", kParser.name);
}
diff --git a/bbb_cape/src/bbb/packet_finder_test.cc b/bbb_cape/src/bbb/packet_finder_test.cc
index 1b8c813..ccd81e5 100644
--- a/bbb_cape/src/bbb/packet_finder_test.cc
+++ b/bbb_cape/src/bbb/packet_finder_test.cc
@@ -160,5 +160,18 @@
}
}
+// Tests to make sure that bitwise-NOTing any byte will result in missing that
+// packet and no future ones.
+TEST_F(PacketFinderTest, InvertAnyByte) {
+ static constexpr auto kTestData = kTestData1;
+ for (int i = 0; i < static_cast<int>(kTestData.size()); ++i) {
+ SCOPED_TRACE("inverting byte " + ::std::to_string(i));
+ ::std::array<uint8_t, kTestData.size()> data;
+ ::std::copy(kTestData.begin(), kTestData.end(), data.begin());
+ data.at(i) ^= 0xFF;
+ ReceivePackets(data, 7, ::std::array<int, 1>{{i / 148 + 1}});
+ }
+}
+
} // namespace testing
} // namespace bbb
diff --git a/bbb_cape/src/bbb/sensor_reader.cc b/bbb_cape/src/bbb/sensor_reader.cc
index 13d6add..2e40903 100644
--- a/bbb_cape/src/bbb/sensor_reader.cc
+++ b/bbb_cape/src/bbb/sensor_reader.cc
@@ -33,13 +33,13 @@
}
const DataStruct *SensorReader::ReadPacket() {
- static constexpr ::aos::time::Time kTimeout =
- ::aos::time::Time::InSeconds(0.5);
+ static constexpr ::aos::time::Time kResetTimeout =
+ ::aos::time::Time::InSeconds(2.5);
while (true) {
- ::aos::time::Time next_timeout = last_received_time_ + kTimeout;
+ ::aos::time::Time next_timeout = last_received_time_ + kResetTimeout;
if (next_timeout <= ::aos::time::Time::Now()) {
- LOG(WARNING, "too long since good packet received\n");
+ LOG(WARNING, "Too long since good packet received. Resetting.\n");
manager_.Reset();
ResetHappened();
}
diff --git a/bbb_cape/src/flasher/flasher.gyp b/bbb_cape/src/flasher/flasher.gyp
index dea4e5c..2d506b8 100644
--- a/bbb_cape/src/flasher/flasher.gyp
+++ b/bbb_cape/src/flasher/flasher.gyp
@@ -16,6 +16,8 @@
'dependencies': [
'<(EXTERNALS):stm32flash',
'<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:gpios',
+ '<(AOS)/common/common.gyp:time',
],
},
],
diff --git a/bbb_cape/src/flasher/stm32_flasher.cc b/bbb_cape/src/flasher/stm32_flasher.cc
index ddddb1b..0f081f5 100644
--- a/bbb_cape/src/flasher/stm32_flasher.cc
+++ b/bbb_cape/src/flasher/stm32_flasher.cc
@@ -8,6 +8,7 @@
#include "aos/common/logging/logging.h"
#include "aos/common/logging/logging_impl.h"
+#include "aos/common/time.h"
extern "C" {
#include "stm32flash/parsers/parser.h"
@@ -17,11 +18,26 @@
#include "stm32flash/init.h"
}
+#include "bbb/gpo.h"
+
int main(int argc, char **argv) {
::aos::logging::Init();
::aos::logging::AddImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
+ {
+ ::bbb::Gpo reset(2, 5, true);
+ ::bbb::Gpo bootloader(2, 2, false);
+ static constexpr ::aos::time::Time kWaitTime =
+ ::aos::time::Time::InSeconds(0.1);
+ reset.Set(false);
+ ::aos::time::SleepFor(kWaitTime);
+ bootloader.Set(false);
+ ::aos::time::SleepFor(kWaitTime);
+ reset.Set(true);
+ ::aos::time::SleepFor(kWaitTime);
+ }
+
if (argc < 2) {
fputs("Need an argument saying which target to download.\n", stderr);
return 1;
diff --git a/frc971/constants.cc b/frc971/constants.cc
index e6a7aa5..7541f7c 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -67,6 +67,15 @@
true,
control_loops::MakeVClutchDrivetrainLoop,
control_loops::MakeClutchDrivetrainLoop,
+ 0.5,
+ 0.1,
+ 0.0,
+ 1.57,
+
+ {0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ {0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ 0.02, // claw_unimportant_epsilon
+ 50505.05, // start_fine_tune_pos
};
break;
case kPracticeTeamNumber:
@@ -86,6 +95,14 @@
false,
control_loops::MakeVDogDrivetrainLoop,
control_loops::MakeDogDrivetrainLoop,
+ 0.5,
+ 0.1,
+ 0.0,
+ 1.57,
+ {0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ {0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
+ 0.02, // claw_unimportant_epsilon
+ 50505.05, //start_fine_tune_pos
};
break;
default:
diff --git a/frc971/constants.h b/frc971/constants.h
index 07cf04f..a3b818e 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -47,6 +47,34 @@
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
+
+ double claw_zeroing_off_speed;
+ double claw_zeroing_speed;
+
+ // claw seperation that would be considered a collision
+ double claw_min_seperation;
+ double claw_max_seperation;
+
+ // Three hall effects are known as front, calib and back
+ struct AnglePair {
+ double lower_angle;
+ double upper_angle;
+ };
+
+ struct Claw {
+ double lower_limit;
+ double upper_limit;
+ AnglePair front;
+ AnglePair calibration;
+ AnglePair back;
+ };
+
+
+ Claw upper_claw;
+ Claw lower_claw;
+
+ double claw_unimportant_epsilon;
+ double start_fine_tune_pos;
};
// Creates (once) a Values instance and returns a reference to it.
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 61217fa..982c6e9 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -93,7 +93,9 @@
void SetRawPosition(double left, double right) {
_raw_right = right;
_raw_left = left;
- loop_->Y << left, right;
+ Eigen::Matrix<double, 2, 1> Y;
+ Y << left, right;
+ loop_->Correct(Y);
}
void SetPosition(
double left, double right, double gyro, bool control_loop_driving) {
@@ -108,8 +110,8 @@
SetRawPosition(left, right);
}
- void Update(bool update_observer, bool stop_motors) {
- loop_->Update(update_observer, stop_motors);
+ void Update(bool stop_motors) {
+ loop_->Update(stop_motors);
}
void SendMotors(Drivetrain::Output *output) {
@@ -569,155 +571,6 @@
constexpr double PolyDrivetrain::Kt;
-
-class DrivetrainMotorsOL {
- public:
- DrivetrainMotorsOL() {
- _old_wheel = 0.0;
- wheel_ = 0.0;
- throttle_ = 0.0;
- quickturn_ = false;
- highgear_ = true;
- _neg_inertia_accumulator = 0.0;
- _left_pwm = 0.0;
- _right_pwm = 0.0;
- }
- void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
- wheel_ = wheel;
- throttle_ = throttle;
- quickturn_ = quickturn;
- highgear_ = highgear;
- _left_pwm = 0.0;
- _right_pwm = 0.0;
- }
- void Update() {
- double overPower;
- float sensitivity = 1.7;
- float angular_power;
- float linear_power;
- double wheel;
-
- double neg_inertia = wheel_ - _old_wheel;
- _old_wheel = wheel_;
-
- double wheelNonLinearity;
- if (highgear_) {
- wheelNonLinearity = 0.1; // used to be csvReader->TURN_NONLIN_HIGH
- // Apply a sin function that's scaled to make it feel better.
- const double angular_range = M_PI / 2.0 * wheelNonLinearity;
- wheel = sin(angular_range * wheel_) / sin(angular_range);
- wheel = sin(angular_range * wheel) / sin(angular_range);
- } else {
- wheelNonLinearity = 0.2; // used to be csvReader->TURN_NONLIN_LOW
- // Apply a sin function that's scaled to make it feel better.
- const double angular_range = M_PI / 2.0 * wheelNonLinearity;
- wheel = sin(angular_range * wheel_) / sin(angular_range);
- wheel = sin(angular_range * wheel) / sin(angular_range);
- wheel = sin(angular_range * wheel) / sin(angular_range);
- }
-
- static const double kThrottleDeadband = 0.05;
- if (::std::abs(throttle_) < kThrottleDeadband) {
- throttle_ = 0;
- } else {
- throttle_ = copysign((::std::abs(throttle_) - kThrottleDeadband) /
- (1.0 - kThrottleDeadband), throttle_);
- }
-
- double neg_inertia_scalar;
- if (highgear_) {
- neg_inertia_scalar = 8.0; // used to be csvReader->NEG_INTERTIA_HIGH
- sensitivity = 1.22; // used to be csvReader->SENSE_HIGH
- } else {
- if (wheel * neg_inertia > 0) {
- neg_inertia_scalar = 5; // used to be csvReader->NEG_INERTIA_LOW_MORE
- } else {
- if (::std::abs(wheel) > 0.65) {
- neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS_EXT
- } else {
- neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS
- }
- }
- sensitivity = 1.24; // used to be csvReader->SENSE_LOW
- }
- double neg_inertia_power = neg_inertia * neg_inertia_scalar;
- _neg_inertia_accumulator += neg_inertia_power;
-
- wheel = wheel + _neg_inertia_accumulator;
- if (_neg_inertia_accumulator > 1) {
- _neg_inertia_accumulator -= 1;
- } else if (_neg_inertia_accumulator < -1) {
- _neg_inertia_accumulator += 1;
- } else {
- _neg_inertia_accumulator = 0;
- }
-
- linear_power = throttle_;
-
- if (quickturn_) {
- double qt_angular_power = wheel;
- if (::std::abs(linear_power) < 0.2) {
- if (qt_angular_power > 1) qt_angular_power = 1.0;
- if (qt_angular_power < -1) qt_angular_power = -1.0;
- } else {
- qt_angular_power = 0.0;
- }
- overPower = 1.0;
- if (highgear_) {
- sensitivity = 1.0;
- } else {
- sensitivity = 1.0;
- }
- angular_power = wheel;
- } else {
- overPower = 0.0;
- angular_power = ::std::abs(throttle_) * wheel * sensitivity;
- }
-
- _right_pwm = _left_pwm = linear_power;
- _left_pwm += angular_power;
- _right_pwm -= angular_power;
-
- if (_left_pwm > 1.0) {
- _right_pwm -= overPower*(_left_pwm - 1.0);
- _left_pwm = 1.0;
- } else if (_right_pwm > 1.0) {
- _left_pwm -= overPower*(_right_pwm - 1.0);
- _right_pwm = 1.0;
- } else if (_left_pwm < -1.0) {
- _right_pwm += overPower*(-1.0 - _left_pwm);
- _left_pwm = -1.0;
- } else if (_right_pwm < -1.0) {
- _left_pwm += overPower*(-1.0 - _right_pwm);
- _right_pwm = -1.0;
- }
- }
-
- void SendMotors(Drivetrain::Output *output) {
- LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
- _left_pwm, _right_pwm, wheel_, throttle_);
- if (output) {
- output->left_voltage = _left_pwm * 12.0;
- output->right_voltage = _right_pwm * 12.0;
- }
- if (highgear_) {
- shifters.MakeWithBuilder().set(false).Send();
- } else {
- shifters.MakeWithBuilder().set(true).Send();
- }
- }
-
- private:
- double _old_wheel;
- double wheel_;
- double throttle_;
- bool quickturn_;
- bool highgear_;
- double _neg_inertia_accumulator;
- double _left_pwm;
- double _right_pwm;
-};
-
void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
const Drivetrain::Position *position,
Drivetrain::Output *output,
@@ -755,7 +608,7 @@
}
}
dt_openloop.SetPosition(position);
- dt_closedloop.Update(position, output == NULL);
+ dt_closedloop.Update(output == NULL);
dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
if (control_loop_driving) {
diff --git a/frc971/control_loops/python/wrists.py b/frc971/control_loops/python/claw.py
similarity index 63%
rename from frc971/control_loops/python/wrists.py
rename to frc971/control_loops/python/claw.py
index d752000..3d6b9fc 100755
--- a/frc971/control_loops/python/wrists.py
+++ b/frc971/control_loops/python/claw.py
@@ -5,9 +5,9 @@
import sys
from matplotlib import pylab
-class Wrist(control_loop.ControlLoop):
- def __init__(self, name="RawWrist"):
- super(Wrist, self).__init__(name)
+class Claw(control_loop.ControlLoop):
+ def __init__(self, name="RawClaw"):
+ super(Claw, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 1.4
# Stall Current in Amps
@@ -16,7 +16,7 @@
self.free_speed = 6200.0
# Free Current in Amps
self.free_current = 1.5
- # Moment of inertia of the wrist in kg m^2
+ # Moment of inertia of the claw in kg m^2
# TODO(aschuh): Measure this in reality. It doesn't seem high enough.
# James measured 0.51, but that can't be right given what I am seeing.
self.J = 2.0
@@ -58,9 +58,9 @@
self.InitializeState()
-class WristDeltaU(Wrist):
- def __init__(self, name="Wrist"):
- super(WristDeltaU, self).__init__(name)
+class ClawDeltaU(Claw):
+ def __init__(self, name="Claw"):
+ super(ClawDeltaU, self).__init__(name)
A_unaugmented = self.A
B_unaugmented = self.B
@@ -97,58 +97,74 @@
self.InitializeState()
-def ClipDeltaU(wrist, delta_u):
- old_u = numpy.matrix([[wrist.X[2, 0]]])
- new_u = numpy.clip(old_u + delta_u, wrist.U_min, wrist.U_max)
+def ClipDeltaU(claw, delta_u):
+ old_u = numpy.matrix([[claw.X[2, 0]]])
+ new_u = numpy.clip(old_u + delta_u, claw.U_min, claw.U_max)
return new_u - old_u
def main(argv):
# Simulate the response of the system to a step input.
- wrist = WristDeltaU()
+ claw = ClawDeltaU()
simulated_x = []
for _ in xrange(100):
- wrist.Update(numpy.matrix([[12.0]]))
- simulated_x.append(wrist.X[0, 0])
+ claw.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(claw.X[0, 0])
pylab.plot(range(100), simulated_x)
pylab.show()
# Simulate the closed loop response of the system to a step input.
- wrist = WristDeltaU()
+ top_claw = ClawDeltaU("TopClaw")
close_loop_x = []
close_loop_u = []
R = numpy.matrix([[1.0], [0.0], [0.0]])
- wrist.X[2, 0] = -5
+ top_claw.X[2, 0] = -5
for _ in xrange(100):
- U = numpy.clip(wrist.K * (R - wrist.X_hat), wrist.U_min, wrist.U_max)
- U = ClipDeltaU(wrist, U)
- wrist.UpdateObserver(U)
- wrist.Update(U)
- close_loop_x.append(wrist.X[0, 0] * 10)
- close_loop_u.append(wrist.X[2, 0])
+ U = numpy.clip(top_claw.K * (R - top_claw.X_hat), top_claw.U_min, top_claw.U_max)
+ U = ClipDeltaU(top_claw, U)
+ top_claw.UpdateObserver(U)
+ top_claw.Update(U)
+ close_loop_x.append(top_claw.X[0, 0] * 10)
+ close_loop_u.append(top_claw.X[2, 0])
pylab.plot(range(100), close_loop_x)
pylab.plot(range(100), close_loop_u)
pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 5:
+ if len(argv) != 9:
print "Expected .h file name and .cc file name for"
print "both the plant and unaugmented plant"
else:
- unaug_wrist = Wrist("RawWrist")
- unaug_loop_writer = control_loop.ControlLoopWriter("RawWrist",
- [unaug_wrist])
- if argv[3][-3:] == '.cc':
- unaug_loop_writer.Write(argv[4], argv[3])
- else:
- unaug_loop_writer.Write(argv[3], argv[4])
-
- loop_writer = control_loop.ControlLoopWriter("Wrist", [wrist])
+ top_unaug_claw = Claw("RawTopClaw")
+ top_unaug_loop_writer = control_loop.ControlLoopWriter("RawTopClaw",
+ [top_unaug_claw])
if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
+ top_unaug_loop_writer.Write(argv[2], argv[1])
else:
- loop_writer.Write(argv[1], argv[2])
+ top_unaug_loop_writer.Write(argv[1], argv[2])
+
+ top_loop_writer = control_loop.ControlLoopWriter("TopClaw", [top_claw])
+ if argv[3][-3:] == '.cc':
+ top_loop_writer.Write(argv[4], argv[3])
+ else:
+ top_loop_writer.Write(argv[3], argv[4])
+
+ bottom_claw = ClawDeltaU("BottomClaw")
+ bottom_unaug_claw = Claw("RawBottomClaw")
+ bottom_unaug_loop_writer = control_loop.ControlLoopWriter(
+ "RawBottomClaw", [bottom_unaug_claw])
+ if argv[5][-3:] == '.cc':
+ bottom_unaug_loop_writer.Write(argv[6], argv[5])
+ else:
+ bottom_unaug_loop_writer.Write(argv[5], argv[6])
+
+ bottom_loop_writer = control_loop.ControlLoopWriter("BottomClaw",
+ [bottom_claw])
+ if argv[7][-3:] == '.cc':
+ bottom_loop_writer.Write(argv[8], argv[7])
+ else:
+ bottom_loop_writer.Write(argv[7], argv[8])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
old mode 100644
new mode 100755
index 802827a..5c6548c
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -13,9 +13,38 @@
namespace frc971 {
namespace control_loops {
+
+void ZeroedStateFeedbackLoop::CapU() {
+ const double old_voltage = voltage_;
+ voltage_ += U(0, 0);
+
+ uncapped_voltage_ = voltage_;
+
+ double limit = zeroing_state() != UNKNOWN_POSITION ? 12.0 : kZeroingMaxVoltage;
+
+ // Make sure that reality and the observer can't get too far off. There is a
+ // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
+ // against last cycle's voltage.
+ if (X_hat(2, 0) > last_voltage_ + 2.0) {
+ voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
+ LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+ } else if (X_hat(2, 0) < last_voltage_ -2.0) {
+ voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
+ LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+ }
+
+ voltage_ = std::min(limit, voltage_);
+ voltage_ = std::max(-limit, voltage_);
+ U(0, 0) = voltage_ - old_voltage;
+ LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
+ LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));
+
+ last_voltage_ = voltage_;
+}
+
ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
: aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
- zeroed_joint_(MakeShooterLoop()) {
+ shooter_(MakeShooterLoop()) {
{
using ::frc971::constants::GetValues;
ZeroedJoint<1>::ConfigurationData config_data;
@@ -33,34 +62,240 @@
}
}
-// Positive is up, and positive power is up.
+
+// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
const ShooterLoop::Goal *goal,
const control_loops::ShooterLoop::Position *position,
- ShooterLoop::Output *output,
- ShooterLoop::Status * status) {
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status * status) {
+ constexpr double dt = 0.01;
+
+ // we must always have these or we have issues.
+ if (goal == NULL || status == NULL) {
+ transform-position_ptr = NULL;
+ if (output) output->voltage = 0;
+ LOG(ERROR, "Thought I would just check for null and die.\n");
+ return;
+ }
// Disable the motors now so that all early returns will return with the
// motors disabled.
- if (output) {
- output->voltage = 0;
- }
+ if (output) output->voltage = 0;
ZeroedJoint<1>::PositionData transformed_position;
ZeroedJoint<1>::PositionData *transformed_position_ptr =
&transformed_position;
- if (!position) {
- transformed_position_ptr = NULL;
- } else {
- //transformed_position.position = position->pos;
- //transformed_position.hall_effects[0] = position->hall_effect;
- //transformed_position.hall_effect_positions[0] = position->calibration;
+ if (position) {
+ transformed_position.position = position->pos;
+ transformed_position.hall_effects[0] = position->hall_effect;
+ transformed_position.hall_effect_positions[0] = position->calibration;
}
- const double voltage = zeroed_joint_.Update(transformed_position_ptr,
+ const double voltage = shooter_.Update(transformed_position_ptr,
output != NULL,
goal->goal, 0.0);
+ const frc971::constants::Values &values = constants::GetValues();
+
+ double absolute_position = postiion->position - calibration_position_;
+
+
+ switch (state_) {
+ case STATE_INITIALIZE:
+ shooter_.zeroing_state() = ZeroedStateFeedbackLoop::UNKNOWN_POSITION;
+
+ // start off with the assumption that we are at the value
+ // futhest back given our sensors
+ if (position && position->pusher_distal_hall_effect){
+ calibration_position_ = position->position -
+ values.pusher_distal_heffect.lower_edge;
+ } else if (position && position->pusher_proximal_hall_effect) {
+ calibration_position_ = position->position -
+ values.pusher_proximal_heffect.lower_edge;
+ } else {
+ calibration_position_ = values.shooter_total_length;
+ }
+
+ state_ = STATE_REQUEST_LOAD;
+
+ // zero out initial goal
+ shooter_.SetGoalPositionVelocity(0.0, 0.0);
+ if (position) {
+ output->latch_pistion = position->plunger_back_hall_effect;
+ } else {
+ // we don't know what is going on so just close the latch to be safe
+ output->latch_piston = true;
+ }
+ output->brake_piston = false;
+ break;
+ case STATE_REQUEST_LOAD:
+ if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ // already latched
+ state_ = STATE_PREPARE_SHOT;
+ } else if (postion->pusher_back_distal_hall_effect ||
+ (relative_position) < 0) {
+ state_ = STATE_LOADING_BACKTRACK;
+ if (relative_position) {
+ calibration_position_ = position->position;
+ }
+ } else {
+ state_ = STATE_LOAD;
+ }
+
+ shooter_.SetGoalPositionVelocity(0.0, 0.0);
+ if (position && output) output->latch_piston = position->plunger_back_hall_effect;
+ output->brake_piston = false;
+ break;
+ case STATE_LOAD_BACKTRACK:
+ if (absolute_position < values.pusher_back_distal_heffect.lower_edge + 0.01) {
+ shooter_.SetGoalPositionVelocity(position->position + values.shooter_zero_speed*dt,
+ values.shooter_zero_speed);
+ } else {
+ state = STATE_LOAD;
+ }
+
+ output->latch_piston = false;
+ output->brake_piston = true;
+ break;
+ case STATE_LOAD:
+ if (position->pusher_proximal_hall_effect &&
+ !last_position_.pusher_back_proximal_hall_effect) {
+ calibration_position_ = position->position -
+ values.pusher_promimal_heffect.lower_edge;
+ }
+ if (position->pusher_distal_hall_effect &&
+ !last_position_.pusher_back_distal_hall_effect) {
+ calibration_position_ = position->position -
+ values.pusher_distal_heffect.lower_edge;
+
+ }
+
+ shooter_.SetGoalPositionVelocity(calibration_position_, 0.0);
+ if (position && output) output->latch_piston = position->plunger_back_hall_effect;
+ if(output) output->brake_piston = false;
+
+ if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ state_ = STATE_PREPARE_SHOT;
+ } else if (position->plunger_back_hall_effect &&
+ position->position == PowerToPosition(goal->shot_power)) {
+ //TODO_ben: I'm worried it will bounce states if the position is drifting slightly
+ state_ = STATE_LOADING_PROBLEM;
+ loading_problem_end_time_ = clock() + 3 * CLOCKS_PER_SECOND;
+ }
+ break;
+ case STATE_LOADING_PROBLEM:
+ if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ state_ = STATE_PREPARE_SHOT;
+ } else if (absolute_position < -0.02 || clock() > loading_problem_end_time_) {
+ state = STATE_UNLOAD;
+ }
+
+ shooter_.SetGoalPositionVelocity(position->position - values.shooter_zero_speed*dt,
+ values.shooter_zero_speed);
+ if (output) output->latch_piston = true;
+ if (output) output->brake_piston = false;
+ break;
+ case STATE_PREPARE_SHOT:
+ shooter_.SetGoalPosition(
+ PowerToPosition(shot_power), 0.0);
+ if (position->position == shooter.goal_position) {
+ state_ = STATE_READY;
+ output->latch_piston = true;
+ output->brake_piston = true;
+ shooter_brake_set_time_ = clock() + 5 * CLOCKS_PER_SECOND;
+ } else {
+ output->latch_piston =true;
+ output->brake_piston = false;
+ }
+ break;
+ case STATE_READY:
+ if (clock() > shooter_brake_set_time_) {
+ shooter_loop_disable = true;
+ if (goal->unload_requested) {
+ state_ = STATE_UNLOAD;
+ } else if (PowerToPosition(goal->shot_power)
+ != position->position) {
+ //TODO_ben: I'm worried it will bounce states if the position is drifting slightly
+ state_ = STATE_PREPARE_SHOT;
+ }else if (goal->shot_requested) {
+ state_ = STATE_REQUEST_FIRE;
+ }
+
+ }
+ output->latch_piston = true;
+ output->brake_piston = true;
+ break;
+ case STATE_REQUEST_FIRE:
+ shooter_loop_disable = true;
+ if (position->plunger_back_hall_effect) {
+ prepare_fire_end_time_ = clock() + 10;
+ state_ = STATE_PREPARE_FIRE;
+ } else {
+ state_ = STATE_REQUEST_LOAD;
+ }
+ break;
+ case STATE_PREPARE_FIRE:
+ shooter_loop_disable = true;
+ if (clock() < prepare_fire_end_time_) {
+ shooter_.ApplySomeVoltage();
+ } else {
+ State_ = STATE_FIRE;
+ cycles_not_moved_ = 0;
+ shot_end_time_ = clock() + 0.5 * CLOCKS_PER_SECOND;
+ }
+
+ output->latch_piston = true;
+ output->brake_piston = true;
+
+ break;
+ case STATE_FIRE:
+ shooter_loop_disable = true;
+ //TODO_ben: need approamately equal
+ if (last_position->position - position->position < 7) {
+ cycles_not_moved++;
+ } else {
+ cycles_not_moved = 0;
+ }
+ output->latch_piston = true;
+ ouput->brake_piston = true;
+ break;
+ case STATE_UNLOAD:
+ if (position->plunger_back_hall_effect && position->latch_piston) {
+ shooter_SetGoalPosition(0.02, 0.0);
+ if (ablsolute_position == 0.02) {
+ output->latch_piston = false;
+ }
+ } else {
+ output->latch_piston = false;
+ state_ = STATE_UNLOAD_MOVE;
+ }
+
+ output->brake_piston = false;
+ break;
+ case STATE_UNLOAD_MOVE:
+ if (position->position > values.shooter_length - 0.03) {
+ shooter_.SetPosition(position->position, 0.0);
+ state_ = STATE_READY_UNLOADED;
+ } else {
+ shooter_.SetPosition(
+ position->position + values.shooter_zeroing_speed*dt
+ values.shooter_zeroing_speed);
+ }
+
+ output->latch_piston = false;
+ output->brake_piston = false;
+ break;
+ case STATE_READY_UNLOAD:
+ if (!goal->unload_requested) {
+ state_ = STATE_REQUEST_LOAD;
+ }
+
+ output->latch_piston = false;
+ output->brake_piston = false;
+ break;
+ }
+
if (position) {
LOG(DEBUG, "pos: hall: absolute: %f\n",
//position->pos,
@@ -68,9 +303,7 @@
zeroed_joint_.absolute_position());
}
- if (output) {
- output->voltage = voltage;
- }
+ output->voltage = voltage;
status->done = ::std::abs(zeroed_joint_.absolute_position() - goal->goal) < 0.004;
}
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
old mode 100644
new mode 100755
index 0bcc97a..30112e1
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -18,6 +18,119 @@
class ShooterTest_NoWindupNegative_Test;
};
+// Note: Everything in this file assumes that there is a 1 cycle delay between
+// power being requested and it showing up at the motor. It assumes that
+// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
+// that isn't true.
+
+// This class implements the CapU function correctly given all the extra
+// information that we know about from the wrist motor.
+// It does not have any zeroing logic in it, only logic to deal with a delta U
+// controller.
+class ZeroedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
+ public:
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop)
+ : StateFeedbackLoop<3, 1, 1>(loop),
+ voltage_(0.0),
+ last_voltage_(0.0),
+ uncapped_voltage_(0.0),
+ offset_(0.0),
+ encoder_(0.0),
+ last_encoder_(0.0){}
+
+ const static int kZeroingMaxVoltage = 5;
+
+ // Caps U, but this time respects the state of the wrist as well.
+ virtual void CapU();
+
+ // Returns the accumulated voltage.
+ double voltage() const { return voltage_; }
+
+ // Returns the uncapped voltage.
+ double uncapped_voltage() const { return uncapped_voltage_; }
+
+ // Zeros the accumulator.
+ void ZeroPower() { voltage_ = 0.0; }
+
+ enum JointZeroingState {
+ // We don't know where the joint is at all.
+ UNKNOWN_POSITION,
+ // Ready for use during teleop.
+ CALIBRATED
+ };
+
+
+ void set_zeroing_state(JointZeroingState zeroing_state) {
+ zeroing_state_ = zeroing_state;
+ }
+
+
+ JointZeroingState zeroing_state() const { return zeroing_state_; }
+
+
+ // Sets the calibration offset given the absolute angle and the corrisponding
+ // encoder value.
+ void SetCalibration(double encoder_val, double known_position) {
+ offset_ = known_position - encoder_val;
+ }
+
+
+ bool SetCalibrationOnEdge(const constants::Values::Claw &claw_values,
+ JointZeroingState zeroing_state) {
+ double edge_encoder;
+ double known_position;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &known_position)) {
+ LOG(INFO, "Calibration edge.\n");
+ SetCalibration(edge_encoder, known_position);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+ }
+
+
+ void SetPositionValues(double poistion) {
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << position;
+ Correct(Y);
+ }
+
+
+ void SetGoalPositionVelocity(double desired_position,
+ double desired_velocity) {
+ // austin said something about which matrix to set, but I didn't under
+ // very much of it
+ //some_matrix = {desired_position, desired_velocity};
+ printf("%s:%d : seg fault?\n", __FILE__, __LINE__);
+ *(const char **)(NULL) = "seg fault";
+ }
+
+ double position() const { return X_hat(0, 0); }
+ double encoder() const { return encoder_; }
+ double last_encoder() const { return last_encoder_; }
+
+ // Returns true if an edge was detected in the last cycle and then sets the
+ // edge_position to the absolute position of the edge.
+ bool GetPositionOfEdge(const constants::Values::Shooter &shooter,
+ double *edge_encoder, double *known_position);
+
+#undef COUNT_SETTER_GETTER
+
+ private:
+ // The accumulated voltage to apply to the motor.
+ double voltage_;
+ double last_voltage_;
+ double uncapped_voltage_;
+ double offset_;
+
+ double previous_position_;
+
+ JointZeroingState zeroing_state_;
+ double encoder_;
+ double last_encoder_;
+};
+
+
class ShooterMotor
: public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
public:
@@ -39,6 +152,23 @@
// True if the state machine is ready.
bool is_ready() const { return zeroed_joint_.is_ready(); }
+enum {
+ STATE_INITIALIZE,
+ STATE_REQUEST_LOAD,
+ STATE_LOAD_BACKTRACK,
+ STATE_LOAD,
+ STATE_LOADING_PROBLEM,
+ STATE_PREPARE_SHOT,
+ STATE_BRAKE_SET,
+ STATE_READY,
+ STATE_REQUEST_FIRE,
+ STATE_PREPARE_FIRE,
+ STATE_FIRE,
+ STATE_UNLOAD,
+ STATE_UNLOAD_MOVE,
+ STATE_READY_UNLOAD
+} State;
+
protected:
virtual void RunIteration(
const ShooterLoop::Goal *goal,
@@ -53,6 +183,7 @@
// The zeroed joint to use.
ZeroedJoint<1> zeroed_joint_;
+ ZeroedStateFeedbackLoop shooter_;
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index cf99c30..4350bd2 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -2,20 +2,19 @@
import "aos/common/control_loop/control_loops.q";
-queue_group ShooterLoop {
+queue_group ShooterGroup {
implements aos.control_loops.ControlLoop;
message Output {
- // The energy to load to in joules.
double voltage;
- // Shoots as soon as this is true.
- bool latched;
- bool locked; //Disc brake locked
+ // true: latch engaged, false: latch open
+ bool latch_piston;
+ // true: brake engaged false: brake released
+ bool brake_piston;
};
message Goal {
// encoder ticks of shot energy.
double shot_power;
- double goal;
// Shoots as soon as this is true.
bool shot_requested;
bool unload_requested;
@@ -49,7 +48,6 @@
// In meters, out is positive.
double position;
- double back_calibration;
// last positive edge
double posedge_value;
@@ -68,18 +66,10 @@
bool done;
};
- message Output {
- // desired motor voltage
- double voltage;
- // true: close latch, false: open latch
- double latch_piston;
- // true: brake engaded, false: brake release
- double brake_piston;
- };
queue Goal goal;
queue Position position;
queue Output output;
queue Status status;
};
-queue_group ShooterLoop shooter_queue_group;
+queue_group ShooterGroup shooter_queue_group;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
old mode 100644
new mode 100755
index 37027f8..3f81a4f
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -19,74 +19,188 @@
// Class which simulates the shooter and sends out queue messages containing the
// position.
-class ShooterMotorSimulation {
+class ShooterSimulation {
public:
- // Constructs a motor simulation. initial_position is the inital angle of the
- // shooter, which will be treated as 0 by the encoder.
- ShooterMotorSimulation(double initial_position)
- : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
- my_shooter_loop_(".frc971.control_loops.shooter",
- 0x1a7b7094, ".frc971.control_loops.shooter.goal",
- ".frc971.control_loops.shooter.position",
- ".frc971.control_loops.shooter.output",
- ".frc971.control_loops.shooter.status") {
-
- printf("test");
+ // Constructs a motor simulation.
+ ShooterSimulation(double initial_position)
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawTopClawPlant()))
+ shooter_queue_group(".frc971.control_loops.claw_queue_group", 0x9f1a99dd,
+ ".frc971.control_loops.claw_queue_group.goal",
+ ".frc971.control_loops.claw_queue_group.position",
+ ".frc971.control_loops.claw_queue_group.output",
+ ".frc971.control_loops.claw_queue_group.status") {
Reinitialize(initial_position);
}
- // Resets the plant so that it starts at initial_position.
+
void Reinitialize(double initial_position) {
- if(initial_position)
- return;
- else return;
+ LOG(INFO, "Reinitializing to {top: %f}\n", initial_position);
+ StateFeedbackPlant<2, 1, 1>* plant = shooter_plant_.get();
+ initial_position_ = initial_position;
+ plant->X(0, 0) = initial_position_;
+ plant->X(1, 0) = 0.0;
+ plant->Y = plant->C() * plant->X;
+ last_voltage_ = 0.0;
+ last_position_.Zero();
+ SetPhysicalSensors(&last_position_);
}
- // Returns the absolute angle of the shooter.
+
+ // Returns the absolute angle of the wrist.
double GetAbsolutePosition() const {
return shooter_plant_->Y(0,0);
}
- // Returns the adjusted angle of the shooter.
+
+ // Returns the adjusted angle of the wrist.
double GetPosition() const {
- return shooter_plant_->Y(0, 0);
+ return GetAbsolutePosition() - initial_position_;
}
- // Sends out the position queue messages.
- void SendPositionMessage() {
- const double current_pos = GetPosition();
- ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
- my_shooter_loop_.position.MakeMessage();
- position->position = current_pos;
+ // Makes sure pos is inside range (inclusive)
+ bool CheckRange(double pos, struct constants::Values::AnglePair pair) {
+ return (pos >= pair.lower_angle && pos <= pair.upper_angle);
+ }
+
+
+ // Sets the values of the physical sensors that can be directly observed
+ // (encoder, hall effect).
+ void SetPhysicalSensors(control_loops::ShooterGroup::Position *position) {
+ const frc971::constants::Values& values = constants::GetValues();
+ position->position = GetPosition();
+
+ LOG(DEBUG, "Physical shooter at {%f}\n", position->position);
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
- double abs_position = GetAbsolutePosition();
- if (abs_position >= constants::GetValues().shooter_lower_physical_limit &&
- abs_position <= constants::GetValues().shooter_upper_physical_limit) {
- //position->plunger_back_hall_effect = true;
- } else {
- //position->plunger_back_hall_effect = false;
+ position->plunger_back_hall_effect =
+ CheckRange(position->position, values.plunger_heffect);
+ position->pusher_distal_hall_effect =
+ CheckRange(position->position, values.pusher_distal_heffect);
+ position->pusher_proximal_hall_effect =
+ CheckRange(position->position, values.pusher_proximal_heffect);
+ position->latch_hall_effect =
+ CheckRange(position->position, values.latch_heffect);
+ }
+
+
+ void UpdateEffectEdge(bool effect, bool last_effect,
+ double upper_angle, double lower_angle, double position,
+ double &posedge_value, double &negedge_value,
+ int &posedge_count, int &negedge_count) {
+ if (effect && !last_effect) {
+ ++posedge_count;
+ if (last_position_.position < lower_angle) {
+ posedge_value = lower_angle - initial_position_;
+ } else {
+ posedge_value = upper_angle - initial_position_;
+ }
+ }
+
+ if (!effect && last_effect) {
+ ++negedge_count;
+ if (position < lower_angle) {
+ negedge_value = lower_angle - initial_position_;
+ } else {
+ negedge_value - upper_angle - initial_position_;
+ }
+ }
+ }
+
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const frc971::constants::Values& values = constants::GetValues();
+ ::aos::ScopedMessagePtr<control_loops::ShooterGroup::Position> position =
+ shooter_queue_group.position.MakeMessage();
+
+ // Initialize all the counters to their previous values.
+ *position = last_position_;
+
+ SetPhysicalSensors(position.get());
+
+ // Handle the front hall effect.
+ if (position->plunger_back_hall_effect &&
+ !last_position_.plunger_back_hall_effect) {
+ ++position->plunger_back_hall_effect_posedge_count;
+
+ if (last_position_.position < values.upper_claw.front.lower_angle) {
+ position->top.posedge_value =
+ values.upper_claw.front.lower_angle - initial_position_;
+ } else {
+ position->top.posedge_value =
+ values.upper_claw.front.upper_angle - initial_position_;
+ }
}
+ if (!position->plunger_back_hall_effect &&
+ last_position_.plunger_back_hall_effect) {
+ ++position->plunger_back_hall_effect_negedge_count;
+
+ if (position->top.position < values.upper_claw.front.lower_angle) {
+ position->top.negedge_value =
+ values.upper_claw.front.lower_angle - initial_position_;
+ } else {
+ position->top.negedge_value =
+ values.upper_claw.front.upper_angle - initial_position_;
+ }
+ }
+
+ // Handle plunger hall effect
+ UpdateEffectEdge(
+ position->plunger_back_hall_effect,
+ last_position_.plunger_back_hall_effect,
+ values.plunger_back.upper_angle,
+ values.plunger_back.lower_angle,
+ position->position,
+ position->posedge_value,
+ position->negedge_value,
+ position->plunger_back_hall_effect_posedge_count,
+ position->plunger_back_hall_effect_negedge_count);
+
+ // Handle pusher distal hall effect
+ UpdateEffectEdge(
+ position->pusher_distal_hall_effect,
+ last_position_.pusher_distal_hall_effect,
+ values.pusher_distal.upper_angle,
+ values.pusher_distal.lower_angle,
+ position->position,
+ position->posedge_value,
+ position->negedge_value,
+ position->pusher_distal_hall_effect_posedge_count,
+ position->pusher_distal_hall_effect_negedge_count);
+
+ // Handle pusher proximal hall effect
+ UpdateEffectEdge(
+ position->pusher_proximal_hall_effect,
+ last_position_.pusher_proximal_hall_effect,
+ values.pusher_proximal.upper_angle,
+ values.pusher_proximal.lower_angle,
+ position->position,
+ position->posedge_value,
+ position->negedge_value,
+ position->pusher_proximal_hall_effect_posedge_count,
+ position->pusher_proximal_hall_effect_negedge_count);
+
+ // Handle latch hall effect
+ UpdateEffectEdge(
+ position->latch_hall_effect,
+ last_position_.latch_hall_effect,
+ values.latch.upper_angle,
+ values.latch.lower_angle,
+ position->position,
+ position->posedge_value,
+ position->negedge_value,
+ position->latch_hall_effect_posedge_count,
+ position->latch_hall_effect_negedge_count);
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- if ((last_position_ <
- constants::GetValues().shooter_lower_physical_limit ||
- last_position_ >
- constants::GetValues().shooter_lower_physical_limit)/* &&
- position->hall_effect*/) {
- calibration_value_ =
- constants::GetValues().shooter_hall_effect_start_position -
- initial_position_;
- }
-
- position->back_calibration = calibration_value_;
- position.Send();
+ last_position_ = *position;
}
- // Simulates the shooter moving for one timestep.
+
+ // Simulates the claw moving for one timestep.
void Simulate() {
last_position_ = shooter_plant_->Y(0, 0);
EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
@@ -100,15 +214,21 @@
last_voltage_ = my_shooter_loop_.output->voltage;
}
+
+ // Top of the claw, the one with rollers
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
+
private:
- ShooterLoop my_shooter_loop_;
+
+ ShooterGroup shooter_queue_group;
double initial_position_;
- double last_position_;
- double calibration_value_;
double last_voltage_;
+
+ control_loops::ShooterGroup::Position last_position_;
};
+
class ShooterTest : public ::testing::Test {
protected:
::aos::common::testing::GlobalCoreInstance my_core;
@@ -116,7 +236,7 @@
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointer to shared memory that
// is no longer valid.
- ShooterLoop my_shooter_loop_;
+ ShooterGroup shooter_queue_group;
// Create a loop and simulation plant.
ShooterMotor shooter_motor_;
@@ -135,6 +255,7 @@
SendDSPacket(true);
}
+
void SendDSPacket(bool enabled) {
::aos::robot_state.MakeWithBuilder().enabled(enabled)
.autonomous(false)
@@ -142,12 +263,12 @@
::aos::robot_state.FetchLatest();
}
+
void VerifyNearGoal() {
- my_shooter_loop_.goal.FetchLatest();
- my_shooter_loop_.position.FetchLatest();
- EXPECT_NEAR(my_shooter_loop_.goal->goal,
- shooter_motor_plant_.GetAbsolutePosition(),
- 1e-4);
+ shooter_queue_group.goal.FetchLatest();
+ shooter_queue_group.position.FetchLatest();
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(shooter_queue_group.goal->shot_power, pos, 1e-4);
}
virtual ~ShooterTest() {
@@ -155,12 +276,12 @@
}
};
-//TEST_F(ShooterTest, EmptyTest) {
-// EXPECT_TRUE(true);
-//}
-// Tests that the shooter zeros correctly and goes to a position.
+
+// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, ZerosCorrectly) {
- my_shooter_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(5050.1)
+ .Send();
for (int i = 0; i < 400; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -170,35 +291,6 @@
VerifyNearGoal();
}
-// Tests that the shooter zeros correctly starting on the hall effect sensor.
-//TEST_F(ShooterTest, ZerosStartingOn) {
-// printf("test");
-// EXPECT_TRUE(true);
-//}
-
-//// Tests that missing positions are correctly handled.
-//TEST_F(ShooterTest, HandleMissingPosition) {
-//}
-//
-//// Tests that losing the encoder for a second triggers a re-zero.
-//TEST_F(ShooterTest, RezeroWithMissingPos) {
-//}
-//
-//// Tests that disabling while zeroing sends the state machine into the
-//// uninitialized state.
-//TEST_F(ShooterTest, DisableGoesUninitialized) {
-//}
-//
-//// Tests that the shooter can't get too far away from the zeroing position if the
-//// zeroing position is saturating the goal.
-//TEST_F(ShooterTest, NoWindupNegative) {
-//}
-//
-//// Tests that the shooter can't get too far away from the zeroing position if the
-//// zeroing position is saturating the goal.
-//TEST_F(ShooterTest, NoWindupPositive) {
-//}
-
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_main.cc b/frc971/control_loops/shooter/shooter_main.cc
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 38ca89c..420a0e7 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -218,7 +218,6 @@
Eigen::Matrix<double, number_of_inputs, 1> U;
Eigen::Matrix<double, number_of_inputs, 1> U_uncapped;
Eigen::Matrix<double, number_of_outputs, 1> U_ff;
- Eigen::Matrix<double, number_of_outputs, 1> Y;
const StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs> &controller() const {
@@ -237,7 +236,6 @@
U.setZero();
U_uncapped.setZero();
U_ff.setZero();
- Y.setZero();
}
StateFeedbackLoop(const StateFeedbackController<
@@ -286,9 +284,14 @@
}
}
- // update_observer is whether or not to use the values in Y.
+ // Corrects X_hat given the observation in Y.
+ void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ Y_ = Y;
+ new_y_ = true;
+ }
+
// stop_motors is whether or not to output all 0s.
- void Update(bool update_observer, bool stop_motors) {
+ void Update(bool stop_motors) {
if (stop_motors) {
U.setZero();
} else {
@@ -296,8 +299,9 @@
CapU();
}
- if (update_observer) {
- X_hat = (A() - L() * C()) * X_hat + L() * Y + B() * U;
+ if (new_y_) {
+ X_hat = (A() - L() * C()) * X_hat + L() * Y_ + B() * U;
+ new_y_ = false;
} else {
X_hat = A() * X_hat + B() * U;
}
@@ -326,6 +330,11 @@
static const int kNumOutputs = number_of_outputs;
static const int kNumInputs = number_of_inputs;
+ // Temporary storage for a measurement until I can figure out why I can't
+ // correct when the measurement is made.
+ Eigen::Matrix<double, number_of_outputs, 1> Y_;
+ bool new_y_ = false;
+
int controller_index_;
};
diff --git a/frc971/control_loops/update_claw.sh b/frc971/control_loops/update_claw.sh
new file mode 100755
index 0000000..083850a
--- /dev/null
+++ b/frc971/control_loops/update_claw.sh
@@ -0,0 +1,14 @@
+#!/bin/bash
+#
+# Updates the claw controllers.
+
+cd $(dirname $0)
+
+./python/claw.py claw/unaugmented_top_claw_motor_plant.h \
+ claw/unaugmented_top_claw_motor_plant.cc \
+ claw/top_claw_motor_plant.h \
+ claw/top_claw_motor_plant.cc \
+ claw/unaugmented_bottom_claw_motor_plant.h \
+ claw/unaugmented_bottom_claw_motor_plant.cc \
+ claw/bottom_claw_motor_plant.h \
+ claw/bottom_claw_motor_plant.cc
diff --git a/frc971/control_loops/update_wrists.sh b/frc971/control_loops/update_wrists.sh
deleted file mode 100755
index bb79a0a..0000000
--- a/frc971/control_loops/update_wrists.sh
+++ /dev/null
@@ -1,10 +0,0 @@
-#!/bin/bash
-#
-# Updates the wrist controllers.
-
-cd $(dirname $0)
-
-./python/wrists.py wrists/top_wrist_motor_plant.h \
- wrists/top_wrist_motor_plant.cc \
- wrists/bottom_wrist_motor_plant.h \
- wrists/bottom_wrist_motor_plant.cc
diff --git a/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc b/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc
deleted file mode 100644
index f08793b..0000000
--- a/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.000326582411818, 0.0631746179893;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeRawWristController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.71581823335, 64.8264890043;
- Eigen::Matrix<double, 1, 2> K;
- K << 130.590421637, 7.48987035533;
- return StateFeedbackController<2, 1, 1>(L, K, MakeRawWristPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeRawWristPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawWristPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeRawWristLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawWristController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/wrists/bottom_wrist_motor_plant.h b/frc971/control_loops/wrists/bottom_wrist_motor_plant.h
deleted file mode 100644
index 00db927..0000000
--- a/frc971/control_loops/wrists/bottom_wrist_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_WRISTS_BOTTOM_WRIST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_WRISTS_BOTTOM_WRIST_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeRawWristController();
-
-StateFeedbackPlant<2, 1, 1> MakeRawWristPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeRawWristLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_WRISTS_BOTTOM_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrists/top_wrist_motor_plant.cc b/frc971/control_loops/wrists/top_wrist_motor_plant.cc
deleted file mode 100644
index 132ef4f..0000000
--- a/frc971/control_loops/wrists/top_wrist_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients() {
- Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 1> B;
- B << 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 1, 3> C;
- C << 1.0, 0.0, 0.0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0.0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<3, 1, 1> MakeWristController() {
- Eigen::Matrix<double, 3, 1> L;
- L << 1.81581823335, 78.6334534274, 142.868137351;
- Eigen::Matrix<double, 1, 3> K;
- K << 92.6004807973, 4.38063492858, 1.11581823335;
- return StateFeedbackController<3, 1, 1>(L, K, MakeWristPlantCoefficients());
-}
-
-StateFeedbackPlant<3, 1, 1> MakeWristPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeWristPlantCoefficients());
- return StateFeedbackPlant<3, 1, 1>(plants);
-}
-
-StateFeedbackLoop<3, 1, 1> MakeWristLoop() {
- ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<3, 1, 1>(MakeWristController());
- return StateFeedbackLoop<3, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/wrists/top_wrist_motor_plant.h b/frc971/control_loops/wrists/top_wrist_motor_plant.h
deleted file mode 100644
index ac657cb..0000000
--- a/frc971/control_loops/wrists/top_wrist_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_WRISTS_TOP_WRIST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_WRISTS_TOP_WRIST_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients();
-
-StateFeedbackController<3, 1, 1> MakeWristController();
-
-StateFeedbackPlant<3, 1, 1> MakeWristPlant();
-
-StateFeedbackLoop<3, 1, 1> MakeWristLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_WRISTS_TOP_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrists/wrists.cc b/frc971/control_loops/wrists/wrists.cc
deleted file mode 100644
index fe9295b..0000000
--- a/frc971/control_loops/wrists/wrists.cc
+++ /dev/null
@@ -1,80 +0,0 @@
-#include "frc971/control_loops/wrists/wrists.h"
-
-#include <stdio.h>
-
-#include <algorithm>
-
-#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/logging/logging.h"
-
-#include "frc971/constants.h"
-#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
-#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
-
-namespace frc971 {
-namespace control_loops {
-
-WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
- : aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
- zeroed_joint_(MakeWristLoop()) {
- {
- using ::frc971::constants::GetValues;
- ZeroedJoint<1>::ConfigurationData config_data;
-
- config_data.lower_limit = GetValues().wrist_lower_limit;
- config_data.upper_limit = GetValues().wrist_upper_limit;
- config_data.hall_effect_start_angle[0] =
- GetValues().wrist_hall_effect_start_angle;
- config_data.zeroing_off_speed = GetValues().wrist_zeroing_off_speed;
- config_data.zeroing_speed = GetValues().wrist_zeroing_speed;
-
- config_data.max_zeroing_voltage = 5.0;
- config_data.deadband_voltage = 0.0;
-
- zeroed_joint_.set_config_data(config_data);
- }
-}
-
-// Positive angle is up, and positive power is up.
-void WristMotor::RunIteration(
- const ::aos::control_loops::Goal *goal,
- const control_loops::WristLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status * status) {
-
- // Disable the motors now so that all early returns will return with the
- // motors disabled.
- if (output) {
- output->voltage = 0;
- }
-
- ZeroedJoint<1>::PositionData transformed_position;
- ZeroedJoint<1>::PositionData *transformed_position_ptr =
- &transformed_position;
- if (!position) {
- transformed_position_ptr = NULL;
- } else {
- transformed_position.position = position->pos;
- transformed_position.hall_effects[0] = position->hall_effect;
- transformed_position.hall_effect_positions[0] = position->calibration;
- }
-
- const double voltage = zeroed_joint_.Update(transformed_position_ptr,
- output != NULL,
- goal->goal, 0.0);
-
- if (position) {
- LOG(DEBUG, "pos: %f hall: %s absolute: %f\n",
- position->pos,
- position->hall_effect ? "true" : "false",
- zeroed_joint_.absolute_position());
- }
-
- if (output) {
- output->voltage = voltage;
- }
- status->done = ::std::abs(zeroed_joint_.absolute_position() - goal->goal) < 0.004;
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/wrists/wrists.gyp b/frc971/control_loops/wrists/wrists.gyp
deleted file mode 100644
index f455e2e..0000000
--- a/frc971/control_loops/wrists/wrists.gyp
+++ /dev/null
@@ -1,66 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'wrist_loops',
- 'type': 'static_library',
- 'sources': ['wrists.q'],
- 'variables': {
- 'header_path': 'frc971/control_loops/wrists',
- },
- 'dependencies': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'wrists_lib',
- 'type': 'static_library',
- 'sources': [
- 'wrists.cc',
- 'top_wrist_motor_plant.cc',
- 'top_wrist_motor_plant.cc',
- ],
- 'dependencies': [
- 'wrist_loops',
- '<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- 'export_dependent_settings': [
- 'wrist_loops',
- '<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- },
- {
- 'target_name': 'wrists_lib_test',
- 'type': 'executable',
- 'sources': [
- 'wrists_lib_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- 'wrist_loops',
- 'wrists_lib',
- '<(AOS)/common/common.gyp:queue_testutils',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- },
- {
- 'target_name': 'wrists',
- 'type': 'executable',
- 'sources': [
- 'wrists_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'wrists_lib',
- ],
- },
- ],
-}
diff --git a/frc971/control_loops/wrists/wrists.h b/frc971/control_loops/wrists/wrists.h
deleted file mode 100644
index 1c1fdf4..0000000
--- a/frc971/control_loops/wrists/wrists.h
+++ /dev/null
@@ -1,63 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
-#define FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
-
-#include <memory>
-
-#include "aos/common/control_loop/ControlLoop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/wrists/wrists.q.h"
-#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
-#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
-
-#include "frc971/control_loops/zeroed_joint.h"
-
-namespace frc971 {
-namespace control_loops {
-namespace testing {
-class WristTest_NoWindupPositive_Test;
-class WristTest_NoWindupNegative_Test;
-};
-
-class WristMotor
- : public aos::control_loops::ControlLoop<control_loops::WristLoop> {
- public:
- explicit WristMotor(
- control_loops::WristLoop *my_wrist = &control_loops::wrist);
-
- // True if the goal was moved to avoid goal windup.
- bool capped_goal() const { return zeroed_joint_.capped_goal(); }
-
- // True if the wrist is zeroing.
- bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
-
- // True if the wrist is zeroing.
- bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
-
- // True if the state machine is uninitialized.
- bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
-
- // True if the state machine is ready.
- bool is_ready() const { return zeroed_joint_.is_ready(); }
-
- protected:
- virtual void RunIteration(
- const ::aos::control_loops::Goal *goal,
- const control_loops::WristLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status *status);
-
- private:
- // Friend the test classes for acces to the internal state.
- friend class testing::WristTest_NoWindupPositive_Test;
- friend class testing::WristTest_NoWindupNegative_Test;
-
- // The zeroed joint to use.
- ZeroedJoint<1> zeroed_joint_;
-
- DISALLOW_COPY_AND_ASSIGN(WristMotor);
-};
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
diff --git a/frc971/control_loops/wrists/wrists.q b/frc971/control_loops/wrists/wrists.q
deleted file mode 100644
index 36159c9..0000000
--- a/frc971/control_loops/wrists/wrists.q
+++ /dev/null
@@ -1,28 +0,0 @@
-package frc971.control_loops;
-
-import "aos/common/control_loop/control_loops.q";
-
-// All angles here are 0 horizontal, positive up.
-queue_group WristsLoop {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // The angle of the bottom wrist.
- double bottom_angle;
- // How much higher the top wrist is.
- double between_angle;
- bool intake;
- };
- message Position {
- double bottom_position, top_position;
- bool bottom_hall_effect, top_hall_effect;
- double bottom_calibration, top_calibration;
- };
-
- queue Goal goal;
- queue Position position;
- queue aos.control_loops.Output output;
- queue aos.control_loops.Status status;
-};
-
-queue_group WristsLoop wrists;
diff --git a/frc971/control_loops/wrists/wrists_lib_test.cc b/frc971/control_loops/wrists/wrists_lib_test.cc
deleted file mode 100644
index 996036e..0000000
--- a/frc971/control_loops/wrists/wrists_lib_test.cc
+++ /dev/null
@@ -1,334 +0,0 @@
-#include <unistd.h>
-
-#include <memory>
-
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/common/queue_testutils.h"
-#include "frc971/control_loops/wrists/wrists.q.h"
-#include "frc971/control_loops/wrists/wrists.h"
-#include "frc971/constants.h"
-
-
-using ::aos::time::Time;
-
-namespace frc971 {
-namespace control_loops {
-namespace testing {
-
-// Class which simulates the wrist and sends out queue messages containing the
-// position.
-class WristMotorSimulation {
- public:
- // Constructs a motor simulation. initial_position is the inital angle of the
- // wrist, which will be treated as 0 by the encoder.
- WristMotorSimulation(double initial_position)
- : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawWristPlant())),
- my_wrist_loop_(".frc971.control_loops.wrists",
- 0x1a7b7094, ".frc971.control_loops.wrists.goal",
- ".frc971.control_loops.wrists.position",
- ".frc971.control_loops.wrists.output",
- ".frc971.control_loops.wrists.status") {
- Reinitialize(initial_position);
- }
-
- // Resets the plant so that it starts at initial_position.
- void Reinitialize(double initial_position) {
- initial_position_ = initial_position;
- wrist_plant_->X(0, 0) = initial_position_;
- wrist_plant_->X(1, 0) = 0.0;
- wrist_plant_->Y = wrist_plant_->C() * wrist_plant_->X;
- last_position_ = wrist_plant_->Y(0, 0);
- calibration_value_ = 0.0;
- last_voltage_ = 0.0;
- }
-
- // Returns the absolute angle of the wrist.
- double GetAbsolutePosition() const {
- return wrist_plant_->Y(0, 0);
- }
-
- // Returns the adjusted angle of the wrist.
- double GetPosition() const {
- return GetAbsolutePosition() - initial_position_;
- }
-
- // Sends out the position queue messages.
- void SendPositionMessage() {
- const double angle = GetPosition();
-
- ::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
- my_wrist_loop_.position.MakeMessage();
- position->pos = angle;
-
- // Signal that the hall effect sensor has been triggered if it is within
- // the correct range.
- double abs_position = GetAbsolutePosition();
- if (abs_position >= constants::GetValues().wrist_hall_effect_start_angle &&
- abs_position <= constants::GetValues().wrist_hall_effect_stop_angle) {
- position->hall_effect = true;
- } else {
- position->hall_effect = false;
- }
-
- // Only set calibration if it changed last cycle. Calibration starts out
- // with a value of 0.
- if ((last_position_ <
- constants::GetValues().wrist_hall_effect_start_angle ||
- last_position_ >
- constants::GetValues().wrist_hall_effect_stop_angle) &&
- position->hall_effect) {
- calibration_value_ =
- constants::GetValues().wrist_hall_effect_start_angle -
- initial_position_;
- }
-
- position->calibration = calibration_value_;
- position.Send();
- }
-
- // Simulates the wrist moving for one timestep.
- void Simulate() {
- last_position_ = wrist_plant_->Y(0, 0);
- EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
- wrist_plant_->U << last_voltage_;
- wrist_plant_->Update();
-
- EXPECT_GE(constants::GetValues().wrist_upper_physical_limit,
- wrist_plant_->Y(0, 0));
- EXPECT_LE(constants::GetValues().wrist_lower_physical_limit,
- wrist_plant_->Y(0, 0));
- last_voltage_ = my_wrist_loop_.output->voltage;
- }
-
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
- private:
- WristLoop my_wrist_loop_;
- double initial_position_;
- double last_position_;
- double calibration_value_;
- double last_voltage_;
-};
-
-class WristTest : public ::testing::Test {
- protected:
- ::aos::common::testing::GlobalCoreInstance my_core;
-
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- WristLoop my_wrist_loop_;
-
- // Create a loop and simulation plant.
- WristMotor wrist_motor_;
- WristMotorSimulation wrist_motor_plant_;
-
- WristTest() : my_wrist_loop_(".frc971.control_loops.wrist",
- 0x1a7b7094, ".frc971.control_loops.wrist.goal",
- ".frc971.control_loops.wrist.position",
- ".frc971.control_loops.wrist.output",
- ".frc971.control_loops.wrist.status"),
- wrist_motor_(&my_wrist_loop_),
- wrist_motor_plant_(0.5) {
- // Flush the robot state queue so we can use clean shared memory for this
- // test.
- ::aos::robot_state.Clear();
- SendDSPacket(true);
- }
-
- void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder().enabled(enabled)
- .autonomous(false)
- .team_id(971).Send();
- ::aos::robot_state.FetchLatest();
- }
-
- void VerifyNearGoal() {
- my_wrist_loop_.goal.FetchLatest();
- my_wrist_loop_.position.FetchLatest();
- EXPECT_NEAR(my_wrist_loop_.goal->goal,
- wrist_motor_plant_.GetAbsolutePosition(),
- 1e-4);
- }
-
- virtual ~WristTest() {
- ::aos::robot_state.Clear();
- }
-};
-
-// Tests that the wrist zeros correctly and goes to a position.
-TEST_F(WristTest, ZerosCorrectly) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 400; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that the wrist zeros correctly starting on the hall effect sensor.
-TEST_F(WristTest, ZerosStartingOn) {
- wrist_motor_plant_.Reinitialize(90 * M_PI / 180.0);
-
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 500; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that missing positions are correctly handled.
-TEST_F(WristTest, HandleMissingPosition) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 400; ++i) {
- if (i % 23) {
- wrist_motor_plant_.SendPositionMessage();
- }
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that loosing the encoder for a second triggers a re-zero.
-TEST_F(WristTest, RezeroWithMissingPos) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 800; ++i) {
- // After 3 seconds, simulate the encoder going missing.
- // This should trigger a re-zero. To make sure it works, change the goal as
- // well.
- if (i < 300 || i > 400) {
- wrist_motor_plant_.SendPositionMessage();
- } else {
- if (i > 310) {
- // Should be re-zeroing now.
- EXPECT_TRUE(wrist_motor_.is_uninitialized());
- }
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
- }
- if (i == 410) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- }
-
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that disabling while zeroing sends the state machine into the
-// uninitialized state.
-TEST_F(WristTest, DisableGoesUninitialized) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 800; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- // After 0.5 seconds, disable the robot.
- if (i > 50 && i < 200) {
- SendDSPacket(false);
- if (i > 100) {
- // Give the loop a couple cycled to get the message and then verify that
- // it is in the correct state.
- EXPECT_TRUE(wrist_motor_.is_uninitialized());
- // When disabled, we should be applying 0 power.
- EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
- EXPECT_EQ(0, my_wrist_loop_.output->voltage);
- }
- } else {
- SendDSPacket(true);
- }
- if (i == 202) {
- // Verify that we are zeroing after the bot gets enabled again.
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- }
-
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- }
- VerifyNearGoal();
-}
-
-// Tests that the wrist can't get too far away from the zeroing position if the
-// zeroing position is saturating the goal.
-TEST_F(WristTest, NoWindupNegative) {
- int capped_count = 0;
- double saved_zeroing_position = 0;
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 500; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- if (i == 50) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- // Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
- wrist_motor_.zeroed_joint_.zeroing_position_ = -100.0;
- } else if (i == 51) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- EXPECT_NEAR(saved_zeroing_position,
- wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
- }
- if (!wrist_motor_.is_ready()) {
- if (wrist_motor_.capped_goal()) {
- ++capped_count;
- // The cycle after we kick the zero position is the only cycle during
- // which we should expect to see a high uncapped power during zeroing.
- ASSERT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
- } else {
- ASSERT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
- }
- }
-
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
- EXPECT_GT(3, capped_count);
-}
-
-// Tests that the wrist can't get too far away from the zeroing position if the
-// zeroing position is saturating the goal.
-TEST_F(WristTest, NoWindupPositive) {
- int capped_count = 0;
- double saved_zeroing_position = 0;
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 500; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- if (i == 50) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- // Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
- wrist_motor_.zeroed_joint_.zeroing_position_ = 100.0;
- } else {
- if (i == 51) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
- }
- }
- if (!wrist_motor_.is_ready()) {
- if (wrist_motor_.capped_goal()) {
- ++capped_count;
- // The cycle after we kick the zero position is the only cycle during
- // which we should expect to see a high uncapped power during zeroing.
- EXPECT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
- } else {
- EXPECT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
- }
- }
-
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
- EXPECT_GT(3, capped_count);
-}
-
-} // namespace testing
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/wrists/wrists_main.cc b/frc971/control_loops/wrists/wrists_main.cc
deleted file mode 100644
index 66d1ea3..0000000
--- a/frc971/control_loops/wrists/wrists_main.cc
+++ /dev/null
@@ -1,11 +0,0 @@
-#include "frc971/control_loops/wrist/wrists.h"
-
-#include "aos/linux_code/init.h"
-
-int main() {
- ::aos::Init();
- frc971::control_loops::WristsLoop wrists;
- wrists.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index eb2bb9a..80c1c9f 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -97,7 +97,7 @@
int main() {
::aos::Init(::bbb::SensorReader::kRelativePriority);
- ::bbb::SensorReader reader("main");
+ ::bbb::SensorReader reader("comp");
while (true) {
::frc971::PacketReceived(reader.ReadPacket(), reader.GetCapeTimestamp());
}
diff --git a/frc971/prime/build.sh b/frc971/prime/build.sh
index 81d7e28..8130f01 100755
--- a/frc971/prime/build.sh
+++ b/frc971/prime/build.sh
@@ -2,5 +2,5 @@
cd $(dirname $0)
+../../aos/build/build.sh linux-amd64 prime.gyp no prime-amd64 "$@" || exit 1
../../aos/build/build.sh linux prime.gyp no prime "$@"
-../../aos/build/build.sh linux-amd64 prime.gyp no prime-amd64 "$@"