Removed y2017_bot3.
y2017_bot3 no longer exists in the real world.
Change-Id: I31f2f66060c43b7afc6b1736e5089b7740c9cbaf
diff --git a/y2017_bot3/BUILD b/y2017_bot3/BUILD
deleted file mode 100644
index b1c68ce..0000000
--- a/y2017_bot3/BUILD
+++ /dev/null
@@ -1,89 +0,0 @@
-load("//aos/downloader:downloader.bzl", "aos_downloader")
-
-cc_binary(
- name = "joystick_reader",
- srcs = [
- "joystick_reader.cc",
- ],
- deps = [
- "//aos:init",
- "//aos/actions:action_lib",
- "//aos/input:drivetrain_input",
- "//aos/input:joystick_input",
- "//aos/logging",
- "//aos/time",
- "//aos/util:log_interval",
- "//frc971/autonomous:auto_queue",
- "//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//y2017_bot3/control_loops/drivetrain:drivetrain_base",
- "//y2017_bot3/control_loops/superstructure:superstructure_lib",
- "//y2017_bot3/control_loops/superstructure:superstructure_queue",
- ],
-)
-
-cc_binary(
- name = "wpilib_interface",
- srcs = [
- "wpilib_interface.cc",
- ],
- restricted_to = ["//tools:roborio"],
- deps = [
- "//aos:init",
- "//aos:make_unique",
- "//aos/controls:control_loop",
- "//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/time",
- "//aos/util:log_interval",
- "//aos/util:phased_loop",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/wpilib:buffered_pcm",
- "//frc971/wpilib:gyro_sender",
- "//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
- "//frc971/wpilib:loop_output_handler",
- "//frc971/wpilib:pdp_fetcher",
- "//frc971/wpilib:sensor_reader",
- "//frc971/wpilib:wpilib_robot_base",
- "//third_party:wpilib",
- "//y2017_bot3/control_loops/drivetrain:polydrivetrain_plants",
- "//y2017_bot3/control_loops/superstructure:superstructure_queue",
- ],
-)
-
-aos_downloader(
- name = "download",
- srcs = [
- "//aos:prime_binaries",
- ],
- restricted_to = ["//tools:roborio"],
- start_srcs = [
- ":wpilib_interface",
- ":joystick_reader",
- "//aos:prime_start_binaries",
- "//y2017_bot3/control_loops/drivetrain:drivetrain",
- "//y2017_bot3/control_loops/superstructure:superstructure",
- ],
-)
-
-aos_downloader(
- name = "download_stripped",
- srcs = [
- "//aos:prime_binaries_stripped",
- ],
- restricted_to = ["//tools:roborio"],
- start_srcs = [
- ":wpilib_interface.stripped",
- ":joystick_reader.stripped",
- "//aos:prime_start_binaries_stripped",
- "//y2017_bot3/control_loops/drivetrain:drivetrain.stripped",
- "//y2017_bot3/control_loops/superstructure:superstructure.stripped",
- ],
-)
-
-py_library(
- name = "python_init",
- srcs = ["__init__.py"],
- visibility = ["//visibility:public"],
-)
diff --git a/y2017_bot3/__init__.py b/y2017_bot3/__init__.py
deleted file mode 100644
index e69de29..0000000
--- a/y2017_bot3/__init__.py
+++ /dev/null
diff --git a/y2017_bot3/control_loops/BUILD b/y2017_bot3/control_loops/BUILD
deleted file mode 100644
index a98593b..0000000
--- a/y2017_bot3/control_loops/BUILD
+++ /dev/null
@@ -1,6 +0,0 @@
-py_library(
- name = "python_init",
- srcs = ["__init__.py"],
- visibility = ["//visibility:public"],
- deps = ["//y2017_bot3:python_init"],
-)
diff --git a/y2017_bot3/control_loops/__init__.py b/y2017_bot3/control_loops/__init__.py
deleted file mode 100644
index e69de29..0000000
--- a/y2017_bot3/control_loops/__init__.py
+++ /dev/null
diff --git a/y2017_bot3/control_loops/drivetrain/BUILD b/y2017_bot3/control_loops/drivetrain/BUILD
deleted file mode 100644
index 08aaab2..0000000
--- a/y2017_bot3/control_loops/drivetrain/BUILD
+++ /dev/null
@@ -1,83 +0,0 @@
-package(default_visibility = ["//visibility:public"])
-
-load("//aos/build:queues.bzl", "queue_library")
-
-genrule(
- name = "genrule_drivetrain",
- outs = [
- "drivetrain_dog_motor_plant.h",
- "drivetrain_dog_motor_plant.cc",
- "kalman_drivetrain_motor_plant.h",
- "kalman_drivetrain_motor_plant.cc",
- ],
- cmd = "$(location //y2017_bot3/control_loops/python:drivetrain) $(OUTS)",
- tools = [
- "//y2017_bot3/control_loops/python:drivetrain",
- ],
- visibility = ["//visibility:private"],
-)
-
-genrule(
- name = "genrule_polydrivetrain",
- outs = [
- "polydrivetrain_dog_motor_plant.h",
- "polydrivetrain_dog_motor_plant.cc",
- "polydrivetrain_cim_plant.h",
- "polydrivetrain_cim_plant.cc",
- "hybrid_velocity_drivetrain.h",
- "hybrid_velocity_drivetrain.cc",
- ],
- cmd = "$(location //y2017_bot3/control_loops/python:polydrivetrain) $(OUTS)",
- tools = [
- "//y2017_bot3/control_loops/python:polydrivetrain",
- ],
- visibility = ["//visibility:private"],
-)
-
-cc_library(
- name = "polydrivetrain_plants",
- srcs = [
- "drivetrain_dog_motor_plant.cc",
- "hybrid_velocity_drivetrain.cc",
- "kalman_drivetrain_motor_plant.cc",
- "polydrivetrain_dog_motor_plant.cc",
- ],
- hdrs = [
- "drivetrain_dog_motor_plant.h",
- "hybrid_velocity_drivetrain.h",
- "kalman_drivetrain_motor_plant.h",
- "polydrivetrain_dog_motor_plant.h",
- ],
- deps = [
- "//frc971/control_loops:hybrid_state_feedback_loop",
- "//frc971/control_loops:state_feedback_loop",
- ],
-)
-
-cc_library(
- name = "drivetrain_base",
- srcs = [
- "drivetrain_base.cc",
- ],
- hdrs = [
- "drivetrain_base.h",
- ],
- deps = [
- ":polydrivetrain_plants",
- "//frc971:shifter_hall_effect",
- "//frc971/control_loops/drivetrain:drivetrain_config",
- ],
-)
-
-cc_binary(
- name = "drivetrain",
- srcs = [
- "drivetrain_main.cc",
- ],
- deps = [
- ":drivetrain_base",
- "//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/control_loops/drivetrain:drivetrain_lib",
- ],
-)
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
deleted file mode 100644
index da4aafb..0000000
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ /dev/null
@@ -1,56 +0,0 @@
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
-
-#include <chrono>
-
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017_bot3/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
-#include "y2017_bot3/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
-#include "y2017_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-
-namespace chrono = ::std::chrono;
-
-namespace y2017_bot3 {
-namespace control_loops {
-namespace drivetrain {
-
-using ::frc971::constants::ShifterHallEffect;
-
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
-
-const DrivetrainConfig<double> &GetDrivetrainConfig() {
- static DrivetrainConfig<double> kDrivetrainConfig{
- ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
- ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
- ::frc971::control_loops::drivetrain::IMUType::IMU_X,
-
- drivetrain::MakeDrivetrainLoop,
- drivetrain::MakeVelocityDrivetrainLoop,
- drivetrain::MakeKFDrivetrainLoop,
- drivetrain::MakeHybridVelocityDrivetrainLoop,
-
- chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(drivetrain::kDt)),
- drivetrain::kRobotRadius, drivetrain::kWheelRadius, drivetrain::kV,
-
- drivetrain::kHighGearRatio, drivetrain::kHighGearRatio,
- drivetrain::kJ,
- drivetrain::kMass,
- kThreeStateDriveShifter, kThreeStateDriveShifter,
- // TODO(Neil): Find out whigh position is default in pneumatics for the
- // gearing
- true /* default_high_gear */, 0 /* down_offset */,
- 0.4 /* wheel_non_linearity */, 1.0 /* quickturn_wheel_multiplier */,
- 1.0 /* wheel_multiplier */,
- };
-
- return kDrivetrainConfig;
-};
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2017_bot3
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_base.h b/y2017_bot3/control_loops/drivetrain/drivetrain_base.h
deleted file mode 100644
index 4fff031..0000000
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_base.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#ifndef Y2017_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
-#define Y2017_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
-
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-
-namespace y2017_bot3 {
-namespace control_loops {
-namespace drivetrain {
-
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2017_bot3
-
-#endif // Y2017_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc
deleted file mode 100644
index ac581eb..0000000
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ /dev/null
@@ -1,18 +0,0 @@
-#include "aos/init.h"
-
-#include "aos/events/shm-event-loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
-
-using ::frc971::control_loops::drivetrain::DrivetrainLoop;
-
-int main() {
- ::aos::Init();
- ::aos::ShmEventLoop event_loop;
- DrivetrainLoop drivetrain(
- ::y2017_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
- &event_loop);
- drivetrain.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/y2017_bot3/control_loops/python/BUILD b/y2017_bot3/control_loops/python/BUILD
deleted file mode 100644
index ed5cc69..0000000
--- a/y2017_bot3/control_loops/python/BUILD
+++ /dev/null
@@ -1,53 +0,0 @@
-package(default_visibility = ["//y2017_bot3:__subpackages__"])
-
-py_binary(
- name = "drivetrain",
- srcs = [
- "drivetrain.py",
- ],
- legacy_create_init = False,
- restricted_to = ["//tools:k8"],
- deps = [
- ":python_init",
- "//external:python-gflags",
- "//external:python-glog",
- "//frc971/control_loops/python:drivetrain",
- ],
-)
-
-py_binary(
- name = "polydrivetrain",
- srcs = [
- "drivetrain.py",
- "polydrivetrain.py",
- ],
- legacy_create_init = False,
- restricted_to = ["//tools:k8"],
- deps = [
- ":python_init",
- "//external:python-gflags",
- "//external:python-glog",
- "//frc971/control_loops/python:polydrivetrain",
- ],
-)
-
-py_library(
- name = "polydrivetrain_lib",
- srcs = [
- "drivetrain.py",
- "polydrivetrain.py",
- ],
- restricted_to = ["//tools:k8"],
- deps = [
- "//external:python-gflags",
- "//external:python-glog",
- "//frc971/control_loops/python:controls",
- ],
-)
-
-py_library(
- name = "python_init",
- srcs = ["__init__.py"],
- visibility = ["//visibility:public"],
- deps = ["//y2017_bot3/control_loops:python_init"],
-)
diff --git a/y2017_bot3/control_loops/python/__init__.py b/y2017_bot3/control_loops/python/__init__.py
deleted file mode 100644
index e69de29..0000000
--- a/y2017_bot3/control_loops/python/__init__.py
+++ /dev/null
diff --git a/y2017_bot3/control_loops/python/drivetrain.py b/y2017_bot3/control_loops/python/drivetrain.py
deleted file mode 100755
index 0272232..0000000
--- a/y2017_bot3/control_loops/python/drivetrain.py
+++ /dev/null
@@ -1,39 +0,0 @@
-#!/usr/bin/python
-
-from frc971.control_loops.python import drivetrain
-import sys
-
-import gflags
-import glog
-
-FLAGS = gflags.FLAGS
-
-gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-
-#TODO(Neil): Update robot moment of inertia, mass, and robot radius
-kDrivetrain = drivetrain.DrivetrainParams(J = 6.0,
- mass = 52,
- robot_radius = 0.59055 / 2.0,
- wheel_radius = 4 * 0.0254 / 2,
- G_high = 14.0 / 40.0 * 34.0 / 50.0 * 52.0 / 60.0,
- G_low = 14.0 / 40.0 * 24.0 / 60.0 * 52.0 / 60.0,
- q_pos_low = 0.12,
- q_pos_high = 0.14,
- q_vel_low = 1.0,
- q_vel_high = 0.95,
- has_imu = False)
-
-def main(argv):
- argv = FLAGS(argv)
- glog.init()
-
- if FLAGS.plot:
- drivetrain.PlotDrivetrainMotions(kDrivetrain)
- elif len(argv) != 5:
- print "Expected .h file name and .cc file name"
- else:
- # Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2017_bot3', kDrivetrain)
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/y2017_bot3/control_loops/python/polydrivetrain.py b/y2017_bot3/control_loops/python/polydrivetrain.py
deleted file mode 100755
index 955f78f..0000000
--- a/y2017_bot3/control_loops/python/polydrivetrain.py
+++ /dev/null
@@ -1,31 +0,0 @@
-#!/usr/bin/python
-
-import sys
-from y2017_bot3.control_loops.python import drivetrain
-from frc971.control_loops.python import polydrivetrain
-
-import gflags
-import glog
-
-__author__ = 'Austin Schuh (austin.linux@gmail.com)'
-
-FLAGS = gflags.FLAGS
-
-try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-except gflags.DuplicateFlagError:
- pass
-
-def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
- 'y2017_bot3', drivetrain.kDrivetrain)
-
-if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
diff --git a/y2017_bot3/control_loops/superstructure/BUILD b/y2017_bot3/control_loops/superstructure/BUILD
deleted file mode 100644
index 9c9f750..0000000
--- a/y2017_bot3/control_loops/superstructure/BUILD
+++ /dev/null
@@ -1,48 +0,0 @@
-package(default_visibility = ["//visibility:public"])
-
-load("//aos/build:queues.bzl", "queue_library")
-
-queue_library(
- name = "superstructure_queue",
- srcs = [
- "superstructure.q",
- ],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:profiled_subsystem_queue",
- "//frc971/control_loops:queues",
- ],
-)
-
-cc_library(
- name = "superstructure_lib",
- srcs = [
- "superstructure.cc",
- ],
- hdrs = [
- "superstructure.h",
- ],
- deps = [
- ":superstructure_queue",
- "//aos:math",
- "//aos/controls:control_loop",
- "//aos/util:trapezoid_profile",
- "//frc971/control_loops:profiled_subsystem",
- "//frc971/control_loops:simple_capped_state_feedback_loop",
- "//frc971/control_loops:state_feedback_loop",
- "//frc971/zeroing",
- ],
-)
-
-cc_binary(
- name = "superstructure",
- srcs = [
- "superstructure_main.cc",
- ],
- deps = [
- ":superstructure_lib",
- ":superstructure_queue",
- "//aos:init",
- "//aos/events:shm-event-loop",
- ],
-)
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.cc b/y2017_bot3/control_loops/superstructure/superstructure.cc
deleted file mode 100644
index e75ffed..0000000
--- a/y2017_bot3/control_loops/superstructure/superstructure.cc
+++ /dev/null
@@ -1,39 +0,0 @@
-#include "y2017_bot3/control_loops/superstructure/superstructure.h"
-
-#include "aos/commonmath.h"
-#include "aos/controls/control_loops.q.h"
-#include "aos/logging/logging.h"
-
-namespace y2017_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-Superstructure::Superstructure(::aos::EventLoop *event_loop,
- const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name) {}
-
-void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position * /*position*/,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status * /*status*/) {
- // Write out all the voltages.
- if (output) {
- output->voltage_rollers = 0.0;
- output->hanger_voltage = 0.0;
- output->fingers_out = false;
- if (unsafe_goal) {
- // Intake.
- output->voltage_rollers = unsafe_goal->voltage_rollers;
- // Fire piston to release gear.
- output->fingers_out = unsafe_goal->fingers_out;
- // Spin Hanger.
- output->hanger_voltage = unsafe_goal->hanger_voltage;
- }
- }
-}
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2017_bot3
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.h b/y2017_bot3/control_loops/superstructure/superstructure.h
deleted file mode 100644
index a9378eb..0000000
--- a/y2017_bot3/control_loops/superstructure/superstructure.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
-#define Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
-
-#include <memory>
-
-#include "aos/controls/control_loop.h"
-#include "aos/util/trapezoid_profile.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/zeroing/zeroing.h"
-#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
-
-namespace y2017_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
- public:
- explicit Superstructure(::aos::EventLoop *event_loop,
- const ::std::string &name =
- ".y2017_bot3.control_loops.superstructure_queue");
-
- static constexpr double kOperatingVoltage = 12.0;
-
- protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position * /*position*/,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status * /*status*/) override;
-
- private:
- DISALLOW_COPY_AND_ASSIGN(Superstructure);
-};
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2017_bot3
-
-#endif // Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.q b/y2017_bot3/control_loops/superstructure/superstructure.q
deleted file mode 100644
index 10a0fca..0000000
--- a/y2017_bot3/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,39 +0,0 @@
-package y2017_bot3.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Voltage from -12 to 12 to send to the rollers. Positive is front surface
- // of the roller is moving down when gear is moving inwards.
- float voltage_rollers;
- // State of finger pistons. True is out, false is in.
- bool fingers_out;
- // Voltage from -12 to 12 sent to the hanger roller. Positive is front
- // surface of the hanger is moving down.
- float hanger_voltage;
- };
-
- message Status {
- };
-
- message Position {
- };
-
- message Output {
- // see above
- float voltage_rollers;
- bool fingers_out;
- float hanger_voltage;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
-
-queue_group SuperstructureQueue superstructure_queue;
diff --git a/y2017_bot3/control_loops/superstructure/superstructure_main.cc b/y2017_bot3/control_loops/superstructure/superstructure_main.cc
deleted file mode 100644
index 38df40e..0000000
--- a/y2017_bot3/control_loops/superstructure/superstructure_main.cc
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "y2017_bot3/control_loops/superstructure/superstructure.h"
-
-#include "aos/init.h"
-
-int main() {
- ::aos::Init();
- ::aos::ShmEventLoop event_loop;
- ::y2017_bot3::control_loops::superstructure::Superstructure superstructure(
- &event_loop);
- superstructure.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/y2017_bot3/joystick_reader.cc b/y2017_bot3/joystick_reader.cc
deleted file mode 100644
index c3cc9cb..0000000
--- a/y2017_bot3/joystick_reader.cc
+++ /dev/null
@@ -1,157 +0,0 @@
-#include <math.h>
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "aos/actions/actions.h"
-#include "aos/input/driver_station_data.h"
-#include "aos/logging/logging.h"
-#include "aos/logging/logging.h"
-#include "aos/util/log_interval.h"
-#include "aos/input/drivetrain_input.h"
-#include "aos/input/joystick_input.h"
-#include "aos/init.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
-
-using ::frc971::control_loops::drivetrain_queue;
-using ::y2017_bot3::control_loops::superstructure_queue;
-
-using ::aos::input::driver_station::ButtonLocation;
-using ::aos::input::driver_station::ControlBit;
-using ::aos::input::driver_station::JoystickAxis;
-using ::aos::input::driver_station::POVLocation;
-using ::aos::input::DrivetrainInputReader;
-
-namespace y2017_bot3 {
-namespace input {
-namespace joysticks {
-
-const ButtonLocation kHangerOn(2, 11);
-const ButtonLocation kGearOut(2, 10);
-const ButtonLocation kRollerOn(2, 7);
-const ButtonLocation kRollerSpit(2, 6);
-
-std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
-
-class Reader : public ::aos::input::JoystickInput {
- public:
- Reader(::aos::EventLoop *event_loop)
- : ::aos::input::JoystickInput(event_loop) {
- // Setting driver station type to Steering Wheel
- drivetrain_input_reader_ = DrivetrainInputReader::Make(
- DrivetrainInputReader::InputType::kSteeringWheel,
- ::y2017_bot3::control_loops::drivetrain::GetDrivetrainConfig());
- }
-
- void RunIteration(const ::aos::input::driver_station::Data &data) override {
- if (!data.GetControlBit(ControlBit::kEnabled)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- }
-
- const bool last_auto_running = auto_running_;
- auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
- data.GetControlBit(ControlBit::kEnabled);
- if (auto_running_ != last_auto_running) {
- if (auto_running_) {
- StartAuto();
- } else {
- StopAuto();
- }
- }
-
- if (!auto_running_) {
- HandleDrivetrain(data);
- HandleTeleop(data);
- }
-
- // Process pending actions.
- action_queue_.Tick();
- was_running_ = action_queue_.Running();
-
- if (!data.GetControlBit(ControlBit::kEnabled)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- }
- }
- void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- drivetrain_input_reader_->HandleDrivetrain(data);
- robot_velocity_ = drivetrain_input_reader_->robot_velocity();
- }
-
- void HandleTeleop(const ::aos::input::driver_station::Data &data) {
- superstructure_queue.status.FetchLatest();
- if (!superstructure_queue.status.get()) {
- LOG(ERROR, "Got no superstructure status packet.\n");
- return;
- }
-
- auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
- new_superstructure_goal->voltage_rollers = 0.0;
-
- if (data.IsPressed(kRollerOn)) {
- new_superstructure_goal->voltage_rollers = 12.0;
- }
-
- if (data.IsPressed(kRollerSpit)) {
- new_superstructure_goal->voltage_rollers = -12.0;
- }
-
- if (data.IsPressed(kHangerOn)) {
- new_superstructure_goal->hanger_voltage = 12.0;
- }
-
-
- if (data.IsPressed(kGearOut)) {
- new_superstructure_goal->fingers_out = true;
- } else {
- new_superstructure_goal->fingers_out = false;
- }
-
- LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
- if (!new_superstructure_goal.Send()) {
- LOG(ERROR, "Sending superstructure goal failed.\n");
- }
- }
-
- private:
- void StartAuto() {
- LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::AutonomousActionParams params;
- ::frc971::autonomous::auto_mode.FetchLatest();
- if (::frc971::autonomous::auto_mode.get() != nullptr) {
- params.mode = ::frc971::autonomous::auto_mode->mode;
- } else {
- LOG(WARNING, "no auto mode values\n");
- params.mode = 0;
- }
- action_queue_.EnqueueAction(
- ::frc971::autonomous::MakeAutonomousAction(params));
- }
-
- void StopAuto() {
- LOG(INFO, "Stopping auto mode\n");
- action_queue_.CancelAllActions();
- }
- double robot_velocity_ = 0.0;
- ::aos::common::actions::ActionQueue action_queue_;
-
- bool was_running_ = false;
- bool auto_running_ = false;
-};
-
-} // namespace joysticks
-} // namespace input
-} // namespace y2017_bot3
-
-int main() {
- ::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
- ::y2017_bot3::input::joysticks::Reader reader(&event_loop);
- reader.Run();
- ::aos::Cleanup();
-}
diff --git a/y2017_bot3/wpilib_interface.cc b/y2017_bot3/wpilib_interface.cc
deleted file mode 100644
index e191e1f..0000000
--- a/y2017_bot3/wpilib_interface.cc
+++ /dev/null
@@ -1,417 +0,0 @@
-#include <inttypes.h>
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-
-#include <array>
-#include <chrono>
-#include <cmath>
-#include <functional>
-#include <thread>
-
-#include "frc971/wpilib/ahal/AnalogInput.h"
-#include "frc971/wpilib/ahal/Compressor.h"
-#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
-#include "frc971/wpilib/ahal/DriverStation.h"
-#include "frc971/wpilib/ahal/Encoder.h"
-#include "frc971/wpilib/ahal/VictorSP.h"
-#undef ERROR
-
-#include "aos/init.h"
-#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/make_unique.h"
-#include "aos/time/time.h"
-#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/wpilib/buffered_pcm.h"
-#include "frc971/wpilib/buffered_solenoid.h"
-#include "frc971/wpilib/dma.h"
-#include "frc971/wpilib/gyro_sender.h"
-#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
-#include "frc971/wpilib/loop_output_handler.h"
-#include "frc971/wpilib/pdp_fetcher.h"
-#include "frc971/wpilib/sensor_reader.h"
-#include "frc971/wpilib/wpilib_robot_base.h"
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-using ::frc971::control_loops::drivetrain_queue;
-using ::y2017_bot3::control_loops::superstructure_queue;
-using ::aos::monotonic_clock;
-namespace chrono = ::std::chrono;
-using namespace frc;
-using aos::make_unique;
-
-namespace y2017_bot3 {
-namespace wpilib {
-namespace {
-
-constexpr double kMaxBringupPower = 12.0;
-
-constexpr double kDrivetrainCyclesPerRevolution = 128.0;
-constexpr double kDrivetrainEncoderCountsPerRevolution =
- kDrivetrainCyclesPerRevolution * 4;
-constexpr double kDrivetrainEncoderRatio = 1.0;
-constexpr double kMaxDrivetrainEncoderPulsesPerSecond =
- control_loops::drivetrain::kFreeSpeed *
- control_loops::drivetrain::kHighOutputRatio /
- kDrivetrainEncoderRatio *
- kDrivetrainEncoderCountsPerRevolution;
-
-// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
-// 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 hall_translate(double in) {
- // Turn voltage from our 3-state halls into a ratio that the loop can use.
- return in / 5.0;
-}
-
-double drivetrain_translate(int32_t in) {
- return -static_cast<double>(in) / (kDrivetrainCyclesPerRevolution /*cpr*/ * 4.0 /*4x*/) *
- kDrivetrainEncoderRatio *
- control_loops::drivetrain::kWheelRadius *
- 2.0 * M_PI;
-}
-
-double drivetrain_velocity_translate(double in) {
- return (1.0 / in) / 256.0 /*cpr*/ *
- kDrivetrainEncoderRatio *
- control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
-}
-
-constexpr double kMaxFastEncoderPulsesPerSecond =
- kMaxDrivetrainEncoderPulsesPerSecond;
-static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
- "fast encoders are too fast");
-
-// Class to send position messages with sensor readings to our loops.
-class SensorReader : public ::frc971::wpilib::SensorReader {
- public:
- SensorReader() {
- // Set to filter out anything shorter than 1/4 of the minimum pulse width
- // we should ever see.
- UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
- }
-
- void set_drivetrain_left_hall(::std::unique_ptr<AnalogInput> analog) {
- drivetrain_left_hall_ = ::std::move(analog);
- }
-
- void set_drivetrain_right_hall(::std::unique_ptr<AnalogInput> analog) {
- drivetrain_right_hall_ = ::std::move(analog);
- }
-
- void RunIteration() {
- {
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
- drivetrain_message->right_encoder =
- -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->right_speed =
- drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
-
- drivetrain_message->left_encoder =
- drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
-
- drivetrain_message->left_shifter_position =
- hall_translate(drivetrain_left_hall_->GetVoltage());
- drivetrain_message->right_shifter_position =
- hall_translate(drivetrain_right_hall_->GetVoltage());
-
- drivetrain_message.Send();
- }
-
- {
- auto superstructure_message = superstructure_queue.position.MakeMessage();
- superstructure_message.Send();
- }
- }
-
- private:
- ::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
-};
-
-class SolenoidWriter {
- public:
- SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
- : pcm_(pcm),
- drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
- superstructure_(
- ".y2017_bot3.control_loops.superstructure_queue.output") {}
-
- void set_compressor(::std::unique_ptr<Compressor> compressor) {
- compressor_ = ::std::move(compressor);
- }
-
- void set_left_drivetrain_shifter(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- left_drivetrain_shifter_ = ::std::move(s);
- }
-
- void set_right_drivetrain_shifter(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- right_drivetrain_shifter_ = ::std::move(s);
- }
-
- void set_fingers(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- fingers_ = ::std::move(s);
- }
-
- void operator()() {
- compressor_->Start();
- ::aos::SetCurrentThreadName("Solenoids");
- ::aos::SetCurrentThreadRealtimePriority(27);
-
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(20),
- ::std::chrono::milliseconds(1));
-
- while (run_) {
- {
- int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
- }
- }
-
- {
- drivetrain_.FetchLatest();
- if (drivetrain_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- left_drivetrain_shifter_->Set(!drivetrain_->left_high);
- right_drivetrain_shifter_->Set(!drivetrain_->right_high);
- }
- }
-
- {
- superstructure_.FetchLatest();
- if (superstructure_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
- fingers_->Set(superstructure_->fingers_out);
- }
- }
-
- {
- ::frc971::wpilib::PneumaticsToLog to_log;
- { to_log.compressor_on = compressor_->Enabled(); }
-
- pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- LOG_STRUCT(DEBUG, "pneumatics info", to_log);
- }
- }
- }
-
- void Quit() { run_ = false; }
-
- private:
- const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid>
- left_drivetrain_shifter_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid>
- right_drivetrain_shifter_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> fingers_;
-
- ::std::unique_ptr<Compressor> compressor_;
-
- ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
- ::aos::Queue<::y2017_bot3::control_loops::SuperstructureQueue::Output>
- superstructure_;
-
- ::std::atomic<bool> run_{true};
-};
-
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_drivetrain_left0_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_left0_victor_ = ::std::move(t);
- }
- void set_drivetrain_left1_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_left1_victor_ = ::std::move(t);
- }
-
- void set_drivetrain_right0_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_right0_victor_ = ::std::move(t);
- }
- void set_drivetrain_right1_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_right1_victor_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left0_victor_->SetSpeed(queue->left_voltage / 12.0);
- drivetrain_left1_victor_->SetSpeed(queue->left_voltage / 12.0);
- drivetrain_right0_victor_->SetSpeed(-queue->right_voltage / 12.0);
- drivetrain_right1_victor_->SetSpeed(-queue->right_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left0_victor_->SetDisabled();
- drivetrain_left1_victor_->SetDisabled();
- drivetrain_right0_victor_->SetDisabled();
- drivetrain_right1_victor_->SetDisabled();
- }
-
- ::std::unique_ptr<::frc::VictorSP> drivetrain_left0_victor_,
- drivetrain_left1_victor_, drivetrain_right0_victor_,
- drivetrain_right1_victor_;
-};
-
-class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_rollers_victor(::std::unique_ptr<::frc::VictorSP> t) {
- rollers_victor_ = ::std::move(t);
- }
-
- void set_hanger0_victor(::std::unique_ptr<::frc::VictorSP> t) {
- hanger0_victor_ = ::std::move(t);
- }
- void set_hanger1_victor(::std::unique_ptr<::frc::VictorSP> t) {
- hanger1_victor_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::y2017_bot3::control_loops::superstructure_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2017_bot3::control_loops::superstructure_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- rollers_victor_->SetSpeed(queue->voltage_rollers / 12.0);
- hanger0_victor_->SetSpeed(queue->hanger_voltage / 12.0);
- hanger1_victor_->SetSpeed(queue->hanger_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "Superstructure output too old.\n");
- rollers_victor_->SetDisabled();
- hanger0_victor_->SetDisabled();
- hanger1_victor_->SetDisabled();
- }
-
- ::std::unique_ptr<::frc::VictorSP> rollers_victor_;
- ::std::unique_ptr<::frc::VictorSP> hanger0_victor_, hanger1_victor_;
-};
-
-class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
- public:
- ::std::unique_ptr<Encoder> make_encoder(int index) {
- return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
- Encoder::k4X);
- }
-
- void Run() override {
- ::aos::InitNRT();
- ::aos::SetCurrentThreadName("StartCompetition");
-
- ::frc971::wpilib::JoystickSender joystick_sender;
- ::std::thread joystick_thread(::std::ref(joystick_sender));
-
- ::frc971::wpilib::PDPFetcher pdp_fetcher;
- ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader;
-
- reader.set_drivetrain_left_encoder(make_encoder(0));
- reader.set_drivetrain_right_encoder(make_encoder(1));
- reader.set_drivetrain_left_hall(make_unique<AnalogInput>(0));
- reader.set_drivetrain_right_hall(make_unique<AnalogInput>(1));
-
- reader.set_pwm_trigger(make_unique<DigitalInput>(0));
- reader.set_dma(make_unique<DMA>());
- ::std::thread reader_thread(::std::ref(reader));
-
- ::frc971::wpilib::GyroSender gyro_sender;
- ::std::thread gyro_thread(::std::ref(gyro_sender));
-
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_drivetrain_left0_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(5)));
- drivetrain_writer.set_drivetrain_left1_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
- drivetrain_writer.set_drivetrain_right0_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
- drivetrain_writer.set_drivetrain_right1_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
- ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
-
- SuperstructureWriter superstructure_writer;
- superstructure_writer.set_rollers_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
- superstructure_writer.set_hanger0_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)));
- superstructure_writer.set_hanger1_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)));
- ::std::thread superstructure_writer_thread(::std::ref(superstructure_writer));
-
- ::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
- new ::frc971::wpilib::BufferedPcm());
- SolenoidWriter solenoid_writer(pcm);
- solenoid_writer.set_left_drivetrain_shifter(pcm->MakeSolenoid(3));
- solenoid_writer.set_right_drivetrain_shifter(pcm->MakeSolenoid(1));
- solenoid_writer.set_fingers(pcm->MakeSolenoid(2));
-
- solenoid_writer.set_compressor(make_unique<Compressor>());
-
- ::std::thread solenoid_thread(::std::ref(solenoid_writer));
-
- // Wait forever. Not much else to do...
- while (true) {
- const int r = select(0, nullptr, nullptr, nullptr, nullptr);
- if (r != 0) {
- PLOG(WARNING, "infinite select failed");
- } else {
- PLOG(WARNING, "infinite select succeeded??\n");
- }
- }
-
- LOG(ERROR, "Exiting WPILibRobot\n");
-
- joystick_sender.Quit();
- joystick_thread.join();
- pdp_fetcher.Quit();
- pdp_fetcher_thread.join();
- reader.Quit();
- reader_thread.join();
- gyro_sender.Quit();
- gyro_thread.join();
-
- drivetrain_writer.Quit();
- drivetrain_writer_thread.join();
- solenoid_writer.Quit();
- solenoid_thread.join();
-
- ::aos::Cleanup();
- }
-};
-
-} // namespace
-} // namespace wpilib
-} // namespace y2017_bot3
-
-AOS_ROBOT_CLASS(::y2017_bot3::wpilib::WPILibRobot);