Merge "Adding a simple codelab. Hopefully this can be useful in training."
diff --git a/NO_BUILD_AMD64 b/NO_BUILD_AMD64
deleted file mode 100644
index 6189481..0000000
--- a/NO_BUILD_AMD64
+++ /dev/null
@@ -1,30 +0,0 @@
--//third_party:wpilib
--//third_party/allwpilib_2016/...
--//third_party/ntcore_2016/...
--//third_party/allwpilib_2017/...
--//third_party/ntcore_2017/...
--//frc971/wpilib/...
--//y2012:wpilib_interface
--//y2012:download
--//y2014:wpilib_interface
--//y2014:download
--//y2014_bot3:wpilib_interface
--//y2014_bot3:download_stripped
--//y2015:wpilib_interface
--//y2015:download
--//y2015_bot3:wpilib_interface
--//y2015_bot3:download
--//y2016:wpilib_interface
--//y2016:download
--//y2016:download_stripped
--//y2016_bot3:wpilib_interface
--//y2016_bot3:download
--//y2016_bot3:download_stripped
--//y2016_bot3/...
--//y2016_bot4:wpilib_interface
--//y2016_bot4:download
--//y2016_bot4:download_stripped
--//y2017:wpilib_interface
--//y2017:download_stripped
--//y2017:download
--//motors/...
diff --git a/NO_BUILD_ROBORIO b/NO_BUILD_ROBORIO
deleted file mode 100644
index 780d909..0000000
--- a/NO_BUILD_ROBORIO
+++ /dev/null
@@ -1,18 +0,0 @@
--@slycot_repo//...
--//aos/vision/...
--//frc971/control_loops/python/...
--//y2012/control_loops/python/...
--//y2014_bot3/control_loops/python/...
--//y2014/control_loops/python/...
--//y2014_bot3/control_loops/python/...
--//y2015/control_loops/python/...
--//y2015_bot3/control_loops/python/...
--//y2016/control_loops/python/...
--//y2016/vision/...
--//y2017/vision/...
--//y2016_bot3/control_loops/python/...
--//y2016_bot4/control_loops/python/...
--//third_party/protobuf/...
--//y2016_bot3/...
--//y2017/control_loops/python/...
--//motors/...
diff --git a/aos/common/BUILD b/aos/common/BUILD
index e3160c4..7a15731 100644
--- a/aos/common/BUILD
+++ b/aos/common/BUILD
@@ -1,6 +1,7 @@
package(default_visibility = ['//visibility:public'])
load('/aos/build/queues', 'queue_library')
+load("//tools:environments.bzl", "mcu_cpus")
queue_library(
name = 'test_queue',
@@ -21,6 +22,7 @@
hdrs = [
'macros.h',
],
+ compatible_with = mcu_cpus,
)
cc_library(
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index c66a4e3..5cc15d1 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -31,8 +31,8 @@
// TODO(Comran): Make one that doesn't depend on the actual values for a
// specific robot.
-const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0,
- 0.0, 0.25, 0.75};
+const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25,
+ 0.75};
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
diff --git a/frc971/shifter_hall_effect.h b/frc971/shifter_hall_effect.h
index aaa5ebd..991386d 100644
--- a/frc971/shifter_hall_effect.h
+++ b/frc971/shifter_hall_effect.h
@@ -4,18 +4,31 @@
namespace frc971 {
namespace constants {
-// Contains the voltages for an analog hall effect sensor on a shifter.
+// Contains the constants for mapping the analog voltages that the shifter
+// sensors return to the shifter position. The code which uses this is trying
+// to sort out if we are in low gear, high gear, or neutral.
struct ShifterHallEffect {
- // The numbers to use for scaling raw voltages to 0-1.
- // Low is near 0.0, high is near 1.0
- double low_gear_middle, low_gear_low;
- double high_gear_high, high_gear_middle;
+ // low_gear_low is the voltage that the shifter position sensor reads when it
+ // is all the way in low gear. high_gear_high is the voltage that the shifter
+ // position sensor reads when it is all the way in high gear. These two
+ // values are used to calculate a position from 0 to 1, where we get 0 when
+ // the shifter is in low gear, and 1 when it is high gear.
+ double low_gear_low;
+ double high_gear_high;
// The numbers for when the dog is clear of each gear.
+ // We are in low gear when the position is less than clear_low, and in high
+ // gear when the shifter position is greater than clear_high.
double clear_low, clear_high;
};
-} // constants
-} // frc971
+struct DualHallShifterHallEffect {
+ ShifterHallEffect shifter_hall_effect;
+ double low_gear_middle;
+ double high_gear_middle;
+};
+
+} // namespace constants
+} // namespace frc971
#endif
diff --git a/motors/BUILD b/motors/BUILD
index ab9f239..e652069 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -1,3 +1,6 @@
+load('//motors:macros.bzl', 'hex_from_elf')
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_binary(
name = 'medium_salsa.elf',
srcs = [
@@ -6,29 +9,21 @@
deps = [
':util',
'//motors/core',
- '//motors/usb',
+ '//motors/usb:legacy',
],
- restricted_to = ['//tools:cortex-m4f'],
+ restricted_to = mcu_cpus,
)
-genrule(
+hex_from_elf(
name = 'medium_salsa',
- srcs = [
- 'medium_salsa.elf',
- ],
- outs = [
- 'medium_salsa.hex',
- ],
- cmd = '$(OBJCOPY) -O ihex $< $@',
- executable = True,
- output_to_bindir = True,
- restricted_to = ['//tools:cortex-m4f'],
+ restricted_to = mcu_cpus,
)
cc_library(
name = 'util',
+ visibility = ['//visibility:public'],
hdrs = [
'util.h',
],
- restricted_to = ['//tools:cortex-m4f'],
+ restricted_to = mcu_cpus,
)
diff --git a/motors/core/BUILD b/motors/core/BUILD
index 73a9d31..107ae5d 100644
--- a/motors/core/BUILD
+++ b/motors/core/BUILD
@@ -1,3 +1,5 @@
+load("//tools:environments.bzl", "mcu_cpus")
+
filegroup(
name = 'linkerscript',
visibility = ['//tools/cpp:__pkg__'],
@@ -17,5 +19,5 @@
'nonstd.c',
'time.c',
],
- restricted_to = ['//tools:cortex-m4f'],
+ restricted_to = mcu_cpus,
)
diff --git a/motors/macros.bzl b/motors/macros.bzl
new file mode 100644
index 0000000..3b1d379
--- /dev/null
+++ b/motors/macros.bzl
@@ -0,0 +1,10 @@
+def hex_from_elf(name, restricted_to=None):
+ native.genrule(
+ name = name,
+ srcs = [ '%s.elf' % name ],
+ outs = [ '%s.hex' % name ],
+ cmd = '$(OBJCOPY) -O ihex $< $@',
+ executable = True,
+ output_to_bindir = True,
+ restricted_to = restricted_to,
+ )
diff --git a/motors/pistol_grip/BUILD b/motors/pistol_grip/BUILD
new file mode 100644
index 0000000..d83b284
--- /dev/null
+++ b/motors/pistol_grip/BUILD
@@ -0,0 +1,20 @@
+load('//motors:macros.bzl', 'hex_from_elf')
+load("//tools:environments.bzl", "mcu_cpus")
+
+cc_binary(
+ name = 'drivers_station.elf',
+ srcs = [
+ 'drivers_station.cc',
+ ],
+ deps = [
+ '//motors:util',
+ '//motors/usb',
+ '//motors/core',
+ ],
+ restricted_to = mcu_cpus,
+)
+
+hex_from_elf(
+ name = 'drivers_station',
+ restricted_to = mcu_cpus,
+)
diff --git a/motors/pistol_grip/drivers_station.cc b/motors/pistol_grip/drivers_station.cc
new file mode 100644
index 0000000..370483e
--- /dev/null
+++ b/motors/pistol_grip/drivers_station.cc
@@ -0,0 +1,66 @@
+// This file has the main for the Teensy in the driver's station that
+// communicates over CAN with the one in the pistol grip controller.
+
+#include <stdio.h>
+
+#include "motors/core/time.h"
+#include "motors/core/kinetis.h"
+#include "motors/usb/usb.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace motors {
+
+extern "C" {
+void *__stack_chk_guard = (void *)0x67111971;
+int _write(int file, char *ptr, int len) {
+ (void)file;
+ (void)ptr;
+ (void)len;
+ return -1;
+}
+} // extern "C"
+
+void __stack_chk_fail(void);
+
+extern "C" int main(void) {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // relative to each other, which means centralizing them here makes it a lot
+ // more manageable.
+ NVIC_SET_SANE_PRIORITY(IRQ_USBOTG, 0x7);
+
+ // Set the LED's pin to output mode.
+ GPIO_BITBAND(GPIOC_PDDR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ delay(100);
+
+ teensy::UsbDevice usb_device(0, 0x16c0, 0x0490);
+ usb_device.Initialize();
+
+ //GPIOC_PSOR = 1 << 5;
+ while (true) {}
+
+ return 0;
+}
+
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+} // namespace motors
+} // namespace frc971
diff --git a/motors/python/BUILD b/motors/python/BUILD
new file mode 100644
index 0000000..20dd786
--- /dev/null
+++ b/motors/python/BUILD
@@ -0,0 +1,12 @@
+py_binary(
+ name = "phase_current",
+ srcs = [
+ "phase_current.py",
+ ],
+ deps = [
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:controls",
+ ],
+ restricted_to = ["//tools:k8"],
+)
diff --git a/motors/python/phase_current.py b/motors/python/phase_current.py
new file mode 100755
index 0000000..2e88fae
--- /dev/null
+++ b/motors/python/phase_current.py
@@ -0,0 +1,563 @@
+#!/usr/bin/python3
+
+import numpy
+from matplotlib import pylab
+import scipy.integrate
+from frc971.control_loops.python import controls
+import time
+import operator
+
+K1 = 1.81e04
+K2 = -2.65e03
+
+# Make the amplitude of the fundamental 1 for ease of playing with.
+K2 /= K1
+K1 = 1
+
+vcc = 30.0 # volts
+R_motor = 0.0055 # ohms for the motor
+R = 0.008 # ohms for system
+
+L = 10.0 * 1e-6 # Henries
+M = L / 10.0
+
+Kv = 22000.0 * 2.0 * numpy.pi / 60.0 / vcc * 2.0
+J = 0.0000007
+
+R_shunt = 0.0003
+
+# RC circuit for current sense filtering.
+R_sense1 = 768.0
+R_sense2 = 1470.0
+C_sense = 10.0 * 1e-9
+
+# So, we measured the inductance by switching between ~5 and ~20 amps through
+# the motor.
+# We then looked at the change in voltage that should give us (assuming duty
+# cycle * vin), and divided it by the corresponding change in current.
+
+# We then looked at the amount of time it took to decay the current to 1/e
+# That gave us the inductance.
+
+# Overrides for experiments
+J = J * 10.0
+
+# Firing phase A -> 0.0
+# Firing phase B -> - numpy.pi * 2.0 / 3.0
+# Firing phase C -> + numpy.pi * 2.0 / 3.0
+
+hz = 20000.0
+
+#switching_pattern = 'front'
+switching_pattern = 'centered'
+#switching_pattern = 'rear'
+#switching_pattern = 'centered front shifted'
+#switching_pattern = 'anticentered'
+
+Vconv = numpy.matrix([[2.0, -1.0, -1.0],
+ [-1.0, 2.0, -1.0],
+ [-1.0, -1.0, 2.0]]) / 3.0
+
+def f_single(theta):
+ return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+
+def g_single(theta):
+ return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+
+def gdot_single(theta):
+ """Derivitive of the current.
+
+ Must be multiplied by omega externally.
+ """
+ return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
+
+f = numpy.vectorize(f_single, otypes=(numpy.float,))
+g = numpy.vectorize(g_single, otypes=(numpy.float,))
+gdot = numpy.vectorize(gdot_single, otypes=(numpy.float,))
+
+def torque(theta):
+ return f(theta) * g(theta)
+
+def phase_a(function, theta):
+ return function(theta)
+
+def phase_b(function, theta):
+ return function(theta + 2 * numpy.pi / 3)
+
+def phase_c(function, theta):
+ return function(theta + 4 * numpy.pi / 3)
+
+def phases(function, theta):
+ return numpy.matrix([[phase_a(function, theta)],
+ [phase_b(function, theta)],
+ [phase_c(function, theta)]])
+
+def all_phases(function, theta_range):
+ return (phase_a(function, theta_range) +
+ phase_b(function, theta_range) +
+ phase_c(function, theta_range))
+
+theta_range = numpy.linspace(start=0, stop=4 * numpy.pi, num=10000)
+one_amp_driving_voltage = R * g(theta_range) + (L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) + M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
+
+max_one_amp_driving_voltage = max(one_amp_driving_voltage)
+
+# The number to divide the product of the unit BEMF and the per phase current
+# by to get motor current.
+one_amp_scalar = (phases(f_single, 0.0).T * phases(g_single, 0.0))[0, 0]
+
+print 'Max BEMF', max(f(theta_range))
+print 'Max current', max(g(theta_range))
+print 'Max drive voltage (one_amp_driving_voltage)', max(one_amp_driving_voltage)
+print 'one_amp_scalar', one_amp_scalar
+
+pylab.figure()
+pylab.subplot(1, 1, 1)
+pylab.plot(theta_range, f(theta_range), label='bemf')
+pylab.plot(theta_range, g(theta_range), label='phase_current')
+pylab.plot(theta_range, torque(theta_range), label='phase_torque')
+pylab.plot(theta_range, all_phases(torque, theta_range), label='sum_torque/current')
+pylab.legend()
+
+
+def full_sample_times(Ton, Toff, dt, n, start_time):
+ """Returns n + 4 samples for the provided switching times.
+
+ We need the timesteps and Us to integrate.
+
+ Returns:
+ array of [t, U matrix]
+ """
+
+ assert((Toff <= 1.0).all())
+
+ if (Ton <= Toff).all():
+ # Verify that they are all ordered correctly.
+ on_before_off = True
+ else:
+ on_before_off = False
+
+ Toff = Toff.copy() * dt
+ Toff[Toff < 100e-9] = -1.0
+ Toff[Toff > dt] = dt
+
+ Ton = Ton.copy() * dt
+ Ton[Ton < 100e-9] = -1.0
+ Ton[Ton > dt - 100e-9] = dt + 1.0
+
+ result = []
+ t = 0
+
+ result_times = numpy.concatenate(
+ (numpy.linspace(0, dt, num=n),
+ numpy.reshape(numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]), (-1,)),
+ numpy.reshape(numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]), (-1,))
+ ))
+ result_times.sort()
+
+ for t in numpy.nditer(result_times):
+ if on_before_off:
+ U = numpy.matrix([[vcc], [vcc], [vcc]])
+ U[t <= Ton] = 0.0
+ U[Toff < t] = 0.0
+ else:
+ U = numpy.matrix([[0.0], [0.0], [0.0]])
+ U[t > Ton] = vcc
+ U[t <= Toff] = vcc
+ result.append((float(t + start_time), U.copy()))
+
+ return result
+
+def sample_times(T, dt, n, start_time):
+ if switching_pattern == 'rear':
+ T = 1.0 - T
+ ans = full_sample_times(T, numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n, start_time)
+ elif switching_pattern == 'centered front shifted':
+ # Centered, but shifted to the beginning of the cycle.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
+
+ tn = min(Ton)[0, 0]
+ Ton -= tn
+ Toff -= tn
+
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'centered':
+ # Centered, looks waaay better.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
+
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'anticentered':
+ # Centered, looks waaay better.
+ Toff = T / 2.0
+ Ton = 1.0 - T / 2.0
+
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'front':
+ ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n, start_time)
+ else:
+ assert(False)
+
+ return ans
+
+class DataLogger(object):
+ def __init__(self, title=None):
+ self.title = title
+ self.ia = []
+ self.ib = []
+ self.ic = []
+ self.ia_goal = []
+ self.ib_goal = []
+ self.ic_goal = []
+ self.ia_controls = []
+ self.ib_controls = []
+ self.ic_controls = []
+ self.isensea = []
+ self.isenseb = []
+ self.isensec = []
+
+ self.va = []
+ self.vb = []
+ self.vc = []
+ self.van = []
+ self.vbn = []
+ self.vcn = []
+
+ self.ea = []
+ self.eb = []
+ self.ec = []
+
+ self.theta = []
+ self.omega = []
+
+ self.i_goal = []
+
+ self.time = []
+ self.controls_time = []
+ self.predicted_time = []
+
+ self.ia_pred = []
+ self.ib_pred = []
+ self.ic_pred = []
+
+ self.voltage_time = []
+ self.estimated_velocity = []
+ self.U_last = numpy.matrix(numpy.zeros((3, 1)))
+
+ def log_predicted(self, current_time, p):
+ self.predicted_time.append(current_time)
+ self.ia_pred.append(p[0, 0])
+ self.ib_pred.append(p[1, 0])
+ self.ic_pred.append(p[2, 0])
+
+ def log_controls(self, current_time, measured_current, In, E, estimated_velocity):
+ self.controls_time.append(current_time)
+ self.ia_controls.append(measured_current[0, 0])
+ self.ib_controls.append(measured_current[1, 0])
+ self.ic_controls.append(measured_current[2, 0])
+
+ self.ea.append(E[0, 0])
+ self.eb.append(E[1, 0])
+ self.ec.append(E[2, 0])
+
+ self.ia_goal.append(In[0, 0])
+ self.ib_goal.append(In[1, 0])
+ self.ic_goal.append(In[2, 0])
+ self.estimated_velocity.append(estimated_velocity)
+
+ def log_data(self, X, U, current_time, Vn, i_goal):
+ self.ia.append(X[0, 0])
+ self.ib.append(X[1, 0])
+ self.ic.append(X[2, 0])
+
+ self.i_goal.append(i_goal)
+
+ self.isensea.append(X[5, 0])
+ self.isenseb.append(X[6, 0])
+ self.isensec.append(X[7, 0])
+
+ self.theta.append(X[3, 0])
+ self.omega.append(X[4, 0])
+
+ self.time.append(current_time)
+
+ self.van.append(Vn[0, 0])
+ self.vbn.append(Vn[1, 0])
+ self.vcn.append(Vn[2, 0])
+
+ if (self.U_last != U).any():
+ self.va.append(self.U_last[0, 0])
+ self.vb.append(self.U_last[1, 0])
+ self.vc.append(self.U_last[2, 0])
+ self.voltage_time.append(current_time)
+
+ self.va.append(U[0, 0])
+ self.vb.append(U[1, 0])
+ self.vc.append(U[2, 0])
+ self.voltage_time.append(current_time)
+ self.U_last = U.copy()
+
+ def plot(self):
+ fig = pylab.figure()
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.controls_time, self.ia_controls, 'ro', label='ia_controls')
+ pylab.plot(self.controls_time, self.ib_controls, 'go', label='ib_controls')
+ pylab.plot(self.controls_time, self.ic_controls, 'bo', label='ic_controls')
+ pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
+ pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
+ pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
+
+ #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
+ #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
+ #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
+ pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
+ pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
+ pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
+ pylab.plot(self.time, self.ia, 'r', label='ia')
+ pylab.plot(self.time, self.ib, 'g', label='ib')
+ pylab.plot(self.time, self.ic, 'b', label='ic')
+ pylab.plot(self.time, self.i_goal, label='i_goal')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.voltage_time, self.va, label='va')
+ pylab.plot(self.voltage_time, self.vb, label='vb')
+ pylab.plot(self.voltage_time, self.vc, label='vc')
+ pylab.plot(self.time, self.van, label='van')
+ pylab.plot(self.time, self.vbn, label='vbn')
+ pylab.plot(self.time, self.vcn, label='vcn')
+ pylab.plot(self.controls_time, self.ea, label='ea')
+ pylab.plot(self.controls_time, self.eb, label='eb')
+ pylab.plot(self.controls_time, self.ec, label='ec')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.time, self.theta, label='theta')
+ pylab.plot(self.time, self.omega, label='omega')
+ pylab.plot(self.controls_time, self.estimated_velocity, label='estimated omega')
+
+ pylab.legend()
+
+ fig = pylab.figure()
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ia_goal, self.ia_controls), 'r', label='ia_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ib_goal, self.ib_controls), 'g', label='ib_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ic_goal, self.ic_controls), 'b', label='ic_error')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
+ pylab.show()
+
+
+# So, from running a bunch of math, we know the following:
+# Van + Vbn + Vcn = 0
+# ia + ib + ic = 0
+# ea + eb + ec = 0
+# d ia/dt + d ib/dt + d ic/dt = 0
+#
+# We also have:
+# [ Van ] [ 2/3 -1/3 -1/3] [Va]
+# [ Vbn ] = [ -1/3 2/3 -1/3] [Vb]
+# [ Vcn ] [ -1/3 -1/3 2/3] [Vc]
+#
+# or,
+#
+# Vabcn = Vconv * V
+#
+# The base equation is:
+#
+# [ Van ] [ R 0 0 ] [ ia ] [ L M M ] [ dia/dt ] [ ea ]
+# [ Vbn ] = [ 0 R 0 ] [ ib ] + [ M L M ] [ dib/dt ] + [ eb ]
+# [ Vbn ] [ 0 0 R ] [ ic ] [ M M L ] [ dic/dt ] [ ec ]
+#
+# or
+#
+# Vabcn = R_matrix * I + L_matrix * I_dot + E
+#
+# We can re-arrange this as:
+#
+# inv(L_matrix) * (Vconv * V - E - R_matrix * I) = I_dot
+# B * V - inv(L_matrix) * E - A * I = I_dot
+class Simulation(object):
+ def __init__(self):
+ self.R_matrix = numpy.matrix(numpy.eye(3)) * R
+ self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
+ self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
+ self.A = self.L_matrix_inv * self.R_matrix
+ self.B = self.L_matrix_inv * Vconv
+ self.A_discrete, self.B_discrete = controls.c2d(-self.A, self.B, 1.0 / hz)
+ self.B_discrete_inverse = numpy.matrix(numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
+
+ self.R_model = R * 1.0
+ self.L_model = L * 1.0
+ self.M_model = M * 1.0
+ self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
+ self.L_matrix_model = numpy.matrix([[self.L_model, self.M_model, self.M_model],
+ [self.M_model, self.L_model, self.M_model],
+ [self.M_model, self.M_model, self.L_model]])
+ self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
+ self.A_model = self.L_matrix_inv_model * self.R_matrix_model
+ self.B_model = self.L_matrix_inv_model * Vconv
+ self.A_discrete_model, self.B_discrete_model = \
+ controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
+ self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
+
+ print 'constexpr double kL = %g;' % self.L_model
+ print 'constexpr double kM = %g;' % self.M_model
+ print 'constexpr double kR = %g;' % self.R_model
+ print 'constexpr float kAdiscrete_diagonal = %gf;' % self.A_discrete_model[0, 0]
+ print 'constexpr float kAdiscrete_offdiagonal = %gf;' % self.A_discrete_model[1, 0]
+ print 'constexpr float kBdiscrete_inv_diagonal = %gf;' % self.B_discrete_inverse_model[0, 0]
+ print 'constexpr float kBdiscrete_inv_offdiagonal = %gf;' % self.B_discrete_inverse_model[1, 0]
+ print 'constexpr double kOneAmpScalar = %g;' % one_amp_scalar
+ print 'constexpr double kMaxOneAmpDrivingVoltage = %g;' % max_one_amp_driving_voltage
+ print('A_discrete', self.A_discrete)
+ print('B_discrete', self.B_discrete)
+ print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
+ print('B_discrete_inv', self.B_discrete_inverse)
+
+ # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
+ # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
+ self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 / (R_sense1 * C_sense))
+ self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0)
+
+ # ia, ib, ic, theta, omega, isensea, isenseb, isensec
+ self.X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
+
+ self.K = 0.05 * Vconv
+ print('A %s' % repr(self.A))
+ print('B %s' % repr(self.B))
+ print('K %s' % repr(self.K))
+
+ print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
+ print('Poles are %s' % repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
+ controllability = controls.ctrb(self.A, self.B)
+ print('Rank of augmented controlability matrix. %d' % numpy.linalg.matrix_rank(
+ controllability))
+
+ self.data_logger = DataLogger(switching_pattern)
+ self.current_time = 0.0
+
+ self.estimated_velocity = self.X[4, 0]
+
+ def motor_diffeq(self, x, t, U):
+ I = numpy.matrix(x[0:3]).T
+ theta = x[3]
+ omega = x[4]
+ Isense = numpy.matrix(x[5:]).T
+
+ dflux = phases(f_single, theta) / Kv
+
+ Xdot = numpy.matrix(numpy.zeros((8, 1)))
+ di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
+ torque = I.T * dflux
+ Xdot[0:3, :] = di_dt
+ Xdot[3, :] = omega
+ Xdot[4, :] = torque / J
+
+ Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
+ return numpy.squeeze(numpy.asarray(Xdot))
+
+ def DoControls(self, goal_current):
+ theta = self.X[3, 0]
+ # Use the actual angular velocity.
+ omega = self.X[4, 0]
+
+ measured_current = self.X[5:, :].copy()
+
+ # Ok, lets now fake it.
+ E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
+ [[-1j],
+ [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
+ [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)]])
+ E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
+ [[-1j],
+ [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
+ [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)]])
+
+ overall_measured_current = ((E_imag1 + E_imag2).real.T * measured_current / one_amp_scalar)[0, 0]
+
+ current_error = goal_current - overall_measured_current
+ #print(current_error)
+ self.estimated_velocity += current_error * 1.0
+ omega = self.estimated_velocity
+
+ # Now, apply the transfer function of the inductor.
+ # Use that to difference the current across the cycle.
+ Icurrent = self.Ilast
+ # No history:
+ #Icurrent = phases(g_single, theta) * goal_current
+ Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
+
+ deltaI = Inext - Icurrent
+
+ H1 = -numpy.linalg.inv(1j * omega * self.L_matrix + self.R_matrix) * omega / Kv
+ H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix + self.R_matrix) * omega / Kv
+ p_imag = H1 * E_imag1 + H2 * E_imag2
+ p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
+ numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
+ p = p_imag.real
+
+ # So, we now know how much the change in current is due to changes in BEMF.
+ # Subtract that, and then run the stock statespace equation.
+ Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete * (Icurrent - p) - p_next_imag.real)
+ print 'Vn_ff', Vn_ff
+ print 'Inext', Inext
+ Vn = Vn_ff + self.K * (Icurrent - measured_current)
+
+ E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
+ self.data_logger.log_controls(self.current_time, measured_current, Icurrent, E, self.estimated_velocity)
+
+ self.Ilast = Inext
+
+ return Vn
+
+ def Simulate(self):
+ start_wall_time = time.time()
+ self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
+ for n in range(200):
+ goal_current = 10.0
+ max_current = (vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ min_current = (-vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ goal_current = max(min_current, min(max_current, goal_current))
+
+ Vn = self.DoControls(goal_current)
+
+ #Vn = numpy.matrix([[0.20], [0.0], [0.0]])
+ #Vn = numpy.matrix([[0.00], [0.20], [0.0]])
+ #Vn = numpy.matrix([[0.00], [0.0], [0.20]])
+
+ # T is the fractional rate.
+ T = Vn / vcc
+ tn = -numpy.min(T)
+ T += tn
+ if (T > 1.0).any():
+ T = T / numpy.max(T)
+
+ for t, U in sample_times(T = T,
+ dt = 1.0 / hz, n = 10,
+ start_time = self.current_time):
+ # Analog amplifier mode!
+ #U = Vn
+
+ self.data_logger.log_data(self.X, (U - min(U)), self.current_time, Vn, goal_current)
+ t_array = numpy.array([self.current_time, t])
+ self.X = numpy.matrix(scipy.integrate.odeint(
+ self.motor_diffeq,
+ numpy.squeeze(numpy.asarray(self.X)),
+ t_array, args=(U,)))[1, :].T
+
+ self.current_time = t
+
+ print 'Took %f to simulate' % (time.time() - start_wall_time)
+
+ self.data_logger.plot()
+
+simulation = Simulation()
+simulation.Simulate()
diff --git a/motors/usb/BUILD b/motors/usb/BUILD
index ddb1bcc..55b1790 100644
--- a/motors/usb/BUILD
+++ b/motors/usb/BUILD
@@ -1,5 +1,7 @@
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_library(
- name = 'usb',
+ name = 'legacy',
visibility = ['//visibility:public'],
hdrs = [
'usb_desc.h',
@@ -21,5 +23,42 @@
deps = [
'//motors/core',
],
- restricted_to = ['//tools:cortex-m4f'],
+ restricted_to = mcu_cpus,
+)
+
+cc_library(
+ name = 'usb',
+ visibility = ['//visibility:public'],
+ hdrs = [
+ 'usb.h',
+ ],
+ srcs = [
+ 'usb.cc',
+ ],
+ deps = [
+ ':constants',
+ '//aos/common:macros',
+ '//motors/core',
+ '//motors:util',
+ ],
+ restricted_to = mcu_cpus,
+)
+
+cc_library(
+ name = 'constants',
+ hdrs = [
+ 'constants.h',
+ ],
+ compatible_with = mcu_cpus,
+)
+
+cc_test(
+ name = 'constants_test',
+ srcs = [
+ 'constants_test.cc',
+ ],
+ deps = [
+ ':constants',
+ '//aos/testing:googletest',
+ ],
)
diff --git a/motors/usb/constants.h b/motors/usb/constants.h
new file mode 100644
index 0000000..58c9ecc
--- /dev/null
+++ b/motors/usb/constants.h
@@ -0,0 +1,114 @@
+#ifndef MOTORS_USB_CONSTANTS_H_
+#define MOTORS_USB_CONSTANTS_H_
+
+#include <stdint.h>
+
+namespace frc971 {
+namespace teensy {
+
+enum class Direction : uint32_t {
+ kTx = 1 << 1,
+ kRx = 0,
+};
+
+enum class EvenOdd : uint32_t {
+ kOdd = 1 << 0,
+ kEven = 0,
+};
+
+constexpr static inline EvenOdd EvenOddInverse(EvenOdd odd) {
+ return static_cast<EvenOdd>(static_cast<uint32_t>(odd) ^
+ static_cast<uint32_t>(EvenOdd::kOdd));
+}
+
+// Returns 0 for kEven and 1 for kOdd. This is useful for indexing into arrays
+// and similar things.
+constexpr static inline int EvenOddIndex(EvenOdd odd) {
+ static_assert(static_cast<int>(EvenOdd::kOdd) == 1, "Value changed");
+ return static_cast<int>(odd);
+}
+
+enum class EndpointBufferState : int {
+ // The values are chosen carefully so bit arithmetic can efficiently
+ // manipulate these values. This math is all encapsulated in methods
+ // immediately following.
+ // Bit 0 is even full.
+ // Bit 1 is odd full.
+ // Bit 2 is which one to fill next (1 for odd).
+ // Bit 3 is which one to empty next (1 for odd).
+
+ // Both are empty and we should fill the even one first.
+ kBothEmptyEvenFirst = 0x0,
+ kBothEmptyOddFirst = 0xC,
+ kEvenFull = 0x5,
+ kOddFull = 0xA,
+ // Both are full and we should empty the even one first.
+ kBothFullEvenFirst = 0x3,
+ kBothFullOddFirst = 0xF,
+};
+
+// kBothEmptyEvenFirst fill even kEvenFull fill odd kBothFullEvenFirst
+// empty even kOddFull empty odd kBothEmptyEvenFirst
+
+// Returns true if state has at least one empty buffer.
+constexpr static inline bool BufferStateHasEmpty(EndpointBufferState state) {
+ return (static_cast<int>(state) & 0x3) != 0x3;
+}
+
+// Returns true if state has at least one full buffer.
+constexpr static inline bool BufferStateHasFull(EndpointBufferState state) {
+ return (static_cast<int>(state) & 0x3) != 0;
+}
+
+// Returns the next buffer to fill from state.
+//
+// This won't make sense if !BufferStateHasEmpty(state).
+constexpr static inline EvenOdd BufferStateToFill(EndpointBufferState state) {
+ return (static_cast<int>(state) & 0x4) ? EvenOdd::kOdd : EvenOdd::kEven;
+}
+
+// Returns the next buffer to empty from state.
+//
+// This won't make sense if !BufferStateHasFull(state).
+constexpr static inline EvenOdd BufferStateToEmpty(EndpointBufferState state) {
+ return (static_cast<int>(state) & 0x8) ? EvenOdd::kOdd : EvenOdd::kEven;
+}
+
+// Returns the new state after filling BufferStateToFill(state).
+//
+// This won't make sense if !BufferStateHasEmpty(state).
+constexpr static inline EndpointBufferState BufferStateAfterFill(
+ EndpointBufferState state) {
+ return static_cast<EndpointBufferState>(
+ // XOR with bit 2 to toggle which is next.
+ (static_cast<int>(state) ^ 0x4) |
+ // Set the bit corresponding to the buffer which was filled.
+ (1 << EvenOddIndex(BufferStateToFill(state))));
+}
+
+// Returns the new state after emptying BufferStateToEmpty(state).
+//
+// This won't make sense if !BufferStateHasFull(state).
+constexpr static inline EndpointBufferState BufferStateAfterEmpty(
+ EndpointBufferState state) {
+ return static_cast<EndpointBufferState>(
+ // XOR with bit 3 to toggle which is next.
+ (static_cast<int>(state) ^ 0x8) &
+ // Clear the bit corresponding to the buffer which was emptied.
+ ~(1 << EvenOddIndex(BufferStateToEmpty(state))));
+}
+
+enum class Data01 : uint32_t {
+ kData1 = 1 << 6,
+ kData0 = 0,
+};
+
+constexpr static inline Data01 Data01Inverse(Data01 toggle) {
+ return static_cast<Data01>(static_cast<uint32_t>(toggle) ^
+ static_cast<uint32_t>(Data01::kData1));
+}
+
+} // namespace teensy
+} // namespace frc971
+
+#endif // MOTORS_USB_CONSTANTS_H_
diff --git a/motors/usb/constants_test.cc b/motors/usb/constants_test.cc
new file mode 100644
index 0000000..4d11349
--- /dev/null
+++ b/motors/usb/constants_test.cc
@@ -0,0 +1,66 @@
+#include "motors/usb/constants.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace teensy {
+namespace testing {
+
+TEST(EndpointBufferStateTest, Filling) {
+ EXPECT_TRUE(BufferStateHasEmpty(EndpointBufferState::kBothEmptyEvenFirst));
+ EXPECT_EQ(EvenOdd::kEven,
+ BufferStateToFill(EndpointBufferState::kBothEmptyEvenFirst));
+
+ EXPECT_TRUE(BufferStateHasEmpty(EndpointBufferState::kBothEmptyOddFirst));
+ EXPECT_EQ(EvenOdd::kOdd,
+ BufferStateToFill(EndpointBufferState::kBothEmptyOddFirst));
+
+ EXPECT_TRUE(BufferStateHasEmpty(EndpointBufferState::kEvenFull));
+ EXPECT_EQ(EvenOdd::kOdd, BufferStateToFill(EndpointBufferState::kEvenFull));
+
+ EXPECT_TRUE(BufferStateHasEmpty(EndpointBufferState::kOddFull));
+ EXPECT_EQ(EvenOdd::kEven, BufferStateToFill(EndpointBufferState::kOddFull));
+
+ EXPECT_FALSE(BufferStateHasEmpty(EndpointBufferState::kBothFullEvenFirst));
+}
+
+TEST(EndpointBufferStateTest, Emptying) {
+ EXPECT_FALSE(BufferStateHasFull(EndpointBufferState::kBothEmptyEvenFirst));
+
+ EXPECT_FALSE(BufferStateHasFull(EndpointBufferState::kBothEmptyOddFirst));
+
+ EXPECT_TRUE(BufferStateHasFull(EndpointBufferState::kEvenFull));
+ EXPECT_EQ(EvenOdd::kEven, BufferStateToEmpty(EndpointBufferState::kEvenFull));
+
+ EXPECT_TRUE(BufferStateHasFull(EndpointBufferState::kOddFull));
+ EXPECT_EQ(EvenOdd::kOdd, BufferStateToEmpty(EndpointBufferState::kOddFull));
+
+ EXPECT_TRUE(BufferStateHasFull(EndpointBufferState::kBothFullEvenFirst));
+ EXPECT_EQ(EvenOdd::kEven,
+ BufferStateToEmpty(EndpointBufferState::kBothFullEvenFirst));
+}
+
+TEST(EndpointBufferStateTest, Transitions) {
+ EXPECT_EQ(EndpointBufferState::kEvenFull,
+ BufferStateAfterFill(EndpointBufferState::kBothEmptyEvenFirst));
+
+ EXPECT_EQ(EndpointBufferState::kOddFull,
+ BufferStateAfterFill(EndpointBufferState::kBothEmptyOddFirst));
+
+ EXPECT_EQ(EndpointBufferState::kBothFullEvenFirst,
+ BufferStateAfterFill(EndpointBufferState::kEvenFull));
+ EXPECT_EQ(EndpointBufferState::kBothEmptyOddFirst,
+ BufferStateAfterEmpty(EndpointBufferState::kEvenFull));
+
+ EXPECT_EQ(EndpointBufferState::kBothFullOddFirst,
+ BufferStateAfterFill(EndpointBufferState::kOddFull));
+ EXPECT_EQ(EndpointBufferState::kBothEmptyEvenFirst,
+ BufferStateAfterEmpty(EndpointBufferState::kOddFull));
+
+ EXPECT_EQ(EndpointBufferState::kOddFull,
+ BufferStateAfterEmpty(EndpointBufferState::kBothFullEvenFirst));
+}
+
+} // namespace testing
+} // namespace teensy
+} // namespace frc971
diff --git a/motors/usb/usb.cc b/motors/usb/usb.cc
new file mode 100644
index 0000000..3067695
--- /dev/null
+++ b/motors/usb/usb.cc
@@ -0,0 +1,812 @@
+#include "motors/usb/usb.h"
+
+#include <string.h>
+
+#include "motors/util.h"
+
+namespace frc971 {
+namespace teensy {
+namespace {
+
+// The mask of interrupts we care about.
+constexpr uint32_t usb_enabled_interrupts() {
+ // Deliberately not turning the sleep interrupt on here because we just
+ // want to ignore that anyways.
+ return USB_INTEN_TOKDNEEN | USB_INTEN_SOFTOKEN | USB_INTEN_ERROREN |
+ USB_INTEN_USBRSTEN;
+}
+
+// The names of all the standard setup requests which come in on endpoint 0.
+namespace standard_setup_requests {
+constexpr int kGetStatus = 0;
+constexpr int kClearFeature = 1;
+constexpr int kSetFeature = 3;
+constexpr int kSetAddress = 5;
+constexpr int kGetDescriptor = 6;
+constexpr int kSetDescriptor = 7;
+constexpr int kGetConfiguration = 8;
+constexpr int kSetConfiguration = 9;
+constexpr int kGetInterface = 10;
+constexpr int kSetInterface = 11;
+constexpr int kSynchFrame = 12;
+} // namespace standard_setup_requests
+
+// The names of the standard feature selectors.
+namespace standard_feature_selectors {
+constexpr int kDeviceRemoteWakeup = 1;
+constexpr int kEndpointHalt = 0;
+constexpr int kTestMode = 2;
+} // namespace standard_feature_selectors
+
+// The names of all the PIDs (Packet IDs) from the USB standard. Note that this
+// USB hardware doesn't expose most of them, especially in device mode.
+enum class UsbPid {
+ kOut = 0x1,
+ kIn = 0x9,
+ kSof = 0x5,
+ kSetup = 0xD,
+ kData0 = 0x3,
+ kData1 = 0xB,
+ kData2 = 0x7,
+ kMData = 0xF,
+ kAck = 0x2,
+ kNak = 0xA,
+ kStall = 0xE,
+ kNYet = 0x6,
+ kPre = 0xC,
+ kErr = 0xC,
+ kSplit = 0x8,
+ kPing = 0x4,
+ kReserved = 0x0,
+};
+
+// The device class for using IADs.
+constexpr uint8_t iad_device_class() { return 0xEF; }
+// The device subclass for using IADs.
+constexpr uint8_t iad_device_subclass() { return 0x02; }
+// The device protocol for using IADs.
+constexpr uint8_t iad_device_protocol() { return 0x01; }
+
+// The total number of endpoints supported by this hardware.
+constexpr int number_endpoints() { return 16; }
+
+__attribute__((aligned(512))) BdtEntry
+ usb0_buffer_descriptor_table[number_endpoints() * 2 /* rx/tx */ *
+ 2 /* even/odd */];
+
+// Returns the specified BDT entry.
+BdtEntry *MutableBdtEntry(int endpoint, Direction direction, EvenOdd odd) {
+ return &usb0_buffer_descriptor_table[static_cast<uint32_t>(endpoint << 2) |
+ static_cast<uint32_t>(direction) |
+ static_cast<uint32_t>(odd)];
+}
+
+// Returns the BDT entry corresponding to a USBx_STAT value.
+BdtEntry *MutableBdtEntryFromStat(uint8_t stat) {
+ return &usb0_buffer_descriptor_table[static_cast<uint32_t>(stat) >> 2];
+}
+
+// A pointer to the object we're going to ask to handle interrupts.
+UsbDevice *volatile global_usb0_device = nullptr;
+
+} // namespace
+
+UsbDevice::UsbDevice(int index, uint16_t vendor_id, uint16_t product_id)
+ : index_(index) {
+ // TODO(Brian): Pass index_ into all the register access macros. Also sort out
+ // how to deal with it for the interrupts.
+ assert(index == 0);
+
+ assert(global_usb0_device == nullptr);
+ global_usb0_device = this;
+
+ // Endpoint 0 isn't a normal endpoint, so it doesn't show up in here.
+ endpoint_mapping_.push_back(nullptr);
+
+ // Set up the "String Descriptor Zero, Specifying Languages Supported by the
+ // Device" (aka english_us_code() only).
+ strings_.emplace_back(4, '\0');
+ strings_.back()[0] = 4;
+ strings_.back()[1] = static_cast<uint8_t>(UsbDescriptorType::kString);
+ strings_.back()[2] = english_us_code() & 0xFF;
+ strings_.back()[3] = (english_us_code() >> 8) & 0xFF;
+
+ device_descriptor_ =
+ device_descriptor_list_.CreateDescriptor(18, UsbDescriptorType::kDevice);
+ device_descriptor_->AddUint16(0x0200); // bcdUSB
+ device_descriptor_->AddByte(iad_device_class()); // bDeviceClass
+ device_descriptor_->AddByte(iad_device_subclass()); // bDeviceSubClass
+ device_descriptor_->AddByte(iad_device_protocol()); // bDeviceProtocol
+ device_descriptor_->AddByte(kEndpoint0MaxSize); // bMaxPacketSize0
+ device_descriptor_->AddUint16(vendor_id); // idVendor
+ device_descriptor_->AddUint16(product_id); // idProduct
+ device_descriptor_->AddUint16(0); // bcdDevice
+ // We might overwrite these string descriptor indices later if we get strings
+ // to put there.
+ device_descriptor_->AddByte(0); // iManufacturer
+ device_descriptor_->AddByte(0); // iProduct
+ device_descriptor_->AddByte(0); // iSerialNumber
+ device_descriptor_->AddByte(1); // bNumConfigurations
+
+ config_descriptor_ = config_descriptor_list_.CreateDescriptor(
+ 9, UsbDescriptorType::kConfiguration);
+}
+
+UsbDevice::~UsbDevice() {
+ NVIC_DISABLE_IRQ(IRQ_USBOTG);
+ dma_memory_barrier();
+ assert(global_usb0_device == this);
+ global_usb0_device = nullptr;
+}
+
+void UsbDevice::Initialize() {
+ assert(!is_set_up_);
+
+ for (UsbFunction *function : functions_) {
+ function->Initialize();
+ }
+
+ config_descriptor_->AddUint16(
+ config_descriptor_list_.CurrentSize()); // wTotalLength
+ config_descriptor_->AddByte(interface_mapping_.size()); // bNumInterfaces
+ config_descriptor_->AddByte(1); // bConfigurationValue
+ // Doesn't seem to be much point naming our one and only configuration.
+ config_descriptor_->AddByte(0); // iConfiguration
+ config_descriptor_->AddByte((1 << 7) /* Reserved */ |
+ (1 << 6) /* Self-powered */); // bmAttribute
+ config_descriptor_->AddByte(2 /* 4mA */); // bMaxPower
+
+ device_descriptor_.reset();
+ config_descriptor_.reset();
+ device_descriptor_list_.CheckFinished();
+ config_descriptor_list_.CheckFinished();
+ is_set_up_ = true;
+
+ // Make sure all the buffer descriptors are clear.
+ for (int i = 0; i < number_endpoints(); ++i) {
+ for (Direction direction : {Direction::kTx, Direction::kRx}) {
+ for (EvenOdd odd : {EvenOdd::kOdd, EvenOdd::kEven}) {
+ MutableBdtEntry(i, direction, odd)->buffer_descriptor = 0;
+ MutableBdtEntry(i, direction, odd)->address = nullptr;
+ }
+ }
+ }
+ dma_memory_barrier();
+
+ // The other startup code handles getting the incoming 48MHz clock running.
+ SIM_SCGC4 |= SIM_SCGC4_USBOTG;
+ MPU_RGDAAC0 |= 0x03000000;
+
+ // Reset it.
+ USB0_USBTRC0 = USB_USBTRC_USBRESET;
+ // TRM says to wait "two USB clock cycles", so assume that's at 48MHz and then
+ // round up, being pessimistic in assuming each read from the peripheral is
+ // only a single core clock. This wildly overapproximates how long we need to
+ // wait, but whatever.
+ for (int i = 0; i < ((F_CPU / 48000000) + 1) * 2; ++i) {
+ while ((USB0_USBTRC0 & USB_USBTRC_USBRESET) != 0) {
+ }
+ }
+
+ USB0_BDTPAGE1 =
+ reinterpret_cast<uintptr_t>(&usb0_buffer_descriptor_table[0]) >> 8;
+ USB0_BDTPAGE2 =
+ reinterpret_cast<uintptr_t>(&usb0_buffer_descriptor_table[0]) >> 16;
+ USB0_BDTPAGE3 =
+ reinterpret_cast<uintptr_t>(&usb0_buffer_descriptor_table[0]) >> 24;
+
+ // The Quick Reference User Guide says to clear all the interrupts.
+ ClearInterrupts();
+ USB0_OTGISTAT = USB_OTGISTAT_ONEMSEC | USB_OTGISTAT_LINE_STATE_CHG;
+
+ // Now enable the module.
+ USB0_CTL = USB_CTL_USBENSOFEN;
+
+ // Un-suspend the transceiver and disable weak pulldowns.
+ USB0_USBCTRL = 0;
+ // And enable the D+ pullup which indicates we're a full-speed device.
+ USB0_CONTROL = USB_CONTROL_DPPULLUPNONOTG;
+
+ // Enable the reset interrupt (which is the first one we care about).
+ USB0_INTEN = USB_INTEN_USBRSTEN;
+
+ dma_memory_barrier();
+ NVIC_ENABLE_IRQ(IRQ_USBOTG);
+}
+
+void usb_isr(void) {
+ UsbDevice *const usb0_device = global_usb0_device;
+ if (usb0_device == nullptr) {
+ NVIC_DISABLE_IRQ(IRQ_USBOTG);
+ } else {
+ usb0_device->HandleInterrupt();
+ }
+}
+
+void UsbDevice::ClearInterrupts() {
+ USB0_ISTAT = USB_ISTAT_ATTACH | USB_ISTAT_RESUME | USB_ISTAT_SLEEP |
+ USB_ISTAT_TOKDNE | USB_ISTAT_SOFTOK | USB_ISTAT_ERROR |
+ USB_ISTAT_USBRST;
+ USB0_ERRSTAT = USB_ERRSTAT_BTSERR | USB_ERRSTAT_DMAERR | USB_ERRSTAT_BTOERR |
+ USB_ERRSTAT_DFN8 | USB_ERRSTAT_CRC16 | USB_ERRSTAT_CRC5EOF |
+ USB_ERRSTAT_PIDERR;
+}
+
+void UsbDevice::HandleInterrupt() {
+ while (true) {
+ const uint32_t status = USB0_ISTAT;
+ if ((status & usb_enabled_interrupts()) == 0) {
+ return;
+ }
+
+ // If we just got a start-of-frame token, then ask all the functions what to
+ // do.
+ if (status & USB_ISTAT_SOFTOK) {
+ // TODO(Brian): Actually ask the functions, maybe only if we're
+ // configured.
+ USB0_ISTAT = USB_ISTAT_SOFTOK;
+ }
+
+ // If we just finished processing a token.
+ if (status & USB_ISTAT_TOKDNE) {
+ const uint8_t stat = USB0_STAT;
+ const int endpoint = G_USB_STAT_ENDP(stat);
+
+ if (endpoint == 0) {
+ HandleEndpoint0Token(stat);
+ } else {
+ BdtEntry *const bdt_entry = MutableBdtEntryFromStat(stat);
+ const UsbPid pid = G_USB_BD_PID(bdt_entry->buffer_descriptor);
+ UsbFunction *const function = endpoint_mapping_[endpoint];
+ if (function == nullptr) {
+ // Should never happen, so stall if we do get here somehow.
+ StallEndpoint(endpoint);
+ } else {
+ switch (pid) {
+ case UsbPid::kOut:
+ function->HandleOutFinished(endpoint, bdt_entry);
+ break;
+
+ case UsbPid::kIn:
+ function->HandleInFinished(
+ endpoint, bdt_entry,
+ (stat & M_USB_STAT_ODD) ? EvenOdd::kOdd : EvenOdd::kEven);
+ break;
+
+ case UsbPid::kSetup:
+ default:
+ // Should never happen, so stall if we do get here somehow.
+ StallEndpoint(endpoint);
+ break;
+ }
+ }
+ }
+
+ USB0_ISTAT = USB_ISTAT_TOKDNE;
+ }
+
+ if (status & USB_ISTAT_USBRST) {
+ // Use DATA0 for all endpoints.
+ USB0_CTL = USB_CTL_ODDRST;
+ endpoint0_tx_odd_ = EvenOdd::kEven;
+ endpoint0_tx_toggle_ = Data01::kData0;
+
+ for (UsbFunction *function : functions_) {
+ function->HandleReset();
+ }
+
+ MutableBdtEntry(0, Direction::kRx, EvenOdd::kEven)->buffer_descriptor =
+ M_USB_BD_OWN | M_USB_BD_DTS | V_USB_BD_BC(kEndpoint0MaxSize);
+ MutableBdtEntry(0, Direction::kRx, EvenOdd::kEven)->address =
+ &endpoint0_receive_buffer_[0][0];
+
+ MutableBdtEntry(0, Direction::kRx, EvenOdd::kOdd)->buffer_descriptor =
+ M_USB_BD_OWN | M_USB_BD_DTS | V_USB_BD_BC(kEndpoint0MaxSize);
+ MutableBdtEntry(0, Direction::kRx, EvenOdd::kOdd)->address =
+ &endpoint0_receive_buffer_[1][0];
+
+ MutableBdtEntry(0, Direction::kTx, EvenOdd::kEven)->buffer_descriptor = 0;
+ MutableBdtEntry(0, Direction::kTx, EvenOdd::kOdd)->buffer_descriptor = 0;
+
+ USB0_ENDPT0 = USB_ENDPT_EPRXEN | USB_ENDPT_EPTXEN | USB_ENDPT_EPHSHK;
+
+ ClearInterrupts();
+
+ // Set the address to 0 for enumeration.
+ USB0_ADDR = 0;
+ new_address_ = 0;
+
+ endpoint0_data_ = nullptr;
+ endpoint0_data_left_ = 0;
+
+ USB0_INTEN = usb_enabled_interrupts();
+ USB0_ERREN = USB_ERREN_BTSERREN | USB_ERREN_DMAERREN |
+ USB_ERREN_BTOERREN | USB_ERREN_DFN8EN | USB_ERREN_CRC16EN |
+ USB_ERREN_CRC5EOFEN | USB_ERREN_PIDERREN;
+
+ // Start the peripheral going.
+ dma_memory_barrier();
+ USB0_CTL = USB_CTL_USBENSOFEN;
+
+ continue;
+ }
+
+ // TODO(Brian): Handle errors more intelligently.
+ if (status & USB_ISTAT_ERROR) {
+ const uint8_t error = USB0_ERRSTAT;
+ USB0_ERRSTAT = error;
+ USB0_ISTAT = USB_ISTAT_ERROR;
+ }
+ }
+}
+
+void UsbDevice::HandleEndpoint0Token(const uint8_t stat) {
+ BdtEntry *const bdt_entry = MutableBdtEntryFromStat(stat);
+ const UsbPid pid = G_USB_BD_PID(bdt_entry->buffer_descriptor);
+ switch (pid) {
+ case UsbPid::kSetup:
+ // Unstall it if it was previously stalled.
+ USB0_ENDPT0 = USB_ENDPT_EPRXEN | USB_ENDPT_EPTXEN | USB_ENDPT_EPHSHK;
+
+ SetupPacket setup_packet;
+ memcpy(&setup_packet, bdt_entry->address, sizeof(setup_packet));
+
+ // Give the buffer back now.
+ dma_memory_barrier();
+ // Next IN and OUT packet for this endpoint (data stage/status stage)
+ // should both be DATA1.
+ // TODO(Brian): Does this actually deal with received toggles correctly?
+ bdt_entry->buffer_descriptor =
+ M_USB_BD_OWN | M_USB_BD_DTS | V_USB_BD_BC(kEndpoint0MaxSize);
+ MutableBdtEntryFromStat(stat ^ M_USB_STAT_ODD)->buffer_descriptor =
+ M_USB_BD_OWN | M_USB_BD_DTS | V_USB_BD_BC(kEndpoint0MaxSize) |
+ M_USB_BD_DATA1;
+ endpoint0_tx_toggle_ = Data01::kData1;
+
+ // TODO(Brian): Tell the functions a new setup packet is starting.
+ // CdcTty: next_endpoint0_out_ = NextEndpoint0Out::kNone;
+
+ // Forget about any pending transactions on this endpoint. There shouldn't
+ // be any, so if we think there are something's out of sync and we should
+ // just drop it. Important to do this before clearing TXD_SUSPEND in
+ // USBx_CTL. Standard says "If a Setup transaction is received by an
+ // endpoint before a previously initiated control transfer is completed,
+ // the device must abort the current transfer/operation".
+ endpoint0_data_ = nullptr;
+ endpoint0_data_left_ = 0;
+ MutableBdtEntry(0, Direction::kTx, EvenOdd::kEven)->buffer_descriptor = 0;
+ MutableBdtEntry(0, Direction::kTx, EvenOdd::kOdd)->buffer_descriptor = 0;
+
+ HandleEndpoint0SetupPacket(setup_packet);
+
+ break;
+
+ case UsbPid::kOut:
+ for (UsbFunction *function : functions_) {
+ switch (function->HandleEndpoint0OutPacket(
+ bdt_entry->address, G_USB_BD_BC(bdt_entry->buffer_descriptor))) {
+ case SetupResponse::kIgnored:
+ break;
+ case SetupResponse::kHandled:
+ dma_memory_barrier();
+ bdt_entry->buffer_descriptor =
+ M_USB_BD_OWN | M_USB_BD_DTS | V_USB_BD_BC(kEndpoint0MaxSize);
+ return;
+ case SetupResponse::kStall:
+ bdt_entry->buffer_descriptor =
+ M_USB_BD_OWN | M_USB_BD_DTS | V_USB_BD_BC(kEndpoint0MaxSize);
+ StallEndpoint0();
+ return;
+ }
+ }
+ bdt_entry->buffer_descriptor =
+ M_USB_BD_OWN | M_USB_BD_DTS | V_USB_BD_BC(kEndpoint0MaxSize);
+ StallEndpoint0();
+ return;
+
+ case UsbPid::kIn:
+ // The functions are allowed to queue data in {endpoint0_data_,
+ // endpoint0_data_left_}, so this case deals with sending their data too.
+
+ // An IN transaction completed, so set up for the next one if appropriate.
+ if (!BufferEndpoint0TxPacket()) {
+ // After we're done, any further requests from the host should result in
+ // stalls (until the next setup token).
+ // TODO(Brian): Keep track of which direction it is and how much we've
+ // finished so we actually know when to stall it, both here and for
+ // kOut tokens.
+ //StallEndpoint0();
+ }
+
+ // If we have a new address, there is nothing left in the setup request
+ // besides a single IN packet forming the status stage, so we know the
+ // changes must be done now.
+ if (new_address_ != 0) {
+ USB0_ADDR = new_address_;
+ new_address_ = 0;
+ }
+
+ break;
+
+ default:
+ // Should never happen, but give the buffer back anyways if necessary.
+ if (!(bdt_entry->buffer_descriptor & M_USB_BD_OWN)) {
+ bdt_entry->buffer_descriptor =
+ M_USB_BD_OWN | M_USB_BD_DTS | V_USB_BD_BC(kEndpoint0MaxSize);
+ }
+ break;
+ }
+
+ // Clear the TXD_SUSPEND flag.
+ dma_memory_barrier();
+ USB0_CTL = USB_CTL_USBENSOFEN;
+}
+
+void UsbDevice::HandleEndpoint0SetupPacket(const SetupPacket &setup_packet) {
+ const bool in = setup_packet.request_type & M_SETUP_REQUEST_TYPE_IN;
+ const uint8_t recipient =
+ G_SETUP_REQUEST_TYPE_RECIPIENT(setup_packet.request_type);
+ switch (G_SETUP_REQUEST_TYPE_TYPE(setup_packet.request_type)) {
+ case SetupRequestType::kStandard:
+ switch (setup_packet.request) {
+ case standard_setup_requests::kSetAddress:
+ if (in || recipient != standard_setup_recipients::kDevice ||
+ setup_packet.index != 0 || setup_packet.length != 0) {
+ break;
+ }
+ new_address_ = setup_packet.value;
+ SendEmptyEndpoint0Packet();
+ return;
+
+ case standard_setup_requests::kSetConfiguration:
+ if (in || recipient != standard_setup_recipients::kDevice ||
+ setup_packet.index != 0 || setup_packet.length != 0) {
+ break;
+ }
+ configuration_ = setup_packet.value;
+
+ // No need to mess with endpoint0_tx_toggle_ because we reset it with
+ // each setup packet anyways.
+
+ for (int endpoint = 0;
+ endpoint < static_cast<int>(endpoint_mapping_.size());
+ ++endpoint) {
+ if (endpoint_mapping_[endpoint]) {
+ endpoint_mapping_[endpoint]->HandleConfigured(endpoint);
+ }
+ }
+
+ SendEmptyEndpoint0Packet();
+ return;
+
+ case standard_setup_requests::kClearFeature:
+ if (in || setup_packet.length != 0) {
+ break;
+ }
+ if (recipient == standard_setup_recipients::kEndpoint &&
+ setup_packet.value == standard_feature_selectors::kEndpointHalt) {
+ const int endpoint =
+ G_SETUP_REQUEST_INDEX_ENDPOINT(setup_packet.index);
+ // Our endpoint 0 doesn't support the halt feature because that's
+ // weird and not recommended by the standard.
+ if (endpoint == 0) {
+ break;
+ }
+ if (endpoint >= number_endpoints()) {
+ break;
+ }
+ USB0_ENDPTn(endpoint) &= ~USB_ENDPT_EPSTALL;
+ if (endpoint_mapping_[endpoint] != nullptr) {
+ endpoint_mapping_[endpoint]->HandleConfigured(endpoint);
+ }
+ SendEmptyEndpoint0Packet();
+ return;
+ }
+ // We should never get kDeviceRemoteWakeup because we don't advertise
+ // support for it in our configuration descriptors.
+ // We should never get kTestMode because we're not high-speed.
+ break;
+
+ case standard_setup_requests::kSetFeature:
+ if (in || setup_packet.length != 0) {
+ break;
+ }
+ if (recipient == standard_setup_recipients::kEndpoint &&
+ setup_packet.value == standard_feature_selectors::kEndpointHalt) {
+ const int endpoint =
+ G_SETUP_REQUEST_INDEX_ENDPOINT(setup_packet.index);
+ // Our endpoint 0 doesn't support the halt feature because that's
+ // weird and not recommended by the standard.
+ if (endpoint == 0) {
+ break;
+ }
+ if (endpoint >= number_endpoints()) {
+ break;
+ }
+ StallEndpoint(endpoint);
+ // TODO(Brian): Tell the appropriate function it's now stalled.
+ SendEmptyEndpoint0Packet();
+ return;
+ }
+ // We should never get kDeviceRemoteWakeup because we don't advertise
+ // support for it in our configuration descriptors.
+ // We should never get kTestMode because we're not high-speed.
+ break;
+
+ case standard_setup_requests::kGetConfiguration:
+ if (!in || recipient != standard_setup_recipients::kDevice ||
+ setup_packet.index != 0 || setup_packet.length != 1) {
+ break;
+ }
+ endpoint0_transmit_buffer_[0] = configuration_;
+ QueueEndpoint0Data(endpoint0_transmit_buffer_, 1);
+ return;
+
+ case standard_setup_requests::kGetInterface:
+ if (!in || recipient != standard_setup_recipients::kInterface ||
+ setup_packet.value != 0 || setup_packet.length != 1) {
+ break;
+ }
+ // Standard says it's unspecified in the default state and must
+ // respond with an error in the address state, so just do an error for
+ // both of them.
+ if (configuration_ == 0) {
+ break;
+ }
+ // TODO(Brian): Ask the appropriate function what alternate setting
+ // the interface has, and stall if there isn't one.
+ endpoint0_transmit_buffer_[0] = 0;
+ QueueEndpoint0Data(endpoint0_transmit_buffer_, 1);
+ return;
+
+ case standard_setup_requests::kSetInterface:
+ if (in || recipient != standard_setup_recipients::kInterface ||
+ setup_packet.length != 0) {
+ break;
+ }
+ // Standard says it's unspecified in the default state and must
+ // respond with an error in the address state, so just do an error for
+ // both of them.
+ if (configuration_ == 0) {
+ break;
+ }
+
+ // TODO(Brian): Pass to the appropriate function instead.
+ if (setup_packet.value != 0) {
+ break;
+ }
+ SendEmptyEndpoint0Packet();
+ return;
+
+ case standard_setup_requests::kGetStatus:
+ if (!in || setup_packet.value != 0 || setup_packet.length != 2) {
+ break;
+ }
+ if (recipient == standard_setup_recipients::kDevice) {
+ if (setup_packet.index != 0) {
+ break;
+ }
+ // Say that we're currently self powered.
+ endpoint0_transmit_buffer_[0] = 1;
+ endpoint0_transmit_buffer_[1] = 0;
+ QueueEndpoint0Data(endpoint0_transmit_buffer_, 2);
+ return;
+ }
+ if ((recipient == standard_setup_recipients::kInterface &&
+ setup_packet.index == 0) ||
+ (recipient == standard_setup_recipients::kEndpoint &&
+ G_SETUP_REQUEST_INDEX_ENDPOINT(setup_packet.index) == 0)) {
+ endpoint0_transmit_buffer_[0] = 0;
+ endpoint0_transmit_buffer_[1] = 0;
+ QueueEndpoint0Data(endpoint0_transmit_buffer_, 2);
+ return;
+ }
+ // Standard says it's unspecified in the default state and must
+ // respond with an error in the address state, so just do an error
+ // for both of them.
+ if (configuration_ == 0) {
+ break;
+ }
+
+ if (recipient == standard_setup_recipients::kInterface) {
+ // TODO(Brian): Check if it's actually an interface we have?
+ endpoint0_transmit_buffer_[0] = 0;
+ endpoint0_transmit_buffer_[1] = 0;
+ QueueEndpoint0Data(endpoint0_transmit_buffer_, 2);
+ return;
+ }
+
+ if (recipient == standard_setup_recipients::kEndpoint) {
+ const int endpoint =
+ G_SETUP_REQUEST_INDEX_ENDPOINT(setup_packet.index);
+ // TODO(Brian): Check if it's actually an endpoint we have?
+ if (USB0_ENDPTn(endpoint) & USB_ENDPT_EPSTALL) {
+ endpoint0_transmit_buffer_[0] = 1;
+ } else {
+ endpoint0_transmit_buffer_[0] = 0;
+ }
+ endpoint0_transmit_buffer_[1] = 0;
+ QueueEndpoint0Data(endpoint0_transmit_buffer_, 2);
+ return;
+ }
+ break;
+
+ case standard_setup_requests::kSetDescriptor:
+ // Not implementing anything for this.
+ break;
+
+ case standard_setup_requests::kSynchFrame:
+ // We don't implement any classes which use this.
+ break;
+
+ case standard_setup_requests::kGetDescriptor:
+ if (!in || recipient != standard_setup_recipients::kDevice) {
+ break;
+ }
+ const uint8_t descriptor_type_byte = (setup_packet.value >> 8) & 0xFF;
+ if (descriptor_type_byte < kUsbDescriptorTypeMin ||
+ descriptor_type_byte > kUsbDescriptorTypeMax) {
+ break;
+ }
+ const UsbDescriptorType descriptor_type =
+ static_cast<UsbDescriptorType>(descriptor_type_byte);
+ const uint8_t descriptor_index = setup_packet.value & 0xFF;
+ switch (descriptor_type) {
+ case UsbDescriptorType::kDevice:
+ if (setup_packet.index != 0 || descriptor_index != 0) {
+ break;
+ }
+ QueueEndpoint0Data(
+ device_descriptor_list_.data_.data(),
+ ::std::min<int>(setup_packet.length,
+ device_descriptor_list_.data_.size()));
+ return;
+
+ case UsbDescriptorType::kConfiguration:
+ if (setup_packet.index != 0 || descriptor_index != 0) {
+ break;
+ }
+ QueueEndpoint0Data(
+ config_descriptor_list_.data_.data(),
+ ::std::min<int>(setup_packet.length,
+ config_descriptor_list_.data_.size()));
+ return;
+
+ case UsbDescriptorType::kString:
+ if (descriptor_index != 0 && setup_packet.index != english_us_code()) {
+ break;
+ }
+ if (descriptor_index >= strings_.size()) {
+ break;
+ }
+ QueueEndpoint0Data(
+ strings_[descriptor_index].data(),
+ ::std::min<int>(setup_packet.length,
+ strings_[descriptor_index].size()));
+ return;
+
+ default:
+ // TODO(Brian): Handle other types of descriptor too.
+ break;
+ }
+ }
+ break;
+
+ default:
+ for (UsbFunction *function : functions_) {
+ switch (function->HandleEndpoint0SetupPacket(setup_packet)) {
+ case SetupResponse::kIgnored:
+ continue;
+ case SetupResponse::kHandled:
+ return;
+ case SetupResponse::kStall:
+ break;
+ }
+ break;
+ }
+ break;
+ }
+
+ StallEndpoint0();
+}
+
+// We're supposed to continue returning stalls until the next kSetup packet.
+// Code might continue putting stuff in the TX buffers, but the hardware won't
+// actually send it as long as the EPSTALL bit is set.
+void UsbDevice::StallEndpoint0() {
+ USB0_ENDPT0 = USB_ENDPT_EPSTALL | USB_ENDPT_EPRXEN | USB_ENDPT_EPTXEN |
+ USB_ENDPT_EPHSHK;
+}
+
+bool UsbDevice::BufferEndpoint0TxPacket() {
+ if (endpoint0_data_ == nullptr) {
+ return false;
+ }
+
+ const int to_transmit = ::std::min(endpoint0_data_left_, kEndpoint0MaxSize);
+ BdtEntry *const tx_bdt_entry =
+ MutableBdtEntry(0, Direction::kTx, endpoint0_tx_odd_);
+ // const_cast is safe because the hardware is only going to read from
+ // this, not write.
+ tx_bdt_entry->address =
+ const_cast<void *>(static_cast<const void *>(endpoint0_data_));
+ dma_memory_barrier();
+ tx_bdt_entry->buffer_descriptor =
+ V_USB_BD_BC(to_transmit) | static_cast<uint32_t>(endpoint0_tx_toggle_) |
+ M_USB_BD_OWN | M_USB_BD_DTS;
+
+ endpoint0_tx_odd_ = EvenOddInverse(endpoint0_tx_odd_);
+ endpoint0_tx_toggle_ = Data01Inverse(endpoint0_tx_toggle_);
+
+ endpoint0_data_ += to_transmit;
+ endpoint0_data_left_ -= to_transmit;
+ if (to_transmit < kEndpoint0MaxSize) {
+ endpoint0_data_ = nullptr;
+ }
+
+ return true;
+}
+
+void UsbDevice::SendEmptyEndpoint0Packet() {
+ // Really doesn't matter what we put here as long as it's not nullptr.
+ endpoint0_data_ = reinterpret_cast<char *>(this);
+ endpoint0_data_left_ = 0;
+ BufferEndpoint0TxPacket();
+}
+
+void UsbDevice::QueueEndpoint0Data(const char *data, int size) {
+ endpoint0_data_ = data;
+ endpoint0_data_left_ = size;
+ // There are 2 TX buffers, so fill them both up.
+ BufferEndpoint0TxPacket();
+ BufferEndpoint0TxPacket();
+}
+
+void UsbDevice::StallEndpoint(int endpoint) {
+ for (Direction direction : {Direction::kTx, Direction::kRx}) {
+ for (EvenOdd odd : {EvenOdd::kOdd, EvenOdd::kEven}) {
+ MutableBdtEntry(endpoint, direction, odd)->buffer_descriptor = 0;
+ dma_memory_barrier();
+ MutableBdtEntry(endpoint, direction, odd)->address = nullptr;
+ }
+ }
+ USB0_ENDPTn(endpoint) |= USB_ENDPT_EPSTALL;
+}
+
+void UsbDevice::ConfigureEndpointFor(int endpoint, bool rx, bool tx,
+ bool handshake) {
+ uint8_t control = 0;
+ if (rx) {
+ control |= USB_ENDPT_EPRXEN;
+ }
+ if (tx) {
+ control |= USB_ENDPT_EPTXEN;
+ }
+ if (handshake) {
+ control |= USB_ENDPT_EPHSHK;
+ }
+ USB0_ENDPTn(endpoint) = control;
+}
+
+int UsbFunction::AddEndpoint() {
+ const int r = device_->endpoint_mapping_.size();
+ assert(r < number_endpoints());
+ device_->endpoint_mapping_.push_back(this);
+ return r;
+}
+
+int UsbFunction::AddInterface() {
+ const int r = device_->interface_mapping_.size();
+ // bInterfaceNumber is only one byte.
+ assert(r < 255);
+ device_->interface_mapping_.push_back(this);
+ return r;
+}
+
+void UsbDevice::SetBdtEntry(int endpoint, Direction direction, EvenOdd odd,
+ BdtEntry bdt_entry) {
+ *MutableBdtEntry(endpoint, direction, odd) = bdt_entry;
+}
+
+} // namespace teensy
+} // namespace frc971
diff --git a/motors/usb/usb.h b/motors/usb/usb.h
new file mode 100644
index 0000000..1322ce1
--- /dev/null
+++ b/motors/usb/usb.h
@@ -0,0 +1,506 @@
+#ifndef MOTORS_USB_USB_H_
+#define MOTORS_USB_USB_H_
+
+#include <assert.h>
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "aos/common/macros.h"
+#include "motors/core/kinetis.h"
+#include "motors/usb/constants.h"
+
+namespace frc971 {
+namespace teensy {
+
+// A sufficient memory barrier between writing some data and telling the USB
+// hardware to read it or having the USB hardware say some data is readable and
+// actually reading it.
+static inline void dma_memory_barrier() {
+ __asm__ __volatile__("" :: : "memory");
+}
+
+// Aligned for faster access via memcpy etc.
+typedef void *DataPointer __attribute__((aligned(4)));
+
+// An entry in the Buffer Descriptor Table.
+struct BdtEntry {
+ uint32_t buffer_descriptor;
+ DataPointer address;
+};
+
+#define V_USB_BD_BC(value) \
+ static_cast<uint32_t>(static_cast<uint32_t>(value) << 16)
+#define G_USB_BD_BC(bd) (((bd) >> 16) & UINT32_C(0x3FF))
+#define M_USB_BD_OWN UINT32_C(1 << 7)
+#define M_USB_BD_DATA1 UINT32_C(1 << 6)
+static_assert(static_cast<uint32_t>(Data01::kData1) == M_USB_BD_DATA1,
+ "Wrong value");
+#define M_USB_BD_KEEP UINT32_C(1 << 5)
+#define M_USB_BD_NINC UINT32_C(1 << 4)
+#define M_USB_BD_DTS UINT32_C(1 << 3)
+#define M_USB_BD_STALL UINT32_C(1 << 2)
+#define V_USB_BD_PID(value) \
+ static_cast<uint32_t>(static_cast<uint32_t>(value) << 2)
+#define G_USB_BD_PID(bd) static_cast<UsbPid>(((bd) >> 2) & UINT32_C(0xF))
+
+#define G_USB_STAT_ENDP(stat) (((stat) >> 4) & UINT32_C(0xF))
+#define M_USB_STAT_TX UINT32_C(1 << 3)
+#define M_USB_STAT_ODD UINT32_C(1 << 2)
+
+// The various types of descriptors defined in the standard for retrieval via
+// GetDescriptor.
+static constexpr uint8_t kUsbDescriptorTypeMin = 1;
+static constexpr uint8_t kUsbDescriptorTypeMax = 11;
+enum class UsbDescriptorType : uint8_t {
+ kDevice = 1,
+ kConfiguration = 2,
+ kString = 3,
+ kInterface = 4,
+ kEndpoint = 5,
+ kDeviceQualifier = 6,
+ kOtherSpeedConfiguration = 7,
+ kInterfacePower = 8,
+ kOtg = 9,
+ kDebug = 10,
+ kInterfaceAssociation = 11,
+};
+
+// The class-specific descriptor types.
+enum class UsbClassDescriptorType : uint8_t {
+ kDevice = 0x21,
+ kConfiguration = 0x22,
+ kString = 0x23,
+ kInterface = 0x24,
+ kEndpoint = 0x25,
+};
+
+// The names of the setup request types from the standard.
+enum class SetupRequestType {
+ kStandard = 0,
+ kClass = 1,
+ kVendor = 2,
+ kReserved = 3,
+};
+
+// Set means device-to-host, clear means host-to-device.
+#define M_SETUP_REQUEST_TYPE_IN UINT8_C(1 << 7)
+#define G_SETUP_REQUEST_TYPE_TYPE(type) \
+ static_cast<SetupRequestType>(((type) >> 5) & UINT8_C(3))
+#define G_SETUP_REQUEST_TYPE_RECIPIENT(type) ((type)&UINT8_C(0x1F))
+#define G_SETUP_REQUEST_INDEX_ENDPOINT(index) ((index)&UINT8_C(0x7F))
+
+// The names of the standard recipients for setup requests.
+namespace standard_setup_recipients {
+constexpr int kDevice = 0;
+constexpr int kInterface = 1;
+constexpr int kEndpoint = 2;
+constexpr int kOther = 3;
+} // namespace standard_setup_recipients
+
+class UsbFunction;
+
+// Allows building up a list of descriptors. This supports a much nicer API than
+// the usual "hard-code a char[] with all the sizes and offsets at compile
+// time". Space for each descriptor is reserved, and then it may be filled out
+// from beginning to end at any time.
+//
+// An instance is the thing that the GetDescriptor operation sends to the host.
+// This is not the concept that the core and class standards call "Foo
+// Descriptor" etc; see Descriptor for that.
+class UsbDescriptorList {
+ public:
+ // Represents a single descriptor. All of the contents must be written before
+ // this object is destroyed.
+ //
+ // Create one via UsbDescriptorList::CreateDescriptor.
+ class Descriptor {
+ public:
+ // All of the allocated space must be filled first.
+ ~Descriptor() {
+ if (descriptor_list_ == nullptr) {
+ return;
+ }
+ // Verify we wrote all the bytes first.
+ assert(next_index_ == end_index_);
+ --descriptor_list_->open_descriptors_;
+ }
+
+ void AddUint16(uint16_t value) {
+ AddByte(value & 0xFF);
+ AddByte((value >> 8) & 0xFF);
+ }
+
+ void AddByte(uint8_t value) {
+ assert(next_index_ < end_index_);
+ data()[next_index_] = value;
+ ++next_index_;
+ }
+
+ // Overwrites an already-written byte.
+ void SetByte(int index, uint8_t value) {
+ assert(index + start_index_ < end_index_);
+ data()[index + start_index_] = value;
+ }
+
+ private:
+ Descriptor(UsbDescriptorList *descriptor_list, int start_index,
+ int end_index)
+ : descriptor_list_(descriptor_list),
+ start_index_(start_index),
+ end_index_(end_index),
+ next_index_(start_index_) {}
+
+ char *data() const {
+ return &descriptor_list_->data_[0];
+ }
+
+ UsbDescriptorList *const descriptor_list_;
+ const int start_index_, end_index_;
+ int next_index_;
+
+ friend class UsbDescriptorList;
+
+ DISALLOW_COPY_AND_ASSIGN(Descriptor);
+ };
+
+ UsbDescriptorList() = default;
+ ~UsbDescriptorList() = default;
+
+ // Creates a new descriptor at the end of the list.
+ // length is the number of bytes, including the length byte.
+ // descriptor_type is the descriptor type, which is the second byte after the
+ // length.
+ ::std::unique_ptr<Descriptor> CreateDescriptor(
+ uint8_t length, UsbDescriptorType descriptor_type) {
+ return CreateDescriptor(length, static_cast<uint8_t>(descriptor_type));
+ }
+
+ ::std::unique_ptr<Descriptor> CreateDescriptor(
+ uint8_t length, UsbClassDescriptorType descriptor_type) {
+ assert(data_.size() > 0);
+ return CreateDescriptor(length, static_cast<uint8_t>(descriptor_type));
+ }
+
+ void CheckFinished() const { assert(open_descriptors_ == 0); }
+
+ int CurrentSize() const { return data_.size(); }
+
+ private:
+ ::std::unique_ptr<Descriptor> CreateDescriptor(uint8_t length,
+ uint8_t descriptor_type) {
+ const int start_index = data_.size();
+ const int end_index = start_index + length;
+ data_.resize(end_index);
+ ++open_descriptors_;
+ auto r = ::std::unique_ptr<Descriptor>(
+ new Descriptor(this, start_index, end_index));
+ r->AddByte(length); // bLength
+ r->AddByte(descriptor_type); // bDescriptorType
+ return r;
+ }
+
+ int open_descriptors_ = 0;
+
+ ::std::string data_;
+
+ friend class UsbDevice;
+
+ DISALLOW_COPY_AND_ASSIGN(UsbDescriptorList);
+};
+
+extern "C" void usb_isr(void);
+
+// USB state events are managed by asking each function if it wants to handle
+// them, sequentially. For the small number of functions which can be
+// practically supported with the limited number of endpoints, this performs
+// better than fancier things like hash maps.
+
+// Manages one of the Teensy's USB peripherals as a USB slave device.
+//
+// This supports being a composite device with multiple functions.
+//
+// Attaching functions etc is called "setup", and must be completed before
+// Initialize() is called.
+//
+// Detaching functions is called "teardown" and must happen after Shutdown().
+// TODO(Brian): Implement Shutdown().
+class UsbDevice final {
+ public:
+ // Represents the data that comes with a UsbPid::kSetup.
+ // Note that the order etc is important because we memcpy into this.
+ struct SetupPacket {
+ uint8_t request_type; // bmRequestType
+ uint8_t request; // bRequest
+ uint16_t value; // wValue
+ uint16_t index; // wIndex
+ uint16_t length; // wLength
+ } __attribute__((aligned(4)));
+ static_assert(sizeof(SetupPacket) == 8, "wrong size");
+
+ enum class SetupResponse {
+ // Indicates this function doesn't recognize the setup packet.
+ kIgnored,
+
+ // Indicates the endpoint should be stalled.
+ //
+ // Don't return this if the packet is for another function.
+ kStall,
+
+ // Indicates this setup packet was handled. Functions must avoid eating
+ // packets intended for other functions.
+ kHandled,
+ };
+
+ static constexpr int kEndpoint0MaxSize = 64;
+
+ // The only language code we support.
+ static constexpr uint16_t english_us_code() { return 0x0409; }
+
+ UsbDevice(int index, uint16_t vendor_id, uint16_t product_id);
+ ~UsbDevice();
+
+ // Ends setup and starts being an actual USB device.
+ void Initialize();
+
+ // Adds a string to the table and returns its index.
+ //
+ // For simplicity, we only support strings with english_us_code().
+ //
+ // May only be called during setup.
+ int AddString(const ::std::string &string) {
+ assert(!is_set_up_);
+ const int r = strings_.size();
+ strings_.emplace_back(string.size() * 2 + 2, '\0');
+ strings_.back()[0] = 2 + string.size() * 2;
+ strings_.back()[1] = static_cast<uint8_t>(UsbDescriptorType::kString);
+ for (size_t i = 0; i < string.size(); ++i) {
+ strings_.back()[i * 2 + 2] = string[i];
+ }
+ return r;
+ }
+
+ // Sets the manufacturer string.
+ //
+ // May only be called during setup.
+ void SetManufacturer(const ::std::string &string) {
+ device_descriptor_->SetByte(14, AddString(string)); // iManufacturer
+ }
+
+ // Sets the product string.
+ //
+ // May only be called during setup.
+ void SetProduct(const ::std::string &string) {
+ device_descriptor_->SetByte(15, AddString(string)); // iProduct
+ }
+
+ // Sets the serial number string.
+ //
+ // May only be called during setup.
+ void SetSerialNumber(const ::std::string &string) {
+ device_descriptor_->SetByte(16, AddString(string)); // iSerialNumber
+ }
+
+ // Queues up an empty IN packet for endpoint 0. This is a common way to
+ // respond to various kinds of configuration commands.
+ //
+ // This may only be called from the appropriate function callbacks.
+ void SendEmptyEndpoint0Packet();
+
+ // Queues some data to send on endpoint 0. This includes putting the initial
+ // packets into the TX buffers.
+ //
+ // This may only be called from the appropriate function callbacks.
+ void QueueEndpoint0Data(const char *data, int size);
+
+ // Stalls an endpoint until it's cleared.
+ //
+ // This should only be called by or on behalf of the function which owns
+ // endpoint.
+ void StallEndpoint(int endpoint);
+
+ // Configures an endpoint to send and/or receive, with or without DATA0/DATA1
+ // handshaking. handshake should probably be true for everything except
+ // isochronous endpoints.
+ //
+ // This should only be called by or on behalf of the function which owns
+ // endpoint.
+ void ConfigureEndpointFor(int endpoint, bool rx, bool tx, bool handshake);
+
+ void SetBdtEntry(int endpoint, Direction direction, EvenOdd odd,
+ BdtEntry bdt_entry);
+
+ private:
+ // Clears all pending interrupts.
+ void ClearInterrupts();
+
+ // Deals with an interrupt that has occured.
+ void HandleInterrupt();
+
+ // Processes a token on endpoint 0.
+ void HandleEndpoint0Token(uint8_t stat);
+
+ // Processes a setup packet on endpoint 0.
+ void HandleEndpoint0SetupPacket(const SetupPacket &setup_packet);
+
+ // Sets endpoint 0 to return STALL tokens. We clear this condition upon
+ // receiving the next SETUP token.
+ void StallEndpoint0();
+
+ // Places the first packet from {endpoint0_data_, endpoint0_data_left_} into
+ // the TX buffers (if there is any data). This may only be called when the
+ // next TX buffer is empty.
+ bool BufferEndpoint0TxPacket();
+
+ // Which USB peripheral this is.
+ const int index_;
+
+ // The string descriptors in order.
+ ::std::vector<::std::string> strings_;
+
+ // TODO(Brian): Refactor into something more generic, because I think this is
+ // shared with all non-isochronous endpoints?
+ Data01 endpoint0_tx_toggle_;
+ EvenOdd endpoint0_tx_odd_;
+ uint8_t endpoint0_receive_buffer_[2][kEndpoint0MaxSize]
+ __attribute__((aligned(4)));
+
+ // A temporary buffer for holding data to transmit on endpoint 0. Sometimes
+ // this is used and sometimes the data is sent directly from some other
+ // location (like for descriptors).
+ char endpoint0_transmit_buffer_[kEndpoint0MaxSize];
+
+ // The data we're waiting to send from endpoint 0. The data must remain
+ // constant until this transmission is done.
+ //
+ // When overwriting this, we ignore if it's already non-nullptr. The host is
+ // supposed to read all of the data before asking for more. If it doesn't do
+ // that, it will just get garbage data because it's unclear what it expects.
+ //
+ // Do note that endpoint0_data_ != nullptr && endpoint0_data_left_ == 0 is an
+ // important state. This means we're going to return a 0-length packet the
+ // next time the host asks. However, depending on the length it asked for,
+ // that might never happen.
+ const char *endpoint0_data_ = nullptr;
+ int endpoint0_data_left_ = 0;
+
+ // If non-0, the new address we're going to start using once the status stage
+ // of the current setup request is finished.
+ uint16_t new_address_ = 0;
+
+ UsbDescriptorList device_descriptor_list_;
+ UsbDescriptorList config_descriptor_list_;
+
+ ::std::unique_ptr<UsbDescriptorList::Descriptor> device_descriptor_,
+ config_descriptor_;
+
+ int configuration_ = 0;
+
+ bool is_set_up_ = false;
+
+ // The function which owns each endpoint.
+ ::std::vector<UsbFunction *> endpoint_mapping_;
+ // The function which owns each interface.
+ ::std::vector<UsbFunction *> interface_mapping_;
+ // All of the functions (without duplicates).
+ ::std::vector<UsbFunction *> functions_;
+
+ friend void usb_isr(void);
+ friend class UsbFunction;
+};
+
+// Represents a USB function. This consists of a set of descriptors and
+// interfaces.
+//
+// Each instance is a single function, so there can be multiple instances of the
+// same subclass in the same devices (ie two serial ports).
+class UsbFunction {
+ public:
+ UsbFunction(UsbDevice *device) : device_(device) {
+ device_->functions_.push_back(this);
+ }
+ virtual ~UsbFunction() = default;
+
+ protected:
+ using SetupResponse = UsbDevice::SetupResponse;
+
+ static constexpr uint8_t iad_descriptor_length() { return 8; }
+ static constexpr uint8_t interface_descriptor_length() { return 9; }
+ static constexpr uint8_t endpoint_descriptor_length() { return 7; }
+
+ static constexpr uint8_t m_endpoint_address_in() { return 1 << 7; }
+ static constexpr uint8_t m_endpoint_attributes_control() { return 0x00; }
+ static constexpr uint8_t m_endpoint_attributes_isochronous() { return 0x01; }
+ static constexpr uint8_t m_endpoint_attributes_bulk() { return 0x03; }
+ static constexpr uint8_t m_endpoint_attributes_interrupt() { return 0x03; }
+
+ // Adds a new endpoint and returns its index.
+ //
+ // Note that at least one descriptor for this newly created endpoint must be
+ // added via CreateConfigDescriptor.
+ //
+ // TODO(Brian): Does this hardware actually only support a single direction
+ // per endpoint number, or can it get a total of 30 endpoints max?
+ //
+ // May only be called during setup.
+ int AddEndpoint();
+
+ // Adds a new interface and returns its index.
+ //
+ // You'll probably want to put this new interface in at least one descriptor
+ // added via CreateConfigDescriptor.
+ //
+ // May only be called during setup.
+ int AddInterface();
+
+ // Adds a new descriptor in the configuration descriptor list. See
+ // UsbDescriptorList::CreateDescriptor for details.
+ //
+ // Note that the order of calls to this is highly significant. In general,
+ // this should only be called from Initialize().
+ //
+ // May only be called during setup.
+ template <typename T>
+ ::std::unique_ptr<UsbDescriptorList::Descriptor> CreateDescriptor(
+ uint8_t length, T descriptor_type) {
+ return device_->config_descriptor_list_.CreateDescriptor(length,
+ descriptor_type);
+ }
+
+ UsbDevice *device() const { return device_; }
+
+ private:
+ virtual void Initialize() = 0;
+
+ virtual SetupResponse HandleEndpoint0SetupPacket(
+ const UsbDevice::SetupPacket & /*setup_packet*/) {
+ return SetupResponse::kIgnored;
+ }
+
+ virtual SetupResponse HandleEndpoint0OutPacket(void * /*data*/,
+ int /*data_length*/) {
+ return SetupResponse::kIgnored;
+ }
+
+ virtual void HandleOutFinished(int endpoint, BdtEntry *bdt_entry) = 0;
+ virtual void HandleInFinished(int endpoint, BdtEntry *bdt_entry,
+ EvenOdd odd) = 0;
+
+ // Called when a given interface is configured (aka "experiences a
+ // configuration event"). This means all rx and tx buffers have been cleared
+ // and should be filled as appropriate, starting from data0. Also,
+ // ConfigureEndpointFor should be called with the appropriate arguments.
+ virtual void HandleConfigured(int endpoint) = 0;
+
+ // Should reset everything to use the even buffers next.
+ virtual void HandleReset() = 0;
+
+ UsbDevice *const device_;
+
+ friend class UsbDevice;
+};
+
+} // namespace teensy
+} // namespace frc971
+
+#endif // MOTORS_USB_USB_H_
diff --git a/motors/util.h b/motors/util.h
index 05edd9d..b215996 100644
--- a/motors/util.h
+++ b/motors/util.h
@@ -147,6 +147,8 @@
#define dma_chN_isr(n) DO_CONCATENATE(dma_ch, n, _isr)
#define IRQ_DMA_CHn(n) DO_CONCATENATE(IRQ_DMA, _CH, n)
+#define USB0_ENDPTn(n) (*(volatile uint8_t *)(0x400720C0 + ((n)*4)))
+
#ifdef __cplusplus
}
#endif
diff --git a/tools/ci/run-tests.sh b/tools/ci/run-tests.sh
index 98762e7..cac8dce 100755
--- a/tools/ci/run-tests.sh
+++ b/tools/ci/run-tests.sh
@@ -3,3 +3,4 @@
bazel --batch test -c opt --curses=no --color=no --jobs=1 //...
bazel --batch build -c opt --curses=no --color=no --jobs=1 //... --cpu=roborio
+bazel --batch build -c opt --curses=no --color=no --jobs=1 //motors/... --cpu=cortex-m4f
diff --git a/tools/cpp/CROSSTOOL b/tools/cpp/CROSSTOOL
index 7e6efdb..6e0c158 100644
--- a/tools/cpp/CROSSTOOL
+++ b/tools/cpp/CROSSTOOL
@@ -242,7 +242,6 @@
compiler_flag: "-Wall"
compiler_flag: "-Wextra"
- compiler_flag: "-Wswitch-enum"
compiler_flag: "-Wpointer-arith"
compiler_flag: "-Wstrict-aliasing"
compiler_flag: "-Wcast-qual"
@@ -489,7 +488,6 @@
compiler_flag: "-Wall"
compiler_flag: "-Wextra"
- compiler_flag: "-Wswitch-enum"
compiler_flag: "-Wpointer-arith"
compiler_flag: "-Wstrict-aliasing"
compiler_flag: "-Wcast-qual"
@@ -744,7 +742,6 @@
compiler_flag: "-Wall"
compiler_flag: "-Wextra"
- compiler_flag: "-Wswitch-enum"
compiler_flag: "-Wpointer-arith"
compiler_flag: "-Wstrict-aliasing"
compiler_flag: "-Wcast-qual"
@@ -889,10 +886,17 @@
compiler_flag: "-D__STDC_FORMAT_MACROS"
compiler_flag: "-D__STDC_CONSTANT_MACROS"
compiler_flag: "-D__STDC_LIMIT_MACROS"
+
+ # Some identifiers for what MCU we're using.
compiler_flag: "-D__MK64FX512__"
compiler_flag: "-DF_CPU=120000000"
+
compiler_flag: "-Wl,--gc-sections"
+ # Newlib's stdint.h does this, but GCC's freestanding stdint.h doesn't use
+ # newlib's so we have to do it manually...
+ compiler_flag: "-D__have_long32"
+
# Make C++ compilation deterministic. Use linkstamping instead of these
# compiler symbols.
unfiltered_cxx_flag: "-Wno-builtin-macro-redefined"
@@ -925,10 +929,10 @@
linker_flag: "-Tmotors/core/mk64fx512.ld"
compiler_flag: "-fmessage-length=80"
+ compiler_flag: "-fmax-errors=20"
compiler_flag: "-Wall"
compiler_flag: "-Wextra"
- compiler_flag: "-Wswitch-enum"
compiler_flag: "-Wpointer-arith"
compiler_flag: "-Wcast-qual"
compiler_flag: "-Wwrite-strings"
@@ -953,10 +957,16 @@
# Enable debug symbols.
compiler_flag: "-g"
- # Commons symbols are weird and not what we want, so just give multiple
+ # Common symbols are weird and not what we want, so just give multiple
# declaration errors instead.
compiler_flag: "-fno-common"
+ # We're not a hosted environment (no file IO, main is called from our code,
+ # etc).
+ compiler_flag: "-ffreestanding"
+ # However, we still want to optimize things like memcpy.
+ compiler_flag: "-fbuiltin"
+
compilation_mode_flags {
mode: OPT
@@ -993,13 +1003,16 @@
action: 'c++-header-preprocessing'
action: 'c++-module-compile'
flag_group {
+ iterate_over: 'quote_include_paths'
flag: '-iquote'
flag: '%{quote_include_paths}'
}
flag_group {
+ iterate_over: 'include_paths'
flag: '-I%{include_paths}'
}
flag_group {
+ iterate_over: 'system_include_paths'
flag: '-iquote'
flag: '%{system_include_paths}'
}
diff --git a/tools/environments.bzl b/tools/environments.bzl
new file mode 100644
index 0000000..68e78de
--- /dev/null
+++ b/tools/environments.bzl
@@ -0,0 +1,5 @@
+# Contains some helpers for working with environments.
+
+mcu_cpus = [
+ "@//tools:cortex-m4f",
+]
diff --git a/y2012/control_loops/drivetrain/drivetrain_base.cc b/y2012/control_loops/drivetrain/drivetrain_base.cc
index 8748d4d..aa8f9e4 100644
--- a/y2012/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_base.cc
@@ -15,7 +15,7 @@
using ::frc971::constants::ShifterHallEffect;
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
diff --git a/y2014/constants.cc b/y2014/constants.cc
index d5a29be..8354e57 100644
--- a/y2014/constants.cc
+++ b/y2014/constants.cc
@@ -39,11 +39,11 @@
const double kPracticeLowGearRatio = kCompLowGearRatio;
const double kPracticeHighGearRatio = kCompHighGearRatio;
-const ShifterHallEffect kCompLeftDriveShifter{2.61, 2.33, 4.25, 3.28, 0.2, 0.7};
-const ShifterHallEffect kCompRightDriveShifter{2.94, 4.31, 4.32, 3.25, 0.2, 0.7};
+const DualHallShifterHallEffect kCompLeftDriveShifter{{2.33, 4.25, 0.2, 0.7}, 2.61, 3.28};
+const DualHallShifterHallEffect kCompRightDriveShifter{{4.31, 4.32, 0.2, 0.7}, 2.94, 3.25};
-const ShifterHallEffect kPracticeLeftDriveShifter{2.80, 3.05, 4.15, 3.2, 0.2, 0.7};
-const ShifterHallEffect kPracticeRightDriveShifter{2.90, 3.75, 3.80, 2.98, 0.2, 0.7};
+const DualHallShifterHallEffect kPracticeLeftDriveShifter{{3.05, 4.15, 0.2, 0.7}, 2.80, 3.2};
+const DualHallShifterHallEffect kPracticeRightDriveShifter{{3.75, 3.80, 0.2, 0.7}, 2.90, 2.98};
const double shooter_zeroing_speed = 0.05;
const double shooter_unload_speed = 0.08;
diff --git a/y2014/constants.h b/y2014/constants.h
index 7fb2507..4ce3714 100644
--- a/y2014/constants.h
+++ b/y2014/constants.h
@@ -9,7 +9,7 @@
namespace y2014 {
namespace constants {
-using ::frc971::constants::ShifterHallEffect;
+using ::frc971::constants::DualHallShifterHallEffect;
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
@@ -35,7 +35,7 @@
// gear.
double low_gear_ratio;
double high_gear_ratio;
- ShifterHallEffect left_drive, right_drive;
+ DualHallShifterHallEffect left_drive, right_drive;
bool clutch_transmission;
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
index ffb0ee3..2c3a6f8 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -30,8 +30,8 @@
constants::GetValues().high_gear_ratio,
constants::GetValues().low_gear_ratio,
- constants::GetValues().left_drive,
- constants::GetValues().right_drive,
+ constants::GetValues().left_drive.shifter_hall_effect,
+ constants::GetValues().right_drive.shifter_hall_effect,
true,
0,
0.25,
diff --git a/y2014/wpilib_interface.cc b/y2014/wpilib_interface.cc
index b5dee33..a130544 100644
--- a/y2014/wpilib_interface.cc
+++ b/y2014/wpilib_interface.cc
@@ -86,14 +86,16 @@
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
}
-float hall_translate(const constants::ShifterHallEffect &k, float in_low,
+float hall_translate(const constants::DualHallShifterHallEffect &k, float in_low,
float in_high) {
const float low_ratio =
- 0.5 * (in_low - static_cast<float>(k.low_gear_low)) /
- static_cast<float>(k.low_gear_middle - k.low_gear_low);
+ 0.5 * (in_low - static_cast<float>(k.shifter_hall_effect.low_gear_low)) /
+ static_cast<float>(k.low_gear_middle - k.shifter_hall_effect.low_gear_low);
const float high_ratio =
- 0.5 + 0.5 * (in_high - static_cast<float>(k.high_gear_middle)) /
- static_cast<float>(k.high_gear_high - k.high_gear_middle);
+ 0.5 +
+ 0.5 * (in_high - static_cast<float>(k.high_gear_middle)) /
+ static_cast<float>(k.shifter_hall_effect.high_gear_high -
+ k.high_gear_middle);
// Return low when we are below 1/2, and high when we are above 1/2.
if (low_ratio + high_ratio < 1.0) {
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
index afb5375..46fcb71 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -15,7 +15,7 @@
using ::frc971::constants::ShifterHallEffect;
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
diff --git a/y2015/constants.cc b/y2015/constants.cc
index ce78344..0ce6ef1 100644
--- a/y2015/constants.cc
+++ b/y2015/constants.cc
@@ -37,14 +37,14 @@
const double kHighGearRatio = kLowGearRatio;
const ::frc971::constants::ShifterHallEffect kCompRightDriveShifter{
- 555, 657, 660, 560, 0.2, 0.7};
+ 0.0, 0.0, 0.2, 0.7};
const ::frc971::constants::ShifterHallEffect kCompLeftDriveShifter{
- 555, 660, 644, 552, 0.2, 0.7};
+ 0.0, 0.0, 0.2, 0.7};
const ::frc971::constants::ShifterHallEffect kPracticeRightDriveShifter{
- 2.95, 3.95, 3.95, 2.95, 0.2, 0.7};
+ 0.0, 0.0, 0.2, 0.7};
const ::frc971::constants::ShifterHallEffect kPracticeLeftDriveShifter{
- 2.95, 4.2, 3.95, 3.0, 0.2, 0.7};
+ 0.0, 0.0, 0.2, 0.7};
const double kToteHeight = 0.3;
// Set by Daniel on 2/13/15.
diff --git a/y2015/control_loops/drivetrain/drivetrain_base.cc b/y2015/control_loops/drivetrain/drivetrain_base.cc
index 26a283b..30e25f8 100644
--- a/y2015/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2015/control_loops/drivetrain/drivetrain_base.cc
@@ -15,7 +15,7 @@
using ::frc971::constants::ShifterHallEffect;
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc
index f7c3d2f..3af05b9 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -15,7 +15,7 @@
using ::frc971::constants::ShifterHallEffect;
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_base.h b/y2015_bot3/control_loops/drivetrain/drivetrain_base.h
index 130d549..1614b88 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -15,10 +15,10 @@
kDrivetrainEncoderRatio * 18.0 / 60.0;
constexpr double kDrivetrainLowGearRatio = kDrivetrainHighGearRatio;
const bool kDrivetrainClutchTransmission = false;
-const ::frc971::constants::ShifterHallEffect kDrivetrainRightShifter{
- 555, 657, 660, 560, 0.2, 0.7};
-const ::frc971::constants::ShifterHallEffect kDrivetrainLeftShifter{
- 555, 660, 644, 552, 0.2, 0.7};
+const ::frc971::constants::ShifterHallEffect kDrivetrainRightShifter{0.0, 0.0,
+ 0.2, 0.7};
+const ::frc971::constants::ShifterHallEffect kDrivetrainLeftShifter{0.0, 0.0,
+ 0.2, 0.7};
// End constants
const ::frc971::control_loops::drivetrain::DrivetrainConfig &
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index f66d963..3f15694 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -16,7 +16,7 @@
using ::frc971::constants::ShifterHallEffect;
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
diff --git a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
index 26a551b..7a99ff3 100644
--- a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -15,7 +15,7 @@
using ::frc971::constants::ShifterHallEffect;
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
diff --git a/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
index 54a7321..1682f30 100644
--- a/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
@@ -15,7 +15,7 @@
using ::frc971::constants::ShifterHallEffect;
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.cc b/y2017/control_loops/drivetrain/drivetrain_base.cc
index 3367fc1..9ff357b 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_base.cc
@@ -16,7 +16,7 @@
using ::frc971::constants::ShifterHallEffect;
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
diff --git a/y2017_bot3/BUILD b/y2017_bot3/BUILD
new file mode 100644
index 0000000..c1dab0f
--- /dev/null
+++ b/y2017_bot3/BUILD
@@ -0,0 +1,64 @@
+load('/aos/downloader/downloader', 'aos_downloader')
+
+cc_binary(
+ name = 'wpilib_interface',
+ srcs = [
+ 'wpilib_interface.cc',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop',
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/messages:robot_state',
+ '//aos/common/util:log_interval',
+ '//aos/common/util:phased_loop',
+ '//aos/common:math',
+ '//aos/common:stl_mutex',
+ '//aos/common:time',
+ '//aos/linux_code:init',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops:queues',
+ '//frc971/wpilib:ADIS16448',
+ '//frc971/wpilib:buffered_pcm',
+ '//frc971/wpilib:dma',
+ '//frc971/wpilib:dma_edge_counting',
+ '//frc971/wpilib:encoder_and_potentiometer',
+ '//frc971/wpilib:gyro_sender',
+ '//frc971/wpilib:interrupt_edge_counting',
+ '//frc971/wpilib:joystick_sender',
+ '//frc971/wpilib:logging_queue',
+ '//frc971/wpilib:loop_output_handler',
+ '//frc971/wpilib:pdp_fetcher',
+ '//frc971/wpilib:wpilib_interface',
+ '//frc971/wpilib:wpilib_robot_base',
+ '//third_party:wpilib',
+ '//y2017_bot3/control_loops/drivetrain:polydrivetrain_plants',
+ ],
+ restricted_to = ['//tools:roborio'],
+)
+
+aos_downloader(
+ name = 'download',
+ start_srcs = [
+ '//aos:prime_start_binaries',
+ '//y2017_bot3/control_loops/drivetrain:drivetrain',
+ ':wpilib_interface',
+ ],
+ srcs = [
+ '//aos:prime_binaries',
+ ],
+ restricted_to = ['//tools:roborio'],
+)
+
+aos_downloader(
+ name = 'download_stripped',
+ start_srcs = [
+ '//aos:prime_start_binaries_stripped',
+ '//y2017_bot3/control_loops/drivetrain:drivetrain.stripped',
+ ':wpilib_interface.stripped',
+ ],
+ srcs = [
+ '//aos:prime_binaries_stripped',
+ ],
+ restricted_to = ['//tools:roborio'],
+)
diff --git a/y2017_bot3/control_loops/drivetrain/BUILD b/y2017_bot3/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..9f37ffc
--- /dev/null
+++ b/y2017_bot3/control_loops/drivetrain/BUILD
@@ -0,0 +1,77 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+genrule(
+ name = 'genrule_drivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017_bot3/control_loops/python:drivetrain) $(OUTS)',
+ tools = [
+ '//y2017_bot3/control_loops/python:drivetrain',
+ ],
+ outs = [
+ 'drivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.cc',
+ ],
+)
+
+genrule(
+ name = 'genrule_polydrivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017_bot3/control_loops/python:polydrivetrain) $(OUTS)',
+ tools = [
+ '//y2017_bot3/control_loops/python:polydrivetrain',
+ ],
+ outs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'polydrivetrain_cim_plant.h',
+ 'polydrivetrain_cim_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'polydrivetrain_plants',
+ srcs = [
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.cc',
+ ],
+ hdrs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'drivetrain_base',
+ srcs = [
+ 'drivetrain_base.cc',
+ ],
+ hdrs = [
+ 'drivetrain_base.h',
+ ],
+ deps = [
+ ':polydrivetrain_plants',
+ '//frc971/control_loops/drivetrain:drivetrain_config',
+ '//frc971:shifter_hall_effect',
+ ],
+)
+
+cc_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain_main.cc',
+ ],
+ deps = [
+ ':drivetrain_base',
+ '//aos/linux_code:init',
+ '//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
new file mode 100644
index 0000000..9c3e66e
--- /dev/null
+++ b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,46 @@
+#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
+
+#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/polydrivetrain_dog_motor_plant.h"
+#include "y2017_bot3/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+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 &GetDrivetrainConfig() {
+ static DrivetrainConfig kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+
+ ::y2017_bot3::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2017_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2017_bot3::control_loops::drivetrain::MakeKFDrivetrainLoop,
+
+ drivetrain::kDt, drivetrain::kRobotRadius, drivetrain::kWheelRadius,
+ drivetrain::kV,
+
+ drivetrain::kHighGearRatio, drivetrain::kHighGearRatio,
+ 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 */
+ };
+
+ 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
new file mode 100644
index 0000000..88f93c9
--- /dev/null
+++ b/y2017_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#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 &
+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
new file mode 100644
index 0000000..3c65c7b
--- /dev/null
+++ b/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,15 @@
+#include "aos/linux_code/init.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();
+ DrivetrainLoop drivetrain(
+ ::y2017_bot3::control_loops::drivetrain::GetDrivetrainConfig());
+ drivetrain.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2017_bot3/control_loops/python/BUILD b/y2017_bot3/control_loops/python/BUILD
new file mode 100644
index 0000000..ab4b383
--- /dev/null
+++ b/y2017_bot3/control_loops/python/BUILD
@@ -0,0 +1,42 @@
+package(default_visibility = ['//y2017_bot3:__subpackages__'])
+
+py_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+ restricted_to = ['//tools:k8'],
+)
+
+py_binary(
+ name = 'polydrivetrain',
+ srcs = [
+ 'polydrivetrain.py',
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+ restricted_to = ['//tools:k8'],
+)
+
+py_library(
+ name = 'polydrivetrain_lib',
+ srcs = [
+ 'polydrivetrain.py',
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+ restricted_to = ['//tools:k8'],
+)
diff --git a/y2017_bot3/control_loops/python/drivetrain.py b/y2017_bot3/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..846fc39
--- /dev/null
+++ b/y2017_bot3/control_loops/python/drivetrain.py
@@ -0,0 +1,360 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+
+class Drivetrain(control_loop.ControlLoop):
+ def __init__(self, name="Drivetrain", left_low=True, right_low=True):
+ super(Drivetrain, self).__init__(name)
+ # Number of motors per side
+ self.num_motors = 2
+ # Stall Torque in N m
+ self.stall_torque = 2.42 * self.num_motors * 0.60
+ # Stall Current in Amps
+ self.stall_current = 133.0 * self.num_motors
+ self.free_speed_rpm = 5500.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60
+ # Free Current in Amps
+ self.free_current = 2.7 * self.num_motors
+ #TODO(Neil): Update robot moment of inertia, mass, and robot radius
+ # Moment of inertia of the drivetrain in kg m^2
+ self.J = 6.0
+ # Mass of the robot, in kg.
+ self.m = 52
+ # Radius of the robot, in meters (requires tuning by hand)
+ self.rb = 0.59055 / 2.0
+ # Radius of the wheels, in meters.
+ self.r = 4 * 0.0254 / 2
+ # Resistance of the motor, divided by the number of motors.
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratios
+ self.G_low = 14.0 / 40.0 * 24.0 / 60.0 * 52.0 / 60.0
+ self.G_high = 14.0 / 40.0 * 34.0 / 50.0 * 52.0 / 60.0
+ if left_low:
+ self.Gl = self.G_low
+ else:
+ self.Gl = self.G_high
+ if right_low:
+ self.Gr = self.G_low
+ else:
+ self.Gr = self.G_high
+
+ # Control loop time step
+ self.dt = 0.00505
+
+ # These describe the way that a given side of a robot will be influenced
+ # by the other side. Units of 1 / kg.
+ self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+ self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+ # The calculations which we will need for A and B.
+ self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.resistance * self.r * self.r)
+ self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.resistance * self.r * self.r)
+ self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
+ self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
+
+ # State feedback matrices
+ # X will be of the format
+ # [[positionl], [velocityl], [positionr], velocityr]]
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, self.msp * self.tcl, 0, self.msn * self.tcr],
+ [0, 0, 0, 1],
+ [0, self.msn * self.tcl, 0, self.msp * self.tcr]])
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [self.msp * self.mpl, self.msn * self.mpr],
+ [0, 0],
+ [self.msn * self.mpl, self.msp * self.mpr]])
+ self.C = numpy.matrix([[1, 0, 0, 0],
+ [0, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ if left_low or right_low:
+ q_pos = 0.12
+ q_vel = 1.0
+ else:
+ q_pos = 0.14
+ q_vel = 0.95
+
+ # Tune the LQR controller
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, name)
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('K %s', repr(self.K))
+
+ self.hlp = 0.3
+ self.llp = 0.4
+ self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
+
+class KFDrivetrain(Drivetrain):
+ def __init__(self, name="KFDrivetrain", left_low=True, right_low=True):
+ super(KFDrivetrain, self).__init__(name, left_low, right_low)
+
+ self.unaugmented_A_continuous = self.A_continuous
+ self.unaugmented_B_continuous = self.B_continuous
+
+ # The practical voltage applied to the wheels is
+ # V_left = U_left + left_voltage_error
+ #
+ # The states are
+ # [left position, left velocity, right position, right velocity,
+ # left voltage error, right voltage error, angular_error]
+ #
+ # The left and right positions are filtered encoder positions and are not
+ # adjusted for heading error.
+ # The turn velocity as computed by the left and right velocities is
+ # adjusted by the gyro velocity.
+ # The angular_error is the angular velocity error between the wheel speed
+ # and the gyro speed.
+ self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
+ self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
+ self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
+ self.A_continuous[0:4,4:6] = self.unaugmented_B_continuous
+ self.B_continuous[0:4,0:2] = self.unaugmented_B_continuous
+ self.A_continuous[0,6] = 1
+ self.A_continuous[2,6] = -1
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0, 0],
+ [0, -0.5 / self.rb, 0, 0.5 / self.rb, 0, 0, 0]])
+
+ self.D = numpy.matrix([[0, 0],
+ [0, 0],
+ [0, 0]])
+
+ q_pos = 0.05
+ q_vel = 1.00
+ q_voltage = 10.0
+ q_encoder_uncertainty = 2.00
+
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, (q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty ** 2.0)]])
+
+ r_pos = 0.0001
+ r_gyro = 0.000001
+ self.R = numpy.matrix([[(r_pos ** 2.0), 0.0, 0.0],
+ [0.0, (r_pos ** 2.0), 0.0],
+ [0.0, 0.0, (r_gyro ** 2.0)]])
+
+ # Solving for kf gains.
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ self.L = self.A * self.KalmanGain
+
+ unaug_K = self.K
+
+ # Implement a nice closed loop controller for use by the closed loop
+ # controller.
+ self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
+ self.K[0:2, 0:4] = unaug_K
+ self.K[0, 4] = 1.0
+ self.K[1, 5] = 1.0
+
+ self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+ qff_pos = 0.005
+ qff_vel = 1.00
+ self.Qff[0, 0] = 1.0 / qff_pos ** 2.0
+ self.Qff[1, 1] = 1.0 / qff_vel ** 2.0
+ self.Qff[2, 2] = 1.0 / qff_pos ** 2.0
+ self.Qff[3, 3] = 1.0 / qff_vel ** 2.0
+ self.Kff = numpy.matrix(numpy.zeros((2, 7)))
+ self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(self.B[0:4,:], self.Qff)
+
+ self.InitializeState()
+
+
+def main(argv):
+ argv = FLAGS(argv)
+ glog.init()
+
+ # Simulate the response of the system to a step input.
+ drivetrain = Drivetrain(left_low=False, right_low=False)
+ simulated_left = []
+ simulated_right = []
+ for _ in xrange(100):
+ drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+ simulated_left.append(drivetrain.X[0, 0])
+ simulated_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), simulated_left)
+ pylab.plot(range(100), simulated_right)
+ pylab.suptitle('Acceleration Test')
+ pylab.show()
+
+ # Simulate forwards motion.
+ drivetrain = Drivetrain(left_low=False, right_low=False)
+ close_loop_left = []
+ close_loop_right = []
+ left_power = []
+ right_power = []
+ R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(300):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+ left_power.append(U[0, 0])
+ right_power.append(U[1, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(300), close_loop_left, label='left position')
+ pylab.plot(range(300), close_loop_right, label='right position')
+ pylab.plot(range(300), left_power, label='left power')
+ pylab.plot(range(300), right_power, label='right power')
+ pylab.suptitle('Linear Move')
+ pylab.legend()
+ pylab.show()
+
+ # Try turning in place
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Angular Move')
+ pylab.show()
+
+ # Try turning just one side.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Pivot')
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ drivetrain_low_low = Drivetrain(
+ name="DrivetrainLowLow", left_low=True, right_low=True)
+ drivetrain_low_high = Drivetrain(
+ name="DrivetrainLowHigh", left_low=True, right_low=False)
+ drivetrain_high_low = Drivetrain(
+ name="DrivetrainHighLow", left_low=False, right_low=True)
+ drivetrain_high_high = Drivetrain(
+ name="DrivetrainHighHigh", left_low=False, right_low=False)
+
+ kf_drivetrain_low_low = KFDrivetrain(
+ name="KFDrivetrainLowLow", left_low=True, right_low=True)
+ kf_drivetrain_low_high = KFDrivetrain(
+ name="KFDrivetrainLowHigh", left_low=True, right_low=False)
+ kf_drivetrain_high_low = KFDrivetrain(
+ name="KFDrivetrainHighLow", left_low=False, right_low=True)
+ kf_drivetrain_high_high = KFDrivetrain(
+ name="KFDrivetrainHighHigh", left_low=False, right_low=False)
+
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['y2017_bot3', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
+ drivetrain_high_low, drivetrain_high_high],
+ namespaces = namespaces)
+ dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
+ drivetrain_low_low.dt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallTorque", "%f",
+ drivetrain_low_low.stall_torque))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
+ drivetrain_low_low.stall_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeed", "%f",
+ drivetrain_low_low.free_speed))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
+ drivetrain_low_low.free_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kJ", "%f",
+ drivetrain_low_low.J))
+ dog_loop_writer.AddConstant(control_loop.Constant("kMass", "%f",
+ drivetrain_low_low.m))
+ dog_loop_writer.AddConstant(control_loop.Constant("kRobotRadius", "%f",
+ drivetrain_low_low.rb))
+ dog_loop_writer.AddConstant(control_loop.Constant("kWheelRadius", "%f",
+ drivetrain_low_low.r))
+ dog_loop_writer.AddConstant(control_loop.Constant("kR", "%f",
+ drivetrain_low_low.resistance))
+ dog_loop_writer.AddConstant(control_loop.Constant("kV", "%f",
+ drivetrain_low_low.Kv))
+ dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
+ drivetrain_low_low.Kt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
+ drivetrain_low_low.G_low))
+ dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
+ drivetrain_high_high.G_high))
+ dog_loop_writer.AddConstant(control_loop.Constant("kHighOutputRatio", "%f",
+ drivetrain_high_high.G_high * drivetrain_high_high.r))
+
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "KFDrivetrain", [kf_drivetrain_low_low, kf_drivetrain_low_high,
+ kf_drivetrain_high_low, kf_drivetrain_high_high],
+ namespaces = namespaces)
+ kf_loop_writer.Write(argv[3], argv[4])
+
+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
new file mode 100755
index 0000000..59ba5cd
--- /dev/null
+++ b/y2017_bot3/control_loops/python/polydrivetrain.py
@@ -0,0 +1,501 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from frc971.control_loops.python import polytope
+from y2017_bot3.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+from frc971.control_loops.python.cim import CIM
+from matplotlib import pylab
+
+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 CoerceGoal(region, K, w, R):
+ """Intersects a line with a region, and finds the closest point to R.
+
+ Finds a point that is closest to R inside the region, and on the line
+ defined by K X = w. If it is not possible to find a point on the line,
+ finds a point that is inside the region and closest to the line. This
+ function assumes that
+
+ Args:
+ region: HPolytope, the valid goal region.
+ K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+ w: float, the offset in the equation above.
+ R: numpy.matrix (2 x 1), the point to be closest to.
+
+ Returns:
+ numpy.matrix (2 x 1), the point.
+ """
+ return DoCoerceGoal(region, K, w, R)[0]
+
+def DoCoerceGoal(region, K, w, R):
+ if region.IsInside(R):
+ return (R, True)
+
+ perpendicular_vector = K.T / numpy.linalg.norm(K)
+ parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+ [-perpendicular_vector[0, 0]]])
+
+ # We want to impose the constraint K * X = w on the polytope H * X <= k.
+ # We do this by breaking X up into parallel and perpendicular components to
+ # the half plane. This gives us the following equation.
+ #
+ # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+ #
+ # Then, substitute this into the polytope.
+ #
+ # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+ #
+ # Substitute K * X = w
+ #
+ # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+ #
+ # Move all the knowns to the right side.
+ #
+ # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+ #
+ # Let t = parallel.T \dot X, the component parallel to the surface.
+ #
+ # H * parallel * t <= k - H * perpendicular * w
+ #
+ # This is a polytope which we can solve, and use to figure out the range of X
+ # that we care about!
+
+ t_poly = polytope.HPolytope(
+ region.H * parallel_vector,
+ region.k - region.H * perpendicular_vector * w)
+
+ vertices = t_poly.Vertices()
+
+ if vertices.shape[0]:
+ # The region exists!
+ # Find the closest vertex
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in vertices:
+ point = parallel_vector * vertex + perpendicular_vector * w
+ length = numpy.linalg.norm(R - point)
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return (closest_point, True)
+ else:
+ # Find the vertex of the space that is closest to the line.
+ region_vertices = region.Vertices()
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in region_vertices:
+ point = vertex.T
+ length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return (closest_point, False)
+
+
+class VelocityDrivetrainModel(control_loop.ControlLoop):
+ def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
+ super(VelocityDrivetrainModel, self).__init__(name)
+ self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
+ right_low=right_low)
+ self.dt = 0.00505
+ self.A_continuous = numpy.matrix(
+ [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
+ [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
+
+ self.B_continuous = numpy.matrix(
+ [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
+ [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
+ self.C = numpy.matrix(numpy.eye(2))
+ self.D = numpy.matrix(numpy.zeros((2, 2)))
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ # FF * X = U (steady state)
+ self.FF = self.B.I * (numpy.eye(2) - self.A)
+
+ self.PlaceControllerPoles([0.90, 0.90])
+ self.PlaceObserverPoles([0.02, 0.02])
+
+ self.G_high = self._drivetrain.G_high
+ self.G_low = self._drivetrain.G_low
+ self.resistance = self._drivetrain.resistance
+ self.r = self._drivetrain.r
+ self.Kv = self._drivetrain.Kv
+ self.Kt = self._drivetrain.Kt
+
+ self.U_max = self._drivetrain.U_max
+ self.U_min = self._drivetrain.U_min
+
+
+class VelocityDrivetrain(object):
+ HIGH = 'high'
+ LOW = 'low'
+ SHIFTING_UP = 'up'
+ SHIFTING_DOWN = 'down'
+
+ def __init__(self):
+ self.drivetrain_low_low = VelocityDrivetrainModel(
+ left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
+ self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
+ self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
+ self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
+
+ # X is [lvel, rvel]
+ self.X = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ self.U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]]),
+ numpy.matrix([[12],
+ [12],
+ [12],
+ [12]]))
+
+ self.U_max = numpy.matrix(
+ [[12.0],
+ [12.0]])
+ self.U_min = numpy.matrix(
+ [[-12.0000000000],
+ [-12.0000000000]])
+
+ self.dt = 0.00505
+
+ self.R = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ self.U_ideal = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ # ttrust is the comprimise between having full throttle negative inertia,
+ # and having no throttle negative inertia. A value of 0 is full throttle
+ # inertia. A value of 1 is no throttle negative inertia.
+ self.ttrust = 1.0
+
+ self.left_gear = VelocityDrivetrain.LOW
+ self.right_gear = VelocityDrivetrain.LOW
+ self.left_shifter_position = 0.0
+ self.right_shifter_position = 0.0
+ self.left_cim = CIM()
+ self.right_cim = CIM()
+
+ def IsInGear(self, gear):
+ return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+
+ def MotorRPM(self, shifter_position, velocity):
+ if shifter_position > 0.5:
+ return (velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ else:
+ return (velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+
+ def CurrentDrivetrain(self):
+ if self.left_shifter_position > 0.5:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_high_high
+ else:
+ return self.drivetrain_high_low
+ else:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_low_high
+ else:
+ return self.drivetrain_low_low
+
+ def SimShifter(self, gear, shifter_position):
+ if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+ shifter_position = min(shifter_position + 0.5, 1.0)
+ else:
+ shifter_position = max(shifter_position - 0.5, 0.0)
+
+ if shifter_position == 1.0:
+ gear = VelocityDrivetrain.HIGH
+ elif shifter_position == 0.0:
+ gear = VelocityDrivetrain.LOW
+
+ return gear, shifter_position
+
+ def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
+ high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+ #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+ high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ high_power = high_torque * high_omega
+ low_power = low_torque * low_omega
+ #if should_print:
+ # print gear_name, "High omega", high_omega, "Low omega", low_omega
+ # print gear_name, "High torque", high_torque, "Low torque", low_torque
+ # print gear_name, "High power", high_power, "Low power", low_power
+
+ # Shift algorithm improvements.
+ # TODO(aschuh):
+ # It takes time to shift. Shifting down for 1 cycle doesn't make sense
+ # because you will end up slower than without shifting. Figure out how
+ # to include that info.
+ # If the driver is still in high gear, but isn't asking for the extra power
+ # from low gear, don't shift until he asks for it.
+ goal_gear_is_high = high_power > low_power
+ #goal_gear_is_high = True
+
+ if not self.IsInGear(current_gear):
+ glog.debug('%s Not in gear.', gear_name)
+ return current_gear
+ else:
+ is_high = current_gear is VelocityDrivetrain.HIGH
+ if is_high != goal_gear_is_high:
+ if goal_gear_is_high:
+ glog.debug('%s Shifting up.', gear_name)
+ return VelocityDrivetrain.SHIFTING_UP
+ else:
+ glog.debug('%s Shifting down.', gear_name)
+ return VelocityDrivetrain.SHIFTING_DOWN
+ else:
+ return current_gear
+
+ def FilterVelocity(self, throttle):
+ # Invert the plant to figure out how the velocity filter would have to work
+ # out in order to filter out the forwards negative inertia.
+ # This math assumes that the left and right power and velocity are equal.
+
+ # The throttle filter should filter such that the motor in the highest gear
+ # should be controlling the time constant.
+ # Do this by finding the index of FF that has the lowest value, and computing
+ # the sums using that index.
+ FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+ min_FF_sum_index = numpy.argmin(FF_sum)
+ min_FF_sum = FF_sum[min_FF_sum_index, 0]
+ min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+ # Compute the FF sum for high gear.
+ high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+
+ # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+ # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+ # - self.K[0, :].sum() * x_avg
+
+ # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+ # (self.K[0, :].sum() + self.FF[0, :].sum())
+
+ # U = (K + FF) * R - K * X
+ # (K + FF) ^-1 * (U + K * X) = R
+
+ # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
+ # have the same velocity goal as high gear, and so that the robot will hold
+ # the same speed for the same throttle for all gears.
+ adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+ return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
+ / (self.ttrust * min_K_sum + min_FF_sum))
+
+ def Update(self, throttle, steering):
+ # Shift into the gear which sends the most power to the floor.
+ # This is the same as sending the most torque down to the floor at the
+ # wheel.
+
+ self.left_gear = self.right_gear = True
+ if True:
+ self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ # Filter the throttle to provide a nicer response.
+ fvel = self.FilterVelocity(throttle)
+
+ # Constant radius means that angualar_velocity / linear_velocity = constant.
+ # Compute the left and right velocities.
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
+
+ # Write this constraint in the form of K * R = w
+ # angular velocity / linear velocity = constant
+ # (left - right) / (left + right) = constant
+ # left - right = constant * left + constant * right
+
+ # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+ # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+ # constant
+ # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+ # (-steering * sign(fvel)) = constant
+ # (-steering * sign(fvel)) * (left + right) = left - right
+ # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+
+ equality_k = numpy.matrix(
+ [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
+ equality_w = 0.0
+
+ self.R[0, 0] = left_velocity
+ self.R[1, 0] = right_velocity
+
+ # Construct a constraint on R by manipulating the constraint on U
+ # Start out with H * U <= k
+ # U = FF * R + K * (R - X)
+ # H * (FF * R + K * R - K * X) <= k
+ # H * (FF + K) * R <= k + H * K * X
+ R_poly = polytope.HPolytope(
+ self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+ self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+
+ # Limit R back inside the box.
+ self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+ FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+ self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
+ else:
+ glog.debug('Not all in gear')
+ if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
+ # TODO(austin): Use battery volts here.
+ R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.U_ideal[0, 0] = numpy.clip(
+ self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
+ self.left_cim.U_min, self.left_cim.U_max)
+ self.left_cim.Update(self.U_ideal[0, 0])
+
+ R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
+ self.U_ideal[1, 0] = numpy.clip(
+ self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
+ self.right_cim.U_min, self.right_cim.U_max)
+ self.right_cim.Update(self.U_ideal[1, 0])
+ else:
+ assert False
+
+ self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+
+ # TODO(austin): Model the robot as not accelerating when you shift...
+ # This hack only works when you shift at the same time.
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+
+ self.left_gear, self.left_shifter_position = self.SimShifter(
+ self.left_gear, self.left_shifter_position)
+ self.right_gear, self.right_shifter_position = self.SimShifter(
+ self.right_gear, self.right_shifter_position)
+
+ glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+ glog.debug('Left shifter %s %d Right shifter %s %d',
+ self.left_gear, self.left_shifter_position,
+ self.right_gear, self.right_shifter_position)
+
+
+def main(argv):
+ vdrivetrain = VelocityDrivetrain()
+
+ if not FLAGS.plot:
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2017_bot3', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high],
+ namespaces=namespaces)
+
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+
+ cim_writer.Write(argv[3], argv[4])
+ return
+
+ vl_plot = []
+ vr_plot = []
+ ul_plot = []
+ ur_plot = []
+ radius_plot = []
+ t_plot = []
+ left_gear_plot = []
+ right_gear_plot = []
+ vdrivetrain.left_shifter_position = 0.0
+ vdrivetrain.right_shifter_position = 0.0
+ vdrivetrain.left_gear = VelocityDrivetrain.LOW
+ vdrivetrain.right_gear = VelocityDrivetrain.LOW
+
+ glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
+
+ if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Left is high')
+ else:
+ glog.debug('Left is low')
+ if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Right is high')
+ else:
+ glog.debug('Right is low')
+
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ elif t < 1.2:
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
+ else:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ t_plot.append(t)
+ vl_plot.append(vdrivetrain.X[0, 0])
+ vr_plot.append(vdrivetrain.X[1, 0])
+ ul_plot.append(vdrivetrain.U[0, 0])
+ ur_plot.append(vdrivetrain.U[1, 0])
+ left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+ right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+
+ fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+ turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+ if abs(fwd_velocity) < 0.0000001:
+ radius_plot.append(turn_velocity)
+ else:
+ radius_plot.append(turn_velocity / fwd_velocity)
+
+ # TODO(austin):
+ # Shifting compensation.
+
+ # Tighten the turn.
+ # Closed loop drive.
+
+ pylab.plot(t_plot, vl_plot, label='left velocity')
+ pylab.plot(t_plot, vr_plot, label='right velocity')
+ pylab.plot(t_plot, ul_plot, label='left voltage')
+ pylab.plot(t_plot, ur_plot, label='right voltage')
+ pylab.plot(t_plot, radius_plot, label='radius')
+ pylab.plot(t_plot, left_gear_plot, label='left gear high')
+ pylab.plot(t_plot, right_gear_plot, label='right gear high')
+ pylab.legend()
+ pylab.show()
+ return 0
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017_bot3/wpilib_interface.cc b/y2017_bot3/wpilib_interface.cc
new file mode 100644
index 0000000..737050a
--- /dev/null
+++ b/y2017_bot3/wpilib_interface.cc
@@ -0,0 +1,517 @@
+#include <inttypes.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <mutex>
+#include <thread>
+
+#include "AnalogInput.h"
+#include "DigitalGlitchFilter.h"
+#include "DriverStation.h"
+#include "Encoder.h"
+#include "Compressor.h"
+#include "VictorSP.h"
+#undef ERROR
+
+#include "aos/common/commonmath.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/messages/robot_state.q.h"
+#include "aos/common/stl_mutex.h"
+#include "aos/common/time.h"
+#include "aos/common/util/compiler_memory_barrier.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/linux_code/init.h"
+
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/gyro_sender.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/dma_edge_counting.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/interrupt_edge_counting.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/wpilib_interface.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+
+#include "y2017_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+
+namespace y2017_bot3 {
+namespace wpilib {
+namespace {
+
+constexpr double kMaxBringupPower = 12.0;
+
+constexpr double kDrivetrainCyclesPerRevolution = 256;
+constexpr double kDrivetrainEncoderCountsPerRevolution =
+ kDrivetrainCyclesPerRevolution * 4;
+constexpr double kDrivetrainEncoderRatio =
+ 1.0 * control_loops::drivetrain::kWheelRadius;
+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): Replace this with ::std::make_unique once all our toolchains
+// have support.
+template <class T, class... U>
+std::unique_ptr<T> make_unique(U &&... u) {
+ return std::unique_ptr<T>(new T(std::forward<U>(u)...));
+}
+
+// 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) / (256.0 /*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:
+ SensorReader() {
+ // Set to filter out anything shorter than 1/4 of the minimum pulse width
+ // we should ever see.
+ fast_encoder_filter_.SetPeriodNanoSeconds(
+ static_cast<int>(1 / 4.0 /* built-in tolerance */ /
+ kMaxFastEncoderPulsesPerSecond * 1e9 +
+ 0.5));
+ hall_filter_.SetPeriodNanoSeconds(100000);
+ }
+
+ void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ drivetrain_left_encoder_ = ::std::move(encoder);
+ }
+
+ void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ drivetrain_right_encoder_ = ::std::move(encoder);
+ }
+
+ 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 set_pwm_trigger(::std::unique_ptr<DigitalInput> pwm_trigger) {
+ medium_encoder_filter_.Add(pwm_trigger.get());
+ pwm_trigger_ = ::std::move(pwm_trigger);
+ }
+
+ // All of the DMA-related set_* calls must be made before this, and it
+ // doesn't
+ // hurt to do all of them.
+ void set_dma(::std::unique_ptr<DMA> dma) {
+ dma_synchronizer_.reset(
+ new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
+ }
+
+ void RunPWMDetecter() {
+ ::aos::SetCurrentThreadRealtimePriority(41);
+
+ pwm_trigger_->RequestInterrupts();
+ // Rising edge only.
+ pwm_trigger_->SetUpSourceEdge(true, false);
+
+ monotonic_clock::time_point last_posedge_monotonic =
+ monotonic_clock::min_time;
+
+ while (run_) {
+ auto ret = pwm_trigger_->WaitForInterrupt(1.0, true);
+ if (ret == InterruptableSensorBase::WaitResult::kRisingEdge) {
+ // Grab all the clocks.
+ const double pwm_fpga_time = pwm_trigger_->ReadRisingTimestamp();
+
+ aos_compiler_memory_barrier();
+ const double fpga_time_before = GetFPGATime() * 1e-6;
+ aos_compiler_memory_barrier();
+ const monotonic_clock::time_point monotonic_now =
+ monotonic_clock::now();
+ aos_compiler_memory_barrier();
+ const double fpga_time_after = GetFPGATime() * 1e-6;
+ aos_compiler_memory_barrier();
+
+ const double fpga_offset =
+ (fpga_time_after + fpga_time_before) / 2.0 - pwm_fpga_time;
+
+ // Compute when the edge was.
+ const monotonic_clock::time_point monotonic_edge =
+ monotonic_now - chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(fpga_offset));
+
+ LOG(INFO, "Got PWM pulse %f spread, %f offset, %lld trigger\n",
+ fpga_time_after - fpga_time_before, fpga_offset,
+ monotonic_edge.time_since_epoch().count());
+
+ // Compute bounds on the timestep and sampling times.
+ const double fpga_sample_length = fpga_time_after - fpga_time_before;
+ const chrono::nanoseconds elapsed_time =
+ monotonic_edge - last_posedge_monotonic;
+
+ last_posedge_monotonic = monotonic_edge;
+
+ // Verify that the values are sane.
+ if (fpga_sample_length > 2e-5 || fpga_sample_length < 0) {
+ continue;
+ }
+ if (fpga_offset < 0 || fpga_offset > 0.00015) {
+ continue;
+ }
+ if (elapsed_time >
+ chrono::microseconds(5050) + chrono::microseconds(4) ||
+ elapsed_time <
+ chrono::microseconds(5050) - chrono::microseconds(4)) {
+ continue;
+ }
+ // Good edge!
+ {
+ ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
+ last_tick_time_monotonic_timepoint_ = last_posedge_monotonic;
+ last_period_ = elapsed_time;
+ }
+ } else {
+ LOG(INFO, "PWM triggered %d\n", ret);
+ }
+ }
+ pwm_trigger_->CancelInterrupts();
+ }
+
+ void operator()() {
+ ::aos::SetCurrentThreadName("SensorReader");
+
+ my_pid_ = getpid();
+ ds_ = &DriverStation::GetInstance();
+
+ dma_synchronizer_->Start();
+
+ ::aos::time::PhasedLoop phased_loop(last_period_,
+ ::std::chrono::milliseconds(3));
+ chrono::nanoseconds filtered_period = last_period_;
+
+ ::std::thread pwm_detecter_thread(
+ ::std::bind(&SensorReader::RunPWMDetecter, this));
+
+ ::aos::SetCurrentThreadRealtimePriority(40);
+ while (run_) {
+ {
+ const int iterations = phased_loop.SleepUntilNext();
+ if (iterations != 1) {
+ LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
+ }
+ }
+ RunIteration();
+
+ monotonic_clock::time_point last_tick_timepoint;
+ chrono::nanoseconds period;
+ {
+ ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
+ last_tick_timepoint = last_tick_time_monotonic_timepoint_;
+ period = last_period_;
+ }
+
+ if (last_tick_timepoint == monotonic_clock::min_time) {
+ continue;
+ }
+ chrono::nanoseconds new_offset = phased_loop.OffsetFromIntervalAndTime(
+ period, last_tick_timepoint + chrono::microseconds(2050));
+
+ // TODO(austin): If this is the first edge in a while, skip to it (plus
+ // an offset). Otherwise, slowly drift time to line up.
+
+ phased_loop.set_interval_and_offset(period, new_offset);
+ }
+ pwm_detecter_thread.join();
+ }
+
+ void RunIteration() {
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
+
+ {
+ 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();
+ }
+
+ dma_synchronizer_->RunIteration();
+ }
+
+ void Quit() { run_ = false; }
+
+ private:
+ double encoder_translate(int32_t value, double counts_per_revolution,
+ double ratio) {
+ return static_cast<double>(value) / counts_per_revolution * ratio *
+ (2.0 * M_PI);
+ }
+
+ int32_t my_pid_;
+ DriverStation *ds_;
+
+ // Mutex to manage access to the period and tick time variables.
+ ::aos::stl_mutex tick_time_mutex_;
+ monotonic_clock::time_point last_tick_time_monotonic_timepoint_ =
+ monotonic_clock::min_time;
+ chrono::nanoseconds last_period_ = chrono::microseconds(5050);
+
+ ::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
+
+ ::std::unique_ptr<Encoder> drivetrain_left_encoder_,
+ drivetrain_right_encoder_;
+
+ ::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
+
+ ::std::unique_ptr<DigitalInput> pwm_trigger_;
+
+ ::std::atomic<bool> run_{true};
+
+ DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_, hall_filter_;
+};
+
+class SolenoidWriter {
+ public:
+ SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
+ : pcm_(pcm),
+ drivetrain_(".y2017_bot3.control_loops.drivetrain_queue.output"){}
+ void set_compressor(::std::unique_ptr<Compressor> compressor) {
+ compressor_ = ::std::move(compressor);
+ }
+
+ void set_drivetrain_shifter(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ drivetrain_shifter_ = ::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_);
+ drivetrain_shifter_->Set(
+ !(drivetrain_->left_high || drivetrain_->right_high));
+ }
+ }
+
+ {
+ ::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> drivetrain_shifter_;
+
+ ::std::unique_ptr<Compressor> compressor_;
+
+ ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
+
+ ::std::atomic<bool> run_{true};
+};
+
+class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_drivetrain_left_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ drivetrain_left_victor_ = ::std::move(t);
+ }
+
+ void set_drivetrain_right_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ drivetrain_right_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_left_victor_->SetSpeed(-queue->left_voltage / 12.0);
+ drivetrain_right_victor_->SetSpeed(queue->right_voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "drivetrain output too old\n");
+ drivetrain_left_victor_->SetDisabled();
+ drivetrain_right_victor_->SetDisabled();
+ }
+
+ ::std::unique_ptr<::frc::VictorSP> drivetrain_left_victor_,
+ drivetrain_right_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;
+
+ // TODO(sabina): Update port numbers
+ 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>(7));
+ 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_left_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(7)));
+ drivetrain_writer.set_drivetrain_right_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+
+ ::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
+ new ::frc971::wpilib::BufferedPcm());
+ SolenoidWriter solenoid_writer(pcm);
+ solenoid_writer.set_drivetrain_shifter(pcm->MakeSolenoid(0));
+
+ 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);