Merge "Add stuff to joystick_reader for robot bringup."
diff --git a/NO_BUILD_AMD64 b/NO_BUILD_AMD64
index 391b1cb..2536534 100644
--- a/NO_BUILD_AMD64
+++ b/NO_BUILD_AMD64
@@ -1,5 +1,4 @@
-//aos/externals:wpilib
--//aos/externals:wpilib_2015
-//third_party/allwpilib_2016/...
-//third_party/ntcore_2016/...
-//frc971/wpilib/...
diff --git a/WORKSPACE b/WORKSPACE
index 964fce2..d4b58f9 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -71,3 +71,34 @@
name = 'ni-libraries',
actual = '@allwpilib_ni_libraries_repo//:ni-libraries',
)
+
+# Downloaded from:
+# https://pypi.python.org/packages/source/s/six/six-1.10.0.tar.gz
+new_http_archive(
+ name = 'six_repo',
+ build_file = 'debian/six.BUILD',
+ sha256 = '105f8d68616f8248e24bf0e9372ef04d3cc10104f1980f54d57b2ce73a5ad56a',
+ url = 'http://frc971.org/Build-Dependencies/six-1.10.0.tar.gz',
+ strip_prefix = 'six-1.10.0',
+)
+
+# For protobuf. Don't use these.
+bind(
+ name = 'six',
+ actual = '@six_repo//:six',
+)
+bind(
+ name = 'gtest',
+ actual = '//third_party/googletest:googlemock',
+)
+bind(
+ name = 'gtest_main',
+ actual = '//third_party/googletest:googlemock_main',
+)
+
+new_http_archive(
+ name = 'python_import_helpers',
+ build_file = 'third_party/python_import_helpers.BUILD',
+ url = 'http://frc971.org/Build-Dependencies/empty.tar.gz',
+ sha256 = '71939a7d75585a57d2e99a33d39f391764d8f930f9a16acf32e00c5d3f432aa0',
+)
diff --git a/aos/common/controls/control_loop_test.cc b/aos/common/controls/control_loop_test.cc
index 6c881f2..ecaafab 100644
--- a/aos/common/controls/control_loop_test.cc
+++ b/aos/common/controls/control_loop_test.cc
@@ -25,7 +25,8 @@
}
void ControlLoopTest::SendMessages(bool enabled) {
- if (current_time_ - last_ds_time_ >= kDSPacketTime) {
+ if (current_time_ - last_ds_time_ >= kDSPacketTime ||
+ last_enabled_ != enabled) {
last_ds_time_ = current_time_;
auto new_state = ::aos::joystick_state.MakeMessage();
new_state->fake = true;
@@ -35,6 +36,7 @@
new_state->team_id = team_id_;
new_state.Send();
+ last_enabled_ = enabled;
}
{
diff --git a/aos/common/controls/control_loop_test.h b/aos/common/controls/control_loop_test.h
index 30115b8..4ab4774 100644
--- a/aos/common/controls/control_loop_test.h
+++ b/aos/common/controls/control_loop_test.h
@@ -53,6 +53,8 @@
::aos::time::Time current_time_ = ::aos::time::Time::InSeconds(0);
::aos::testing::TestSharedMemory my_shm_;
+
+ bool last_enabled_ = false;
};
} // namespace testing
diff --git a/aos/externals/BUILD b/aos/externals/BUILD
index d85de28..4a03d83 100644
--- a/aos/externals/BUILD
+++ b/aos/externals/BUILD
@@ -1,55 +1,8 @@
-_header_dirs = [
- 'allwpilib/wpilibc/wpilibC++/include',
- 'allwpilib/wpilibc/wpilibC++Devices/include',
- 'allwpilib/hal/include',
- 'allwpilib/hal/lib/Athena/FRC_FPGA_ChipObject',
- 'allwpilib/hal/lib/Athena',
-]
-
-cc_library(
- name = 'wpilib_2015',
- visibility = ['//visibility:public'],
- srcs = glob([
- 'allwpilib/wpilibc/wpilibC++/src/*.cpp',
- 'allwpilib/wpilibc/wpilibC++Devices/src/*.cpp',
- 'allwpilib/wpilibc/wpilibC++Devices/src/Internal/*.cpp',
- 'allwpilib/hal/lib/Athena/*.cpp',
- 'allwpilib/hal/lib/Athena/ctre/*.cpp',
- ], [
- # This looks like an older version of the file, so we want to use the other
- # one.
- 'allwpilib/wpilibc/wpilibC++Devices/include/NetworkCommunication/FRCComm.h',
- ]) + [
- 'allwpilib/hal/lib/Athena/NetworkCommunication/FRCComm.h',
- 'allwpilib/ni-libraries/libFRC_NetworkCommunication.so.1.5.0',
- 'allwpilib/ni-libraries/libRoboRIO_FRC_ChipObject.so.1.2.0',
- 'allwpilib/ni-libraries/libNiFpgaLv.so.14.0.0',
- 'allwpilib/ni-libraries/libNiFpga.so.14.0.0',
- 'allwpilib/ni-libraries/libNiRioSrv.so.14.0.0',
- 'allwpilib/ni-libraries/libspi.so.1.0.0',
- 'allwpilib/ni-libraries/libi2c.so.1.0.0',
- ],
- defines = [
- 'WPILIB2015=1',
- ],
- copts = [
- '-Wno-error',
- '-Wno-unused-parameter',
- '-Wno-switch-enum',
- ],
- hdrs = glob([d + '/**/*.h' for d in _header_dirs] + [d + '/**/*.hpp' for d in _header_dirs]),
- includes = _header_dirs,
- linkopts = [
- '-lpthread',
- ],
-)
-
cc_library(
name = 'wpilib',
visibility = ['//visibility:public'],
deps = [
- ':wpilib_2015',
- #'//third_party/allwpilib_2016:wpilib',
+ '//third_party/allwpilib_2016:wpilib',
],
includes = [
'forwpilib',
diff --git a/build_tests/BUILD b/build_tests/BUILD
index cce2e01..be6a201 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -1,5 +1,6 @@
load('/tools/build_rules/ruby', 'ruby_binary')
load('/aos/build/queues', 'queue_library')
+load('/tools/build_rules/protobuf', 'proto_cc_library')
cc_test(
name = 'gflags_build_test',
@@ -69,3 +70,20 @@
],
size = 'small',
)
+
+proto_cc_library(
+ name = 'proto_build_test_library',
+ src = 'proto.proto',
+)
+
+cc_test(
+ name = 'proto_build_test',
+ srcs = [
+ 'proto.cc',
+ ],
+ deps = [
+ ':proto_build_test_library',
+ '//aos/testing:googletest',
+ ],
+ size = 'small',
+)
diff --git a/build_tests/proto.cc b/build_tests/proto.cc
new file mode 100644
index 0000000..23ff879
--- /dev/null
+++ b/build_tests/proto.cc
@@ -0,0 +1,16 @@
+#include "gtest/gtest.h"
+
+#include "build_tests/proto.pb.h"
+
+TEST(ProtoBuildTest, Serialize) {
+ ::frc971::TestProto test_proto1, test_proto2;
+ test_proto1.set_s("Hi!");
+ test_proto1.set_i(971);
+
+ ::std::string serialized;
+ ASSERT_TRUE(test_proto1.SerializeToString(&serialized));
+ ASSERT_TRUE(test_proto2.ParseFromString(serialized));
+
+ EXPECT_EQ("Hi!", test_proto2.s());
+ EXPECT_EQ(971, test_proto2.i());
+}
diff --git a/build_tests/proto.proto b/build_tests/proto.proto
new file mode 100644
index 0000000..367a650
--- /dev/null
+++ b/build_tests/proto.proto
@@ -0,0 +1,12 @@
+syntax = "proto3";
+
+package frc971;
+
+import "google/protobuf/empty.proto";
+
+message TestProto {
+ string s = 1;
+ int32 i = 2;
+ // Making sure that well-known protos work.
+ .google.protobuf.Empty empty = 3;
+}
diff --git a/debian/six.BUILD b/debian/six.BUILD
new file mode 100644
index 0000000..87cd318
--- /dev/null
+++ b/debian/six.BUILD
@@ -0,0 +1,7 @@
+py_library(
+ name = 'six',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'six.py',
+ ],
+)
diff --git a/frc971/analysis/analysis.py b/frc971/analysis/analysis.py
index e477bdd..48cc8bc 100755
--- a/frc971/analysis/analysis.py
+++ b/frc971/analysis/analysis.py
@@ -44,6 +44,8 @@
None
"""
pline = ParseLine(line)
+ pline_data = None
+
for key in self.signal:
value = self.signal[key]
binary = key[0]
@@ -61,10 +63,12 @@
# Make sure that we're looking at the right binary structure instance.
if binary == pline.name:
if pline.msg.startswith(struct_instance_name + ': '):
- # Parse the structure and traverse it as specified in
- # `data_search_path`. This lets the user access very deeply nested
- # structures.
- _, _, data = pline.ParseStruct()
+ # Parse the structure once.
+ if pline_data is None:
+ _, _, pline_data = pline.ParseStruct()
+ # Traverse the structure as specified in `data_search_path`.
+ # This lets the user access very deeply nested structures.
+ data = pline_data
for path in data_search_path:
data = data[path]
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 369de78..ee1efdd 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -115,7 +115,7 @@
'//frc971/control_loops:state_feedback_loop',
'//frc971/queues:gyro',
'//aos/common:queues',
- '//y2014:constants',
- '//y2014/control_loops/drivetrain:polydrivetrain_plants',
+ '//y2016:constants',
+ '//y2016/control_loops/drivetrain:polydrivetrain_plants',
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index ca395cb..50c5df7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -47,6 +47,14 @@
kf_.set_controller_index(dt_openloop_.controller_index());
+ bool gyro_valid = false;
+ if (gyro_reading.FetchLatest()) {
+ LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+ last_gyro_heading_ = gyro_reading->angle;
+ last_gyro_rate_ = gyro_reading->velocity;
+ gyro_valid = true;
+ }
+
{
Eigen::Matrix<double, 3, 1> Y;
Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
@@ -75,12 +83,9 @@
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
- if (gyro_reading.FetchLatest()) {
- LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+ if (gyro_valid) {
dt_closedloop_.SetPosition(left_encoder, right_encoder,
gyro_reading->angle);
- last_gyro_heading_ = gyro_reading->angle;
- last_gyro_rate_ = gyro_reading->velocity;
} else {
dt_closedloop_.SetRawPosition(left_encoder, right_encoder);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index dda65bd..a2d9fdd 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -37,7 +37,6 @@
double v; // Motor velocity constant.
double t; // Torque constant.
- double turn_width; // Robot turn width, in meters.
// Gear ratios, from wheel to motor shaft.
double high_gear_ratio;
double low_gear_ratio;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 9d7e223..554a93e 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -7,15 +7,16 @@
#include "aos/common/controls/polytope.h"
#include "aos/common/controls/control_loop_test.h"
-#include "y2014/constants.h"
+#include "y2016/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "y2014/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2016/constants.h"
#include "frc971/queues/gyro.q.h"
namespace frc971 {
@@ -23,57 +24,42 @@
namespace drivetrain {
namespace testing {
-using ::y2014::control_loops::drivetrain::MakeDrivetrainPlant;
+using ::y2016::control_loops::drivetrain::MakeDrivetrainPlant;
// TODO(Comran): Make one that doesn't depend on the actual values for a
// specific robot.
-const DrivetrainConfig kDrivetrainConfig {
- ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
+const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0,
+ 0.0, 0.25, 0.75};
- ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
- ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop,
+const DrivetrainConfig &GetDrivetrainConfig() {
+ static DrivetrainConfig kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
- ::y2014::control_loops::drivetrain::kDt,
- ::y2014::control_loops::drivetrain::kStallTorque,
- ::y2014::control_loops::drivetrain::kStallCurrent,
- ::y2014::control_loops::drivetrain::kFreeSpeedRPM,
- ::y2014::control_loops::drivetrain::kFreeCurrent,
- ::y2014::control_loops::drivetrain::kJ,
- ::y2014::control_loops::drivetrain::kMass,
- ::y2014::control_loops::drivetrain::kRobotRadius,
- ::y2014::control_loops::drivetrain::kWheelRadius,
- ::y2014::control_loops::drivetrain::kR,
- ::y2014::control_loops::drivetrain::kV,
- ::y2014::control_loops::drivetrain::kT,
+ ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
- ::y2014::constants::GetValuesForTeam(971).turn_width,
- ::y2014::constants::GetValuesForTeam(971).high_gear_ratio,
- ::y2014::constants::GetValuesForTeam(971).low_gear_ratio,
- ::y2014::constants::GetValuesForTeam(971).left_drive,
- ::y2014::constants::GetValuesForTeam(971).right_drive
+ ::y2016::control_loops::drivetrain::kDt,
+ ::y2016::control_loops::drivetrain::kStallTorque,
+ ::y2016::control_loops::drivetrain::kStallCurrent,
+ ::y2016::control_loops::drivetrain::kFreeSpeedRPM,
+ ::y2016::control_loops::drivetrain::kFreeCurrent,
+ ::y2016::control_loops::drivetrain::kJ,
+ ::y2016::control_loops::drivetrain::kMass,
+ ::y2016::control_loops::drivetrain::kRobotRadius,
+ ::y2016::control_loops::drivetrain::kWheelRadius,
+ ::y2016::control_loops::drivetrain::kR,
+ ::y2016::control_loops::drivetrain::kV,
+ ::y2016::control_loops::drivetrain::kT,
+ ::y2016::control_loops::drivetrain::kHighGearRatio,
+ ::y2016::control_loops::drivetrain::kLowGearRatio,
+
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter};
+
+ return kDrivetrainConfig;
};
-class Environment : public ::testing::Environment {
- public:
- virtual ~Environment() {}
- // how to set up the environment.
- virtual void SetUp() {
- aos::controls::HPolytope<0>::Init();
- }
-};
-::testing::Environment* const holder_env =
- ::testing::AddGlobalTestEnvironment(new Environment);
-
-class TeamNumberEnvironment : public ::testing::Environment {
- public:
- // Override this to define how to set up the environment.
- virtual void SetUp() { aos::network::OverrideTeamNumber(971); }
-};
-
-::testing::Environment* const team_number_env =
- ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
-
// Class which simulates the drivetrain and sends out queue messages containing
// the position.
class DrivetrainSimulation {
@@ -83,11 +69,11 @@
DrivetrainSimulation()
: drivetrain_plant_(
new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
- my_drivetrain_queue_(".frc971.control_loops.drivetrain",
- 0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
- ".frc971.control_loops.drivetrain.position",
- ".frc971.control_loops.drivetrain.output",
- ".frc971.control_loops.drivetrain.status") {
+ my_drivetrain_queue_(".frc971.control_loops.drivetrain", 0x8a8dde77,
+ ".frc971.control_loops.drivetrain.goal",
+ ".frc971.control_loops.drivetrain.position",
+ ".frc971.control_loops.drivetrain.output",
+ ".frc971.control_loops.drivetrain.status") {
Reinitialize();
}
@@ -128,6 +114,7 @@
}
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
+
private:
::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
double last_left_position_;
@@ -145,14 +132,14 @@
DrivetrainLoop drivetrain_motor_;
DrivetrainSimulation drivetrain_motor_plant_;
- DrivetrainTest() : my_drivetrain_queue_(".frc971.control_loops.drivetrain",
- 0x8a8dde77,
- ".frc971.control_loops.drivetrain.goal",
- ".frc971.control_loops.drivetrain.position",
- ".frc971.control_loops.drivetrain.output",
- ".frc971.control_loops.drivetrain.status"),
- drivetrain_motor_(kDrivetrainConfig, &my_drivetrain_queue_),
- drivetrain_motor_plant_() {
+ DrivetrainTest()
+ : my_drivetrain_queue_(".frc971.control_loops.drivetrain", 0x8a8dde77,
+ ".frc971.control_loops.drivetrain.goal",
+ ".frc971.control_loops.drivetrain.position",
+ ".frc971.control_loops.drivetrain.output",
+ ".frc971.control_loops.drivetrain.status"),
+ drivetrain_motor_(GetDrivetrainConfig(), &my_drivetrain_queue_),
+ drivetrain_motor_plant_() {
::frc971::sensors::gyro_reading.Clear();
}
@@ -160,23 +147,21 @@
my_drivetrain_queue_.goal.FetchLatest();
my_drivetrain_queue_.position.FetchLatest();
EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
- drivetrain_motor_plant_.GetLeftPosition(),
- 1e-2);
+ drivetrain_motor_plant_.GetLeftPosition(), 1e-2);
EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
- drivetrain_motor_plant_.GetRightPosition(),
- 1e-2);
+ drivetrain_motor_plant_.GetRightPosition(), 1e-2);
}
- virtual ~DrivetrainTest() {
- ::frc971::sensors::gyro_reading.Clear();
- }
+ virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
};
// Tests that the drivetrain converges on a goal.
TEST_F(DrivetrainTest, ConvergesCorrectly) {
- my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .control_loop_driving(true)
.left_goal(-1.0)
- .right_goal(1.0).Send();
+ .right_goal(1.0)
+ .Send();
for (int i = 0; i < 200; ++i) {
drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
@@ -188,9 +173,11 @@
// Tests that it survives disabling.
TEST_F(DrivetrainTest, SurvivesDisabling) {
- my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .control_loop_driving(true)
.left_goal(-1.0)
- .right_goal(1.0).Send();
+ .right_goal(1.0)
+ .Send();
for (int i = 0; i < 500; ++i) {
drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
@@ -270,14 +257,14 @@
double x2_min, double x2_max) {
Eigen::Matrix<double, 4, 2> box_H;
box_H << /*[[*/ 1.0, 0.0 /*]*/,
- /*[*/-1.0, 0.0 /*]*/,
- /*[*/ 0.0, 1.0 /*]*/,
- /*[*/ 0.0,-1.0 /*]]*/;
+ /*[*/ -1.0, 0.0 /*]*/,
+ /*[*/ 0.0, 1.0 /*]*/,
+ /*[*/ 0.0, -1.0 /*]]*/;
Eigen::Matrix<double, 4, 1> box_k;
box_k << /*[[*/ x1_max /*]*/,
- /*[*/-x1_min /*]*/,
- /*[*/ x2_max /*]*/,
- /*[*/-x2_min /*]]*/;
+ /*[*/ -x1_min /*]*/,
+ /*[*/ x2_max /*]*/,
+ /*[*/ -x2_min /*]]*/;
::aos::controls::HPolytope<2> t_poly(box_H, box_k);
return t_poly;
}
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 9fdfe1c..19bbef1 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -139,8 +139,7 @@
}
void DrivetrainMotorsSS::SetPosition(double left, double right, double gyro) {
// Decay the offset quickly because this gyro is great.
- const double offset =
- (right - left - gyro * dt_config_.turn_width) / 2.0;
+ const double offset = (right - left) / 2.0 - gyro * dt_config_.robot_radius;
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
SetRawPosition(left, right);
diff --git a/third_party/googletest/BUILD b/third_party/googletest/BUILD
index f342e66..fcec287 100644
--- a/third_party/googletest/BUILD
+++ b/third_party/googletest/BUILD
@@ -95,6 +95,15 @@
)
cc_library(
+ name = "googlemock_main",
+ visibility = ["//visibility:public"],
+ deps = [
+ ":googlemock",
+ ":googletest_main",
+ ],
+)
+
+cc_library(
name = "googletest_sample_libs",
hdrs = [
"googletest/samples/prime_tables.h",
diff --git a/third_party/protobuf/BUILD b/third_party/protobuf/BUILD
index 3cac4a8..6686800 100644
--- a/third_party/protobuf/BUILD
+++ b/third_party/protobuf/BUILD
@@ -2,18 +2,34 @@
licenses(["notice"])
+load("/tools/build_rules/select", "compiler_select")
+
################################################################################
# Protobuf Runtime Library
################################################################################
COPTS = [
"-DHAVE_PTHREAD",
+ "-DGOOGLE_THIRD_PARTY_PROTOBUF",
"-Wall",
"-Wwrite-strings",
"-Woverloaded-virtual",
"-Wno-sign-compare",
- "-Wno-error=unused-function",
-]
+ "-Wno-unused-function",
+ "-Wno-unused-parameter",
+ "-Wno-format-nonliteral",
+ "-Wno-switch-enum",
+ "-Wno-missing-field-initializers",
+ "-Wno-ignored-qualifiers",
+] + compiler_select({
+ "gcc": [
+ "-Wno-error=cast-align",
+ ],
+ "clang": [
+ "-Wno-unused-const-variable",
+ "-Wno-unused-private-field",
+ ],
+})
# Bazel should provide portable link_opts for pthread.
LINK_OPTS = ["-lpthread"]
@@ -127,6 +143,7 @@
deps = [":protobuf_lite"],
)
+'''
objc_library(
name = "protobuf_objc",
hdrs = ["objectivec/GPBProtocolBuffers.h"],
@@ -134,6 +151,7 @@
non_arc_srcs = ["objectivec/GPBProtocolBuffers.m"],
visibility = ["//visibility:public"],
)
+'''
RELATIVE_WELL_KNOWN_PROTOS = [
# AUTOGEN(well_known_protos)
@@ -153,6 +171,12 @@
WELL_KNOWN_PROTOS = ["src/" + s for s in RELATIVE_WELL_KNOWN_PROTOS]
+filegroup(
+ name = "well_known_protos",
+ srcs = WELL_KNOWN_PROTOS,
+ visibility = ["//visibility:public"],
+)
+
cc_proto_library(
name = "cc_wkt_protos",
srcs = WELL_KNOWN_PROTOS,
@@ -342,6 +366,7 @@
default_runtime = ":protobuf",
protoc = ":protoc",
deps = [":cc_wkt_protos"],
+ copts = COPTS,
)
COMMON_TEST_SRCS = [
@@ -366,6 +391,7 @@
":protoc_lib",
"//external:gtest",
],
+ copts = COPTS,
)
cc_test(
@@ -465,7 +491,7 @@
"wellknown.srcjar"
],
cmd = "$(location :protoc) --java_out=$(@D)/wellknown.jar" +
- " -Isrc $(SRCS) " +
+ " -Ithird_party/protobuf/src $(SRCS) " +
" && mv $(@D)/wellknown.jar $(@D)/wellknown.srcjar",
tools = [":protoc"],
)
@@ -517,6 +543,7 @@
include = "python",
)
+'''
cc_binary(
name = "internal/_api_implementation.so",
srcs = ["python/google/protobuf/internal/api_implementation.cc"],
@@ -527,7 +554,7 @@
linkstatic = 1,
deps = select({
"//conditions:default": [],
- ":use_fast_cpp_protos": ["//util/python:python_headers"],
+ ":use_fast_cpp_protos": ["//third_party/protobuf/util/python:python_headers"],
}),
)
@@ -550,9 +577,10 @@
":protobuf",
] + select({
"//conditions:default": [],
- ":use_fast_cpp_protos": ["//util/python:python_headers"],
+ ":use_fast_cpp_protos": ["//third_party/protobuf/util/python:python_headers"],
}),
)
+'''
config_setting(
name = "use_fast_cpp_protos",
diff --git a/third_party/protobuf/protobuf.bzl b/third_party/protobuf/protobuf.bzl
index f674a6c..0cc195b 100644
--- a/third_party/protobuf/protobuf.bzl
+++ b/third_party/protobuf/protobuf.bzl
@@ -1,14 +1,15 @@
# -*- mode: python; -*- PYTHON-PREPROCESSING-REQUIRED
def _GetPath(ctx, path):
- if ctx.label.workspace_root:
- return ctx.label.workspace_root + '/' + path
- else:
- return path
+ if str(ctx.label).startswith('@'):
+ fail('External labels not supported for now')
+ return path
def _GenDir(ctx):
if not ctx.attr.includes:
- return ctx.label.workspace_root
+ if str(ctx.label).startswith('@'):
+ fail('External labels not supported for now')
+ return ''
if not ctx.attr.includes[0]:
return _GetPath(ctx, ctx.label.package)
if not ctx.label.package:
@@ -34,13 +35,17 @@
path = path[len(include):]
- if not path.startswith(PACKAGE_NAME):
- fail("The package %s is not within the path %s" % (PACKAGE_NAME, path))
+ package_name = PACKAGE_NAME
+ if not package_name.startswith('third_party/protobuf'):
+ fail('The package %s is not a protobuf package' % package_name)
+ package_name = package_name[len('third_party/protobuf/'):]
+ if not path.startswith(package_name):
+ fail("The package %s is not within the path %s" % (package_name, path))
- if not PACKAGE_NAME:
+ if not package_name:
return path
- return path[len(PACKAGE_NAME)+1:]
+ return path[len(package_name)+1:]
def _proto_gen_impl(ctx):
"""General implementation for generating protos"""
@@ -265,6 +270,7 @@
if default_runtime and not default_runtime in py_libs + deps:
py_libs += [default_runtime]
+ py_libs += ['@python_import_helpers//:google_protobuf_importer']
native.py_library(
name=name,
diff --git a/third_party/protobuf/python/google/protobuf/internal/test_util.py b/third_party/protobuf/python/google/protobuf/internal/test_util.py
index ac88fa8..091cb61 100755
--- a/third_party/protobuf/python/google/protobuf/internal/test_util.py
+++ b/third_party/protobuf/python/google/protobuf/internal/test_util.py
@@ -596,9 +596,9 @@
# Search up the directory tree looking for the C++ protobuf source code.
path = '.'
while os.path.exists(path):
- if os.path.exists(os.path.join(path, 'src/google/protobuf')):
+ if os.path.exists(os.path.join(path, 'third_party/protobuf/src/google/protobuf')):
# Found it. Load the golden file from the testdata directory.
- full_path = os.path.join(path, 'src/google/protobuf/testdata', filename)
+ full_path = os.path.join(path, 'third_party/protobuf/src/google/protobuf/testdata', filename)
return open(full_path, 'rb')
path = os.path.join(path, '..')
diff --git a/third_party/protobuf/util/python/BUILD b/third_party/protobuf/util/python/BUILD
index 358c381..073ea63 100644
--- a/third_party/protobuf/util/python/BUILD
+++ b/third_party/protobuf/util/python/BUILD
@@ -1,3 +1,5 @@
+licenses(["notice"])
+
# This is a placeholder for python headers. Projects needing to use
# fast cpp protos in protobuf's python interface should build with
# --define=use_fast_cpp_protos=true, and in addition, provide
diff --git a/third_party/python_import_helpers.BUILD b/third_party/python_import_helpers.BUILD
new file mode 100644
index 0000000..38a68ec
--- /dev/null
+++ b/third_party/python_import_helpers.BUILD
@@ -0,0 +1,19 @@
+# This repository is used to create helper __init__.py files which import things
+# under third_party at the root of the Python import namespace instead of
+# requiring third_party.xyz. in front of them.
+
+genrule(
+ name = 'google_protobuf_importer_init',
+ outs = ['google/__init__.py'],
+ cmd = 'echo "%s" > $@' % '\n'.join([
+ 'import sys',
+ 'import third_party.protobuf.google.protobuf',
+ 'sys.modules[\'google.protobuf\'] = third_party.protobuf.google.protobuf',
+ ]),
+)
+
+py_library(
+ name = 'google_protobuf_importer',
+ visibility = ['//visibility:public'],
+ srcs = [':google_protobuf_importer_init'],
+)
diff --git a/tools/build_rules/protobuf.bzl b/tools/build_rules/protobuf.bzl
new file mode 100644
index 0000000..fe6af4f
--- /dev/null
+++ b/tools/build_rules/protobuf.bzl
@@ -0,0 +1,71 @@
+def _do_proto_cc_library_impl(ctx):
+ message = 'Building %s and %s from %s' % (ctx.outputs.pb_h.short_path,
+ ctx.outputs.pb_cc.short_path,
+ ctx.file.src.short_path)
+ ctx.action(
+ inputs = ctx.files.src + ctx.files._well_known_protos,
+ executable = ctx.executable._protoc,
+ arguments = [
+ '--cpp_out=%s' % ctx.configuration.genfiles_dir.path,
+ '-I.',
+ '-Ithird_party/protobuf/src',
+ ctx.file.src.path,
+ ],
+ mnemonic = 'ProtocCc',
+ progress_message = message,
+ outputs = [
+ ctx.outputs.pb_h,
+ ctx.outputs.pb_cc,
+ ],
+ )
+
+def _do_proto_cc_library_outputs(attr):
+ basename = attr.src.name[:-len('.proto')]
+ return {
+ 'pb_h': '%s.pb.h' % basename,
+ 'pb_cc': '%s.pb.cc' % basename,
+ }
+
+_do_proto_cc_library = rule(
+ implementation = _do_proto_cc_library_impl,
+ attrs = {
+ 'src': attr.label(
+ allow_files = FileType(['.proto']),
+ mandatory = True,
+ single_file = True,
+ ),
+ '_protoc': attr.label(
+ default = Label('//third_party/protobuf:protoc'),
+ executable = True,
+ cfg = HOST_CFG,
+ ),
+ '_well_known_protos': attr.label(
+ default = Label('//third_party/protobuf:well_known_protos'),
+ ),
+ },
+ outputs = _do_proto_cc_library_outputs,
+ output_to_genfiles = True,
+)
+
+def proto_cc_library(name, src, visibility = None):
+ '''Generates a cc_library from a single .proto file. Does not support
+ dependencies on any .proto files except the well-known ones protobuf comes
+ with (which are unconditionally depended on).
+
+ Attrs:
+ src: The .proto file.
+ '''
+
+ _do_proto_cc_library(
+ name = '%s__proto_srcs' % name,
+ src = src,
+ visibility = ['//visibility:private'],
+ )
+ basename = src[:-len('.proto')]
+ native.cc_library(
+ name = name,
+ srcs = [ '%s.pb.cc' % basename ],
+ hdrs = [ '%s.pb.h' % basename ],
+ deps = [ '//third_party/protobuf' ],
+ visibility = visibility,
+ )
diff --git a/y2012/joystick_reader.cc b/y2012/joystick_reader.cc
index b1af0bb..b977672 100644
--- a/y2012/joystick_reader.cc
+++ b/y2012/joystick_reader.cc
@@ -27,7 +27,7 @@
namespace joysticks {
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kShiftHigh(2, 3), kShiftLow(2, 1);
const ButtonLocation kQuickTurn(1, 5);
const ButtonLocation kCatch(3, 10);
@@ -93,10 +93,10 @@
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
- if (data.PosEdge(kShiftHigh)) {
+ if (data.PosEdge(kShiftLow)) {
is_high_gear_ = false;
}
- if (data.PosEdge(kShiftLow)) {
+ if (data.PosEdge(kShiftHigh)) {
is_high_gear_ = true;
}
if (!drivetrain_queue.goal.MakeWithBuilder()
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 4cb69ca..6c7427b 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -11,9 +11,10 @@
#include "aos/common/commonmath.h"
#include "aos/common/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/actors/drivetrain_actor.h"
#include "y2014/constants.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
namespace y2014 {
namespace actors {
@@ -29,7 +30,7 @@
const double yoffset = params.y_offset;
const double turn_offset =
- params.theta_offset * constants::GetValues().turn_width / 2.0;
+ params.theta_offset * control_loops::drivetrain::kRobotRadius;
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
@@ -41,18 +42,18 @@
profile.set_maximum_acceleration(params.maximum_acceleration);
profile.set_maximum_velocity(params.maximum_velocity);
- turn_profile.set_maximum_acceleration(params.maximum_turn_acceleration *
- constants::GetValues().turn_width /
- 2.0);
+ turn_profile.set_maximum_acceleration(
+ params.maximum_turn_acceleration *
+ control_loops::drivetrain::kRobotRadius);
turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- constants::GetValues().turn_width / 2.0);
+ control_loops::drivetrain::kRobotRadius);
while (true) {
::aos::time::PhasedLoopXMS(5, 2500);
::frc971::control_loops::drivetrain_queue.status.FetchLatest();
if (::frc971::control_loops::drivetrain_queue.status.get()) {
- const auto& status = *::frc971::control_loops::drivetrain_queue.status;
+ const auto &status = *::frc971::control_loops::drivetrain_queue.status;
if (::std::abs(status.uncapped_left_voltage -
status.uncapped_right_voltage) > 24) {
LOG(DEBUG, "spinning in place\n");
@@ -140,14 +141,16 @@
if (ShouldCancel()) return true;
const double kPositionThreshold = 0.05;
- const double left_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->filtered_left_position -
- (left_goal_state(0, 0) + params.left_initial_position));
- const double right_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->filtered_right_position -
- (right_goal_state(0, 0) + params.right_initial_position));
- const double velocity_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status->robot_speed);
+ const double left_error =
+ ::std::abs(::frc971::control_loops::drivetrain_queue.status
+ ->filtered_left_position -
+ (left_goal_state(0, 0) + params.left_initial_position));
+ const double right_error =
+ ::std::abs(::frc971::control_loops::drivetrain_queue.status
+ ->filtered_right_position -
+ (right_goal_state(0, 0) + params.right_initial_position));
+ const double velocity_error = ::std::abs(
+ ::frc971::control_loops::drivetrain_queue.status->robot_speed);
if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
velocity_error < 0.2) {
break;
@@ -162,7 +165,7 @@
}
::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2014::actors::DrivetrainActionParams& params) {
+ const ::y2014::actors::DrivetrainActionParams ¶ms) {
return ::std::unique_ptr<DrivetrainAction>(
new DrivetrainAction(&::y2014::actors::drivetrain_action, params));
}
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index e954637..9c39f27 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -10,12 +10,13 @@
#include "aos/common/actions/actions.h"
#include "frc971/autonomous/auto.q.h"
-#include "y2014/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/actors/shoot_actor.h"
#include "y2014/actors/drivetrain_actor.h"
+#include "y2014/actors/shoot_actor.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014/control_loops/shooter/shooter.q.h"
#include "y2014/queues/auto_mode.q.h"
#include "y2014/queues/hot_goal.q.h"
@@ -83,9 +84,9 @@
void StepDrive(double distance, double theta) {
double left_goal = (left_initial_position + distance -
- theta * constants::GetValues().turn_width / 2.0);
+ theta * control_loops::drivetrain::kRobotRadius);
double right_goal = (right_initial_position + distance +
- theta * constants::GetValues().turn_width / 2.0);
+ theta * control_loops::drivetrain::kRobotRadius);
::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
.highgear(true)
@@ -197,9 +198,9 @@
auto drivetrain_action = actors::MakeDrivetrainAction(params);
drivetrain_action->Start();
left_initial_position +=
- distance - theta * constants::GetValues().turn_width / 2.0;
+ distance - theta * control_loops::drivetrain::kRobotRadius;
right_initial_position +=
- distance + theta * constants::GetValues().turn_width / 2.0;
+ distance + theta * control_loops::drivetrain::kRobotRadius;
return ::std::move(drivetrain_action);
}
diff --git a/y2014/constants.cc b/y2014/constants.cc
index 8cf9076..ad6c3e6 100644
--- a/y2014/constants.cc
+++ b/y2014/constants.cc
@@ -45,8 +45,6 @@
const ShifterHallEffect kPracticeLeftDriveShifter{2.80, 3.05, 4.15, 3.2, 0.2, 0.7};
const ShifterHallEffect kPracticeRightDriveShifter{2.90, 3.75, 3.80, 2.98, 0.2, 0.7};
-const double kRobotWidth = 25.0 / 100.0 * 2.54;
-
const double shooter_zeroing_speed = 0.05;
const double shooter_unload_speed = 0.08;
@@ -63,7 +61,6 @@
kCompLeftDriveShifter,
kCompRightDriveShifter,
false,
- 0.5,
::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
5.0, // drivetrain max speed
@@ -100,7 +97,6 @@
kCompLeftDriveShifter,
kCompRightDriveShifter,
false,
- kRobotWidth,
::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
5.0, // drivetrain max speed
@@ -146,7 +142,6 @@
kPracticeLeftDriveShifter,
kPracticeRightDriveShifter,
false,
- kRobotWidth,
::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
5.0, // drivetrain max speed
diff --git a/y2014/constants.h b/y2014/constants.h
index a31e9c0..4ad06bc 100644
--- a/y2014/constants.h
+++ b/y2014/constants.h
@@ -38,8 +38,6 @@
ShifterHallEffect left_drive, right_drive;
bool clutch_transmission;
- double turn_width;
-
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
index b4ef447..1e18b68 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -25,7 +25,7 @@
drivetrain::kMass, drivetrain::kRobotRadius, drivetrain::kWheelRadius,
drivetrain::kR, drivetrain::kV, drivetrain::kT,
- constants::GetValues().turn_width, constants::GetValues().high_gear_ratio,
+ constants::GetValues().high_gear_ratio,
constants::GetValues().low_gear_ratio,
constants::GetValues().left_drive, constants::GetValues().right_drive};
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index 137856f..d2d1375 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -35,7 +35,7 @@
const ButtonLocation kDriveControlLoopEnable1(1, 7),
kDriveControlLoopEnable2(1, 11);
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kShiftHigh(2, 3), kShiftLow(2, 1);
const ButtonLocation kQuickTurn(1, 5);
const ButtonLocation kCatch(3, 10);
@@ -241,10 +241,10 @@
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
- if (data.PosEdge(kShiftHigh)) {
+ if (data.PosEdge(kShiftLow)) {
is_high_gear_ = false;
}
- if (data.PosEdge(kShiftLow)) {
+ if (data.PosEdge(kShiftHigh)) {
is_high_gear_ = true;
}
}
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index 3e6e3e2..6dfcbb4 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -30,7 +30,7 @@
// Joystick & button addresses.
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kShiftHigh(2, 3), kShiftLow(2, 1);
const ButtonLocation kQuickTurn(1, 5);
const ButtonLocation kFrontRollersIn(3, 8);
@@ -75,11 +75,11 @@
LOG(WARNING, "sending stick values failed\n");
}
- if (data.PosEdge(kShiftHigh)) {
+ if (data.PosEdge(kShiftLow)) {
is_high_gear_ = false;
}
- if (data.PosEdge(kShiftLow)) {
+ if (data.PosEdge(kShiftHigh)) {
is_high_gear_ = true;
}
}
diff --git a/y2015/joystick_reader.cc b/y2015/joystick_reader.cc
index f949c1a..4b5ced8 100644
--- a/y2015/joystick_reader.cc
+++ b/y2015/joystick_reader.cc
@@ -65,7 +65,7 @@
constexpr actors::ProfileParams kElevatorMove{0.3, 1.0};
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kShiftHigh(2, 3), kShiftLow(2, 1);
const ButtonLocation kQuickTurn(1, 5);
//const ButtonLocation kClawClosed(3, 5);
diff --git a/y2015_bot3/joystick_reader.cc b/y2015_bot3/joystick_reader.cc
index be9bd7f..a65bfa9 100644
--- a/y2015_bot3/joystick_reader.cc
+++ b/y2015_bot3/joystick_reader.cc
@@ -48,7 +48,7 @@
// Joystick & button addresses.
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kShiftHigh(2, 3), kShiftLow(2, 1);
const ButtonLocation kQuickTurn(1, 5);
// essential
diff --git a/y2016/actors/drivetrain_actor.cc b/y2016/actors/drivetrain_actor.cc
index 94835c8..325f6e4 100644
--- a/y2016/actors/drivetrain_actor.cc
+++ b/y2016/actors/drivetrain_actor.cc
@@ -30,7 +30,7 @@
const double yoffset = params.y_offset;
const double turn_offset =
- params.theta_offset * constants::Values::kTurnWidth / 2.0;
+ params.theta_offset * control_loops::drivetrain::kRobotRadius;
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
@@ -42,11 +42,11 @@
profile.set_maximum_acceleration(params.maximum_acceleration);
profile.set_maximum_velocity(params.maximum_velocity);
- turn_profile.set_maximum_acceleration(params.maximum_turn_acceleration *
- constants::Values::kTurnWidth /
- 2.0);
+ turn_profile.set_maximum_acceleration(
+ params.maximum_turn_acceleration *
+ control_loops::drivetrain::kRobotRadius);
turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- constants::Values::kTurnWidth / 2.0);
+ control_loops::drivetrain::kRobotRadius);
while (true) {
::aos::time::PhasedLoopXMS(5, 2500);
diff --git a/y2016/autonomous/auto.cc b/y2016/autonomous/auto.cc
index 56844e0..b315223 100644
--- a/y2016/autonomous/auto.cc
+++ b/y2016/autonomous/auto.cc
@@ -15,6 +15,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/actors/drivetrain_actor.h"
#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/queues/profile_params.q.h"
using ::aos::time::Time;
@@ -85,9 +86,9 @@
auto drivetrain_action = actors::MakeDrivetrainAction(params);
drivetrain_action->Start();
left_initial_position +=
- distance - theta * constants::Values::kTurnWidth / 2.0;
+ distance - theta * control_loops::drivetrain::kRobotRadius;
right_initial_position +=
- distance + theta * constants::Values::kTurnWidth / 2.0;
+ distance + theta * control_loops::drivetrain::kRobotRadius;
return ::std::move(drivetrain_action);
}
diff --git a/y2016/constants.cc b/y2016/constants.cc
index 8af39a1..0785646 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -28,8 +28,7 @@
// ///// Mutual constants between robots. /////
const int Values::kZeroingSampleSize;
-constexpr double Values::kDrivetrainEncoderRatio, Values::kLowGearRatio,
- Values::kHighGearRatio, Values::kTurnWidth, Values::kShooterEncoderRatio,
+constexpr double Values::kDrivetrainEncoderRatio, Values::kShooterEncoderRatio,
Values::kIntakeEncoderRatio, Values::kShoulderEncoderRatio,
Values::kWristEncoderRatio, Values::kIntakePotRatio,
Values::kShoulderPotRatio, Values::kWristPotRatio,
diff --git a/y2016/constants.h b/y2016/constants.h
index 87b56cf..6e7c1a9 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -28,14 +28,7 @@
// The ratio from the encoder shaft to the drivetrain wheels.
static constexpr double kDrivetrainEncoderRatio =
- (18.0 / 36.0) /*output reduction*/ * (44.0 / 30.0);
-
- // The gear ratios from motor shafts to the drivetrain wheels for high and low
- // gear.
- static constexpr double kLowGearRatio = 18.0 / 60.0 * 18.0 / 36.0;
- static constexpr double kHighGearRatio = 28.0 / 50.0 * 18.0 / 36.0;
-
- static constexpr double kTurnWidth = 0.601; // Robot width.
+ (18.0 / 36.0) /*output reduction*/ * (30.0 / 44.0) /*encoder gears*/;
// Ratios for our subsystems.
static constexpr double kShooterEncoderRatio = 1.0;
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 1b11690..cd0462b 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -37,10 +37,9 @@
drivetrain::kR,
drivetrain::kV,
drivetrain::kT,
+ drivetrain::kHighGearRatio,
+ drivetrain::kLowGearRatio,
- constants::Values::kTurnWidth,
- constants::Values::kHighGearRatio,
- constants::Values::kLowGearRatio,
kThreeStateDriveShifter,
kThreeStateDriveShifter};
diff --git a/y2016/control_loops/python/drivetrain.py b/y2016/control_loops/python/drivetrain.py
index f94b456..bb86625 100755
--- a/y2016/control_loops/python/drivetrain.py
+++ b/y2016/control_loops/python/drivetrain.py
@@ -76,10 +76,10 @@
self.J = 2.8
# Mass of the robot, in kg.
self.m = 68
- # Radius of the robot, in meters (from last year).
- self.rb = 0.647998644 / 2.0
+ # Radius of the robot, in meters (requires tuning by hand)
+ self.rb = 0.601 / 2.0
# Radius of the wheels, in meters.
- self.r = .04445
+ self.r = 0.097155
# Resistance of the motor, divided by the number of motors.
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
@@ -88,8 +88,8 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
- self.G_low = 18.0 / 60.0 * 18.0 / 50.0
- self.G_high = 28.0 / 50.0 * 18.0 / 50.0
+ self.G_low = 14.0 / 48.0 * 18.0 / 60.0 * 18.0 / 36.0
+ self.G_high = 14.0 / 48.0 * 28.0 / 50.0 * 18.0 / 36.0
if left_low:
self.Gl = self.G_low
else:
@@ -341,6 +341,10 @@
drivetrain_low_low.Kv))
dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
drivetrain_low_low.Kt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
+ drivetrain_low_low.G_low))
+ dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
+ drivetrain_high_high.G_high))
dog_loop_writer.Write(argv[1], argv[2])
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 9b87ff9..759c5ca 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -15,6 +15,8 @@
namespace {
constexpr double kZeroingVoltage = 4.0;
+constexpr double kOperatingVoltage = 12.0;
+constexpr double kLandingShoulderDownVoltage = -2.0;
// The maximum voltage the intake roller will be allowed to use.
constexpr float kMaxIntakeVoltage = 8.0;
@@ -481,6 +483,16 @@
constants::Values::kShoulderRange.lower);
requested_wrist = 0.0;
requested_intake = unsafe_goal->angle_intake;
+ // Transition to landing once the profile is close to finished for the
+ // shoulder.
+ if (arm_.goal(0, 0) > kShoulderTransitionToLanded + 1e-4 ||
+ arm_.unprofiled_goal(0, 0) > kShoulderTransitionToLanded + 1e-4) {
+ if (state_ == LANDING_RUNNING) {
+ state_ = RUNNING;
+ } else {
+ state_ = SLOW_RUNNING;
+ }
+ }
} else {
// Otherwise, give the user what he asked for.
arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
@@ -498,7 +510,9 @@
// Transition to landing once the profile is close to finished for the
// shoulder.
- if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4) {
+ if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4 &&
+ arm_.unprofiled_goal(0, 0) <=
+ kShoulderTransitionToLanded + 1e-4) {
if (state_ == RUNNING) {
state_ = LANDING_RUNNING;
} else {
@@ -525,11 +539,24 @@
}
// Set the voltage limits.
- const double max_voltage =
- (state_ == RUNNING || state_ == LANDING_RUNNING) ? 12.0 : kZeroingVoltage;
+ const double max_voltage = (state_ == RUNNING || state_ == LANDING_RUNNING)
+ ? kOperatingVoltage
+ : kZeroingVoltage;
arm_.set_max_voltage(max_voltage, max_voltage);
intake_.set_max_voltage(max_voltage);
+ if (IsRunning()) {
+ // We don't want lots of negative voltage when we are near the bellypan on
+ // the shoulder...
+ // TODO(austin): Do I want to push negative power into the belly pan at this
+ // point? Maybe just put the goal slightly below the bellypan and call that
+ // good enough.
+ if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4) {
+ arm_.set_shoulder_asymetric_limits(kLandingShoulderDownVoltage,
+ max_voltage);
+ }
+ }
+
// Calculate the loops for a cycle.
arm_.Update(disable);
intake_.Update(disable);
@@ -551,8 +578,7 @@
// Save debug/internal state.
// TODO(austin): Save the voltage errors.
- status->zeroed = (state_ == RUNNING || state_ == LANDING_RUNNING ||
- state_ == SLOW_RUNNING || state_ == LANDING_SLOW_RUNNING);
+ status->zeroed = IsRunning();
status->shoulder.angle = arm_.X_hat(0, 0);
status->shoulder.angular_velocity = arm_.X_hat(1, 0);
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 23491bf..0b72c77 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -20,6 +20,8 @@
class SuperstructureTest_ArmZeroingErrorTest_Test;
class SuperstructureTest_IntakeZeroingErrorTest_Test;
class SuperstructureTest_UpperHardstopStartup_Test;
+class SuperstructureTest_DisabledWhileZeroingHigh_Test;
+class SuperstructureTest_DisabledWhileZeroingLow_Test;
} // namespace testing
// Helper class to prevent parts from crashing into each other. The parts in
@@ -170,6 +172,11 @@
ESTOP = 16,
};
+ bool IsRunning() const {
+ return (state_ == SLOW_RUNNING || state_ == RUNNING ||
+ state_ == LANDING_SLOW_RUNNING || state_ == LANDING_RUNNING);
+ }
+
State state() const { return state_; }
// Returns the value to move the joint to such that it will stay below
@@ -197,6 +204,8 @@
friend class testing::SuperstructureTest_IntakeZeroingErrorTest_Test;
friend class testing::SuperstructureTest_RespectsRange_Test;
friend class testing::SuperstructureTest_UpperHardstopStartup_Test;
+ friend class testing::SuperstructureTest_DisabledWhileZeroingHigh_Test;
+ friend class testing::SuperstructureTest_DisabledWhileZeroingLow_Test;
Intake intake_;
Arm arm_;
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index ce50b4b..3bc41cb 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -137,6 +137,11 @@
double max_angular_acceleration_wrist);
void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage);
+ void set_shoulder_asymetric_limits(double shoulder_min_voltage,
+ double shoulder_max_voltage) {
+ loop_->set_asymetric_voltage(0, shoulder_min_voltage, shoulder_max_voltage);
+ }
+
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
// Resets the internal state.
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index eff5017..bc33c9b 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -163,6 +163,31 @@
superstructure_queue_.output->voltage_wrist +
arm_plant_->wrist_voltage_offset();
+ // Verify that the correct power limits are being respected depending on
+ // which mode we are in.
+ EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
+ if (superstructure_queue_.status->state == Superstructure::RUNNING ||
+ superstructure_queue_.status->state ==
+ Superstructure::LANDING_RUNNING) {
+ CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
+ 12.00001);
+ CHECK_LE(::std::abs(superstructure_queue_.output->voltage_shoulder),
+ 12.00001);
+ CHECK_LE(::std::abs(superstructure_queue_.output->voltage_wrist),
+ 12.00001);
+ } else {
+ CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
+ 4.00001);
+ CHECK_LE(::std::abs(superstructure_queue_.output->voltage_shoulder),
+ 4.00001);
+ CHECK_LE(::std::abs(superstructure_queue_.output->voltage_wrist),
+ 4.00001);
+ }
+ if (arm_plant_->X(0, 0) <=
+ Superstructure::kShoulderTransitionToLanded + 1e-4) {
+ CHECK_GE(superstructure_queue_.output->voltage_shoulder, -2.00001);
+ }
+
// Use the plant to generate the next physical state given the voltages to
// the motors.
intake_plant_->Update();
@@ -575,6 +600,135 @@
EXPECT_NE(0.0, superstructure_.arm_.goal(2, 0));
}
+// Tests that disabling while zeroing at any state restarts from beginning
+TEST_F(SuperstructureTest, DisabledWhileZeroingHigh) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.upper);
+ superstructure_plant_.InitializeShoulderPosition(
+ constants::Values::kShoulderRange.upper);
+ superstructure_plant_.InitializeAbsoluteWristPosition(
+ constants::Values::kWristRange.upper);
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.upper)
+ .angle_shoulder(constants::Values::kShoulderRange.upper)
+ .angle_wrist(constants::Values::kWristRange.upper)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
+
+ // Expected states to cycle through and check in order.
+ Superstructure::State ExpectedStateOrder[] = {
+ Superstructure::DISABLED_INITIALIZED,
+ Superstructure::HIGH_ARM_ZERO_LIFT_ARM,
+ Superstructure::HIGH_ARM_ZERO_LEVEL_SHOOTER,
+ Superstructure::HIGH_ARM_ZERO_MOVE_INTAKE_OUT,
+ Superstructure::HIGH_ARM_ZERO_LOWER_ARM,
+ };
+
+ // Cycle through until arm_ and intake_ are initialized in superstructure.cc
+ while (superstructure_.state() < Superstructure::DISABLED_INITIALIZED) {
+ RunIteration(true);
+ }
+
+ static const int kNumberOfStates =
+ sizeof(ExpectedStateOrder) / sizeof(ExpectedStateOrder[0]);
+
+ // Next state when reached to disable
+ for (int i = 0; i < kNumberOfStates; i++) {
+ // Next expected state after being disabled that is expected until next
+ // state to disable at is reached
+ for (int j = 0; superstructure_.state() != ExpectedStateOrder[i] && j <= i;
+ j++) {
+ // RunIteration until next expected state is reached with a maximum
+ // of 10000 times to ensure a breakout
+ for (int o = 0;
+ superstructure_.state() < ExpectedStateOrder[j] && o < 10000; o++) {
+ RunIteration(true);
+ }
+ EXPECT_EQ(ExpectedStateOrder[j], superstructure_.state());
+ }
+
+ EXPECT_EQ(ExpectedStateOrder[i], superstructure_.state());
+
+ // Disable
+ RunIteration(false);
+
+ EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
+ }
+
+ RunForTime(Time::InSeconds(10));
+ EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
+}
+
+// Tests that disabling while zeroing at any state restarts from beginning
+TEST_F(SuperstructureTest, DisabledWhileZeroingLow) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.lower);
+ superstructure_plant_.InitializeShoulderPosition(
+ constants::Values::kShoulderRange.lower);
+ superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.lower)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(constants::Values::kWristRange.lower)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
+
+ // Expected states to cycle through and check in order.
+ Superstructure::State ExpectedStateOrder[] = {
+ Superstructure::DISABLED_INITIALIZED,
+ Superstructure::LOW_ARM_ZERO_LOWER_INTAKE,
+ Superstructure::LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER,
+ Superstructure::LOW_ARM_ZERO_LIFT_SHOULDER,
+ Superstructure::LOW_ARM_ZERO_LEVEL_SHOOTER,
+ };
+
+ // Cycle through until arm_ and intake_ are initialized in superstructure.cc
+ while (superstructure_.state() < Superstructure::DISABLED_INITIALIZED) {
+ RunIteration(true);
+ }
+
+ static const int kNumberOfStates =
+ sizeof(ExpectedStateOrder) / sizeof(ExpectedStateOrder[0]);
+
+ // Next state when reached to disable
+ for (int i = 0; i < kNumberOfStates; i++) {
+ // Next expected state after being disabled that is expected until next
+ // state to disable at is reached
+ for (int j = 0; superstructure_.state() != ExpectedStateOrder[i] && j <= i;
+ j++) {
+ // RunIteration until next expected state is reached with a maximum
+ // of 10000 times to ensure a breakout
+ for (int o = 0;
+ superstructure_.state() < ExpectedStateOrder[j] && o < 10000; o++) {
+ RunIteration(true);
+ }
+ EXPECT_EQ(ExpectedStateOrder[j], superstructure_.state());
+ }
+
+ EXPECT_EQ(ExpectedStateOrder[i], superstructure_.state());
+
+ // Disable
+ RunIteration(false);
+
+ EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
+ }
+
+ RunForTime(Time::InSeconds(10));
+ EXPECT_EQ(Superstructure::LANDING_RUNNING, superstructure_.state());
+}
+
// Tests that MoveButKeepBelow returns sane values.
TEST_F(SuperstructureTest, MoveButKeepBelowTest) {
EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 10.0, 1.0));
@@ -592,8 +746,7 @@
constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
constants::Values::kShoulderRange.lower);
- superstructure_plant_.InitializeRelativeWristPosition(
- constants::Values::kWristRange.lower);
+ superstructure_plant_.InitializeRelativeWristPosition(0.0);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
@@ -905,12 +1058,21 @@
ASSERT_TRUE(
superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.upper) // stowed
+ .angle_shoulder(constants::Values::kShoulderRange.lower) // Down
+ .angle_wrist(0.0) // Stowed
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(constants::Values::kIntakeRange.upper) // stowed
.angle_shoulder(M_PI / 4.0) // in the collision area
.angle_wrist(M_PI / 2.0) // down
.Send());
- RunForTime(Time::InSeconds(10));
+ RunForTime(Time::InSeconds(3));
superstructure_queue_.status.FetchLatest();
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
@@ -956,25 +1118,165 @@
// Make sure that we can properly detect a collision.
TEST_F(SuperstructureTest, DetectAndFixCollisionBetweenArmAndIntake) {
- superstructure_plant_.InitializeIntakePosition(
- constants::Values::kIntakeRange.upper); // upper limit
- superstructure_plant_.InitializeShoulderPosition(M_PI * 0.25); // 45° up
- superstructure_plant_.InitializeAbsoluteWristPosition(0); // level
+ // Zero & go straight up with the shoulder.
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(M_PI * 0.5)
+ .angle_wrist(0.0)
+ .Send());
+
+ RunForTime(Time::InSeconds(6));
+ VerifyNearGoal();
// Since we're explicitly checking for collisions, we don't want to fail the
// test because of collisions.
check_for_collisions_ = false;
- RunForTime(Time::InSeconds(1));
+ // Move shoulder down until collided by applying a voltage offset while
+ // disabled.
+ superstructure_plant_.set_power_error(0.0, -1.0, 0.0);
+ while (!collided()) {
+ RunIteration(false);
+ }
+ RunForTime(Time::InSeconds(0.5), false); // Move a bit further down.
+
ASSERT_TRUE(collided());
+ EXPECT_EQ(Superstructure::SLOW_RUNNING, superstructure_.state());
+ superstructure_plant_.set_power_error(0.0, 0.0, 0.0);
// Make sure that the collision avoidance will properly move the limbs out of
// the collision area.
RunForTime(Time::InSeconds(10));
ASSERT_FALSE(collided());
+ EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
}
-// TODO(austin): Landed to unlanded needs to go fast!
+// Make sure that we can properly detect a collision.
+TEST_F(SuperstructureTest, DetectAndFixShoulderInDrivebase) {
+ // Zero & go straight up with the shoulder.
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(0.0)
+ .Send());
+
+ RunForTime(Time::InSeconds(6));
+ VerifyNearGoal();
+
+ // Since we're explicitly checking for collisions, we don't want to fail the
+ // test because of collisions.
+ check_for_collisions_ = false;
+
+ // Move wrist up until on top of the bellypan
+ superstructure_plant_.set_power_error(0.0, 0.0, -1.0);
+ while (superstructure_plant_.wrist_angle() > -0.2) {
+ RunIteration(false);
+ }
+
+ ASSERT_TRUE(collided());
+ EXPECT_EQ(Superstructure::LANDING_SLOW_RUNNING, superstructure_.state());
+
+ // Make sure that the collision avoidance will properly move the limbs out of
+ // the collision area.
+ superstructure_plant_.set_power_error(0.0, 0.0, 0.0);
+ RunForTime(Time::InSeconds(3));
+ ASSERT_FALSE(collided());
+ EXPECT_EQ(Superstructure::LANDING_RUNNING, superstructure_.state());
+}
+
+// Make sure that the landing voltage limit works.
+TEST_F(SuperstructureTest, LandingDownVoltageLimit) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.lower);
+ superstructure_plant_.InitializeShoulderPosition(0.0);
+ superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(constants::Values::kShoulderRange.lower)
+ .angle_wrist(0.0) // intentionally asking for forward
+ .Send());
+
+ RunForTime(Time::InSeconds(6));
+ VerifyNearGoal();
+
+ // If we are near the bottom of the range, we won't have enough power to
+ // compensate for the offset. This means that we fail if we get to the goal.
+ superstructure_plant_.set_power_error(0.0, 3.0, 0.0);
+ RunForTime(Time::InSeconds(2));
+ superstructure_plant_.set_power_error(0.0, 6.0, 0.0);
+ RunForTime(Time::InSeconds(2));
+ EXPECT_LE(0.0, superstructure_queue_.goal->angle_shoulder);
+}
+
+// Make sure that we land slowly.
+TEST_F(SuperstructureTest, LandSlowly) {
+ // Zero & go to initial position.
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(M_PI * 0.25)
+ .angle_wrist(0.0)
+ .Send());
+ RunForTime(Time::InSeconds(8));
+
+ // Tell it to land in the bellypan as fast as possible.
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(0.0)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
+
+ // Wait until we hit the transition point.
+ do {
+ RunIteration();
+ superstructure_queue_.status.FetchLatest();
+ } while (superstructure_plant_.shoulder_angle() >
+ Superstructure::kShoulderTransitionToLanded);
+
+ set_peak_shoulder_velocity(0.55);
+ RunForTime(Time::InSeconds(4));
+}
+
+// Make sure that we quickly take off from a land.
+TEST_F(SuperstructureTest, TakeOffQuickly) {
+ // Zero & go to initial position.
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(0.0)
+ .angle_wrist(0.0)
+ .Send());
+ RunForTime(Time::InSeconds(8));
+
+ // Tell it to take off as fast as possible.
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(M_PI / 2.0)
+ .angle_wrist(0.0)
+ .max_angular_velocity_intake(20)
+ .max_angular_acceleration_intake(20)
+ .max_angular_velocity_shoulder(20)
+ .max_angular_acceleration_shoulder(20)
+ .max_angular_velocity_wrist(20)
+ .max_angular_acceleration_wrist(20)
+ .Send());
+
+ // Wait until we hit the transition point.
+ do {
+ RunIteration();
+ superstructure_queue_.status.FetchLatest();
+ } while (superstructure_plant_.shoulder_angle() <
+ Superstructure::kShoulderTransitionToLanded);
+
+ // Make sure we are faster than the limited speed (which would indicate that
+ // we are still holding the shoulder back even when it's taking off).
+ EXPECT_GE(superstructure_plant_.shoulder_angular_velocity(), 0.55);
+}
} // namespace testing
} // namespace superstructure
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index e3842e6..190692f 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -32,7 +32,7 @@
namespace joysticks {
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kShiftHigh(2, 3), kShiftHigh2(2, 2), kShiftLow(2, 1);
const ButtonLocation kQuickTurn(1, 5);
// Buttons on the lexan driver station to get things running on bring-up day.
@@ -92,11 +92,11 @@
LOG(WARNING, "sending stick values failed\n");
}
- if (data.PosEdge(kShiftHigh)) {
+ if (data.PosEdge(kShiftLow)) {
is_high_gear_ = false;
}
- if (data.PosEdge(kShiftLow)) {
+ if (data.PosEdge(kShiftHigh) || data.PosEdge(kShiftHigh2)) {
is_high_gear_ = true;
}
}
diff --git a/y2016/wpilib/BUILD b/y2016/wpilib/BUILD
index eb8b289..ff8722b 100644
--- a/y2016/wpilib/BUILD
+++ b/y2016/wpilib/BUILD
@@ -31,6 +31,7 @@
'//frc971/wpilib:wpilib_interface',
'//frc971/wpilib:pdp_fetcher',
'//y2016:constants',
+ '//y2016/control_loops/drivetrain:polydrivetrain_plants',
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_queue',
],
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index d43af31..408ea8c 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -34,6 +34,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
@@ -83,17 +84,17 @@
double drivetrain_translate(int32_t in) {
return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
constants::Values::kDrivetrainEncoderRatio *
- (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
+ control_loops::drivetrain::kWheelRadius * 2.0 / 2.0;
}
double drivetrain_velocity_translate(double in) {
return (1.0 / in) / 256.0 /*cpr*/ *
constants::Values::kDrivetrainEncoderRatio *
- (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
+ control_loops::drivetrain::kWheelRadius * 2.0 / 2.0;
}
double shooter_translate(int32_t in) {
- return -static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
+ return -static_cast<double>(in) / (128.0 /*cpr*/ * 4.0 /*4x*/) *
constants::Values::kShooterEncoderRatio * (2 * M_PI /*radians*/);
}
@@ -114,17 +115,17 @@
double intake_pot_translate(double voltage) {
return voltage * constants::Values::kIntakePotRatio *
- (5.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
double shoulder_pot_translate(double voltage) {
return voltage * constants::Values::kShoulderPotRatio *
- (5.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
double wrist_pot_translate(double voltage) {
return voltage * constants::Values::kWristPotRatio *
- (5.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
constexpr double kMaxDrivetrainEncoderPulsesPerSecond =