Start using DMA sampling with ADC
The various code used to make DMA-based sampling work.
Tested (except for some non-functional changes to improve code) on
cart, although not extensively or under any real load.
Change-Id: I187066e857e5077cb017f9f371c8eeaf0a2e2d15
diff --git a/motors/fet12/BUILD b/motors/fet12/BUILD
index 0eed7ef..a637a55 100644
--- a/motors/fet12/BUILD
+++ b/motors/fet12/BUILD
@@ -33,6 +33,7 @@
"//motors:util",
"//motors/core",
"//motors/peripheral:adc",
+ "//motors/peripheral:adc_dma",
"//motors/peripheral:can",
"//motors/peripheral:uart",
],
@@ -80,20 +81,37 @@
],
)
-py_binary(
- name = "current_equalize",
+py_library(
+ name = "calib_sensors",
srcs = [
- "current_equalize.py",
+ "calib_sensors.py",
],
data = [
"@python_repo//:scipy",
],
)
+py_binary(
+ name = "current_equalize",
+ srcs = [
+ "current_equalize.py",
+ ],
+ data = [
+ ":calib_sensors",
+ "@python_repo//:scipy",
+ ],
+)
+
genrule(
name = "gen_current_equalization",
+ srcs = [
+ "calib_data_6030.csv",
+ "calib_data_60a.csv",
+ "calib_data_60b.csv",
+ "calib_data_60c.csv",
+ ],
outs = ["current_equalization.h"],
- cmd = "./$(location current_equalize) > \"$@\"",
+ cmd = "./$(location current_equalize) $(location calib_data_60a.csv) $(location calib_data_60b.csv) $(location calib_data_60c.csv) $(location calib_data_6030.csv) > \"$@\"",
restricted_to = mcu_cpus,
tools = ["current_equalize"],
)
diff --git a/motors/fet12/calib_data_6030.csv b/motors/fet12/calib_data_6030.csv
new file mode 100644
index 0000000..eb6105e
--- /dev/null
+++ b/motors/fet12/calib_data_6030.csv
@@ -0,0 +1,124 @@
+60,30,0, 75,73,-80, -154,-80,-151
+60,30,0, 74,72,-88, -160,-88,-156
+60,30,0, 67,64,-93, -157,-96,-156
+60,30,0, 72,68,-84, -154,-86,-150
+60,30,0, 70,69,-89, -156,-88,-153
+60,30,0, 90,94,-65, -155,-64,-153
+60,30,0, 81,79,-80, -157,-78,-156
+60,30,0, 66,65,-94, -156,-93,-159
+60,30,0, 70,73,-81, -154,-83,-151
+60,30,0, 75,77,-87, -157,-85,-159
+60,30,0, 83,81,-75, -156,-77,-157
+60,30,0, 71,72,-89, -155,-89,-154
+60,30,0, 88,91,-72, -155,-68,-161
+60,30,0, 71,67,-90, -158,-91,-155
+60,30,0, 71,74,-84, -153,-81,-151
+60,30,0, 85,84,-81, -158,-76,-161
+60,30,0, 88,88,-68, -153,-68,-154
+60,30,0, 74,75,-83, -152,-78,-153
+60,30,0, 87,89,-74, -162,-75,-159
+60,30,0, 77,77,-81, -159,-81,-155
+60,30,0, 65,69,-84, -154,-86,-154
+60,30,0, 75,78,-78, -151,-76,-151
+60,30,0, 69,68,-89, -153,-86,-156
+60,30,0, 94,97,-66, -161,-66,-161
+60,30,0, 82,81,-75, -157,-78,-153
+60,30,0, 82,84,-79, -156,-76,-162
+60,30,0, 78,73,-87, -161,-88,-154
+60,30,0, 69,68,-90, -153,-90,-151
+60,30,0, 76,73,-83, -154,-79,-154
+60,30,0, 73,74,-79, -149,-81,-151
+60,30,0, 83,83,-75, -155,-77,-157
+60,30,0, 82,84,-75, -161,-79,-155
+60,30,0, 80,83,-78, -156,-79,-157
+60,30,0, 66,65,-91, -159,-94,-155
+60,30,0, 78,78,-82, -157,-82,-154
+60,30,0, 84,84,-76, -161,-79,-154
+60,30,0, 94,96,-65, -154,-63,-154
+60,30,0, 73,76,-79, -157,-83,-155
+60,30,0, 79,79,-82, -156,-81,-156
+60,30,0, 67,67,-86, -154,-86,-151
+60,30,0, 72,72,-86, -150,-84,-154
+60,30,0, 75,79,-80, -154,-81,-154
+60,30,0, 66,66,-88, -152,-89,-152
+60,30,0, 76,74,-85, -157,-85,-156
+60,30,0, 78,81,-78, -158,-81,-157
+60,30,0, 83,82,-71, -151,-69,-151
+60,30,0, 69,72,-83, -151,-82,-156
+60,30,0, 64,71,-82, -146,-82,-148
+60,30,0, 82,80,-78, -154,-77,-156
+ 0,60,30, -127,-129,-58, 78,-54,76
+ 0,60,30, -112,-112,-45, 69,-42,74
+ 0,60,30, -120,-120,-47, 75,-45,76
+ 0,60,30, -112,-115,-47, 74,-42,74
+ 0,60,30, -127,-126,-51, 77,-50,77
+ 0,60,30, -121,-120,-46, 75,-46,77
+ 0,60,30, -128,-128,-55, 76,-52,78
+ 0,60,30, -119,-122,-53, 71,-48,75
+ 0,60,30, -124,-121,-52, 74,-49,74
+ 0,60,30, -122,-120,-52, 75,-47,74
+ 0,60,30, -127,-126,-54, 73,-54,77
+ 0,60,30, -123,-122,-56, 74,-52,70
+ 0,60,30, -121,-121,-50, 72,-49,75
+ 0,60,30, -121,-122,-56, 72,-51,69
+ 0,60,30, -120,-115,-48, 73,-46,71
+ 0,60,30, -115,-113,-37, 76,-40,77
+ 0,60,30, -118,-115,-53, 73,-45,70
+ 0,60,30, -129,-126,-48, 83,-46,80
+ 0,60,30, -129,-129,-55, 76,-52,76
+ 0,60,30, -123,-119,-52, 82,-45,71
+ 0,60,30, -119,-120,-50, 76,-45,78
+ 0,60,30, -120,-118,-46, 77,-42,76
+ 0,60,30, -104,-105,-30, 77,-28,79
+ 0,60,30, -114,-112,-44, 76,-40,73
+ 0,60,30, -127,-128,-57, 76,-54,80
+ 0,60,30, -119,-118,-51, 71,-48,74
+ 0,60,30, -113,-113,-36, 79,-34,80
+ 0,60,30, -130,-126,-54, 79,-49,80
+ 0,60,30, -125,-120,-56, 78,-50,71
+ 0,60,30, -134,-136,-60, 76,-60,81
+ 0,60,30, -122,-121,-51, 74,-49,73
+ 0,60,30, -104,-106,-32, 77,-29,77
+ 0,60,30, -118,-117,-46, 72,-47,76
+ 0,60,30, -128,-128,-59, 77,-53,70
+ 0,60,30, -125,-126,-55, 75,-50,76
+ 0,60,30, -127,-126,-47, 78,-47,84
+ 0,60,30, -126,-126,-56, 72,-55,76
+ 0,60,30, -119,-118,-49, 75,-44,74
+ 0,60,30, -121,-119,-49, 76,-43,74
+ 0,60,30, -130,-131,-61, 79,-56,77
+ 0,60,30, -122,-120,-55, 70,-52,73
+ 0,60,30, -121,-121,-50, 77,-46,77
+ 0,60,30, -124,-125,-52, 74,-50,75
+ 0,60,30, -126,-125,-55, 77,-51,75
+ 0,60,30, -113,-113,-40, 77,-37,78
+ 0,60,30, -127,-126,-52, 78,-51,78
+ 0,60,30, -120,-116,-44, 78,-43,77
+ 0,60,30, -127,-126,-54, 74,-53,73
+ 0,60,30, -127,-126,-56, 75,-52,77
+ 0,60,30, -120,-118,-50, 72,-48,71
+ 0,60,30, -129,-128,-54, 76,-52,79
+ 0,60,30, -122,-117,-48, 77,-44,73
+ 30,0,60, 64,65,139, 80,140,75
+ 30,0,60, 50,50,126, 78,127,77
+ 30,0,60, 69,68,136, 79,143,72
+ 30,0,60, 65,64,133, 74,135,76
+ 30,0,60, 69,69,135, 70,137,68
+ 30,0,60, 60,59,132, 78,132,74
+ 30,0,60, 57,58,136, 76,136,77
+ 30,0,60, 58,60,135, 78,134,75
+ 30,0,60, 68,65,135, 73,135,74
+ 30,0,60, 67,68,141, 76,141,75
+ 30,0,60, 69,68,142, 77,144,75
+ 30,0,60, 56,62,129, 79,131,70
+ 30,0,60, 62,65,135, 75,135,73
+ 30,0,60, 72,71,145, 73,144,78
+ 30,0,60, 65,64,136, 74,135,73
+ 30,0,60, 62,59,136, 74,137,77
+ 30,0,60, 64,65,137, 76,140,78
+ 30,0,60, 64,62,141, 76,138,79
+ 30,0,60, 67,70,135, 72,137,71
+ 30,0,60, 74,74,147, 73,146,77
+ 30,0,60, 68,67,142, 75,144,78
+ 30,0,60, 59,59,126, 75,128,68
+
diff --git a/motors/fet12/calib_data_60a.csv b/motors/fet12/calib_data_60a.csv
new file mode 100644
index 0000000..3ef66cf
--- /dev/null
+++ b/motors/fet12/calib_data_60a.csv
@@ -0,0 +1,94 @@
+60,0,0, 139,140,-16, -153,-13,-153
+60,0,0, 141,147,-12, -152,-11,-157
+60,0,0, 145,148,-8, -152,-8,-150
+60,0,0, 137,141,-17, -154,-18,-157
+60,0,0, 146,143,-14, -158,-15,-155
+60,0,0, 141,147,-14, -156,-16,-161
+60,0,0, 143,145,-15, -154,-15,-161
+60,0,0, 141,147,-13, -153,-12,-158
+60,0,0, 141,145,-17, -156,-18,-162
+60,0,0, 144,151,-10, -156,-11,-160
+60,0,0, 142,146,-14, -156,-16,-159
+60,0,0, 149,152,-11, -156,-10,-161
+60,0,0, 137,144,-11, -152,-13,-157
+60,0,0, 135,141,-17, -151,-12,-155
+60,0,0, 136,142,-16, -152,-13,-156
+60,0,0, 144,147,-6, -151,-9,-151
+60,0,0, 143,149,-11, -155,-10,-158
+60,0,0, 139,143,-14, -151,-11,-154
+60,0,0, 140,150,-6, -151,-8,-154
+60,0,0, 143,147,-12, -150,-7,-153
+60,0,0, 139,145,-7, -145,-8,-155
+60,0,0, 139,142,-16, -152,-14,-158
+60,0,0, 142,147,-12, -154,-11,-157
+60,0,0, 137,139,-22, -154,-18,-159
+60,0,0, 138,140,-12, -152,-15,-150
+60,0,0, 138,146,-8, -147,-6,-152
+60,0,0, 144,148,-14, -155,-15,-160
+60,0,0, 141,141,-15, -151,-12,-156
+60,0,0, 141,144,-19, -154,-18,-163
+60,0,0, 135,140,-15, -150,-16,-153
+60,0,0, 140,142,-13, -149,-12,-157
+60,0,0, 150,157,-8, -154,-3,-160
+60,0,0, 141,145,-12, -153,-11,-154
+60,0,0, 147,146,-14, -156,-15,-158
+60,0,0, 140,142,-15, -150,-13,-155
+60,0,0, 136,139,-18, -147,-16,-153
+60,0,0, 136,139,-17, -152,-18,-153
+60,0,0, 146,147,-6, -149,-2,-150
+60,0,0, 140,143,-19, -155,-18,-159
+60,0,0, 142,141,-16, -152,-11,-154
+60,0,0, 137,142,-13, -148,-9,-154
+60,0,0, 143,149,-18, -153,-14,-163
+60,0,0, 145,153,-5, -152,-6,-158
+60,0,0, 130,135,-27, -154,-25,-158
+60,0,0, 146,151,-6, -155,-7,-154
+60,0,0, 146,151,-11, -154,-7,-158
+60,0,0, 137,142,-16, -151,-12,-157
+60,0,0, 142,145,-12, -152,-11,-155
+60,0,0, 143,144,-17, -156,-15,-161
+60,0,0, 138,139,-13, -148,-13,-149
+60,0,0, 139,141,-15, -148,-11,-152
+60,0,0, 142,144,-13, -148,-12,-155
+60,0,0, 137,140,-20, -152,-18,-157
+60,0,0, 141,141,-13, -154,-15,-153
+60,0,0, 145,146,-13, -151,-8,-154
+60,0,0, 139,143,-15, -148,-12,-156
+60,0,0, 138,140,-16, -151,-14,-150
+60,0,0, 140,143,-14, -150,-12,-155
+60,0,0, 138,140,-12, -152,-13,-153
+60,0,0, 139,144,-9, -147,-9,-155
+60,0,0, 141,142,-18, -154,-17,-160
+60,0,0, 142,142,-13, -154,-15,-152
+60,0,0, 133,135,-18, -148,-19,-154
+60,0,0, 138,134,-19, -155,-21,-152
+60,0,0, 145,147,-12, -156,-13,-159
+60,0,0, 148,149,-5, -155,-6,-153
+60,0,0, 143,142,-14, -157,-16,-156
+60,0,0, 135,139,-21, -152,-18,-158
+60,0,0, 143,147,-6, -150,-8,-151
+60,0,0, 137,143,-19, -154,-20,-163
+60,0,0, 136,140,-16, -150,-17,-156
+60,0,0, 146,147,-8, -156,-12,-154
+60,0,0, 145,145,-13, -152,-9,-153
+60,0,0, 137,140,-18, -151,-14,-157
+60,0,0, 137,142,-18, -153,-17,-158
+60,0,0, 143,146,-14, -154,-11,-159
+60,0,0, 141,143,-10, -149,-11,-156
+60,0,0, 139,138,-13, -151,-14,-156
+60,0,0, 135,138,-12, -148,-15,-153
+60,0,0, 132,140,-13, -148,-15,-153
+60,0,0, 141,147,-12, -154,-12,-158
+60,0,0, 143,146,-7, -150,-8,-152
+60,0,0, 137,140,-17, -152,-14,-158
+60,0,0, 137,139,-12, -149,-13,-152
+60,0,0, 145,150,-7, -152,-5,-158
+60,0,0, 140,142,-9, -148,-10,-152
+60,0,0, 144,151,-4, -145,-3,-155
+60,0,0, 134,136,-15, -147,-14,-154
+60,0,0, 139,140,-10, -148,-10,-147
+60,0,0, 142,146,-10, -152,-7,-153
+60,0,0, 154,153,6, -145,7,-149
+60,0,0, 140,137,-14, -150,-14,-152
+60,0,0, 138,143,-17, -151,-13,-155
+60,0,0, 149,151,-2, -151,0,-154
diff --git a/motors/fet12/calib_data_60b.csv b/motors/fet12/calib_data_60b.csv
new file mode 100644
index 0000000..e44b7ff
--- /dev/null
+++ b/motors/fet12/calib_data_60b.csv
@@ -0,0 +1,61 @@
+ 0,60,0, -118,-115,-130, -10,-126,-12
+ 0,60,0, -108,-106,-120, -9,-116,-10
+ 0,60,0, -114,-113,-133, -12,-125,-17
+ 0,60,0, -116,-114,-131, -11,-128,-14
+ 0,60,0, -112,-109,-124, -12,-120,-10
+ 0,60,0, -111,-109,-120, -7,-115,-10
+ 0,60,0, -112,-111,-130, -15,-124,-12
+ 0,60,0, -100,-98,-110, -4,-108,-6
+ 0,60,0, -114,-110,-117, -1,-116,-4
+ 0,60,0, -116,-115,-122, -7,-121,-4
+ 0,60,0, -123,-118,-132, -7,-130,-12
+ 0,60,0, -106,-103,-119, -8,-115,-11
+ 0,60,0, -120,-119,-130, -11,-128,-9
+ 0,60,0, -116,-116,-130, -6,-122,-9
+ 0,60,0, -117,-115,-126, -5,-124,-9
+ 0,60,0, -107,-104,-117, -6,-112,-9
+ 0,60,0, -103,-106,-120, -11,-120,-13
+ 0,60,0, -114,-114,-129, -9,-123,-15
+ 0,60,0, -102,-100,-111, -9,-109,-12
+ 0,60,0, -114,-110,-124, -8,-120,-11
+ 0,60,0, -115,-113,-124, -6,-121,-6
+ 0,60,0, -118,-118,-128, -6,-125,-6
+ 0,60,0, -115,-111,-127, -11,-127,-15
+ 0,60,0, -112,-109,-129, -11,-123,-13
+ 0,60,0, -115,-111,-129, -11,-125,-14
+ 0,60,0, -117,-115,-128, -12,-128,-13
+ 0,60,0, -114,-110,-126, -10,-124,-15
+ 0,60,0, -115,-111,-129, -11,-126,-14
+ 0,60,0, -115,-116,-129, -9,-124,-11
+ 0,60,0, -116,-112,-130, -11,-127,-10
+ 0,60,0, -116,-113,-119, -4,-120,-5
+ 0,60,0, -109,-108,-126, -14,-123,-16
+ 0,60,0, -113,-115,-130, -11,-127,-12
+ 0,60,0, -119,-118,-138, -11,-131,-22
+ 0,60,0, -104,-104,-120, -10,-117,-12
+ 0,60,0, -113,-108,-115, -3,-116,-7
+ 0,60,0, -108,-106,-122, -6,-119,-14
+ 0,60,0, -110,-107,-113, -6,-114,-5
+ 0,60,0, -104,-102,-113, -7,-110,-8
+ 0,60,0, -118,-118,-132, -8,-127,-11
+ 0,60,0, -117,-113,-127, -7,-123,-14
+ 0,60,0, -118,-115,-128, -9,-125,-10
+ 0,60,0, -109,-108,-128, -12,-122,-13
+ 0,60,0, -103,-104,-113, -8,-113,-10
+ 0,60,0, -112,-110,-130, -11,-125,-12
+ 0,60,0, -115,-112,-126, -7,-122,-12
+ 0,60,0, -115,-112,-127, -6,-122,-12
+ 0,60,0, -113,-112,-127, -10,-123,-13
+ 0,60,0, -112,-112,-128, -11,-123,-15
+ 0,60,0, -115,-108,-121, -4,-121,-12
+ 0,60,0, -111,-109,-126, -7,-121,-14
+ 0,60,0, -112,-111,-128, -7,-122,-12
+ 0,60,0, -104,-101,-114, -7,-109,-12
+ 0,60,0, -117,-120,-135, -4,-129,-9
+ 0,60,0, -119,-117,-128, -7,-124,-11
+ 0,60,0, -111,-112,-122, -7,-118,-6
+ 0,60,0, -119,-116,-125, -4,-122,-9
+ 0,60,0, -112,-109,-121, -9,-120,-7
+ 0,60,0, -116,-115,-130, -6,-124,-14
+ 0,60,0, -116,-115,-127, -8,-123,-9
+ 0,60,0, -104,-105,-118, -10,-114,-12
diff --git a/motors/fet12/calib_data_60c.csv b/motors/fet12/calib_data_60c.csv
new file mode 100644
index 0000000..eb1bc52
--- /dev/null
+++ b/motors/fet12/calib_data_60c.csv
@@ -0,0 +1,49 @@
+ 0,0,60, -14,-18,135, 151,135,156
+ 0,0,60, -11,-8,139, 155,142,148
+ 0,0,60, -1,2,156, 156,155,155
+ 0,0,60, -14,-15,140, 155,138,158
+ 0,0,60, -6,-7,143, 155,148,157
+ 0,0,60, -16,-15,133, 158,136,156
+ 0,0,60, -17,-20,135, 155,134,155
+ 0,0,60, -14,-11,138, 149,139,156
+ 0,0,60, -19,-15,135, 157,135,152
+ 0,0,60, -18,-17,140, 162,141,161
+ 0,0,60, -8,-6,149, 157,147,156
+ 0,0,60, -3,-6,142, 149,144,155
+ 0,0,60, -6,-2,144, 150,145,152
+ 0,0,60, -17,-15,138, 155,138,156
+ 0,0,60, -11,-8,145, 151,142,156
+ 0,0,60, -15,-15,140, 150,136,158
+ 0,0,60, -16,-12,145, 156,141,156
+ 0,0,60, -20,-17,135, 156,134,159
+ 0,0,60, -6,0,151, 149,148,155
+ 0,0,60, -17,-14,140, 156,141,156
+ 0,0,60, -16,-13,141, 161,146,157
+ 0,0,60, -9,-9,150, 159,146,162
+ 0,0,60, -16,-13,136, 150,132,150
+ 0,0,60, -22,-19,140, 159,140,163
+ 0,0,60, -14,-13,140, 155,138,156
+ 0,0,60, -14,-15,139, 158,139,158
+ 0,0,60, -8,-6,150, 157,150,159
+ 0,0,60, -4,0,153, 158,154,156
+ 0,0,60, -16,-11,138, 158,140,152
+ 0,0,60, -14,-15,138, 156,140,158
+ 0,0,60, 6,7,157, 156,158,155
+ 0,0,60, -2,-4,150, 151,146,157
+ 0,0,60, -15,-14,139, 153,135,154
+ 0,0,60, -7,-6,143, 155,146,157
+ 0,0,60, -7,-9,144, 156,146,155
+ 0,0,60, -10,-8,150, 156,150,162
+ 0,0,60, -7,-4,144, 155,147,152
+ 0,0,60, -18,-14,143, 158,142,160
+ 0,0,60, -10,-10,140, 155,141,154
+ 0,0,60, -19,-19,135, 156,135,156
+ 0,0,60, -7,-11,140, 151,140,153
+ 0,0,60, -3,-6,151, 157,150,158
+ 0,0,60, -12,-10,143, 156,141,153
+ 0,0,60, -10,-4,148, 155,146,156
+ 0,0,60, -12,-9,140, 153,141,151
+ 0,0,60, -7,-9,146, 154,148,156
+ 0,0,60, -21,-19,137, 160,136,157
+ 0,0,60, -20,-21,131, 158,135,154
+ 0,0,60, -14,-12,141, 157,141,155
diff --git a/motors/fet12/calib_sensors.py b/motors/fet12/calib_sensors.py
new file mode 100755
index 0000000..777ea37
--- /dev/null
+++ b/motors/fet12/calib_sensors.py
@@ -0,0 +1,43 @@
+#!/usr/bin/python3
+import sys
+import numpy as np
+
+# Note on associated data files:
+# calib_data_60*.csv has each output channel set at a constant value of 60.
+# calib_data_6030.csv actuates two channels.
+
+def calibrate(fnames):
+ """Do fitting to calibrate ADC data given csv files.
+
+ CSVs should be of format:
+ command_a, command_b, command_c, adc0, adc0, adc1, adc2, adc1, adc2
+ Where The adc columns in this case are the 6 samples taken from the
+ ADC where each pair of columns with the same name correspond with
+ the same measurement (we average samples that are of the same value
+ because otherwise the solution matrix can't be solved for in a stable
+ manner).
+ """
+ data = np.zeros((1, 9))
+ for fname in fnames:
+ data = np.vstack((data, np.genfromtxt(fname, delimiter=',')))
+ data = data[1:, :]
+
+ if data.shape[1] == 9:
+ data[:, 3] = (data[:, 3] + data[:, 4]) / 2.0
+ data[:, 4] = (data[:, 5] + data[:, 7]) / 2.0
+ data[:, 5] = (data[:, 6] + data[:, 8]) / 2.0
+ data = data[:, :6]
+
+ b = data[:, 0:3]
+ b = b - np.tile(np.mean(b, axis=1), (3, 1)).T
+ # Vcc / 3000 / R
+ b *= 30.8 / 3000.0 / 0.0084
+ A = data[:, 3:]
+
+ return np.linalg.lstsq(A, b[:])[0].T
+
+if __name__ == "__main__":
+ if len(sys.argv) < 2:
+ print("Need filenames for data")
+ sys.exit(1)
+ print(calibrate(sys.argv[1:]))
diff --git a/motors/fet12/current_equalize.py b/motors/fet12/current_equalize.py
index 698983f..ea44916 100755
--- a/motors/fet12/current_equalize.py
+++ b/motors/fet12/current_equalize.py
@@ -2,8 +2,9 @@
import numpy
import sys
+import calib_sensors
-def main():
+def manual_calibrate():
# 38 27 -84
# 36 -64 39
# -74 21 35
@@ -17,6 +18,13 @@
[-current / 2.0, current, -current / 2.0],
[-current / 2.0, -current / 2.0, current]])
transform = I * numpy.linalg.inv(Is)
+ return transform
+
+def main():
+ transform = manual_calibrate()
+
+ if len(sys.argv) > 1:
+ transform = calib_sensors.calibrate(sys.argv[1:])
print("#ifndef MOTORS_FET12_CURRENT_MATRIX_")
print("#define MOTORS_FET12_CURRENT_MATRIX_")
diff --git a/motors/fet12/fet12v2.cc b/motors/fet12/fet12v2.cc
index d95d4a2..5f8568b 100644
--- a/motors/fet12/fet12v2.cc
+++ b/motors/fet12/fet12v2.cc
@@ -10,6 +10,7 @@
#include "motors/fet12/motor_controls.h"
#include "motors/motor.h"
#include "motors/peripheral/adc.h"
+#include "motors/peripheral/adc_dma.h"
#include "motors/peripheral/can.h"
#include "motors/peripheral/uart.h"
#include "motors/util.h"
@@ -25,6 +26,10 @@
constexpr double kR = 0.0084;
struct Fet12AdcReadings {
+ // Averages of the pairs of ADC DMA channels corresponding with each channel
+ // pair. Individual values in motor_currents correspond to current sensor
+ // values, rather than the actual currents themselves (and so they still need
+ // to be decoupled).
int16_t motor_currents[3];
int16_t throttle, fuse_voltage;
};
@@ -57,34 +62,9 @@
PORTB_PCR3 = PORT_PCR_MUX(0);
}
-Fet12AdcReadings AdcReadFet12(const DisableInterrupts &) {
- Fet12AdcReadings r;
-
- ADC1_SC1A = 5;
- ADC0_SC1A = 23;
- while (!(ADC1_SC1A & ADC_SC1_COCO)) {
- }
- ADC1_SC1A = 6;
- r.motor_currents[0] = static_cast<int16_t>(ADC1_RA) - 2032;
- while (!(ADC0_SC1A & ADC_SC1_COCO)) {
- }
- ADC0_SC1A = 13;
- r.throttle = static_cast<int16_t>(ADC0_RA);
- while (!(ADC1_SC1A & ADC_SC1_COCO)) {
- }
- ADC1_SC1A = 7;
- r.motor_currents[1] = static_cast<int16_t>(ADC1_RA) - 2032;
- while (!(ADC0_SC1A & ADC_SC1_COCO)) {
- }
- r.fuse_voltage = static_cast<int16_t>(ADC0_RA);
- while (!(ADC1_SC1A & ADC_SC1_COCO)) {
- }
- r.motor_currents[2] = static_cast<int16_t>(ADC1_RA) - 2032;
-
- return r;
-}
-
::std::atomic<Motor *> global_motor{nullptr};
+::std::atomic<teensy::AdcDmaSampler *> global_adc_dma{nullptr};
+
::std::atomic<teensy::InterruptBufferedUart *> global_stdout{nullptr};
extern "C" {
@@ -154,20 +134,31 @@
DebugBuffer global_debug_buffer;
void ftm0_isr(void) {
- const auto wrapped_encoder =
- global_motor.load(::std::memory_order_relaxed)->wrapped_encoder();
+ static uint32_t i = 0;
+ teensy::AdcDmaSampler *const adc_dma =
+ global_adc_dma.load(::std::memory_order_relaxed);
+
Fet12AdcReadings adc_readings;
- {
- DisableInterrupts disable_interrupts;
- adc_readings = AdcReadFet12(disable_interrupts);
+ // TODO(Brian): Switch to the DMA interrupt instead of spinning.
+ while (!adc_dma->CheckDone()) {
}
+
+ adc_readings.motor_currents[0] =
+ (adc_dma->adc_result(0, 0) + adc_dma->adc_result(0, 1)) / 2;
+ adc_readings.motor_currents[1] =
+ (adc_dma->adc_result(0, 2) + adc_dma->adc_result(1, 1)) / 2;
+ adc_readings.motor_currents[2] =
+ (adc_dma->adc_result(1, 0) + adc_dma->adc_result(1, 2)) / 2;
+ adc_readings.throttle = adc_dma->adc_result(0, 3);
const ::std::array<float, 3> decoupled =
DecoupleCurrents(adc_readings.motor_currents);
-
+ adc_dma->Reset();
+ const uint32_t wrapped_encoder =
+ global_motor.load(::std::memory_order_relaxed)->wrapped_encoder();
+#if 1
const BalancedReadings balanced =
BalanceSimpleReadings(decoupled);
- static int i = 0;
static float fuse_badness = 0;
static uint32_t cycles_since_start = 0u;
@@ -265,17 +256,6 @@
->SetGoalCurrent(goal_current);
global_motor.load(::std::memory_order_relaxed)
->HandleInterrupt(balanced, wrapped_encoder);
-#else
- (void)balanced;
- FTM0->SC &= ~FTM_SC_TOF;
- FTM0->C0V = 0;
- FTM0->C1V = 0;
- FTM0->C2V = 0;
- FTM0->C3V = 0;
- FTM0->C4V = 0;
- FTM0->C5V = 60;
- FTM0->PWMLOAD = FTM_PWMLOAD_LDOK;
-#endif
global_debug_buffer.count.fetch_add(1);
@@ -322,7 +302,9 @@
global_debug_buffer.size.fetch_add(1);
}
+ ++i;
if (buffer_size == global_debug_buffer.samples.size()) {
+ i = 0;
GPIOC_PCOR = (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4);
GPIOD_PCOR = (1 << 4) | (1 << 5);
@@ -340,11 +322,19 @@
PORTD_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(1);
PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
}
+#else
+#endif
+#else
+ FTM0->SC &= ~FTM_SC_TOF;
+ FTM0->C0V = 0;
+ FTM0->C1V = 0;
+ FTM0->C2V = 0;
+ FTM0->C3V = 0;
+ FTM0->C4V = 0;
+ FTM0->C5V = 0;
+ FTM0->PWMLOAD = FTM_PWMLOAD_LDOK;
+#endif
- ++i;
- if (i > 1000) {
- i = 0;
- }
}
} // extern "C"
@@ -501,8 +491,69 @@
motor.set_encoder_offset(810);
motor.set_deadtime_compensation(9);
ConfigurePwmFtm(FTM0);
+
+ FTM2->CONF = FTM_CONF_GTBEEN;
+ FTM2->MODE = FTM_MODE_WPDIS;
+ FTM2->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+ FTM2->SC = FTM_SC_CLKS(0) /* Disable counting for now */;
+ FTM2->CNTIN = 0;
+ FTM2->CNT = 0;
+ // TODO(Brian): Don't duplicate this.
+ FTM2->MOD = BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY;
+ FTM2->OUTINIT = 0;
+ // All of the channels are active high.
+ FTM2->POL = 0;
+ FTM2->SYNCONF = FTM_SYNCONF_HWWRBUF | FTM_SYNCONF_SWWRBUF |
+ FTM_SYNCONF_SWRSTCNT | FTM_SYNCONF_SYNCMODE;
+ // Don't want any intermediate loading points.
+ FTM2->PWMLOAD = 0;
+
+ // Need to set them to some kind of output mode so we can actually change
+ // them.
+ FTM2->C0SC = FTM_CSC_MSA;
+ FTM2->C1SC = FTM_CSC_MSA;
+
+ // This has to happen after messing with SYNCONF, and should happen after
+ // messing with various other things so the values can get flushed out of the
+ // buffers.
+ FTM2->SYNC =
+ FTM_SYNC_SWSYNC /* Flush everything out right now */ |
+ FTM_SYNC_CNTMAX /* Load new values at the end of the cycle */;
+ // Wait for the software synchronization to finish.
+ while (FTM2->SYNC & FTM_SYNC_SWSYNC) {
+ }
+ FTM2->SC = FTM_SC_CLKS(1) /* Use the system clock */ |
+ FTM_SC_PS(0) /* Don't prescale the clock */;
+ // TODO:
+ //FTM2->MODE &= ~FTM_MODE_WPDIS;
+
+ FTM2->EXTTRIG = FTM_EXTTRIG_CH0TRIG | FTM_EXTTRIG_CH1TRIG;
+
+ teensy::AdcDmaSampler adc_dma;
+ // ADC0_Dx0 is 1-0
+ // ADC0_Dx2 is 1-2
+ // ADC0_Dx3 is 2-0
+ // ADC1_Dx0 is 2-0
+ // ADC1_Dx3 is 1-0
+ // Sample 0: 1-2,2-0
+ // Sample 1: 1-2,1-0
+ // Sample 2: 1-0,2-0
+ // Sample 3: 23(SENSE0),18(VIN)
+ adc_dma.set_adc0_samples({V_ADC_ADCH(2) | M_ADC_DIFF,
+ V_ADC_ADCH(2) | M_ADC_DIFF,
+ V_ADC_ADCH(0) | M_ADC_DIFF, V_ADC_ADCH(23)});
+ adc_dma.set_adc1_samples({V_ADC_ADCH(0) | M_ADC_DIFF,
+ V_ADC_ADCH(3) | M_ADC_DIFF,
+ V_ADC_ADCH(0) | M_ADC_DIFF, V_ADC_ADCH(18)});
+ adc_dma.set_ftm_delays({&FTM2->C0V, &FTM2->C1V});
+ adc_dma.set_pdb_input(PDB_IN_FTM2);
+
+ adc_dma.Initialize();
+ FTM0->CONF = FTM_CONF_GTBEEN;
motor.Init();
global_motor.store(&motor, ::std::memory_order_relaxed);
+ global_adc_dma.store(&adc_dma, ::std::memory_order_relaxed);
+
// Output triggers to things like the PDBs on initialization.
FTM0_EXTTRIG = FTM_EXTTRIG_INITTRIGEN;
// Don't let any memory accesses sneak past here, because we actually
@@ -529,7 +580,11 @@
printf("Zeroed motor!\n");
// Give stuff a chance to recover from interrupts-disabled.
delay(100);
+ adc_dma.Reset();
motor.Start();
+ // Now poke the GTB to actually start both timers.
+ FTM0->CONF = FTM_CONF_GTBEEN | FTM_CONF_GTBEOUT;
+
NVIC_ENABLE_IRQ(IRQ_FTM0);
GPIOC_PSOR = 1 << 5;