Merge "Final competition code"
diff --git a/WORKSPACE b/WORKSPACE
index 21b41f3..cc93fcb 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -12,6 +12,14 @@
"//debian:patch.bzl",
patch_debs = "files",
)
+load(
+ "//debian:pandoc.bzl",
+ pandoc_debs = "files",
+)
+load(
+ "//debian:libusb.bzl",
+ libusb_debs = "files",
+)
load("//debian:packages.bzl", "generate_repositories_for_debs")
generate_repositories_for_debs(python_debs)
@@ -20,6 +28,10 @@
generate_repositories_for_debs(patch_debs)
+generate_repositories_for_debs(pandoc_debs)
+
+generate_repositories_for_debs(libusb_debs)
+
new_http_archive(
name = "python_repo",
build_file = "debian/python.BUILD",
@@ -139,21 +151,6 @@
url = "http://frc971.org/Build-Dependencies/empty.tar.gz",
)
-new_local_repository(
- name = "libusb",
- build_file = "debian/libusb.BUILD",
- path = "/usr",
-)
-
-# Created by combining libusb-1.0-0_2%3a1.0.19-1_amd64,
-# libusb-1.0-0-dev_2%3a1.0.19-1, and libudev1_215-17+deb8u7.
-new_http_archive(
- name = "libusb_1_0",
- build_file = "debian/libusb-1.0.BUILD",
- sha256 = "12acb30faacd10e9aa7f3a5e074701e167ce9bbd45694db37d13d55de5398816",
- url = "http://frc971.org/Build-Dependencies/libusb-1.0-1.0.19.tar.xz",
-)
-
# Recompressed from libusb-1.0.21.7z.
http_file(
name = "libusb_1_0_windows",
@@ -184,3 +181,17 @@
sha256 = "b5ce139648a2e04f5585948ddad2fdae24dd4ee7976ac5a22d6ae7bd5674631e",
url = "http://frc971.org/Build-Dependencies/patch.tar.gz",
)
+
+new_http_archive(
+ name = "pandoc",
+ build_file = "debian/pandoc.BUILD",
+ sha256 = "9f7a7adb3974a1f14715054c349ff3edc2909e920dbe3438fca437a83845f3c4",
+ url = "http://frc971.org/Build-Dependencies/pandoc.tar.gz",
+)
+
+new_http_archive(
+ name = "libusb",
+ build_file = "debian/libusb.BUILD",
+ sha256 = "3ca5cc2d317226f6646866ff9e8c443db3b0f6c82f828e800240982727531590",
+ url = "http://frc971.org/Build-Dependencies/libusb.tar.gz",
+)
diff --git a/aos/linux_code/ipc_lib/aos_sync.cc b/aos/linux_code/ipc_lib/aos_sync.cc
index 3c1debb..d0baaa0 100644
--- a/aos/linux_code/ipc_lib/aos_sync.cc
+++ b/aos/linux_code/ipc_lib/aos_sync.cc
@@ -134,7 +134,11 @@
// actually make the syscall.
// The actual macro that we key off of to use the inline versions or not.
-#define ARM_EABI_INLINE_SYSCALL defined(__ARM_EABI__)
+#if defined(__ARM_EABI__)
+#define ARM_EABI_INLINE_SYSCALL 1
+#else
+#define ARM_EABI_INLINE_SYSCALL 0
+#endif
// Used for FUTEX_WAIT, FUTEX_LOCK_PI, and FUTEX_TRYLOCK_PI.
inline int sys_futex_wait(int op, aos_futex *addr1, int val1,
diff --git a/debian/BUILD b/debian/BUILD
index e9d3823..f1f38f4 100644
--- a/debian/BUILD
+++ b/debian/BUILD
@@ -12,6 +12,14 @@
":patch.bzl",
patch_debs = "files",
)
+load(
+ ":pandoc.bzl",
+ pandoc_debs = "files",
+)
+load(
+ ":libusb.bzl",
+ libusb_debs = "files",
+)
load("//debian:packages.bzl", "download_packages", "generate_deb_tarball")
py_binary(
@@ -62,6 +70,23 @@
],
)
+download_packages(
+ name = "download_pandoc_deps",
+ packages = [
+ "pandoc",
+ ],
+)
+
+download_packages(
+ name = "download_libusb_deps",
+ packages = [
+ "libusb-0.1-4",
+ "libusb-1.0-0",
+ "libusb-1.0-0-dev",
+ "libusb-dev",
+ ],
+)
+
generate_deb_tarball(
name = "python",
files = python_debs,
@@ -76,3 +101,13 @@
name = "patch",
files = patch_debs,
)
+
+generate_deb_tarball(
+ name = "pandoc",
+ files = pandoc_debs,
+)
+
+generate_deb_tarball(
+ name = "libusb",
+ files = libusb_debs,
+)
diff --git a/debian/libusb-1.0.BUILD b/debian/libusb-1.0.BUILD
deleted file mode 100644
index a307cec..0000000
--- a/debian/libusb-1.0.BUILD
+++ /dev/null
@@ -1,14 +0,0 @@
-cc_library(
- name = 'libusb_1_0',
- visibility = ['//visibility:public'],
- hdrs = [
- 'usr/include/libusb-1.0/libusb.h',
- ],
- srcs = [
- 'usr/lib/x86_64-linux-gnu/libusb-1.0.so',
- ],
- includes = [
- 'usr/include',
- ],
- restricted_to = ['@//tools:k8'],
-)
diff --git a/debian/libusb.BUILD b/debian/libusb.BUILD
index 3ccffe1..7c41058 100644
--- a/debian/libusb.BUILD
+++ b/debian/libusb.BUILD
@@ -1,13 +1,28 @@
cc_library(
- name = 'libusb',
- visibility = ['//visibility:public'],
- srcs = [
- 'lib/x86_64-linux-gnu/libusb.so',
- ],
- hdrs = [
- 'include/usb.h',
- ],
- includes = [
- 'include',
- ],
+ name = "libusb",
+ srcs = [
+ "usr/lib/x86_64-linux-gnu/libusb.so",
+ ],
+ hdrs = [
+ "usr/include/usb.h",
+ ],
+ includes = [
+ "usr/include",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "libusb_1_0",
+ srcs = [
+ "usr/lib/x86_64-linux-gnu/libusb-1.0.so",
+ ],
+ hdrs = [
+ "usr/include/libusb-1.0/libusb.h",
+ ],
+ includes = [
+ "usr/include",
+ ],
+ restricted_to = ["@//tools:k8"],
+ visibility = ["//visibility:public"],
)
diff --git a/debian/libusb.bzl b/debian/libusb.bzl
new file mode 100644
index 0000000..1bf64f5
--- /dev/null
+++ b/debian/libusb.bzl
@@ -0,0 +1,7 @@
+files = {
+ "libudev1_215-17+deb8u7_amd64.deb": "35cbd0325c5300e86d0bb5c5bf3a538635b78b56e503dc2f22826b2d53343fcd",
+ "libusb-0.1-4_0.1.12-25_amd64.deb": "18a028fec2fc1de8c24c85f53d6af2972f6688f2765a5e6202674da00179b573",
+ "libusb-1.0-0-dev_1.0.19-1_amd64.deb": "e68962767b1f134e910bcf85995c2a87f047a430c6688b06d5955feb6c56c1a1",
+ "libusb-1.0-0_1.0.19-1_amd64.deb": "c37c63f3496145484c93d10ba2ae3edf73ebb19164d477b0e5e96322bef9aa5d",
+ "libusb-dev_0.1.12-25_amd64.deb": "44db0da49d44e508c494367c1d90e5f7a711a2c7ab5e15c29c98caa8730e07b9",
+}
diff --git a/debian/pandoc.BUILD b/debian/pandoc.BUILD
new file mode 100644
index 0000000..7e3084e
--- /dev/null
+++ b/debian/pandoc.BUILD
@@ -0,0 +1,25 @@
+genrule(
+ name = "gen_wrapper",
+ outs = ["pandoc_wrapper"],
+ cmd = "\n".join([
+ "cat > $@ <<END",
+ "#!/bin/bash",
+ "export LD_LIBRARY_PATH=external/pandoc/usr/lib/x86_64-linux-gnu:external/pandoc/lib/x86_64-linux-gnu",
+ "exec external/pandoc/usr/bin/pandoc \"\\$$@\"",
+ "END",
+ ]),
+ executable = True,
+)
+
+filegroup(
+ name = "pandoc",
+ srcs = ["pandoc_wrapper"],
+ data = glob(["**"]),
+ visibility = ["//visibility:public"],
+)
+
+filegroup(
+ name = "all_files",
+ srcs = glob(["**"]),
+ visibility = ["//visibility:public"],
+)
diff --git a/debian/pandoc.bzl b/debian/pandoc.bzl
new file mode 100644
index 0000000..f203db0
--- /dev/null
+++ b/debian/pandoc.bzl
@@ -0,0 +1,10 @@
+files = {
+ "libffi6_3.1-2+deb8u1_amd64.deb": "100343fca79ff265abc62467c7085fca68b8764e8c2551302ab741c771e7f0aa",
+ "libgmp10_6.0.0+dfsg-6_amd64.deb": "155a31b0f716aa3dcd7ee68e9bd57e0b76a6b31f4e41fb2d953e986315437082",
+ "libicu52_52.1-8+deb8u7_amd64.deb": "708d4499e2f6344a77d903c2c03a958f5edac32f9adf5ff3da7b572bf307e980",
+ "liblua5.1-0_5.1.5-7.1_amd64.deb": "91dfecba874f9330c8a67f17060594ed148c34c44532b60cdc10a480060e38a9",
+ "libstdc++6_4.9.2-10+deb8u1_amd64.deb": "a8f4ef6773b90bb39a8a8a0a5e3e20ca8501de6896204f665eb114d5b79f164f",
+ "libyaml-0-2_0.1.6-3_amd64.deb": "5885db15ac425eb7231c436903525b78381e034bcc53928a97997a745295d222",
+ "pandoc-data_1.12.4.2~dfsg-1_all.deb": "52c4cebd39582e3c2636febd40806a5f1ec71c7be9653addcabbe2732f52a730",
+ "pandoc_1.12.4.2~dfsg-1+b14_amd64.deb": "e2e2a33d398d92ead214391c93afdbd3f590f6ab50d9a88492e2421826484d1c",
+}
diff --git a/motors/BUILD b/motors/BUILD
index 50a4bce..395765e 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -43,7 +43,7 @@
"NOTES.html",
],
cmd = " ".join([
- "pandoc",
+ "$(location @pandoc)",
"-f",
"markdown_github-hard_line_breaks",
"-t",
@@ -52,6 +52,10 @@
"$@",
"$<",
]),
+ tools = [
+ "@pandoc",
+ "@pandoc//:all_files",
+ ],
)
cc_library(
diff --git a/motors/pistol_grip/BUILD b/motors/pistol_grip/BUILD
index 6e6a428..0a24ba8 100644
--- a/motors/pistol_grip/BUILD
+++ b/motors/pistol_grip/BUILD
@@ -58,7 +58,7 @@
deps = [
# Don't add anything else here. :usb_forward_windows still has to build it
# without any other dependencies.
- "@libusb_1_0",
+ "@libusb//:libusb_1_0",
],
)
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index edbe406..cb1dd9c 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -27,21 +27,23 @@
.count();
}
-const ProfileParameters kFinalSwitchDrive = {0.5, 0.5};
+const ProfileParameters kFinalSwitchDrive = {0.5, 1.5};
const ProfileParameters kDrive = {5.0, 2.5};
const ProfileParameters kThirdBoxDrive = {3.0, 2.5};
const ProfileParameters kSlowDrive = {1.5, 2.5};
const ProfileParameters kScaleTurnDrive = {3.0, 2.5};
const ProfileParameters kFarSwitchTurnDrive = {2.0, 2.5};
+const ProfileParameters kFarScaleFinalTurnDrive = kFarSwitchTurnDrive;
const ProfileParameters kTurn = {4.0, 2.0};
const ProfileParameters kSweepingTurn = {5.0, 7.0};
+const ProfileParameters kFarScaleSweepingTurn = kSweepingTurn;
const ProfileParameters kFastTurn = {5.0, 7.0};
const ProfileParameters kReallyFastTurn = {1.5, 9.0};
const ProfileParameters kThirdBoxSlowBackup = {0.35, 1.5};
const ProfileParameters kThirdBoxSlowTurn = {1.5, 4.0};
-const ProfileParameters kThirdBoxPlaceDrive = {4.0, 2.3};
+const ProfileParameters kThirdBoxPlaceDrive = {4.0, 2.0};
const ProfileParameters kFinalFrontFarSwitchDrive = {2.0, 2.0};
@@ -58,318 +60,53 @@
LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
Reset();
+ // Switch
+ /*
switch (params.mode) {
+ case 0:
+ case 2: {
+ if (FarSwitch(start_time, true)) return true;
+ } break;
+
+ case 3:
case 1: {
- constexpr double kDriveDistance = 3.2;
- // Start on the left. Drive, arc a turn, and drop in the close switch.
- StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
- if (!WaitForDriveNear(2.0, M_PI / 2.0)) return true;
-
- // Now, close so let's move the arm up.
- set_arm_goal_position(arm::BackSwitchIndex());
- SendSuperstructureGoal();
-
- StartDrive(0.0, 0.0, kSlowDrive, kSweepingTurn);
- if (!WaitForDriveNear(1.6, M_PI / 2.0)) return true;
-
- StartDrive(0.0, -M_PI / 4.0 - 0.2, kSlowDrive, kSweepingTurn);
- if (!WaitForDriveNear(0.2, 0.2)) return true;
- set_max_drivetrain_voltage(6.0);
- ::std::this_thread::sleep_for(chrono::milliseconds(300));
-
- set_open_claw(true);
- SendSuperstructureGoal();
-
- ::std::this_thread::sleep_for(chrono::milliseconds(500));
- set_max_drivetrain_voltage(12.0);
- StartDrive(0.2, 0.0, kDrive, kTurn);
- if (!WaitForTurnProfileDone()) return true;
-
- set_arm_goal_position(arm::NeutralIndex());
- SendSuperstructureGoal();
-
+ if (CloseSwitch(start_time)) return true;
} break;
- case 0: {
- StartDrive(-3.2, 0.0, kDrive, kTurn);
- if (!WaitForDriveProfileDone()) return true;
- } break;
- case 200:
- {
- constexpr bool kDriveBehind = true;
- if (kDriveBehind) {
- // Start on the left. Hit the switch.
- constexpr double kFullDriveLength = 9.95;
- constexpr double kTurnDistance = 4.40;
- StartDrive(-kFullDriveLength, 0.0, kDrive, kTurn);
- if (!WaitForDriveProfileNear(kFullDriveLength - (kTurnDistance - 1.4)))
- return true;
- StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
-
- if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance))
- return true;
- StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
- if (!WaitForTurnProfileDone()) return true;
- StartDrive(0.0, 0.0, kDrive, kTurn);
- if (!WaitForDriveProfileDone()) return true;
-
- // Now, close so let's move the arm up.
- set_arm_goal_position(arm::FrontSwitchAutoIndex());
- SendSuperstructureGoal();
-
- StartDrive(0.0, M_PI / 2.0, kDrive, kFastTurn);
- if (!WaitForTurnProfileDone()) return true;
-
- set_max_drivetrain_voltage(6.0);
- StartDrive(0.35, 0.0, kFinalSwitchDrive, kTurn);
- if (!WaitForArmTrajectoryClose(0.001)) return true;
- // if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
- ::std::this_thread::sleep_for(chrono::milliseconds(1500));
-
- set_open_claw(true);
- SendSuperstructureGoal();
-
- ::std::this_thread::sleep_for(chrono::milliseconds(1500));
- set_arm_goal_position(arm::NeutralIndex());
- SendSuperstructureGoal();
- if (ShouldCancel()) return true;
- set_max_drivetrain_voltage(12.0);
- StartDrive(-0.5, 0.0, kDrive, kTurn);
- if (!WaitForDriveProfileDone()) return true;
- } else {
- // Start on the left. Hit the switch.
- constexpr double kFullDriveLength = 5.55;
- constexpr double kTurnDistance = 0.35;
- StartDrive(-kFullDriveLength, 0.0, kFarSwitchTurnDrive, kTurn);
-
- if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance))
- return true;
- StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
- if (!WaitForTurnProfileDone()) return true;
- StartDrive(0.0, 0.0, kDrive, kTurn);
- if (!WaitForDriveProfileDone()) return true;
-
- // Now, close so let's move the arm up.
- set_arm_goal_position(arm::FrontSwitchIndex());
- SendSuperstructureGoal();
-
- StartDrive(0.0, -M_PI / 2.0, kDrive, kFastTurn);
- if (!WaitForTurnProfileDone()) return true;
-
- set_max_drivetrain_voltage(10.0);
- StartDrive(1.1, 0.0, kDrive, kTurn);
- if (!WaitForArmTrajectoryClose(0.001)) return true;
- if (!WaitForDriveNear(0.6, M_PI / 2.0)) return true;
- StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
-
- if (!WaitForDriveNear(0.3, M_PI / 2.0)) return true;
- set_max_drivetrain_voltage(6.0);
- StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
-
- // if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
- ::std::this_thread::sleep_for(chrono::milliseconds(300));
-
- set_open_claw(true);
- SendSuperstructureGoal();
-
- ::std::this_thread::sleep_for(chrono::milliseconds(1000));
- set_arm_goal_position(arm::NeutralIndex());
- SendSuperstructureGoal();
- if (ShouldCancel()) return true;
- set_max_drivetrain_voltage(12.0);
- StartDrive(-0.5, 0.0, kDrive, kTurn);
- if (!WaitForDriveProfileDone()) return true;
- }
+ }
+ */
+ // Scale
+ switch (params.mode) {
+ case 0:
+ case 1: {
+ if (FarScale(start_time)) return true;
} break;
case 3:
case 2: {
- // Start on the left. Hit the scale.
- constexpr double kDriveDistance = 6.95;
- // Distance and angle to do the big drive to the third cube.
- constexpr double kThirdCubeDrive = 1.57;
- constexpr double kThirdCubeTurn = M_PI / 4.0 - 0.1;
- // Angle to do the slow pickup turn on the third box.
- constexpr double kThirdBoxEndTurnAngle = 0.20;
-
- // Distance to drive back to the scale with the third cube.
- constexpr double kThirdCubeDropDrive = kThirdCubeDrive + 0.30;
-
- // Drive.
- StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
- if (!WaitForDriveNear(kDriveDistance - 1.0, M_PI / 2.0)) return true;
- // Once we are away from the wall, start the arm.
- set_arm_goal_position(arm::BackMiddle2BoxIndex());
- SendSuperstructureGoal();
-
- // We are starting to get close. Slow down for the turn.
- if (!WaitForDriveNear(4.0, M_PI / 2.0)) return true;
- StartDrive(0.0, 0.0, kScaleTurnDrive, kFastTurn);
-
- // Once we've gotten slowed down a bit, start turning.
- if (!WaitForDriveNear(3.25, M_PI / 2.0)) return true;
- StartDrive(0.0, -M_PI / 6.0, kScaleTurnDrive, kFastTurn);
-
- if (!WaitForDriveNear(1.0, M_PI / 2.0)) return true;
- StartDrive(0.0, M_PI / 6.0, kScaleTurnDrive, kFastTurn);
-
- // Get close and open the claw.
- if (!WaitForDriveNear(0.15, 0.25)) return true;
- set_open_claw(true);
- SendSuperstructureGoal();
- set_intake_angle(-0.40);
- LOG(INFO, "Dropped first box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
-
- ::std::this_thread::sleep_for(chrono::milliseconds(500));
-
- set_grab_box(true);
- SendSuperstructureGoal();
-
- ::std::this_thread::sleep_for(chrono::milliseconds(200));
-
- LOG(INFO, "Starting second box drive %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
- constexpr double kSecondBoxSwerveAngle = 0.35;
- constexpr double kSecondBoxDrive = 1.43;
- StartDrive(kSecondBoxDrive, 0.0, kDrive, kFastTurn);
- if (!WaitForDriveNear(kSecondBoxDrive - 0.2, M_PI / 2.0)) return true;
-
- StartDrive(0.0, kSecondBoxSwerveAngle, kDrive, kFastTurn);
- if (!WaitForDriveNear(0.5, M_PI / 2.0)) return true;
-
- set_open_claw(true);
- set_disable_box_correct(false);
- SendSuperstructureGoal();
-
- StartDrive(0.0, -kSecondBoxSwerveAngle, kDrive, kFastTurn);
-
- if (!WaitForDriveProfileDone()) return true;
- set_intake_angle(0.10);
- set_arm_goal_position(arm::BackHighBoxIndex());
- set_open_claw(false);
-
- set_roller_voltage(10.0);
- SendSuperstructureGoal();
-
- LOG(INFO, "Grabbing second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
- if (!WaitForBoxGrabed()) return true;
-
- LOG(INFO, "Got second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
- ::std::this_thread::sleep_for(chrono::milliseconds(600));
-
- set_grab_box(false);
- set_arm_goal_position(arm::UpIndex());
- set_roller_voltage(0.0);
- set_disable_box_correct(false);
- SendSuperstructureGoal();
- LOG(INFO, "Driving to place second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
-
- StartDrive(-kSecondBoxDrive + 0.2, kSecondBoxSwerveAngle, kDrive,
- kFastTurn);
- if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
-
- constexpr double kSecondBoxEndExtraAngle = 0.3;
- StartDrive(0.0, -kSecondBoxSwerveAngle - kSecondBoxEndExtraAngle, kDrive,
- kFastTurn);
-
- LOG(INFO, "Starting throw %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
- if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
- set_arm_goal_position(arm::BackHighBoxIndex());
- SendSuperstructureGoal();
-
- // Throw the box.
- if (!WaitForArmTrajectoryClose(0.03)) return true;
-
- set_open_claw(true);
- set_intake_angle(-M_PI / 4.0);
- LOG(INFO, "Releasing second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
- SendSuperstructureGoal();
-
- ::std::this_thread::sleep_for(chrono::milliseconds(700));
- set_open_claw(false);
- set_grab_box(true);
- SendSuperstructureGoal();
-
- LOG(INFO, "Driving to third box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
- StartDrive(kThirdCubeDrive, kSecondBoxEndExtraAngle, kThirdBoxDrive,
- kFastTurn);
- if (!WaitForDriveNear(kThirdCubeDrive - 0.1, M_PI / 4.0)) return true;
-
- StartDrive(0.0, kThirdCubeTurn, kThirdBoxDrive, kFastTurn);
- if (!WaitForDriveNear(0.3, M_PI / 4.0)) return true;
-
- set_intake_angle(0.05);
- set_roller_voltage(9.0);
- SendSuperstructureGoal();
-
- if (!WaitForDriveProfileDone()) return true;
- if (!WaitForTurnProfileDone()) return true;
- StartDrive(0.30, kThirdBoxEndTurnAngle, kThirdBoxSlowBackup, kThirdBoxSlowTurn);
- if (!WaitForDriveProfileDone()) return true;
-
- if (!WaitForBoxGrabed()) return true;
- LOG(INFO, "Third box grabbed %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
- const bool too_late =
- monotonic_clock::now() > start_time + chrono::milliseconds(12000);
- if (too_late) {
- LOG(INFO, "Third box too long, going up. %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
- set_grab_box(false);
- set_arm_goal_position(arm::UpIndex());
- set_roller_voltage(0.0);
- SendSuperstructureGoal();
- }
- ::std::this_thread::sleep_for(chrono::milliseconds(600));
-
- set_grab_box(false);
- if (!too_late) {
- set_arm_goal_position(arm::BackMiddle2BoxIndex());
- }
- set_roller_voltage(0.0);
- SendSuperstructureGoal();
-
- StartDrive(-kThirdCubeDropDrive, 0.0,
- kThirdBoxPlaceDrive, kReallyFastTurn);
-
- if (!WaitForDriveNear(1.40, M_PI / 4.0)) return true;
- StartDrive(0.0, -kThirdCubeTurn - 0.2 - kThirdBoxEndTurnAngle - 0.3,
- kThirdBoxPlaceDrive, kReallyFastTurn);
-
- if (!WaitForArmTrajectoryClose(0.005)) return true;
- if (!WaitForDriveProfileDone()) return true;
- if (!WaitForTurnProfileDone()) return true;
-
- if (!too_late) {
- set_open_claw(true);
- set_intake_angle(-M_PI / 4.0);
- set_roller_voltage(0.0);
- SendSuperstructureGoal();
-
- LOG(INFO, "Final open %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
- }
-
- ::std::this_thread::sleep_for(chrono::milliseconds(14750) -
- (monotonic_clock::now() - start_time));
-
- set_arm_goal_position(arm::UpIndex());
- SendSuperstructureGoal();
-
- ::std::this_thread::sleep_for(chrono::milliseconds(15000) -
- (monotonic_clock::now() - start_time));
-
- set_close_claw(true);
- SendSuperstructureGoal();
-
+ if (ThreeCubeAuto(start_time)) return true;
} break;
}
+ /*
+ switch (params.mode) {
+ case 1: {
+ if (FarScale(start_time)) return true;
+ //if (CloseSwitch(start_time)) return true;
+ } break;
+ case 0: {
+ if (DriveStraight()) return true;
+ } break;
+ case 200: {
+ if (FarSwitch(start_time)) return true;
+ } break;
+
+ case 3:
+ case 2: {
+ if (ThreeCubeAuto(start_time)) {
+ return true;
+ }
+ } break;
+ }
+ */
LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
@@ -384,5 +121,469 @@
return true;
}
+bool AutonomousActor::FarSwitch(monotonic_clock::time_point start_time,
+ bool drive_behind, bool left) {
+ const double turn_scalar = left ? 1.0 : -1.0;
+ if (drive_behind) {
+ // Start on the left. Hit the switch.
+ constexpr double kFullDriveLength = 9.93;
+ constexpr double kTurnDistance = 4.40;
+ StartDrive(-kFullDriveLength, 0.0, kDrive, kTurn);
+ set_arm_goal_position(arm::NeutralIndex());
+ set_grab_box(false);
+ SendSuperstructureGoal();
+ if (!WaitForDriveProfileNear(kFullDriveLength - (kTurnDistance - 1.4)))
+ return true;
+ StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
+
+ if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance)) return true;
+ StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kFarSwitchTurnDrive, kSweepingTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ // Now, close so let's move the arm up.
+ set_arm_goal_position(arm::FrontSwitchAutoIndex());
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileNear(2.0)) return true;
+ StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kFastTurn);
+
+ if (!WaitForDriveProfileNear(1.3)) return true;
+
+ StartDrive(0.0, turn_scalar * M_PI / 2.0, kFarSwitchTurnDrive, kFastTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ constexpr double kGentlePushDrive = 0.70;
+ StartDrive(kGentlePushDrive, 0.0, kSlowDrive, kTurn);
+
+ if (!WaitForDriveProfileNear(kGentlePushDrive - 0.25)) return true;
+ // Turn down the peak voltage when we push against the wall so we don't blow
+ // breakers or pull the battery voltage down too far.
+ set_max_drivetrain_voltage(6.0);
+ StartDrive(0.00, 0.0, kFinalSwitchDrive, kTurn);
+
+ if (!WaitForArmTrajectoryClose(0.001)) return true;
+ LOG(INFO, "Arm close at %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(1000));
+
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(1500));
+ set_arm_goal_position(arm::NeutralIndex());
+ SendSuperstructureGoal();
+ if (ShouldCancel()) return true;
+ set_max_drivetrain_voltage(12.0);
+ StartDrive(-0.5, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+ } else {
+ // Start on the left. Hit the switch.
+ constexpr double kFullDriveLength = 5.55;
+ constexpr double kTurnDistance = 0.35;
+ StartDrive(-kFullDriveLength, 0.0, kFarSwitchTurnDrive, kTurn);
+
+ if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance)) return true;
+ StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kFarSwitchTurnDrive,
+ kSweepingTurn);
+ if (!WaitForTurnProfileDone()) return true;
+ StartDrive(0.0, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+
+ // Now, close so let's move the arm up.
+ set_arm_goal_position(arm::FrontSwitchIndex());
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kDrive, kFastTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ set_max_drivetrain_voltage(10.0);
+ StartDrive(1.1, 0.0, kDrive, kTurn);
+ if (!WaitForArmTrajectoryClose(0.001)) return true;
+ if (!WaitForDriveNear(0.6, M_PI / 2.0)) return true;
+ StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
+
+ if (!WaitForDriveNear(0.3, M_PI / 2.0)) return true;
+ set_max_drivetrain_voltage(6.0);
+ StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
+
+ // if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
+ ::std::this_thread::sleep_for(chrono::milliseconds(300));
+
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(1000));
+ set_arm_goal_position(arm::NeutralIndex());
+ SendSuperstructureGoal();
+ if (ShouldCancel()) return true;
+ set_max_drivetrain_voltage(12.0);
+ StartDrive(-0.5, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+ }
+ return false;
+}
+
+bool AutonomousActor::FarScale(monotonic_clock::time_point start_time) {
+ // Start on the left. Hit the switch.
+ constexpr double kFullDriveLength = 11.40;
+ constexpr double kFirstTurnDistance = 4.40;
+ constexpr double kSecondTurnDistance = 9.30;
+ StartDrive(-kFullDriveLength, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileNear(kFullDriveLength - (kFirstTurnDistance - 1.4)))
+ return true;
+ StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
+
+ if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance)) return true;
+ StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ SendSuperstructureGoal();
+ if (!WaitForTurnProfileDone()) return true;
+
+ StartDrive(0.0, 0.0, kDrive, kTurn);
+
+ if (!WaitForDriveProfileNear(kFullDriveLength - (kSecondTurnDistance - 1.4)))
+ return true;
+
+ StartDrive(0.0, 0.0, kFarScaleFinalTurnDrive, kTurn);
+ if (!WaitForDriveProfileNear(kFullDriveLength - (kSecondTurnDistance)))
+ return true;
+ LOG(INFO, "Final turn at %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ StartDrive(0.0, M_PI / 2.0, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
+ if (!WaitForDriveProfileNear(0.15)) return true;
+
+ StartDrive(0.0, 0.3, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
+
+ LOG(INFO, "Dropping at %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(1000));
+ LOG(INFO, "Backing up at %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ StartDrive(1.5, -0.55, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
+
+ if (!WaitForDriveProfileNear(1.4)) return true;
+ set_arm_goal_position(arm::NeutralIndex());
+ set_open_claw(false);
+ set_grab_box(true);
+ SendSuperstructureGoal();
+
+ if (!WaitForDriveProfileNear(0.3)) return true;
+
+ set_intake_angle(0.15);
+ set_roller_voltage(10.0);
+ SendSuperstructureGoal();
+
+ set_max_drivetrain_voltage(6.0);
+ StartDrive(0.0, 0.0, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
+
+ if (!WaitForDriveProfileDone()) return true;
+ if (!WaitForTurnProfileDone()) return true;
+
+ StartDrive(0.2, 0.0, kThirdBoxSlowBackup, kThirdBoxSlowTurn);
+
+ if (!WaitForBoxGrabed()) return true;
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ set_intake_angle(-3.0);
+ set_roller_voltage(0.0);
+ set_grab_box(false);
+ SendSuperstructureGoal();
+
+ StartDrive(-1.40, 0.15, kThirdBoxPlaceDrive, kFarScaleSweepingTurn);
+
+ if (!WaitForDriveProfileNear(0.1)) return true;
+
+ StartDrive(0.0, 0.5, kThirdBoxPlaceDrive, kFarScaleSweepingTurn);
+
+ if (!WaitForDriveProfileDone()) return true;
+ if (!WaitForTurnProfileDone()) return true;
+ ::std::this_thread::sleep_for(chrono::milliseconds(500));
+
+ LOG(INFO, "Dropping second box at %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(1000));
+
+ StartDrive(1.4, -0.7, kThirdBoxPlaceDrive, kFarScaleSweepingTurn);
+ ::std::this_thread::sleep_for(chrono::milliseconds(200));
+ set_arm_goal_position(arm::NeutralIndex());
+ set_open_claw(false);
+ SendSuperstructureGoal();
+
+ if (!WaitForDriveProfileNear(1.0)) return true;
+
+ StartDrive(0.0, -0.6, kThirdBoxPlaceDrive, kFarScaleSweepingTurn);
+ return false;
+}
+
+bool AutonomousActor::FarReadyScale(monotonic_clock::time_point start_time) {
+ // Start on the left. Hit the switch.
+ constexpr double kFullDriveLength = 7.5;
+ constexpr double kFirstTurnDistance = 4.40;
+ StartDrive(-kFullDriveLength, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileNear(kFullDriveLength - (kFirstTurnDistance - 1.4)))
+ return true;
+ StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
+
+ if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance)) return true;
+ StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
+ set_arm_goal_position(arm::UpIndex());
+ SendSuperstructureGoal();
+ LOG(INFO, "Lifting arm at %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ if (!WaitForTurnProfileDone()) return true;
+
+ StartDrive(0.0, 0.0, kDrive, kTurn);
+ return false;
+}
+
+bool AutonomousActor::DriveStraight() {
+ StartDrive(-3.2, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+ return false;
+}
+
+bool AutonomousActor::CloseSwitch(monotonic_clock::time_point start_time,
+ bool left) {
+ const double turn_scalar = left ? 1.0 : -1.0;
+
+ constexpr double kDriveDistance = 3.2;
+ // Start on the left. Drive, arc a turn, and drop in the close switch.
+ StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
+ if (!WaitForDriveNear(2.5, M_PI / 2.0)) return true;
+
+ // Now, close so let's move the arm up.
+ set_arm_goal_position(arm::BackSwitchIndex());
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, 0.0, kSlowDrive, kSweepingTurn);
+ if (!WaitForDriveNear(1.6, M_PI / 2.0)) return true;
+
+ StartDrive(0.0, turn_scalar * (-M_PI / 4.0 - 0.2), kSlowDrive, kSweepingTurn);
+ if (!WaitForDriveNear(0.2, 0.2)) return true;
+ set_max_drivetrain_voltage(6.0);
+ LOG(INFO, "Lowered drivetrain voltage %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ ::std::this_thread::sleep_for(chrono::milliseconds(300));
+
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(500));
+ set_max_drivetrain_voltage(12.0);
+ StartDrive(0.7, 0.0, kDrive, kTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ set_arm_goal_position(arm::NeutralIndex());
+ SendSuperstructureGoal();
+ return false;
+}
+
+bool AutonomousActor::ThreeCubeAuto(monotonic_clock::time_point start_time) {
+ // Start on the left. Hit the scale.
+ constexpr double kDriveDistance = 6.95;
+ // Distance and angle to do the big drive to the third cube.
+ constexpr double kThirdCubeDrive = 1.57 + 0.05 + 0.05;
+ constexpr double kThirdCubeTurn = M_PI / 4.0 + 0.1;
+ // Angle to do the slow pickup turn on the third box.
+ constexpr double kThirdBoxEndTurnAngle = 0.30;
+
+ // Distance to drive back to the scale with the third cube.
+ constexpr double kThirdCubeDropDrive = kThirdCubeDrive + 0.40;
+
+ // Drive.
+ StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
+ if (!WaitForDriveNear(kDriveDistance - 1.0, M_PI / 2.0)) return true;
+ // Once we are away from the wall, start the arm.
+ set_arm_goal_position(arm::BackMiddle2BoxIndex());
+ SendSuperstructureGoal();
+
+ // We are starting to get close. Slow down for the turn.
+ if (!WaitForDriveNear(4.0, M_PI / 2.0)) return true;
+ StartDrive(0.0, 0.0, kScaleTurnDrive, kFastTurn);
+
+ // Once we've gotten slowed down a bit, start turning.
+ if (!WaitForDriveNear(3.25, M_PI / 2.0)) return true;
+ StartDrive(0.0, -M_PI / 6.0, kScaleTurnDrive, kFastTurn);
+
+ if (!WaitForDriveNear(1.0, M_PI / 2.0)) return true;
+ StartDrive(0.0, M_PI / 6.0, kScaleTurnDrive, kFastTurn);
+
+ // Get close and open the claw.
+ if (!WaitForDriveNear(0.15, 0.25)) return true;
+ set_open_claw(true);
+ SendSuperstructureGoal();
+ set_intake_angle(-0.60);
+ LOG(INFO, "Dropped first box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(700));
+
+ set_grab_box(true);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Starting second box drive %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ constexpr double kSecondBoxSwerveAngle = 0.35;
+ constexpr double kSecondBoxDrive = 1.38;
+ StartDrive(kSecondBoxDrive, 0.0, kDrive, kFastTurn);
+ if (!WaitForDriveNear(kSecondBoxDrive - 0.2, M_PI / 2.0)) return true;
+
+ StartDrive(0.0, kSecondBoxSwerveAngle, kDrive, kFastTurn);
+ if (!WaitForDriveNear(0.5, M_PI / 2.0)) return true;
+
+ set_open_claw(true);
+ set_disable_box_correct(false);
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, -kSecondBoxSwerveAngle, kDrive, kFastTurn);
+
+ if (!WaitForDriveProfileDone()) return true;
+ set_max_drivetrain_voltage(6.0);
+ StartDrive(0.0, 0.0, kDrive, kFastTurn);
+
+ set_intake_angle(0.15);
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ set_open_claw(false);
+
+ set_roller_voltage(10.0);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Grabbing second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ ::std::this_thread::sleep_for(chrono::milliseconds(200));
+ StartDrive(-0.04, 0.0, kThirdBoxSlowBackup, kThirdBoxSlowTurn);
+
+ if (!WaitForBoxGrabed()) return true;
+ set_max_drivetrain_voltage(12.0);
+
+ LOG(INFO, "Got second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ ::std::this_thread::sleep_for(chrono::milliseconds(500));
+
+ set_grab_box(false);
+ //set_arm_goal_position(arm::UpIndex());
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ set_roller_voltage(0.0);
+ set_disable_box_correct(false);
+ SendSuperstructureGoal();
+ LOG(INFO, "Driving to place second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ StartDrive(-kSecondBoxDrive + 0.16, kSecondBoxSwerveAngle, kDrive, kFastTurn);
+ if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
+
+ constexpr double kSecondBoxEndExtraAngle = 0.3;
+ StartDrive(0.0, -kSecondBoxSwerveAngle - kSecondBoxEndExtraAngle, kDrive,
+ kFastTurn);
+
+ LOG(INFO, "Starting throw %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
+ if (!WaitForArmTrajectoryClose(0.25)) return true;
+ SendSuperstructureGoal();
+
+ // Throw the box.
+ //if (!WaitForArmTrajectoryClose(0.20)) return true;
+ if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
+
+ set_open_claw(true);
+ set_intake_angle(-M_PI / 4.0);
+ LOG(INFO, "Releasing second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(700));
+ set_open_claw(false);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Driving to third box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ StartDrive(kThirdCubeDrive, kSecondBoxEndExtraAngle, kThirdBoxDrive,
+ kFastTurn);
+ if (!WaitForDriveNear(kThirdCubeDrive - 0.1, M_PI / 4.0)) return true;
+ set_grab_box(true);
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, kThirdCubeTurn, kThirdBoxDrive, kFastTurn);
+ if (!WaitForDriveNear(0.05, M_PI / 4.0)) return true;
+
+ set_intake_angle(0.05);
+ set_roller_voltage(9.0);
+ SendSuperstructureGoal();
+
+ if (!WaitForDriveProfileDone()) return true;
+ if (!WaitForTurnProfileDone()) return true;
+ StartDrive(0.35, kThirdBoxEndTurnAngle, kThirdBoxSlowBackup,
+ kThirdBoxSlowTurn);
+ if (!WaitForDriveProfileDone()) return true;
+
+ LOG(INFO, "Waiting for third box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ if (!WaitForBoxGrabed()) return true;
+ LOG(INFO, "Third box grabbed %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ const bool too_late =
+ monotonic_clock::now() > start_time + chrono::milliseconds(12500);
+ if (too_late) {
+ LOG(INFO, "Third box too long, going up. %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ set_grab_box(false);
+ set_arm_goal_position(arm::UpIndex());
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
+ }
+ ::std::this_thread::sleep_for(chrono::milliseconds(400));
+
+ set_grab_box(false);
+ if (!too_late) {
+ set_arm_goal_position(arm::BackMiddle2BoxIndex());
+ }
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
+
+ StartDrive(-kThirdCubeDropDrive, 0.0, kThirdBoxPlaceDrive, kReallyFastTurn);
+
+ if (!WaitForDriveNear(kThirdCubeDropDrive - 0.23, M_PI / 4.0)) return true;
+ StartDrive(0.0, -kThirdCubeTurn - 0.2 - kThirdBoxEndTurnAngle - 0.3,
+ kThirdBoxPlaceDrive, kReallyFastTurn);
+
+ if (!WaitForDriveNear(0.30, M_PI / 4.0 + 0.2)) return true;
+
+ if (!too_late) {
+ set_open_claw(true);
+ set_intake_angle(-M_PI / 4.0);
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Final open %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ }
+
+ if (!WaitForDriveProfileDone()) return true;
+ if (!WaitForTurnProfileDone()) return true;
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(14750) -
+ (monotonic_clock::now() - start_time));
+
+ set_arm_goal_position(arm::UpIndex());
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(15000) -
+ (monotonic_clock::now() - start_time));
+
+ set_close_claw(true);
+ SendSuperstructureGoal();
+ return false;
+}
+
} // namespace actors
} // namespace y2018
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index d564f5d..32f8287 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -87,12 +87,23 @@
new_superstructure_goal->open_claw = open_claw_;
new_superstructure_goal->close_claw = close_claw_;
new_superstructure_goal->deploy_fork = deploy_fork_;
+ new_superstructure_goal->trajectory_override = false;
if (!new_superstructure_goal.Send()) {
LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
+ bool ThreeCubeAuto(::aos::monotonic_clock::time_point start_time);
+ bool CloseSwitch(::aos::monotonic_clock::time_point start_time,
+ bool left = true);
+ bool FarSwitch(::aos::monotonic_clock::time_point start_time,
+ bool drive_behind = true, bool left = true);
+ bool FarReadyScale(::aos::monotonic_clock::time_point start_time);
+ bool DriveStraight();
+
+ bool FarScale(::aos::monotonic_clock::time_point start_time);
+
bool WaitForArmTrajectoryOrDriveClose(double drive_threshold,
double arm_threshold) {
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
diff --git a/y2018/analysis/arm b/y2018/analysis/arm
index 3aa101d..2e99daa 100644
--- a/y2018/analysis/arm
+++ b/y2018/analysis/arm
@@ -2,6 +2,8 @@
superstructure.stripped output voltage_proximal
superstructure.stripped status arm path_distance_to_go
+superstructure.stripped goal arm_goal_position
+superstructure.stripped status arm current_node
superstructure.stripped status arm proximal_estimator_state position
superstructure.stripped status arm goal_theta0
diff --git a/y2018/control_loops/python/graph_edit.py b/y2018/control_loops/python/graph_edit.py
index fbf5f8b..dde67c5 100644
--- a/y2018/control_loops/python/graph_edit.py
+++ b/y2018/control_loops/python/graph_edit.py
@@ -172,7 +172,7 @@
def __init__(self):
super(Silly, self).__init__()
- self.theta_version = True
+ self.theta_version = False
self.reinit_extents()
self.last_pos = (numpy.pi / 2.0, 1.0)
@@ -182,6 +182,8 @@
self.segments = []
self.prev_segment_pt = None
self.now_segment_pt = None
+ self.spline_edit = 0
+ self.edit_control1 = True
def reinit_extents(self):
if self.theta_version:
@@ -410,17 +412,44 @@
self.theta_version = not self.theta_version
self.reinit_extents()
+
+ elif keyval == Gdk.KEY_z:
+ self.edit_control1 = not self.edit_control1
+ if self.edit_control1:
+ self.now_segment_pt = self.segments[0].control1
+ else:
+ self.now_segment_pt = self.segments[0].control2
+ if not self.theta_version:
+ data = to_xy(self.now_segment_pt[0], self.now_segment_pt[1])
+ self.last_pos = (data[0], data[1])
+ else:
+ self.last_pos = self.now_segment_pt
+
+ print("self.last_pos: ", self.last_pos, " ci: ",
+ self.circular_index_select)
+
self.redraw()
def do_button_press(self, event):
self.last_pos = (event.x, event.y)
self.now_segment_pt = self.cur_pt_in_theta()
+
+ if self.edit_control1:
+ self.segments[0].control1 = self.now_segment_pt
+ else:
+ self.segments[0].control2 = self.now_segment_pt
+
print('Clicked at theta: %s' % (repr(self.now_segment_pt,)))
if not self.theta_version:
print('Clicked at xy, circular index: (%f, %f, %f)' %
(self.last_pos[0], self.last_pos[1],
self.circular_index_select))
+ print('c1: numpy.array([%f, %f])' % (self.segments[0].control1[0],
+ self.segments[0].control1[1]))
+ print('c2: numpy.array([%f, %f])' % (self.segments[0].control2[0],
+ self.segments[0].control2[1]))
+
self.redraw()
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 642a006..dae7caa 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -531,7 +531,7 @@
self_hang = numpy.array(
[numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
partner_hang = numpy.array(
- [numpy.pi / 2.0 - (-0.25), numpy.pi / 2.0])
+ [numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
above_hang = numpy.array(
[numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
@@ -663,11 +663,17 @@
tall_to_back_high_c2 = numpy.array([1.508610, 0.946147])
# If true, only plot the first named segment
-isolate = 0
+isolate = False
long_alpha_unitizer = numpy.matrix([[1.0 / 17.0, 0.0], [0.0, 1.0 / 17.0]])
+neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
+neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
+
+
named_segments = [
+ ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2, back_switch, "BackSwitch"),
+
ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2, back_high_box, "NeutralBoxToHigh", alpha_unitizer=long_alpha_unitizer),
ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2, back_middle2_box, "NeutralBoxToMiddle2", long_alpha_unitizer),
ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2, back_middle1_box, "NeutralBoxToMiddle1", long_alpha_unitizer),
@@ -713,7 +719,6 @@
SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
- AngleSegment(neutral, back_switch),
XYSegment(ready_above_box, front_low_box),
XYSegment(ready_above_box, front_switch),
diff --git a/y2018/control_loops/python/intake.py b/y2018/control_loops/python/intake.py
index 8879dff..a9479e0 100755
--- a/y2018/control_loops/python/intake.py
+++ b/y2018/control_loops/python/intake.py
@@ -165,7 +165,7 @@
# space.
q_output_vel = 1.0
- q_spring_pos = 0.5
+ q_spring_pos = 0.10
q_spring_vel = 2.0
q_voltage = 1000000000000.0
self.Q_lqr = numpy.matrix(
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index d79a293..812f091 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -42,10 +42,10 @@
bool close_claw, const control_loops::ArmPosition *position,
const bool claw_beambreak_triggered,
const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, double *proximal_output,
+ const bool intake_clear_of_box, bool suicide,
+ bool trajectory_override, double *proximal_output,
double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status,
- bool suicide) {
+ bool *claw_closed, control_loops::ArmStatus *status) {
::Eigen::Matrix<double, 2, 1> Y;
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
@@ -160,6 +160,11 @@
case State::GOTO_PATH:
if (outputs_disabled) {
state_ = State::DISABLED;
+ } else if (trajectory_override) {
+ follower_.SwitchTrajectory(nullptr);
+ current_node_ = filtered_goal;
+ follower_.set_theta(points_[current_node_]);
+ state_ = State::GOTO_PATH;
} else if (close_enough_for_full_power_) {
state_ = State::RUNNING;
grab_state_ = GrabState::NORMAL;
@@ -175,6 +180,11 @@
state_ = State::ESTOP;
} else if (outputs_disabled) {
state_ = State::DISABLED;
+ } else if (trajectory_override) {
+ follower_.SwitchTrajectory(nullptr);
+ current_node_ = filtered_goal;
+ follower_.set_theta(points_[current_node_]);
+ state_ = State::GOTO_PATH;
} else if (suicide) {
state_ = State::PREP_CLIMB;
climb_count_ = 50;
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 6b2864a..d3dd2df 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -38,10 +38,11 @@
bool close_claw, const control_loops::ArmPosition *position,
const bool claw_beambreak_triggered,
const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, double *proximal_output,
+ const bool intake_clear_of_box,
+ bool suicide, bool trajectory_override,
+ double *proximal_output,
double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status,
- bool suicide);
+ bool *claw_closed, control_loops::ArmStatus *status);
void Reset();
@@ -56,12 +57,12 @@
};
enum class GrabState : int32_t {
- NORMAL,
- WAIT_FOR_BOX,
- TALL_BOX,
- SHORT_BOX,
- CLAW_CLOSE,
- OPEN_INTAKE,
+ NORMAL = 0,
+ WAIT_FOR_BOX = 1,
+ TALL_BOX = 2,
+ SHORT_BOX = 3,
+ CLAW_CLOSE = 4,
+ OPEN_INTAKE = 5,
};
State state() const { return state_; }
@@ -71,6 +72,10 @@
// intake position to release the box when the state machine is lifting.
double max_intake_override() const { return max_intake_override_; }
+ uint32_t current_node() const { return current_node_; }
+
+ double path_distance_to_go() { return follower_.path_distance_to_go(); }
+
private:
bool AtState(uint32_t state) const { return current_node_ == state; }
bool NearEnd(double threshold = 0.03) const {
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 2098cd0..26f75be 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -96,18 +96,28 @@
const bool intake_clear_of_box =
intake_left_.clear_of_box() && intake_right_.clear_of_box();
+
+ bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw : false;
+ if (unsafe_goal) {
+ if (unsafe_goal->open_threshold != 0.0) {
+ if (arm_.current_node() != unsafe_goal->arm_goal_position ||
+ arm_.path_distance_to_go() > unsafe_goal->open_threshold) {
+ open_claw = false;
+ }
+ }
+ }
arm_.Iterate(
unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
- unsafe_goal != nullptr ? unsafe_goal->grab_box : false,
- unsafe_goal != nullptr ? unsafe_goal->open_claw : false,
+ unsafe_goal != nullptr ? unsafe_goal->grab_box : false, open_claw,
unsafe_goal != nullptr ? unsafe_goal->close_claw : false,
&(position->arm), position->claw_beambreak_triggered,
position->box_back_beambreak_triggered, intake_clear_of_box,
+ unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false,
+ unsafe_goal != nullptr ? unsafe_goal->trajectory_override : false,
output != nullptr ? &(output->voltage_proximal) : nullptr,
output != nullptr ? &(output->voltage_distal) : nullptr,
output != nullptr ? &(output->release_arm_brake) : nullptr,
- output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm),
- unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false);
+ output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm));
if (output) {
if (unsafe_goal) {
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index e2cad73..fc1d935 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -140,7 +140,11 @@
double voltage_winch;
+ double open_threshold;
+
bool disable_box_correct;
+
+ bool trajectory_override;
};
message Status {
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 61e3d9f..173e6ef 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -66,6 +66,9 @@
const ButtonLocation kArmAboveHang(3, 15);
const ButtonLocation kArmBelowHang(3, 16);
+const ButtonLocation kEmergencyUp(3, 5);
+const ButtonLocation kEmergencyDown(3, 6);
+
const ButtonLocation kWinch(4, 2);
const ButtonLocation kArmNeutral(3, 8);
@@ -297,6 +300,16 @@
arm_goal_position_ = arm::BelowHangIndex();
}
+ if (data.IsPressed(kEmergencyDown)) {
+ arm_goal_position_ = arm::NeutralIndex();
+ new_superstructure_goal->trajectory_override = true;
+ } else if (data.IsPressed(kEmergencyUp)) {
+ arm_goal_position_ = arm::UpIndex();
+ new_superstructure_goal->trajectory_override = true;
+ } else {
+ new_superstructure_goal->trajectory_override = false;
+ }
+
new_superstructure_goal->deploy_fork =
data.IsPressed(kArmAboveHang) && data.IsPressed(kClawOpen);
@@ -314,9 +327,16 @@
new_superstructure_goal->hook_release = data.IsPressed(kArmBelowHang);
new_superstructure_goal->arm_goal_position = arm_goal_position_;
+ if (data.IsPressed(kArmFrontSwitch) || data.IsPressed(kArmBackSwitch)) {
+ new_superstructure_goal->open_threshold = 0.35;
+ } else {
+ new_superstructure_goal->open_threshold = 0.0;
+ }
if ((data.IsPressed(kClawOpen) && data.IsPressed(kDriverClawOpen)) ||
- data.PosEdge(kArmPickupBoxFromIntake)) {
+ data.PosEdge(kArmPickupBoxFromIntake) ||
+ (data.IsPressed(kClawOpen) &&
+ (data.IsPressed(kArmFrontSwitch) || data.IsPressed(kArmBackSwitch)))) {
new_superstructure_goal->open_claw = true;
} else {
new_superstructure_goal->open_claw = false;
diff --git a/y2018/vision/deploy.sh b/y2018/vision/deploy.sh
index b2fa3d2..ba9d40d 100755
--- a/y2018/vision/deploy.sh
+++ b/y2018/vision/deploy.sh
@@ -1,5 +1,7 @@
#!/bin/bash
+set -e
+
JETSON="root@$1"
# To build for the Jetson, use