Merge changes I4058077b,I25ec37a6,If21a0a97,Iff72d608,If1750d4b, ...
* changes:
Fixed fridge lurching down.
Score is faster.
Horizontal can is faster now.
No longer lower the claw in as many places and stack faster.
Rounded score set down corner.
Sped up vertical can pickup
Sped up stack.
Start fridge and claw at a reasonable position after zeroing.
Added XY to the fridge profile lib.
diff --git a/aos/build/aos_all.gyp b/aos/build/aos_all.gyp
index bd333a3..10d5a5b 100644
--- a/aos/build/aos_all.gyp
+++ b/aos/build/aos_all.gyp
@@ -34,6 +34,8 @@
'<(AOS)/common/common.gyp:mutex_test',
'<(AOS)/common/common.gyp:condition_test',
'<(AOS)/common/common.gyp:once_test',
+ '<(AOS)/common/common.gyp:event_test',
+ '<(AOS)/common/common.gyp:queue_testutils_test',
'<(AOS)/common/logging/logging.gyp:logging_impl_test',
'<(AOS)/common/util/util.gyp:options_test',
'<(AOS)/common/common.gyp:queue_test',
diff --git a/aos/build/build.py b/aos/build/build.py
index 6281821..5be0950 100755
--- a/aos/build/build.py
+++ b/aos/build/build.py
@@ -442,7 +442,7 @@
''.join(mv_cmd).format(TMPDIR=TEMP_DIR, TO_DIR=TARGET_DIR)))
def build_env(self):
- OTHER_SYSROOT = '/usr/lib/llvm-3.5'
+ OTHER_SYSROOT = '/usr/lib/llvm-3.5/'
SYMBOLIZER_PATH = OTHER_SYSROOT + 'bin/llvm-symbolizer'
r = {}
if self.compiler() == 'clang' or self.compiler() == 'gcc_4.8':
@@ -670,6 +670,7 @@
Defaults to everything.
extra_flag... Extra flags associated with the targets.
--gtest_*: Arguments to pass on to tests.
+ --print_logs, --log_file=*: More test arguments.
Specifying targets:
Targets are combinations of architecture, compiler, and debug flags. Which
@@ -755,12 +756,13 @@
continue
if arg == '-h' or arg == '--help':
print_help(0)
- if re.match('^--gtest_.*$', arg):
+ if (re.match('^--gtest_.*$', arg) or arg == '--print-logs' or
+ re.match('^--log_file=.*$', arg)):
if args.action_name == 'tests':
args.extra_flags.append(arg)
continue
else:
- print_help(1, '--gtest_* is only valid for tests')
+ print_help(1, '%s is only valid for tests' % arg)
if args.platform:
args.targets.append(arg)
else:
diff --git a/aos/build/externals.gyp b/aos/build/externals.gyp
index 315f0bf..2bfe241 100644
--- a/aos/build/externals.gyp
+++ b/aos/build/externals.gyp
@@ -136,11 +136,9 @@
],
'dependencies': [
'gtest_prod',
- '<(AOS)/common/common.gyp:queue_testutils',
],
'export_dependent_settings': [
'gtest_prod',
- '<(AOS)/common/common.gyp:queue_testutils',
],
'cflags!': ['-Werror'],
'direct_dependent_settings': {
diff --git a/aos/common/actions/action_test.cc b/aos/common/actions/action_test.cc
index eb863e3..59d8d79 100644
--- a/aos/common/actions/action_test.cc
+++ b/aos/common/actions/action_test.cc
@@ -10,6 +10,7 @@
#include "aos/common/actions/actions.h"
#include "aos/common/actions/actions.q.h"
#include "aos/common/actions/test_action.q.h"
+#include "aos/common/event.h"
using ::aos::time::Time;
@@ -424,7 +425,58 @@
EXPECT_FALSE(action_queue_.Running());
}
-} // namespace testing.
-} // namespace actions.
-} // namespace common.
-} // namespace aos.
+// Tests that cancelling an action before the message confirming it started is
+// received works.
+// Situations like this used to lock the action queue up waiting for an action
+// to report that it successfully cancelled.
+// This situation is kind of a race condition, but it happens very consistently
+// when hitting buttons while the robot is in teleop-disabled. To hit the race
+// condition consistently in the test, there are a couple of Events inserted in
+// between various things running.
+TEST_F(ActionTest, CancelBeforeStart) {
+ Event thread_ready, ready_to_start, ready_to_stop;
+ ::std::thread action_thread(
+ [this, &thread_ready, &ready_to_start, &ready_to_stop]() {
+ TestActorNOP nop_act(&actions::test_action);
+ nop_act.Initialize();
+ thread_ready.Set();
+ ready_to_start.Wait();
+ nop_act.WaitForActionRequest();
+ LOG(DEBUG, "got a request to run\n");
+ const uint32_t running_id = nop_act.RunIteration();
+ LOG(DEBUG, "waiting for %" PRIx32 " to be stopped\n", running_id);
+ ready_to_stop.Set();
+ nop_act.WaitForStop(running_id);
+ });
+
+ action_queue_.CancelAllActions();
+ EXPECT_FALSE(action_queue_.Running());
+ thread_ready.Wait();
+ LOG(DEBUG, "starting action\n");
+ action_queue_.EnqueueAction(MakeTestActionNOP());
+ action_queue_.Tick();
+ action_queue_.CancelAllActions();
+ ready_to_start.Set();
+ LOG(DEBUG, "started action\n");
+ EXPECT_TRUE(action_queue_.Running());
+ ready_to_stop.Wait();
+ EXPECT_TRUE(action_queue_.Running());
+ LOG(DEBUG, "action is ready to stop\n");
+
+ action_queue_.Tick();
+ action_queue_.CancelAllActions();
+ EXPECT_FALSE(action_queue_.Running());
+ action_queue_.Tick();
+ action_queue_.CancelAllActions();
+ ASSERT_FALSE(action_queue_.Running());
+ action_thread.join();
+
+ action_queue_.Tick();
+ action_queue_.CancelAllActions();
+ ASSERT_FALSE(action_queue_.Running());
+}
+
+} // namespace testing
+} // namespace actions
+} // namespace common
+} // namespace aos
diff --git a/aos/common/actions/actions.gyp b/aos/common/actions/actions.gyp
index 623569b..69c9c7a 100644
--- a/aos/common/actions/actions.gyp
+++ b/aos/common/actions/actions.gyp
@@ -63,7 +63,8 @@
'<(AOS)/common/logging/logging.gyp:queue_logging',
'<(AOS)/common/common.gyp:queues',
'<(AOS)/common/common.gyp:time',
- 'action_queue'
+ 'action_queue',
+ '<(AOS)/common/common.gyp:event',
],
},
],
diff --git a/aos/common/common.gyp b/aos/common/common.gyp
index a0750fe..461200e 100644
--- a/aos/common/common.gyp
+++ b/aos/common/common.gyp
@@ -21,6 +21,7 @@
'<(AOS)/build/aos.gyp:logging',
'once',
'<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:shared_mem',
+ 'mutex',
],
'export_dependent_settings': [
'<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:shared_mem',
@@ -244,11 +245,35 @@
'dependencies': [
'<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:aos_sync',
'<(AOS)/build/aos.gyp:logging_interface',
- 'die',
],
'export_dependent_settings': [
'<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:aos_sync',
- 'die',
+ ],
+ },
+ {
+ 'target_name': 'event',
+ 'type': 'static_library',
+ 'sources': [
+ '<(AOS)/linux_code/ipc_lib/event.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:aos_sync',
+ '<(AOS)/build/aos.gyp:logging_interface',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/linux_code/ipc_lib/ipc_lib.gyp:aos_sync',
+ ],
+ },
+ {
+ 'target_name': 'queue_testutils_test',
+ 'type': 'executable',
+ 'sources': [
+ 'queue_testutils_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'queue_testutils',
+ '<(AOS)/build/aos.gyp:logging',
],
},
{
@@ -269,6 +294,19 @@
],
},
{
+ 'target_name': 'event_test',
+ 'type': 'executable',
+ 'sources': [
+ 'event_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'event',
+ 'queue_testutils',
+ 'time',
+ ],
+ },
+ {
'target_name': 'condition_test',
'type': 'executable',
'sources': [
diff --git a/aos/common/event.h b/aos/common/event.h
new file mode 100644
index 0000000..9839d41
--- /dev/null
+++ b/aos/common/event.h
@@ -0,0 +1,53 @@
+#ifndef AOS_COMMON_EVENT_H_
+#define AOS_COMMON_EVENT_H_
+
+#include "aos/linux_code/ipc_lib/aos_sync.h"
+
+namespace aos {
+
+// An abstraction of an event which is easy to implement for Linux and in other
+// environments.
+// On Linux at least, this is definitely safe for passing through C code with
+// memcpy etc.
+//
+// An event is either "set" or "unset". Any thread can transition it between
+// these two states and other threads can wait for an unset->set transition.
+// This is not a particularly powerful synchronization primitive, but it can be
+// much easier to use than more complicated ones in some situations. The name is
+// taken from Python's implementation of the same thing.
+//
+// An event is equivalent to a semaphore which is either set to 0 or infinity.
+// It is also equivalent to a condition variable with the monitored condition
+// being "is it set or not".
+//
+// IMPORTANT: You can NOT use this to successfully replace a standard condition
+// variable in most cases. When the condition being monitored changes separately
+// from the actual state of the condition variable/event, there WILL be race
+// conditions if you try to use this class.
+//
+// It is undefined behavior to destroy an Event while there are current
+// Wait()ers.
+class Event {
+ public:
+ // Creates an unset event.
+ Event();
+ // There must be no waiters when an Event is destroyed.
+ ~Event() = default;
+
+ // Waits for the event to be set. Returns immediately if it is already set.
+ void Wait();
+
+ // Wakes up all Wait()ers and sets the event (atomically).
+ void Set();
+ // Unsets the event so future Wait() callers will block instead of returning
+ // immediately.
+ // Returns true if the event was previously set.
+ bool Clear();
+
+ private:
+ aos_futex impl_;
+};
+
+} // namespace aos
+
+#endif // AOS_COMMON_EVENT_H_
diff --git a/aos/common/event_test.cc b/aos/common/event_test.cc
new file mode 100644
index 0000000..d8671eb
--- /dev/null
+++ b/aos/common/event_test.cc
@@ -0,0 +1,71 @@
+#include "aos/common/event.h"
+
+#include <thread>
+
+#include "gtest/gtest.h"
+
+#include "aos/common/queue_testutils.h"
+#include "aos/common/time.h"
+
+namespace aos {
+namespace testing {
+
+class EventTest : public ::testing::Test {
+ public:
+ Event test_event;
+
+ protected:
+ void SetUp() override {
+ ::aos::common::testing::EnableTestLogging();
+ }
+};
+
+// Makes sure that basic operations with no blocking or anything work.
+TEST_F(EventTest, Basic) {
+ EXPECT_FALSE(test_event.Clear());
+ EXPECT_FALSE(test_event.Clear());
+
+ test_event.Set();
+ test_event.Wait();
+ EXPECT_TRUE(test_event.Clear());
+ EXPECT_FALSE(test_event.Clear());
+}
+
+// Tests that tsan understands that events establish a happens-before
+// relationship.
+TEST_F(EventTest, ThreadSanitizer) {
+ for (int i = 0; i < 3000; ++i) {
+ int variable = 0;
+ test_event.Clear();
+ ::std::thread thread([this, &variable]() {
+ test_event.Wait();
+ --variable;
+ });
+ ++variable;
+ test_event.Set();
+ thread.join();
+ EXPECT_EQ(0, variable);
+ }
+}
+
+// Tests that an event blocks correctly.
+TEST_F(EventTest, Blocks) {
+ time::Time start_time, finish_time;
+ // Without this, it sometimes manages to fail under tsan.
+ Event started;
+ ::std::thread thread([this, &start_time, &finish_time, &started]() {
+ start_time = time::Time::Now();
+ started.Set();
+ test_event.Wait();
+ finish_time = time::Time::Now();
+ });
+ static const time::Time kWaitTime = time::Time::InSeconds(0.05);
+ started.Wait();
+ time::SleepFor(kWaitTime);
+ test_event.Set();
+ thread.join();
+ EXPECT_GE(finish_time - start_time, kWaitTime);
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/common/mutex_test.cc b/aos/common/mutex_test.cc
index bd2365a..56f7772 100644
--- a/aos/common/mutex_test.cc
+++ b/aos/common/mutex_test.cc
@@ -4,6 +4,8 @@
#include <math.h>
#include <pthread.h>
+#include <thread>
+
#include "gtest/gtest.h"
#include "aos/linux_code/ipc_lib/aos_sync.h"
@@ -145,6 +147,24 @@
EXPECT_EQ(2, counter);
}
+// Verifiers that ThreadSanitizer understands how a mutex works.
+// For some reason this used to fail when the other tests didn't...
+TEST_F(MutexTest, ThreadSanitizerMutexLocker) {
+ int counter = 0;
+ ::std::thread thread([&counter, this]() {
+ for (int i = 0; i < 1000; ++i) {
+ MutexLocker locker(&test_mutex);
+ ++counter;
+ }
+ });
+ for (int i = 0; i < 1000; ++i) {
+ MutexLocker locker(&test_mutex);
+ --counter;
+ }
+ thread.join();
+ EXPECT_EQ(0, counter);
+}
+
// Verifies that ThreadSanitizer understands that an uncontended mutex
// establishes a happens-before relationship.
TEST_F(MutexTest, ThreadSanitizerUncontended) {
diff --git a/aos/common/queue_testutils.cc b/aos/common/queue_testutils.cc
index d71d023..a40b5e9 100644
--- a/aos/common/queue_testutils.cc
+++ b/aos/common/queue_testutils.cc
@@ -10,6 +10,7 @@
#include "aos/common/queue.h"
#include "aos/common/logging/logging_impl.h"
#include "aos/common/once.h"
+#include "aos/common/mutex.h"
using ::aos::logging::LogMessage;
@@ -31,11 +32,13 @@
// Clears out all of the messages already recorded.
void ClearMessages() {
+ ::aos::MutexLocker locker(&messages_mutex_);
messages_.clear();
}
// Prints out all of the messages (like when a test fails).
void PrintAllMessages() {
+ ::aos::MutexLocker locker(&messages_mutex_);
for (auto it = messages_.begin(); it != messages_.end(); ++it) {
logging::internal::PrintMessage(stdout, *it);
}
@@ -66,6 +69,7 @@
}
virtual void HandleMessage(const LogMessage &message) override {
+ ::aos::MutexLocker locker(&messages_mutex_);
if (message.level == FATAL || print_as_messages_come_in_) {
logging::internal::PrintMessage(output_file_, message);
}
@@ -76,6 +80,7 @@
::std::vector<LogMessage> messages_;
bool print_as_messages_come_in_ = false;
FILE *output_file_ = stdout;
+ ::aos::Mutex messages_mutex_;
};
class MyTestEventListener : public ::testing::EmptyTestEventListener {
diff --git a/aos/common/queue_testutils_test.cc b/aos/common/queue_testutils_test.cc
new file mode 100644
index 0000000..2defe74
--- /dev/null
+++ b/aos/common/queue_testutils_test.cc
@@ -0,0 +1,31 @@
+#include "aos/common/queue_testutils.h"
+
+#include <thread>
+
+#include "gtest/gtest.h"
+
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+namespace common {
+namespace testing {
+
+// Tests logging from multiple threads.
+// tsan used to complain about this.
+TEST(QueueTestutilsTest, MultithreadedLog) {
+ EnableTestLogging();
+
+ ::std::thread thread([]() {
+ for (int i = 0; i < 1000; ++i) {
+ LOG(INFO, "test from thread\n");
+ }
+ });
+ for (int i = 0; i < 1000; ++i) {
+ LOG(INFO, "not from thread\n");
+ }
+ thread.join();
+}
+
+} // namespace testing
+} // namespace common
+} // namespace aos
diff --git a/aos/externals/gtest/gtest_main.cc b/aos/externals/gtest/gtest_main.cc
index 024c502..0f6b60e 100644
--- a/aos/externals/gtest/gtest_main.cc
+++ b/aos/externals/gtest/gtest_main.cc
@@ -2,7 +2,18 @@
#include <getopt.h>
#include "gtest/gtest.h"
-#include "aos/common/queue_testutils.h"
+
+namespace aos {
+namespace common {
+namespace testing {
+
+// Actually declared/defined in aos/common/queue_testutils.
+void SetLogFileName(const char* filename) __attribute__((weak));
+void ForcePrintLogsDuringTests() __attribute__((weak));
+
+} // namespace testing
+} // namespace common
+} // namespace aos
GTEST_API_ int main(int argc, char **argv) {
static const struct option long_options[] = {
@@ -36,11 +47,15 @@
break;
case 'p':
- aos::common::testing::ForcePrintLogsDuringTests();
+ if (::aos::common::testing::ForcePrintLogsDuringTests) {
+ ::aos::common::testing::ForcePrintLogsDuringTests();
+ }
break;
case 'o':
- aos::common::testing::SetLogFileName(optarg);
+ if (::aos::common::testing::SetLogFileName) {
+ ::aos::common::testing::SetLogFileName(optarg);
+ }
break;
case '?':
diff --git a/aos/linux_code/ipc_lib/aos_sync.cc b/aos/linux_code/ipc_lib/aos_sync.cc
index efda532..311d88d 100644
--- a/aos/linux_code/ipc_lib/aos_sync.cc
+++ b/aos/linux_code/ipc_lib/aos_sync.cc
@@ -155,21 +155,10 @@
}
// Returns true if it succeeds and false if it fails.
-// This is the same as __sync_bool_compare_and_swap, except it fixes that being
-// broken under tsan.
+// This is the same as __sync_bool_compare_and_swap, except it provides an easy
+// place to switch implementations and/or work around bugs.
inline bool compare_and_swap(aos_futex *f, uint32_t before, uint32_t after) {
-#ifdef AOS_SANITIZER_thread
- // TODO(brians): Figure out how exactly tsan breaks this and fix it + make
- // sure our workaround actually works.
- // This workaround is unsafe in the general case, but does not change the
- // effect in our specific case if the primitive works correctly (and seems to
- // still do the right thing even when tsan's version falsely reports failing).
- if (__atomic_load_n(f, __ATOMIC_SEQ_CST) == after) return false;
- if (__sync_bool_compare_and_swap(f, before, after)) return true;
- return __atomic_load_n(f, __ATOMIC_SEQ_CST) == after;
-#else
return __sync_bool_compare_and_swap(f, before, after);
-#endif
}
pid_t do_get_tid() {
@@ -267,6 +256,11 @@
}
}
+#ifdef AOS_SANITIZER_thread
+ // Help tsan figure out that we're synchronizing on this.
+ __sync_fetch_and_add(&m->futex, 0);
+#endif
+
return 0;
}
@@ -417,6 +411,10 @@
return -1;
}
}
+#ifdef AOS_SANITIZER_thread
+ // Help tsan figure out that we're synchronizing on this.
+ __sync_add_and_fetch(m, 0);
+#endif
return 0;
}
diff --git a/aos/linux_code/ipc_lib/event.cc b/aos/linux_code/ipc_lib/event.cc
new file mode 100644
index 0000000..e95b45e
--- /dev/null
+++ b/aos/linux_code/ipc_lib/event.cc
@@ -0,0 +1,35 @@
+#include "aos/common/event.h"
+
+#include "aos/common/type_traits.h"
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+
+Event::Event() : impl_(0) {
+ static_assert(shm_ok<Event>::value,
+ "Event is not safe for use in shared memory.");
+}
+
+void Event::Wait() {
+ int ret;
+ do {
+ ret = futex_wait(&impl_);
+ } while (ret == 1);
+ if (ret == 0) return;
+ CHECK_EQ(-1, ret);
+ PLOG(FATAL, "futex_wait(%p) failed", &impl_);
+}
+
+// We're not going to expose the number woken because that's not easily portable
+// to condition variable-based implementations.
+void Event::Set() {
+ if (futex_set(&impl_) == -1) {
+ PLOG(FATAL, "futex_set(%p) failed", &impl_);
+ }
+}
+
+bool Event::Clear() {
+ return !futex_unset(&impl_);
+}
+
+} // namespace aos
diff --git a/aos/linux_code/logging/linux_logging.cc b/aos/linux_code/logging/linux_logging.cc
index c27e94b..b13a740 100644
--- a/aos/linux_code/logging/linux_logging.cc
+++ b/aos/linux_code/logging/linux_logging.cc
@@ -108,7 +108,7 @@
if (__builtin_expect(dropped_messages > 0, false)) {
::aos::time::Time message_time =
::aos::time::Time(msg->seconds, msg->nseconds);
- if (backoff_start - message_time < kDropBackoff) {
+ if (message_time - backoff_start < kDropBackoff) {
++dropped_messages;
queue->FreeMessage(msg);
return;
diff --git a/aos/linux_code/starter/starter.sh b/aos/linux_code/starter/starter.sh
index ee2ae1f..b81bf62 100755
--- a/aos/linux_code/starter/starter.sh
+++ b/aos/linux_code/starter/starter.sh
@@ -3,5 +3,7 @@
# NI already has a core pattern, so we probably shouldn't change it.
#echo '/home/driver/tmp/robot_logs/%e-%s-%p-%t.coredump' > /proc/sys/kernel/core_pattern
-export PATH=$PATH:/home/admin/robot_code
-exec starter_exe /home/admin/robot_code/start_list.txt
+while true; do
+ export PATH=$PATH:/home/admin/robot_code
+ starter_exe /home/admin/robot_code/start_list.txt
+done
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 74c9060..068836c 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -231,7 +231,6 @@
claw_position() <= values.claw.wrist.lower_hard_limit) {
LOG(ERROR, "Claw at %f out of bounds [%f, %f].\n", claw_position(),
values.claw.wrist.lower_limit, values.claw.wrist.upper_limit);
- state_ = ESTOP;
}
}
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 7696d3b..8da1c20 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -14,11 +14,11 @@
// Angle of wrist joint.
double angle;
// Angular velocity of wrist.
- double angular_velocity;
+ float angular_velocity;
// Maximum profile velocity, or 0 for the default.
- double max_velocity;
+ float max_velocity;
// Maximum profile acceleration, or 0 for the default.
- double max_acceleration;
+ float max_acceleration;
// Voltage of intake rollers. Positive means sucking in, negative means
// spitting out.
double intake;
@@ -54,11 +54,11 @@
// Estimated angle of wrist joint.
double angle;
// Estimated angular velocity of wrist.
- double angular_velocity;
+ float angular_velocity;
// Goal angle of wrist joint.
double goal_angle;
- double goal_velocity;
+ float goal_velocity;
// Voltage of intake rollers. Positive means sucking in, negative means
// spitting out.
double intake;
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 213a92c..b95bf12 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -25,7 +25,7 @@
StateFeedbackController<2, 1, 1> MakeClawController() {
Eigen::Matrix<double, 2, 1> L;
- L << 0.165471565307, 0.0290418032388;
+ L << 0.998251427366, 26.9874526231;
Eigen::Matrix<double, 1, 2> K;
K << 74.4310031124, 4.72251126222;
Eigen::Matrix<double, 2, 2> A_inv;
diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
index 637f499..257374d 100644
--- a/frc971/control_loops/fridge/fridge.q
+++ b/frc971/control_loops/fridge/fridge.q
@@ -42,19 +42,19 @@
double height;
// Angular velocity of the arm.
- double angular_velocity;
+ float angular_velocity;
// Linear velocity of the elevator.
- double velocity;
+ float velocity;
// Maximum arm profile angular velocity or 0 for the default.
- double max_angular_velocity;
+ float max_angular_velocity;
// Maximum elevator profile velocity or 0 for the default.
- double max_velocity;
+ float max_velocity;
// Maximum arm profile acceleration or 0 for the default.
- double max_angular_acceleration;
+ float max_angular_acceleration;
// Maximum elevator profile acceleration or 0 for the default.
- double max_acceleration;
+ float max_acceleration;
// X position of the fridge.
double x;
@@ -62,19 +62,19 @@
double y;
// Velocity of the x position of the fridge.
- double x_velocity;
+ float x_velocity;
// Velocity of the y position of the fridge.
- double y_velocity;
+ float y_velocity;
// Maximum x profile velocity or 0 for the default.
- double max_x_velocity;
+ float max_x_velocity;
// Maximum y profile velocity or 0 for the default.
- double max_y_velocity;
+ float max_y_velocity;
// Maximum x profile acceleration or 0 for the default.
- double max_x_acceleration;
+ float max_x_acceleration;
// Maximum y profile acceleration or 0 for the default.
- double max_y_acceleration;
+ float max_y_acceleration;
// TODO(austin): Do I need acceleration here too?
@@ -93,34 +93,34 @@
// Estimated angle of the arm.
double angle;
// Estimated angular velocity of the arm.
- double angular_velocity;
+ float angular_velocity;
// Estimated height of the elevator.
double height;
// Estimated velocity of the elvator.
- double velocity;
+ float velocity;
// state of the grabber pistons
GrabberPistons grabbers;
// Goal angle and velocity of the arm.
double goal_angle;
- double goal_angular_velocity;
+ float goal_angular_velocity;
// Goal height and velocity of the elevator.
double goal_height;
- double goal_velocity;
+ float goal_velocity;
// Estimated X/Y position of the fridge.
// These are translated directly from the height/angle statuses.
double x;
double y;
- double x_velocity;
- double y_velocity;
+ float x_velocity;
+ float y_velocity;
// X/Y goals of the fridge.
// These are translated directly from the height/angle goals.
double goal_x;
double goal_y;
- double goal_x_velocity;
- double goal_y_velocity;
+ float goal_x_velocity;
+ float goal_y_velocity;
// If true, we have aborted.
bool estopped;
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index fd123a3..4ca50e9 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -86,12 +86,12 @@
print 'L is', self.L
- q_pos = 0.018
- q_vel = 0.05
+ q_pos = 0.05
+ q_vel = 2.65
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
[0.0, (q_vel ** 2.0)]])
- r_volts = 0.1
+ r_volts = 0.025
self.R = numpy.matrix([[(r_volts ** 2.0)]])
self.KalmanGain, self.Q_steady = controls.kalman(