Merge "Added shooter and tests."
diff --git a/aos/BUILD b/aos/BUILD
index ac5c3e9..47de872 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -475,6 +475,7 @@
"testdata/expected.json",
"testdata/expected_multinode.json",
"testdata/good_multinode.json",
+ "testdata/good_multinode_hostnames.json",
"testdata/invalid_destination_node.json",
"testdata/invalid_nodes.json",
"testdata/invalid_source_node.json",
diff --git a/aos/configuration.cc b/aos/configuration.cc
index ee343c2..ac897bf 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -362,7 +362,7 @@
// Check that if there is a node list, all the source nodes are filled out and
// valid, and all the destination nodes are valid (and not the source). This
// is a basic consistency check.
- if (result.message().has_nodes()) {
+ if (result.message().has_nodes() && config.message().has_channels()) {
for (const Channel *c : *config.message().channels()) {
CHECK(c->has_source_node()) << ": Channel " << FlatbufferToJson(c)
<< " is missing \"source_node\"";
@@ -597,9 +597,16 @@
const Node *GetNodeFromHostname(const Configuration *config,
std::string_view hostname) {
for (const Node *node : *config->nodes()) {
- if (node->hostname()->string_view() == hostname) {
+ if (node->has_hostname() && node->hostname()->string_view() == hostname) {
return node;
}
+ if (node->has_hostnames()) {
+ for (const auto &candidate : *node->hostnames()) {
+ if (candidate->string_view() == hostname) {
+ return node;
+ }
+ }
+ }
}
return nullptr;
}
diff --git a/aos/configuration.fbs b/aos/configuration.fbs
index 317c100..576616e 100644
--- a/aos/configuration.fbs
+++ b/aos/configuration.fbs
@@ -126,6 +126,12 @@
hostname:string;
// Port to serve forwarded data from.
port:ushort = 9971;
+
+ // An alternative to hostname which allows specifying multiple hostnames,
+ // any of which will match this node.
+ //
+ // Don't specify a hostname in multiple nodes in the same configuration.
+ hostnames:[string];
}
// Overall configuration datastructure for the pubsub.
diff --git a/aos/configuration_test.cc b/aos/configuration_test.cc
index 0c0874b..c36bd93 100644
--- a/aos/configuration_test.cc
+++ b/aos/configuration_test.cc
@@ -677,6 +677,46 @@
GetNodeOrDie(&config.message(), config2.message().nodes()->Get(0)));
}
+TEST_F(ConfigurationTest, GetNodeFromHostname) {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/good_multinode.json");
+ EXPECT_EQ("pi1",
+ CHECK_NOTNULL(GetNodeFromHostname(&config.message(), "raspberrypi"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ("pi2", CHECK_NOTNULL(
+ GetNodeFromHostname(&config.message(), "raspberrypi2"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "raspberrypi3"));
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "localhost"));
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "3"));
+}
+
+TEST_F(ConfigurationTest, GetNodeFromHostnames) {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/good_multinode_hostnames.json");
+ EXPECT_EQ("pi1",
+ CHECK_NOTNULL(GetNodeFromHostname(&config.message(), "raspberrypi"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ("pi2", CHECK_NOTNULL(
+ GetNodeFromHostname(&config.message(), "raspberrypi2"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ("pi2", CHECK_NOTNULL(
+ GetNodeFromHostname(&config.message(), "raspberrypi3"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ("pi2", CHECK_NOTNULL(
+ GetNodeFromHostname(&config.message(), "other"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "raspberrypi4"));
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "localhost"));
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "3"));
+}
+
} // namespace testing
} // namespace configuration
} // namespace aos
diff --git a/aos/network/team_number.cc b/aos/network/team_number.cc
index b031299..14fadab 100644
--- a/aos/network/team_number.cc
+++ b/aos/network/team_number.cc
@@ -35,6 +35,29 @@
return std::nullopt;
}
+std::optional<uint16_t> ParsePiTeamNumber(const std::string &hostname) {
+ if (hostname.substr(0, 3) != "pi-") {
+ return std::nullopt;
+ }
+ size_t first_separator = hostname.find('-');
+ if (first_separator == hostname.npos ||
+ first_separator >= hostname.size() - 2) {
+ return std::nullopt;
+ }
+ ++first_separator;
+ const size_t second_separator = hostname.find('-', first_separator);
+ if (second_separator == hostname.npos) {
+ return std::nullopt;
+ }
+ const std::string number_string =
+ hostname.substr(first_separator, second_separator - first_separator);
+ int number;
+ if (!util::StringToNumber(number_string, &number)) {
+ return std::nullopt;
+ }
+ return number;
+}
+
} // namespace team_number_internal
namespace {
@@ -65,6 +88,13 @@
return *result;
}
}
+ {
+ const auto result = team_number_internal::ParsePiTeamNumber(hostname);
+ if (result) {
+ LOG(INFO) << "Pi hostname team number is: " << *result;
+ return *result;
+ }
+ }
LOG(FATAL) << "Failed to parse a team number from hostname: " << hostname;
}
diff --git a/aos/network/team_number.h b/aos/network/team_number.h
index 6eb1335..618affb 100644
--- a/aos/network/team_number.h
+++ b/aos/network/team_number.h
@@ -28,6 +28,8 @@
std::optional<uint16_t> ParseRoborioTeamNumber(const std::string &hostname);
+std::optional<uint16_t> ParsePiTeamNumber(const std::string &hostname);
+
} // namespace team_number_internal
} // namespace network
} // namespace aos
diff --git a/aos/network/team_number_test.cc b/aos/network/team_number_test.cc
index 03001de..c11f0a0 100644
--- a/aos/network/team_number_test.cc
+++ b/aos/network/team_number_test.cc
@@ -7,6 +7,7 @@
namespace testing {
using team_number_internal::ParseRoborioTeamNumber;
+using team_number_internal::ParsePiTeamNumber;
TEST(TeamNumberTest, Parse2015TeamNumber) {
EXPECT_EQ(971u, *ParseRoborioTeamNumber("roboRIO-971"));
@@ -29,6 +30,17 @@
EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO--FRC"));
}
+TEST(TeamNumberTest, ParsePiTeamNumber) {
+ EXPECT_EQ(971u, *ParsePiTeamNumber("pi-971-1"));
+ EXPECT_EQ(8971u, *ParsePiTeamNumber("pi-8971-22"));
+ EXPECT_EQ(8971u, *ParsePiTeamNumber("pi-8971-"));
+
+ EXPECT_FALSE(ParseRoborioTeamNumber("pi"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("pi-"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("pi-971"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("pi-971a-1"));
+}
+
} // namespace testing
} // namespace network
} // namespace aos
diff --git a/aos/testdata/good_multinode_hostnames.json b/aos/testdata/good_multinode_hostnames.json
new file mode 100644
index 0000000..b82f6f3
--- /dev/null
+++ b/aos/testdata/good_multinode_hostnames.json
@@ -0,0 +1,18 @@
+{
+ "nodes": [
+ {
+ "name": "pi1",
+ "hostnames": [
+ "raspberrypi"
+ ]
+ },
+ {
+ "name": "pi2",
+ "hostnames": [
+ "raspberrypi2",
+ "raspberrypi3",
+ "other"
+ ]
+ }
+ ]
+}
diff --git a/frc971/config/BUILD b/frc971/config/BUILD
index 0f4f7ba..098d1f8 100644
--- a/frc971/config/BUILD
+++ b/frc971/config/BUILD
@@ -8,6 +8,7 @@
srcs = ["setup_roborio.sh"],
data = [
":rio_robotCommand",
+ ":sctp.ko",
"@arm_frc_linux_gnueabi_repo//:compiler_pieces",
],
visibility = ["//visibility:public"],
diff --git a/frc971/config/sctp.ko b/frc971/config/sctp.ko
new file mode 100644
index 0000000..81e41ec
--- /dev/null
+++ b/frc971/config/sctp.ko
Binary files differ
diff --git a/frc971/config/setup_roborio.sh b/frc971/config/setup_roborio.sh
index 8c2be86..f288df8 100755
--- a/frc971/config/setup_roborio.sh
+++ b/frc971/config/setup_roborio.sh
@@ -34,6 +34,15 @@
ssh "admin@${ROBOT_HOSTNAME}" ln -s /media/sda1/aos_log-current robot_code/aos_log-current
fi
+if [[ "$(ssh admin@${ROBOT_HOSTNAME} uname -r)" != "4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189" ]]; then
+ echo "Target roboRIO has the wrong kernel"
+ exit 1
+fi
+
+ssh "admin@${ROBOT_HOSTNAME}" mkdir "/lib/modules/4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189/kernel/net/sctp/"
+scp frc971/config/sctp.ko "admin@${ROBOT_HOSTNAME}:/lib/modules/4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189/kernel/net/sctp/sctp.ko"
+ssh "admin@${ROBOT_HOSTNAME}" depmod
+
# This fails if the code isn't running.
ssh "admin@${ROBOT_HOSTNAME}" 'PATH="${PATH}":/usr/local/natinst/bin/ /usr/local/frc/bin/frcKillRobot.sh -r -t' || true
diff --git a/frc971/constants.h b/frc971/constants.h
index a52add2..debfb55 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -98,6 +98,8 @@
double upper;
double middle() const { return (lower_hard + upper_hard) / 2.0; }
+
+ double range() const { return upper_hard - lower_hard; }
};
} // namespace constants
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 36da41a..bd4447b 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -22,6 +22,13 @@
kalman_q_voltage,
kalman_r_position,
dt=0.00505):
+ """Constructs an AngularSystemParams object.
+
+ Args:
+ motor: Motor object with the motor constants.
+ G: float, Gear ratio. Less than 1 means output moves slower than the
+ input.
+ """
self.name = name
self.motor = motor
self.G = G
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 919968e..f124852 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -685,3 +685,7 @@
(12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
+ # Motor inertia in kg m^2
+ # Diameter of 1.9", weight of: 100 grams
+ # TODO(austin): Get a number from Scott Westbrook for the mass
+ self.motor_inertia = 0.1 * ((0.95 * 0.0254) ** 2.0)
diff --git a/y2020/BUILD b/y2020/BUILD
index 79dc651..9a233d4 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -33,10 +33,12 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plants",
+ "//y2020/control_loops/superstructure/control_panel:control_panel_plants",
+ "//y2020/control_loops/superstructure/finisher:finisher_plants",
"//y2020/control_loops/superstructure/hood:hood_plants",
"//y2020/control_loops/superstructure/intake:intake_plants",
"//y2020/control_loops/superstructure/turret:turret_plants",
- "//y2020/control_loops/superstructure/control_panel:control_panel_plants",
"@com_google_absl//absl/base",
],
)
diff --git a/y2020/constants.cc b/y2020/constants.cc
index cc5c1fb..a35798f 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -92,6 +92,11 @@
turret_params->zeroing_constants.moving_buffer_size = 20;
turret_params->zeroing_constants.allowable_encoder_error = 0.9;
+ CHECK_LE(hood->range.range(),
+ hood->zeroing_constants.one_revolution_distance);
+ CHECK_LE(intake->range.range(),
+ intake->zeroing_constants.one_revolution_distance);
+
switch (team) {
// A set of constants for tests.
case 1:
diff --git a/y2020/constants.h b/y2020/constants.h
index 16a3e8b..ed58777 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -10,7 +10,9 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
#include "y2020/control_loops/superstructure/control_panel/control_panel_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
#include "y2020/control_loops/superstructure/hood/hood_plant.h"
#include "y2020/control_loops/superstructure/intake/intake_plant.h"
#include "y2020/control_loops/superstructure/turret/turret_plant.h"
@@ -25,10 +27,10 @@
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
}
- static constexpr double kDrivetrainEncoderRatio() { return (24.0 / 52.0); }
+ static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
- control_loops::drivetrain::kHighOutputRatio /
+ control_loops::drivetrain::kHighGearRatio /
constants::Values::kDrivetrainEncoderRatio() *
kDrivetrainEncoderCountsPerRevolution();
}
@@ -36,22 +38,20 @@
// Hood
static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
- // TODO(sabina): Update constants
- static constexpr double kHoodEncoderRatio() { return 1.0; }
+ static constexpr double kHoodEncoderRatio() { return 8.0 / 72.0; }
static constexpr double kMaxHoodEncoderPulsesPerSecond() {
- return control_loops::superstructure::hood::kFreeSpeed *
+ return control_loops::superstructure::hood::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::hood::kOutputRatio /
- kHoodEncoderRatio() / (2.0 * M_PI) *
- kHoodEncoderCountsPerRevolution();
+ kHoodEncoderRatio() * kHoodEncoderCountsPerRevolution();
}
static constexpr ::frc971::constants::Range kHoodRange() {
return ::frc971::constants::Range{
- 0.00, // Back Hard
- 0.79, // Front Hard
- 0.14, // Back Soft
- 0.78 // Front Soft
+ -0.01, // Back Hard
+ 0.65, // Front Hard
+ 0.0, // Back Soft
+ 0.64 // Front Soft
};
}
@@ -65,10 +65,9 @@
static constexpr double kIntakeEncoderRatio() { return (16.0 / 32.0); }
static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
- return control_loops::superstructure::intake::kFreeSpeed *
+ return control_loops::superstructure::intake::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::intake::kOutputRatio /
- kIntakeEncoderRatio() / (2.0 * M_PI) *
- kIntakeEncoderCountsPerRevolution();
+ kIntakeEncoderRatio() * kIntakeEncoderCountsPerRevolution();
}
// TODO(sabina): update range
@@ -92,18 +91,16 @@
static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kTurretEncoderRatio() {
- return 1.0; // TODO (Kai): Get Gear Ratios when ready
+ return (26.0 / 150.0) * (130.0 / 40.0);
}
static constexpr double kMaxTurretEncoderPulsesPerSecond() {
- return control_loops::superstructure::turret::kFreeSpeed *
+ return control_loops::superstructure::turret::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::turret::kOutputRatio /
- kTurretEncoderRatio() / (2.0 * M_PI) *
- kTurretEncoderCountsPerRevolution();
+ kTurretEncoderRatio() * kTurretEncoderCountsPerRevolution();
}
- // TODO(austin): Figure out the actual constant here.
- static constexpr double kTurretPotRatio() { return 1.0; }
+ static constexpr double kTurretPotRatio() { return (26.0 / 150.0); }
static constexpr ::frc971::constants::Range kTurretRange() {
return ::frc971::constants::Range{
@@ -135,23 +132,40 @@
static constexpr double kControlPanelEncoderRatio() { return (56.0 / 28.0); }
static constexpr double kMaxControlPanelEncoderPulsesPerSecond() {
- return control_loops::superstructure::control_panel::kFreeSpeed *
+ return control_loops::superstructure::control_panel::kFreeSpeed /
+ (2.0 * M_PI) *
control_loops::superstructure::control_panel::kOutputRatio /
- kControlPanelEncoderRatio() / (2.0 * M_PI) *
+ kControlPanelEncoderRatio() *
kControlPanelEncoderCountsPerRevolution();
}
// Shooter
- // TODO: Get actual constants.
static constexpr double kFinisherEncoderCountsPerRevolution() {
return 4096.0;
}
- static constexpr double kFinisherEncoderRatio() { return 1.0; }
+ static constexpr double kFinisherEncoderRatio() { return 30.0 / 40.0; }
+
+ static constexpr double kMaxFinisherEncoderPulsesPerSecond() {
+ return control_loops::superstructure::finisher::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::finisher::kOutputRatio /
+ kFinisherEncoderRatio() * kFinisherEncoderCountsPerRevolution();
+ }
+
static constexpr double kAcceleratorEncoderCountsPerRevolution() {
return 4096.0;
}
- static constexpr double kAcceleratorEncoderRatio() { return 1.0; }
+ static constexpr double kAcceleratorEncoderRatio() {
+ return (1.2 * 1.2 * 1.2) * (30.0 / 40.0);
+ }
+
+ static constexpr double kMaxAcceleratorEncoderPulsesPerSecond() {
+ return control_loops::superstructure::accelerator::kFreeSpeed /
+ (2.0 * M_PI) *
+ control_loops::superstructure::accelerator::kOutputRatio /
+ kAcceleratorEncoderRatio() *
+ kAcceleratorEncoderCountsPerRevolution();
+ }
// Climber
static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 257ed23..64ae12e 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -13,22 +13,32 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+# Inertia for a single 4" diameter, 1" wide neopreme wheel.
+J_wheel = 0.000319
+# Gear ratio between wheels (speed up!)
+G_per_wheel = 1.2
+# Gear ratio to the final wheel.
+G = (30.0 / 40.0) * numpy.power(G_per_wheel, 3.0)
+# Overall flywheel inertia.
+J = J_wheel * (
+ 1.0 + numpy.power(G, -2.0) + numpy.power(G, -4.0) + numpy.power(G, -6.0))
+
+# The position and velocity are measured for the final wheel.
kAccelerator = flywheel.FlywheelParams(
name='Accelerator',
motor=control_loop.Falcon(),
- G=1.0,
- J=0.006,
+ G=G,
+ J=J,
q_pos=0.08,
q_vel=4.00,
- q_voltage=0.3,
+ q_voltage=0.4,
r_pos=0.05,
- controller_poles=[.87],
- dt=0.00505)
+ controller_poles=[.80])
def main(argv):
if FLAGS.plot:
- R = numpy.matrix([[0.0], [100.0], [0.0]])
+ R = numpy.matrix([[0.0], [500.0], [0.0]])
flywheel.PlotSpinup(kAccelerator, goal=R, iterations=200)
return 0
diff --git a/y2020/control_loops/python/control_panel.py b/y2020/control_loops/python/control_panel.py
index 206a39e..e02aa13 100644
--- a/y2020/control_loops/python/control_panel.py
+++ b/y2020/control_loops/python/control_panel.py
@@ -17,12 +17,11 @@
except gflags.DuplicateFlagError:
pass
-#TODO(sabina): update moment
kControlPanel = angular_system.AngularSystemParams(
name='ControlPanel',
motor=control_loop.BAG(),
- G=(1.0),
- J=0.3,
+ G=1.0,
+ J=0.000009,
q_pos=0.20,
q_vel=5.0,
kalman_q_pos=0.12,
diff --git a/y2020/control_loops/python/drivetrain.py b/y2020/control_loops/python/drivetrain.py
index 54745dd..95b9fcf 100644
--- a/y2020/control_loops/python/drivetrain.py
+++ b/y2020/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
from frc971.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
import sys
import gflags
@@ -11,19 +12,20 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
kDrivetrain = drivetrain.DrivetrainParams(
- J=1.5,
- mass=38.5,
- robot_radius=0.45 / 2.0,
- wheel_radius=4.0 * 0.0254 / 2.0,
- G=9.0 / 52.0,
+ J=6.0,
+ mass=68.0,
+ # TODO(austin): Measure radius a bit better.
+ robot_radius=0.7 / 2.0,
+ wheel_radius=6.0 * 0.0254 / 2.0,
+ motor_type=control_loop.Falcon(),
+ G=(8.0 / 70.0) * (17.0 / 24.0),
q_pos=0.14,
q_vel=1.30,
efficiency=0.80,
has_imu=True,
force=True,
kf_q_voltage=13.0,
- controller_poles=[0.82, 0.82],
- robot_cg_offset=0.0)
+ controller_poles=[0.82, 0.82])
def main(argv):
@@ -38,5 +40,6 @@
# Write the generated constants out to a file.
drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2020', kDrivetrain)
+
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 0b0fbb4..0d46bb9 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -13,22 +13,32 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+# Inertia for a single 4" diameter, 2" wide neopreme wheel.
+J_wheel = 0.000319 * 2.0
+# Gear ratio to the final wheel.
+# 40 tooth on the flywheel
+# 48 for the falcon.
+# 60 tooth on the outer wheel.
+G = 48.0 / 40.0
+# Overall flywheel inertia.
+J = J_wheel * (1.0 + (40.0 / 60.0)**2.0)
+
+# The position and velocity are measured for the final wheel.
kFinisher = flywheel.FlywheelParams(
name='Finisher',
motor=control_loop.Falcon(),
- G=1.0,
- J=0.006,
+ G=G,
+ J=J,
q_pos=0.08,
q_vel=4.00,
- q_voltage=0.3,
+ q_voltage=0.4,
r_pos=0.05,
- controller_poles=[.87],
- dt=0.00505)
+ controller_poles=[.80])
def main(argv):
if FLAGS.plot:
- R = numpy.matrix([[0.0], [100.0], [0.0]])
+ R = numpy.matrix([[0.0], [500.0], [0.0]])
flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=200)
return 0
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 9a6fc46..630d223 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -214,7 +214,7 @@
offset.append(observer_flywheel.X_hat[2, 0])
applied_U = U.copy()
- if i > 30:
+ if i > 100:
applied_U += 2
flywheel.Update(applied_U)
@@ -273,10 +273,10 @@
loop_writer = control_loop.ControlLoopWriter(
name, flywheels, namespaces=namespace)
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio' + params.name, '%f',
+ control_loop.Constant('kOutputRatio', '%f',
flywheels[0].G))
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed' + params.name, '%f',
+ control_loop.Constant('kFreeSpeed', '%f',
flywheels[0].motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index 8c99940..b4d8c84 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -17,29 +17,33 @@
except gflags.DuplicateFlagError:
pass
-'''
-Hood is an angular subsystem due to the mounting of the encoder on the hood joint.
-We are currently electing to ignore potential non-linearity.
-'''
-#TODO: update constants
+# Hood is an angular subsystem due to the mounting of the encoder on the hood
+# joint. We are currently electing to ignore potential non-linearity.
+
+range_of_travel_radians = (37.0 * numpy.pi / 180.0)
+# 0.083 inches/turn
+# 6.38 inches of travel
+turns_of_leadscrew_per_range_of_travel = 6.38 / 0.083
+
+radians_per_turn = range_of_travel_radians / turns_of_leadscrew_per_range_of_travel
+
kHood = angular_system.AngularSystemParams(
name='Hood',
motor=control_loop.BAG(),
- # meters / rad (used the displacement of the lead screw and the angle)
- G=(0.1601 / 0.6457),
- J=0.3,
- q_pos=0.20,
- q_vel=5.0,
+ G=radians_per_turn,
+ J=0.08389,
+ q_pos=0.55,
+ q_vel=14.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
- kalman_q_voltage=4.0,
+ kalman_q_voltage=2.5,
kalman_r_position=0.05)
def main(argv):
if FLAGS.plot:
- R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ R = numpy.matrix([[numpy.pi / 4.0], [0.0]])
angular_system.PlotKick(kHood, R)
angular_system.PlotMotion(kHood, R)
diff --git a/y2020/control_loops/python/intake.py b/y2020/control_loops/python/intake.py
index f65679a..9e80244 100644
--- a/y2020/control_loops/python/intake.py
+++ b/y2020/control_loops/python/intake.py
@@ -17,14 +17,13 @@
except gflags.DuplicateFlagError:
pass
-#TODO: update constants
kIntake = angular_system.AngularSystemParams(
name='Intake',
motor=control_loop.BAG(),
- G=(1.0 / 1.0),
- J=0.3,
- q_pos=0.20,
- q_vel=5.0,
+ G=(12.0 / 24.0) * (1.0 / 7.0) * (1.0 / 7.0) * (16.0 / 32.0),
+ J=3.0 * 0.139 * 0.139,
+ q_pos=0.40,
+ q_vel=20.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
kalman_q_voltage=4.0,
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index 58ea7d8..e276457 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -21,18 +21,21 @@
kTurret = angular_system.AngularSystemParams(
name='Turret',
motor=control_loop.Vex775Pro(),
- #TODO: Update Gear Ratios when they are ready
- G=(6.0 / 60.0) * (20.0 / 100.0) * (24.0 / 84.0),
- #TODO: Get number from Bryan (moment of inertia)
- J=0.30,
- q_pos=0.20,
- q_vel=5.0,
+ G=(6.0 / 60.0) * (26.0 / 150.0),
+ J=0.11,
+ q_pos=0.40,
+ q_vel=7.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
- kalman_q_voltage=4.0,
+ kalman_q_voltage=3.0,
kalman_r_position=0.05)
def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi], [0.0]])
+ angular_system.PlotKick(kTurret, R)
+ angular_system.PlotMotion(kTurret, R)
+
# Write the generated constants out to a file.
if len(argv) != 5:
glog.fatal(
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index de15415..d81b6b5 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -95,6 +95,7 @@
":superstructure_status_fbs",
"//aos:math",
"//aos/controls:control_loop_test",
+ "//aos/events/logging:logger",
"//aos/testing:googletest",
"//aos/time",
"//frc971/control_loops:capped_test_plant",
diff --git a/y2020/control_loops/superstructure/accelerator/BUILD b/y2020/control_loops/superstructure/accelerator/BUILD
index 2c680bf..30fca08 100644
--- a/y2020/control_loops/superstructure/accelerator/BUILD
+++ b/y2020/control_loops/superstructure/accelerator/BUILD
@@ -7,11 +7,28 @@
outs = [
"accelerator_plant.h",
"accelerator_plant.cc",
- "accelerator_integral_plant.h",
- "accelerator_integral_plant.cc",
+ "integral_accelerator_plant.h",
+ "integral_accelerator_plant.cc",
],
cmd = "$(location //y2020/control_loops/python:accelerator) $(OUTS)",
tools = [
"//y2020/control_loops/python:accelerator",
],
)
+
+cc_library(
+ name = "accelerator_plants",
+ srcs = [
+ "accelerator_plant.cc",
+ "integral_accelerator_plant.cc",
+ ],
+ hdrs = [
+ "accelerator_plant.h",
+ "integral_accelerator_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/finisher/BUILD b/y2020/control_loops/superstructure/finisher/BUILD
index b9bfc4f..2009886 100644
--- a/y2020/control_loops/superstructure/finisher/BUILD
+++ b/y2020/control_loops/superstructure/finisher/BUILD
@@ -7,11 +7,28 @@
outs = [
"finisher_plant.h",
"finisher_plant.cc",
- "finisher_integral_plant.h",
- "finisher_integral_plant.cc",
+ "integral_finisher_plant.h",
+ "integral_finisher_plant.cc",
],
cmd = "$(location //y2020/control_loops/python:finisher) $(OUTS)",
tools = [
"//y2020/control_loops/python:finisher",
],
)
+
+cc_library(
+ name = "finisher_plants",
+ srcs = [
+ "finisher_plant.cc",
+ "integral_finisher_plant.cc",
+ ],
+ hdrs = [
+ "finisher_plant.h",
+ "integral_finisher_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index d63231b..fe8a29a 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -4,18 +4,6 @@
cc_library(
name = "shooter_plants",
- srcs = [
- "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.cc",
- "//y2020/control_loops/superstructure/accelerator:accelerator_plant.cc",
- "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.cc",
- "//y2020/control_loops/superstructure/finisher:finisher_plant.cc",
- ],
- hdrs = [
- "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.h",
- "//y2020/control_loops/superstructure/accelerator:accelerator_plant.h",
- "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.h",
- "//y2020/control_loops/superstructure/finisher:finisher_plant.h",
- ],
deps = [
"//frc971/control_loops:state_feedback_loop",
],
@@ -49,10 +37,11 @@
"flywheel_controller.h",
],
deps = [
- ":shooter_plants",
"//aos/controls:control_loop",
"//frc971/control_loops:profiled_subsystem",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plants",
+ "//y2020/control_loops/superstructure/finisher:finisher_plants",
],
)
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index 5853097..a3e9bdb 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -6,8 +6,8 @@
#include "aos/controls/control_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/accelerator/accelerator_integral_plant.h"
-#include "y2020/control_loops/superstructure/finisher/finisher_integral_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
+#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
namespace y2020 {
diff --git a/y2020/control_loops/superstructure/shooter_plot.pb b/y2020/control_loops/superstructure/shooter_plot.pb
new file mode 100644
index 0000000..01a1e20
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter_plot.pb
@@ -0,0 +1,44 @@
+channel {
+ name: "/superstructure"
+ type: "y2020.control_loops.superstructure.Status"
+ alias: "Status"
+}
+channel {
+ name: "/superstructure"
+ type: "y2020.control_loops.superstructure.Output"
+ alias: "Output"
+}
+channel {
+ name: "/superstructure"
+ type: "y2020.control_loops.superstructure.Position"
+ alias: "Position"
+}
+channel {
+ name: "/superstructure"
+ type: "y2020.control_loops.superstructure.Goal"
+ alias: "Goal"
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "Status"
+ field: "hood.position"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Goal"
+ field: "hood.unsafe_goal"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Position"
+ field: "hood.encoder"
+ }
+ }
+ ylabel: "hood position"
+ }
+}
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 9df845a..8ada461 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -4,6 +4,7 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
+#include "aos/events/logging/logger.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
@@ -15,6 +16,9 @@
#include "y2020/control_loops/superstructure/intake/intake_plant.h"
#include "y2020/control_loops/superstructure/superstructure.h"
+DEFINE_string(output_file, "",
+ "If set, logs all channels to the provided logfile.");
+
namespace y2020 {
namespace control_loops {
namespace superstructure {
@@ -386,6 +390,15 @@
superstructure_plant_event_loop_(MakeEventLoop("plant")),
superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {
set_team_id(::frc971::control_loops::testing::kTeamNumber);
+
+ if (!FLAGS_output_file.empty()) {
+ unlink(FLAGS_output_file.c_str());
+ log_buffer_writer_ = std::make_unique<aos::logger::DetachedBufferWriter>(
+ FLAGS_output_file);
+ logger_event_loop_ = MakeEventLoop("logger");
+ logger_ = std::make_unique<aos::logger::Logger>(log_buffer_writer_.get(),
+ logger_event_loop_.get());
+ }
}
void VerifyNearGoal() {
@@ -485,6 +498,10 @@
::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
SuperstructureSimulation superstructure_plant_;
+
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::DetachedBufferWriter> log_buffer_writer_;
+ std::unique_ptr<aos::logger::Logger> logger_;
};
// Tests that the superstructure does nothing when the goal is to remain
@@ -536,8 +553,10 @@
SetEnabled(true);
// Set a reasonable goal.
- superstructure_plant_.InitializeHoodPosition(0.7);
- superstructure_plant_.InitializeIntakePosition(0.7);
+ superstructure_plant_.InitializeHoodPosition(
+ constants::Values::kHoodRange().middle());
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange().middle());
WaitUntilZeroed();
{
@@ -545,12 +564,12 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), 0.2,
+ *builder.fbb(), constants::Values::kHoodRange().upper,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), 0.2,
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
@@ -759,9 +778,6 @@
SetEnabled(true);
// Set a reasonable goal.
- superstructure_plant_.InitializeHoodPosition(0.7);
- superstructure_plant_.InitializeIntakePosition(0.7);
-
WaitUntilZeroed();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 023a242..e8f7637 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -21,7 +21,7 @@
finisher:FlywheelControllerStatus;
// The subsystem to accelerate the ball before the finisher
- // Velocity is the slowest (lowest) wheel
+ // Velocity is the fastest (top) wheel
accelerator_left:FlywheelControllerStatus;
accelerator_right:FlywheelControllerStatus;
}
@@ -33,7 +33,7 @@
// If true, we have aborted. This is if one or all subsystem estops.
estopped:bool;
- //Subsystem status.
+ // Subsystem status.
hood:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index ab8b967..a1083eb 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -66,18 +66,6 @@
// DMA stuff and then removing the * 2.0 in *_translate.
// The low bit is direction.
-// TODO(brian): Use ::std::max instead once we have C++14 so that can be
-// constexpr.
-template <typename T>
-constexpr T max(T a, T b) {
- return (a > b) ? a : b;
-}
-
-template <typename T, typename... Rest>
-constexpr T max(T a, T b, T c, Rest... rest) {
- return max(max(a, b), c, rest...);
-}
-
double drivetrain_translate(int32_t in) {
return ((static_cast<double>(in) /
Values::kDrivetrainEncoderCountsPerRevolution()) *
@@ -98,15 +86,19 @@
(10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
-// TODO(Ravago): check with constants which are fast.
constexpr double kMaxFastEncoderPulsesPerSecond =
- Values::kMaxDrivetrainEncoderPulsesPerSecond();
-static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ std::max({Values::kMaxControlPanelEncoderPulsesPerSecond(),
+ Values::kMaxFinisherEncoderPulsesPerSecond(),
+ Values::kMaxAcceleratorEncoderPulsesPerSecond()});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1000000.0,
"fast encoders are too fast");
constexpr double kMaxMediumEncoderPulsesPerSecond =
- kMaxFastEncoderPulsesPerSecond;
+ std::max({Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxHoodEncoderPulsesPerSecond(),
+ Values::kMaxIntakeEncoderPulsesPerSecond(),
+ Values::kMaxTurretEncoderPulsesPerSecond()});
-static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
+static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000.0,
"medium encoders are too fast");
} // namespace
@@ -193,12 +185,13 @@
// Control Panel
void set_control_panel_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- medium_encoder_filter_.Add(encoder.get());
+ fast_encoder_filter_.Add(encoder.get());
control_panel_encoder_ = ::std::move(encoder);
}
// Auto mode switches.
void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+ medium_encoder_filter_.Add(sensor.get());
autonomous_modes_.at(i) = ::std::move(sensor);
}