Merge "Add support for robots without DMA."
diff --git a/README.md b/README.md
index eef71a6..e572ea9 100644
--- a/README.md
+++ b/README.md
@@ -98,7 +98,7 @@
```console
# Freshly imaged roboRIOs need to be configured to run the 971 code
# at startup. This is done by using the setup_roborio.sh script.
-setup_roborio.sh roboRIO-971-frc.local
+bazel run -c opt //aos/config:setup_roborio -- roboRIO-XXX-frc.local
```
### Some other useful packages
diff --git a/aos/input/BUILD b/aos/input/BUILD
index dd79c5e..0da8d01 100644
--- a/aos/input/BUILD
+++ b/aos/input/BUILD
@@ -48,3 +48,18 @@
"//aos/robot_state",
],
)
+
+cc_library(
+ name = "action_joystick_input",
+ srcs = ["action_joystick_input.cc"],
+ hdrs = ["action_joystick_input.h"],
+ deps = [
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/input:drivetrain_input",
+ "//aos/input:joystick_input",
+ "//aos/logging",
+ "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:base_autonomous_actor",
+ ],
+)
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
new file mode 100644
index 0000000..991ea17
--- /dev/null
+++ b/aos/input/action_joystick_input.cc
@@ -0,0 +1,50 @@
+#include "aos/input/action_joystick_input.h"
+
+#include "aos/input/driver_station_data.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+
+using ::aos::input::driver_station::ControlBit;
+
+namespace aos {
+namespace input {
+
+void ActionJoystickInput::RunIteration(
+ const ::aos::input::driver_station::Data &data) {
+ 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_) {
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+ drivetrain_input_reader_->HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+
+ // Process pending actions.
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+}
+
+void ActionJoystickInput::StartAuto() {
+ LOG(INFO, "Starting auto mode\n");
+ action_queue_.EnqueueAction(::frc971::autonomous::MakeAutonomousAction(0));
+}
+
+void ActionJoystickInput::StopAuto() {
+ LOG(INFO, "Stopping auto mode\n");
+ action_queue_.CancelAllActions();
+}
+
+} // namespace input
+} // namespace aos
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
new file mode 100644
index 0000000..d4023ca
--- /dev/null
+++ b/aos/input/action_joystick_input.h
@@ -0,0 +1,48 @@
+#ifndef AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
+#define AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
+
+#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
+#include "aos/input/joystick_input.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+
+namespace aos {
+namespace input {
+
+// Class to abstract out managing actions, autonomous mode, and drivetrains.
+// Turns out we do the same thing every year, so let's stop copying it.
+class ActionJoystickInput : public ::aos::input::JoystickInput {
+ public:
+ ActionJoystickInput(
+ ::aos::EventLoop *event_loop,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config)
+ : ::aos::input::JoystickInput(event_loop),
+ drivetrain_input_reader_(DrivetrainInputReader::Make(
+ DrivetrainInputReader::InputType::kPistol, dt_config)) {}
+
+ virtual ~ActionJoystickInput() {}
+
+ private:
+ // Handles any year specific superstructure code.
+ virtual void HandleTeleop(const ::aos::input::driver_station::Data &data) = 0;
+
+ void RunIteration(const ::aos::input::driver_station::Data &data) override;
+
+ void StartAuto();
+ void StopAuto();
+
+ // True if the internal state machine thinks auto is running right now.
+ bool auto_running_ = false;
+ // True if an action was running last cycle.
+ bool was_running_ = false;
+
+ ::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
+ ::aos::common::actions::ActionQueue action_queue_;
+};
+
+} // namespace input
+} // namespace aos
+
+#endif // AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 8d375f8..a31390a 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -371,6 +371,10 @@
angular_system = AngularSystem(params, params.name)
loop_writer = control_loop.ControlLoopWriter(
angular_system.name, [angular_system], namespaces=year_namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', angular_system.G))
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', angular_system.motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
integral_angular_system = IntegralAngularSystem(params,
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 9edbb47..52bd8d1 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -144,7 +144,6 @@
self.selected_points = []
self.splines = []
- self.spline = []
self.reinit_extents()
self.inStart = None
@@ -204,8 +203,6 @@
self.startSet = True
else:
self.inEnd = [self.index_of_edit, self.findDistance()]
- self.spline[self.spline_edit].addConstraint(
- self.inStart, self.inEnd, self.inConstraint, self.inValue)
self.startSet = False
self.mode = Mode.kEditing
self.spline_edit = -1
@@ -237,7 +234,7 @@
set_color(cr, palette["GREY"])
cr.paint()
#Scale the field to fit within drawing area
- cr.scale(0.5,0.5)
+ cr.scale(0.5, 0.5)
# Draw a extents rectangle
set_color(cr, palette["WHITE"])
@@ -277,14 +274,6 @@
cr.fill()
y = 0
- for x, i in enumerate(self.spline):
- for j in i.constraints:
- cr.move_to(-650, -y * 10 + 320)
- set_color(cr, palette["BLACK"])
- display_text(
- cr, str("Spline " + str(x) + ": " + str(j.toString())),
- 0.5, 0.5, 2, 2)
- y += 1
# update all the things
@@ -489,9 +478,6 @@
self.splines[spline_edit][1] = f * 2 + e * -1
self.splines[spline_edit][2] = d + f * 4 + e * -4
- self.spline[spline_edit].point = self.splines[spline_edit]
- self.spline[spline_edit].math()
-
if not self.spline_edit == 0:
spline_edit = self.spline_edit - 1
a = self.splines[self.spline_edit][0]
@@ -501,11 +487,6 @@
self.splines[spline_edit][4] = a * 2 + b * -1
self.splines[spline_edit][3] = c + a * 4 + b * -4
- self.spline[spline_edit].point = self.splines[spline_edit]
- self.spline[spline_edit].math()
-
- self.spline[self.spline_edit].edit(self.index_of_edit,
- [self.x, self.y])
self.index_of_edit = -1
self.spline_edit = -1
else:
@@ -641,4 +622,4 @@
window = GridWindow()
-basic_window.RunApp()
+basic_window.RunApp()
\ No newline at end of file
diff --git a/frc971/wpilib/ahal/Spark.cc b/frc971/wpilib/ahal/Spark.cc
new file mode 100644
index 0000000..9127931
--- /dev/null
+++ b/frc971/wpilib/ahal/Spark.cc
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc971/wpilib/ahal/Spark.h"
+
+#include <HAL/HAL.h>
+
+using namespace frc;
+
+Spark::Spark(int channel) : PWM(channel) {
+ /* Note that the Spark uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the Spark User Manual available
+ * from REV Robotics.
+ *
+ * 2.003ms = full "forward"
+ * 1.55ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.46ms = the "low end" of the deadband range
+ * 0.999ms = full "reverse"
+ */
+ SetBounds(2.003, 1.55, 1.50, 1.46, .999);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
+}
diff --git a/frc971/wpilib/ahal/Spark.h b/frc971/wpilib/ahal/Spark.h
new file mode 100644
index 0000000..f33130b
--- /dev/null
+++ b/frc971/wpilib/ahal/Spark.h
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc971/wpilib/ahal/PWM.h"
+
+namespace frc {
+
+/**
+ * Vex Robotics Victor SP Speed Controller
+ */
+class Spark : public PWM {
+ public:
+ explicit Spark(int channel);
+ virtual ~Spark() = default;
+};
+
+} // namespace frc
diff --git a/frc971/wpilib/encoder_and_potentiometer.h b/frc971/wpilib/encoder_and_potentiometer.h
index 3516168..42fb86b 100644
--- a/frc971/wpilib/encoder_and_potentiometer.h
+++ b/frc971/wpilib/encoder_and_potentiometer.h
@@ -271,6 +271,24 @@
::std::unique_ptr<frc::AnalogInput> potentiometer_;
};
+class AbsoluteEncoder {
+ public:
+ void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
+ duty_cycle_.set_input(::std::move(input));
+ }
+
+ void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ encoder_ = ::std::move(encoder);
+ }
+
+ double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
+ int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
+
+ private:
+ DutyCycleReader duty_cycle_;
+ ::std::unique_ptr<frc::Encoder> encoder_;
+};
+
} // namespace wpilib
} // namespace frc971
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index dc34e1c..c843735 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -122,6 +122,25 @@
encoder_ratio);
}
+ // Copies a Absolute Encoder with the correct unit
+ // and direction changes.
+ void CopyPosition(
+ const ::frc971::wpilib::AbsoluteEncoder &encoder,
+ ::frc971::AbsolutePosition *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+
+ position->absolute_encoder =
+ (reverse ? (1.0 - encoder.ReadAbsoluteEncoder())
+ : encoder.ReadAbsoluteEncoder()) *
+ encoder_ratio * (2.0 * M_PI);
+ }
+
double encoder_translate(int32_t value, double counts_per_revolution,
double ratio) {
return static_cast<double>(value) / counts_per_revolution * ratio *
diff --git a/y2019/BUILD b/y2019/BUILD
index 9421a11..ad35ead 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -55,6 +55,30 @@
"//third_party/Phoenix-frc-lib:phoenix",
],
)
+cc_library(
+ name = "joystick_reader",
+ srcs = [
+ ":joystick_reader.cc",
+ ],
+ deps = [
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/input:drivetrain_input",
+ "//aos/input:joystick_input",
+ "//aos/input:action_joystick_input",
+ "//aos/logging",
+ "//aos/network:team_number",
+ "//aos/stl_mutex",
+ "//aos/time",
+ "//aos/util:log_interval",
+ "//aos/vision/events:udp",
+ "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//y2019/control_loops/drivetrain:drivetrain_base",
+ "//y2019/control_loops/superstructure:superstructure_queue",
+ ],
+)
py_library(
name = "python_init",
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
new file mode 100644
index 0000000..7bb32b2
--- /dev/null
+++ b/y2019/joystick_reader.cc
@@ -0,0 +1,185 @@
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/input/action_joystick_input.h"
+#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
+#include "aos/input/joystick_input.h"
+#include "aos/logging/logging.h"
+#include "aos/logging/logging.h"
+#include "aos/util/log_interval.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+using ::y2019::control_loops::superstructure::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;
+
+namespace y2019 {
+namespace input {
+namespace joysticks {
+
+// TODO(sabina): update button locations when the board is done
+const ButtonLocation kElevatorUp(0, 0);
+const ButtonLocation kElevatorDown(0, 0);
+const ButtonLocation kDiskLoad(0, 0);
+const ButtonLocation kDiskRocketMiddle(0, 0);
+const ButtonLocation kDiskRocketTop(0, 0);
+const ButtonLocation kCargoLoad(0, 0);
+const ButtonLocation kCargoBay(0, 0);
+const ButtonLocation kCargoRocketBase(0, 0);
+const ButtonLocation kCargoRocketMiddle(0, 0);
+const ButtonLocation kCargoRocketTop(0, 0);
+const ButtonLocation kStow(0, 0);
+const ButtonLocation kIntakeExtend(0, 0);
+const ButtonLocation kIntake(0, 0);
+const ButtonLocation kSpit(0, 0);
+const ButtonLocation kCargoSuction(0, 0);
+const ButtonLocation kDiskSuction(0, 0);
+const ButtonLocation kSuctionOut(0, 0);
+const ButtonLocation kDeployStilt(0, 0);
+const ButtonLocation kRetractStilt(0, 0);
+const ButtonLocation kBackwards(0, 0);
+
+class Reader : public ::aos::input::ActionJoystickInput {
+ public:
+ Reader(::aos::EventLoop *event_loop)
+ : ::aos::input::ActionJoystickInput(
+ event_loop,
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {}
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ superstructure_queue.position.FetchLatest();
+ superstructure_queue.status.FetchLatest();
+ if (!superstructure_queue.status.get() ||
+ !superstructure_queue.position.get()) {
+ LOG(ERROR, "Got no superstructure status packet.\n");
+ return;
+ }
+
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+
+ if (data.IsPressed(kElevatorUp)) {
+ elevator_height_ += 0.1;
+ } else if (data.IsPressed(kElevatorDown)) {
+ elevator_height_ -= 0.1;
+ } else if (data.IsPressed(kDiskLoad)) {
+ elevator_height_ = 0.48;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kDiskRocketMiddle)) {
+ elevator_height_ = 1.19;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kDiskRocketTop)) {
+ elevator_height_ = 1.90;
+ wrist_angle_ = M_PI;
+ }
+
+ // TODO(sabina): do we need an angle here?
+ else if (data.IsPressed(kCargoLoad)) {
+ elevator_height_ = 1.12;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kCargoBay)) {
+ elevator_height_ = 0.0;
+ wrist_angle_ = M_PI / 3;
+ } else if (data.IsPressed(kCargoRocketBase)) {
+ elevator_height_ = 0.7;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kCargoRocketMiddle)) {
+ elevator_height_ = 1.41;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kCargoRocketTop)) {
+ elevator_height_ = 2.12;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kStow)) {
+ elevator_height_ = 0.5;
+ wrist_angle_ = 0.0;
+ } else {
+ }
+
+ // TODO(sabina): get accurate angle.
+ if (data.IsPressed(kIntakeExtend)) {
+ new_superstructure_goal->intake.joint_angle = 0.5;
+ } else {
+ new_superstructure_goal->intake.joint_angle = 0.0;
+ }
+
+ if (data.IsPressed(kIntake)) {
+ new_superstructure_goal->suction.bottom = true;
+ if (superstructure_queue.status->has_piece == false) {
+ new_superstructure_goal->intake.roller_voltage = 12.0;
+ } else {
+ new_superstructure_goal->intake.roller_voltage = 0.0;
+ }
+ } else if (data.IsPressed(kSpit)) {
+ new_superstructure_goal->suction.bottom = false;
+ if (superstructure_queue.status->has_piece == false) {
+ new_superstructure_goal->intake.roller_voltage = 12.0;
+ } else {
+ new_superstructure_goal->intake.roller_voltage = 0.0;
+ }
+ } else {
+ new_superstructure_goal->intake.roller_voltage = 0.0;
+ }
+
+ // TODO(sabina): decide if we should really have disk suction as its own
+ // button
+ if (data.IsPressed(kCargoSuction)) {
+ new_superstructure_goal->suction.top = false;
+ new_superstructure_goal->suction.bottom = true;
+ } else if (data.IsPressed(kDiskSuction)) {
+ new_superstructure_goal->suction.top = true;
+ new_superstructure_goal->suction.bottom = true;
+ } else if (data.IsPressed(kSuctionOut)) {
+ new_superstructure_goal->suction.top = true;
+ new_superstructure_goal->suction.bottom = true;
+ } else {
+ }
+
+ // TODO(sabina): max height please?
+ if (data.IsPressed(kDeployStilt)) {
+ new_superstructure_goal->stilts.height = 0;
+ } else if (data.IsPressed(kRetractStilt)) {
+ new_superstructure_goal->stilts.height = 0;
+ } else {
+ }
+
+ if (data.IsPressed(kBackwards)) {
+ wrist_angle_ = -wrist_angle_;
+ }
+
+ new_superstructure_goal->elevator.height = elevator_height_;
+ new_superstructure_goal->wrist.angle = wrist_angle_;
+
+ LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
+
+ private:
+ // Current goals here.
+ double elevator_height_ = 0.0;
+ double wrist_angle_ = 0.0;
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2019
+
+int main() {
+ ::aos::Init(-1);
+ ::aos::ShmEventLoop event_loop;
+ ::y2019::input::joysticks::Reader reader(&event_loop);
+ reader.Run();
+ ::aos::Cleanup();
+}