Clean up the new pistol grip code
I ended up with various pieces left over after committing all the
pieces...
Change-Id: I129299f988c236a602a289fb0ad6b92f265f8ea2
diff --git a/motors/core/time.cc b/motors/core/time.cc
index 48800a2..ba09876 100644
--- a/motors/core/time.cc
+++ b/motors/core/time.cc
@@ -5,7 +5,10 @@
// The systick interrupt increments this every 1ms.
volatile uint32_t systick_millis_count = 0;
-uint32_t micros(void) {
+namespace {
+
+template<int kMultiplier>
+uint32_t do_time(void) {
__disable_irq();
uint32_t current = SYST_CVR;
uint32_t count = systick_millis_count;
@@ -15,9 +18,16 @@
// up to its max, then add another ms.
if ((istatus & SCB_ICSR_PENDSTSET) && current > 50) count++;
current = ((F_CPU / 1000) - 1) - current;
- return count * 1000 + current / (F_CPU / 1000000);
+ return count * (1000 * kMultiplier) +
+ current * kMultiplier / (F_CPU / 1000000);
}
+} // namespace
+
+uint32_t nanos(void) { return do_time<1000>(); }
+
+uint32_t micros(void) { return do_time<1>(); }
+
void delay(uint32_t ms) { delay_from(micros(), ms); }
uint32_t delay_from(uint32_t start, uint32_t ms) {
diff --git a/motors/core/time.h b/motors/core/time.h
index 9befab6..3d5f262 100644
--- a/motors/core/time.h
+++ b/motors/core/time.h
@@ -8,6 +8,9 @@
{
#endif
+// Returns the current number of nanoseconds. This will wrap naturally.
+uint32_t nanos(void);
+
// Returns the current number of microseconds. This will wrap naturally.
uint32_t micros(void);
diff --git a/motors/decode_dump.py b/motors/decode_dump.py
index 1010e88..27e1757 100755
--- a/motors/decode_dump.py
+++ b/motors/decode_dump.py
@@ -31,28 +31,30 @@
def current(reading, ref):
reading_voltage = reading / 4096 * 3.3 / 1.47 * (0.768 + 1.47)
+ reading_voltage = reading / 4096 * 3.3 / 18.0 * (18.0 + 10.0)
#reading_ref = ref / 4096 * 3.3
reading_ref = 2.5
- reading_ref = 0
- return (reading_voltage - reading_ref) / 50 / 0.0003
+ #reading_ref = 0
+ #return (reading_voltage - reading_ref) / 50 / 0.0003
+ return (reading_voltage - reading_ref) / 0.195
with open(sys.argv[1], 'w') as out:
- out.write('current0.0,current1.0,current2.0,current0.1,current1.1,current2.1,count\n')
+ out.write('balanced0,balanced1,balanced2,current0.0,current1.0,current2.0,current0.1,current1.1,current2.1,count\n')
#for point in decoded[2000:7200]:
for point in decoded:
out.write(','.join(str(d) for d in (
current(point[0], point[6]),
- current(point[1], point[6]),
- current(point[2], point[6]),
- #current(point[3], point[6]),
- #current(point[4], point[6]),
- #current(point[5], point[6]),
- point[3] / 100.0,
- point[4] / 100.0,
- point[5] / 100.0,
- point[6] / 100.0,
- point[7] / 100.0,
- point[8] / 100.0,
+ current(point[1], point[6]),
+ current(point[2], point[6]),
+ current(point[3], point[6]),
+ current(point[4], point[6]),
+ current(point[5], point[6]),
+ current(point[6], point[6]),
+ current(point[7], point[6]),
+ current(point[8], point[6]),
+ #point[6] / 100.0,
+ #point[7] / 100.0,
+ #point[8] / 100.0,
point[9] / 100.0,
point[10] / 100.0,
)) + '\n')
diff --git a/motors/pistol_grip/BUILD b/motors/pistol_grip/BUILD
index 503291f..6e6a428 100644
--- a/motors/pistol_grip/BUILD
+++ b/motors/pistol_grip/BUILD
@@ -1,102 +1,102 @@
-load('//motors:macros.bzl', 'hex_from_elf')
+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/peripheral:can',
- '//motors/core',
- '//motors/usb',
- '//motors/usb:cdc',
- '//motors/usb:hid',
- '//motors/usb:interrupt_out',
- ],
- restricted_to = mcu_cpus,
+ name = "drivers_station.elf",
+ srcs = [
+ "drivers_station.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//motors:util",
+ "//motors/core",
+ "//motors/peripheral:can",
+ "//motors/usb",
+ "//motors/usb:cdc",
+ "//motors/usb:hid",
+ "//motors/usb:interrupt_out",
+ ],
)
hex_from_elf(
- name = 'drivers_station',
- restricted_to = mcu_cpus,
+ name = "drivers_station",
+ restricted_to = mcu_cpus,
)
cc_binary(
- name = 'controller.elf',
- srcs = [
- 'vtable_wheel.cc',
- 'vtable_trigger.cc',
- 'controller.cc',
- ],
- deps = [
- ':motor_controls',
- '//motors:util',
- '//motors:motor',
- '//motors/core',
- '//motors/peripheral:can',
- '//motors/peripheral:adc',
- '//motors/usb',
- '//motors/usb:cdc',
- '//frc971/control_loops/drivetrain:haptic_input_uc',
- ],
- restricted_to = mcu_cpus,
+ name = "controller.elf",
+ srcs = [
+ "controller.cc",
+ "vtable_trigger.cc",
+ "vtable_wheel.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":motor_controls",
+ "//frc971/control_loops/drivetrain:haptic_input_uc",
+ "//motors:motor",
+ "//motors:util",
+ "//motors/core",
+ "//motors/peripheral:adc",
+ "//motors/peripheral:can",
+ "//motors/usb",
+ "//motors/usb:cdc",
+ ],
)
hex_from_elf(
- name = 'controller',
- restricted_to = mcu_cpus,
+ name = "controller",
+ restricted_to = mcu_cpus,
)
cc_binary(
- name = 'usb_forward_linux',
- srcs = [
- 'usb_forward.cc',
- ],
- deps = [
- # Don't add anything else here. :usb_forward_windows still has to build it
- # without any other dependencies.
- '@libusb_1_0',
- ],
- restricted_to = ['//tools:k8'],
+ name = "usb_forward_linux",
+ srcs = [
+ "usb_forward.cc",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ # Don't add anything else here. :usb_forward_windows still has to build it
+ # without any other dependencies.
+ "@libusb_1_0",
+ ],
)
genrule(
- name = 'usb_forward_windows',
- outs = [
- 'usb_forward.exe',
- ],
- srcs = [
- 'usb_forward.cc',
- '@libusb_1_0_windows//file',
- ],
- tools = [
- 'usb_forward_windows_build.sh',
- ],
- cmd = ' '.join([
- '$(location usb_forward_windows_build.sh)',
- '$(location usb_forward.cc)',
- '$(location @libusb_1_0_windows//file)',
- '$@',
- ]),
- output_to_bindir = True,
+ name = "usb_forward_windows",
+ srcs = [
+ "usb_forward.cc",
+ "@libusb_1_0_windows//file",
+ ],
+ outs = [
+ "usb_forward.exe",
+ ],
+ cmd = " ".join([
+ "$(location usb_forward_windows_build.sh)",
+ "$(location usb_forward.cc)",
+ "$(location @libusb_1_0_windows//file)",
+ "$@",
+ ]),
+ output_to_bindir = True,
+ tools = [
+ "usb_forward_windows_build.sh",
+ ],
)
cc_library(
- name = 'motor_controls',
- visibility = ['//visibility:public'],
- hdrs = [
- 'motor_controls.h',
- ],
- srcs = [
- 'motor_controls.cc',
- ],
- deps = [
- '//motors:math',
- '//motors:motor',
- '//motors/peripheral:configuration',
- '//third_party/eigen',
- ],
- restricted_to = mcu_cpus,
+ name = "motor_controls",
+ srcs = [
+ "motor_controls.cc",
+ ],
+ hdrs = [
+ "motor_controls.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//motors:math",
+ "//motors:motor",
+ "//motors/peripheral:configuration",
+ "//third_party/eigen",
+ ],
)
diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
index 4460355..dd3a4b8 100644
--- a/motors/pistol_grip/controller.cc
+++ b/motors/pistol_grip/controller.cc
@@ -1,11 +1,13 @@
#include "motors/core/kinetis.h"
-#include <stdio.h>
#include <inttypes.h>
+#include <stdio.h>
#include <atomic>
#include <cmath>
+#include "frc971/control_loops/drivetrain/integral_haptic_trigger.h"
+#include "frc971/control_loops/drivetrain/integral_haptic_wheel.h"
#include "motors/core/time.h"
#include "motors/motor.h"
#include "motors/peripheral/adc.h"
@@ -14,8 +16,6 @@
#include "motors/usb/cdc.h"
#include "motors/usb/usb.h"
#include "motors/util.h"
-#include "frc971/control_loops/drivetrain/integral_haptic_wheel.h"
-#include "frc971/control_loops/drivetrain/integral_haptic_trigger.h"
#define MOTOR0_PWM_FTM FTM3
#define MOTOR0_ENCODER_FTM FTM2
@@ -195,11 +195,9 @@
global_motor0.load(::std::memory_order_relaxed)
->HandleInterrupt(BalanceSimpleReadings(readings.currents), encoder);
-
global_trigger_angle.store(trigger_angle);
}
-
int ConvertFloat16(float val) {
int result = static_cast<int>(val * 32768.0f) + 32768;
if (result > 0xffff) {
diff --git a/motors/plot.py b/motors/plot.py
index 013f308..3d43080 100755
--- a/motors/plot.py
+++ b/motors/plot.py
@@ -3,7 +3,7 @@
import numpy
from matplotlib import pylab
-data = numpy.loadtxt('/tmp/dump3.csv',
+data = numpy.loadtxt('/tmp/jkalsdjflsd.csv',
delimiter=',',
skiprows=1)
x = range(len(data))
@@ -15,11 +15,16 @@
pylab.plot(x, [d[3] for d in data], 'r--', label='ia_goal')
pylab.plot(x, [d[4] for d in data], 'g--', label='ib_goal')
pylab.plot(x, [d[5] for d in data], 'b--', label='ic_goal')
-pylab.plot(x, [d[6] for d in data], 'rx', label='i_overall')
-pylab.plot(x, [d[7] for d in data], 'gx', label='omega')
-pylab.plot(x, [d[8] for d in data], 'r', label='van')
-pylab.plot(x, [d[9] for d in data], 'g', label='vbn')
-pylab.plot(x, [d[10] for d in data], 'b', label='vcn')
+pylab.plot(x, [d[6] for d in data], 'rx', label='va')
+pylab.plot(x, [d[7] for d in data], 'gx', label='vb')
+pylab.plot(x, [d[8] for d in data], 'bx', label='vc')
+#pylab.plot(x, [d[6] for d in data], 'rx', label='i_overall')
+#pylab.plot(x, [d[7] for d in data], 'gx', label='omega')
+#pylab.plot(x, [d[8] for d in data], 'r', label='van')
+#pylab.plot(x, [d[9] for d in data], 'g', label='vbn')
+#pylab.plot(x, [d[10] for d in data], 'b', label='vcn')
+pylab.plot(x, [d[9] / 1000.0 for d in data], 'yx', label='pos')
+pylab.plot(x, [d[10] / 1000.0 for d in data], 'yo', label='pos')
pylab.legend()
pylab.show()
diff --git a/motors/python/BUILD b/motors/python/BUILD
index 458399f..299f4d2 100644
--- a/motors/python/BUILD
+++ b/motors/python/BUILD
@@ -3,12 +3,12 @@
srcs = [
"phase_current.py",
],
+ restricted_to = ["//tools:k8"],
deps = [
"//external:python-gflags",
"//external:python-glog",
"//frc971/control_loops/python:controls",
],
- restricted_to = ["//tools:k8"],
)
py_binary(
@@ -16,10 +16,10 @@
srcs = [
"haptic_phase_current.py",
],
+ restricted_to = ["//tools:k8"],
deps = [
"//external:python-gflags",
"//external:python-glog",
"//frc971/control_loops/python:controls",
],
- restricted_to = ["//tools:k8"],
)