code

1 files changed, 386 insertions, 0 deletions

diff --git a/CODE/debugprewk/debug/motordriver/motordriver.ino b/CODE/debugprewk/debug/motordriver/motordriver.ino
new file mode 100644
index 0000000..2f1c225
--- /dev/null
+++ b/CODE/debugprewk/debug/motordriver/motordriver.ino
@@ -0,0 +1,386 @@
+#include <Adafruit_MCP23X17.h>
+#include <TimerThree.h>
+//moveohmi = https://stackoverflow.com/questions/3748037/how-to-control-a-kiwi-drive-robot
+//https://www.pololu.com/product/2997
+//https://www.pololu.com/product/4861
+//https://ww1.microchip.com/downloads/en/devicedoc/20001952c.pdf
+
+
+#define ArrayElementCount(needRPM) (sizeof(needRPM) / sizeof(needRPM[0]))
+
+// motor one/A  front
+#define  m1enA 3   // GPA3  PWM1
+#define  m1enB 2   // GPA2
+#define  m1pwm1 0  // GPA0
+#define  m1pwm2 1  // GPA1
+#define  m1encA 12 // GPB4
+#define  m1encB 13 // GPB5
+// motor two/B  left
+#define  m2enA 7   // GPA7  PWM2
+#define  m2enB 6   // GPA6
+#define  m2pwm1 4  // GPA4
+#define  m2pwm2 5  // GPA5
+#define  m2encA 14 // GPB6
+#define  m2encB 15 // GPB7
+// motor three/C    right
+#define  m3enA 11  // GPB3  PWM3
+#define  m3enB 10  // GPB2
+#define  m3pwm1 8  // GPB0
+#define  m3pwm2 9  // GPB1
+#define  m3encA 24   // teensy port 24
+#define  m3encB 9   // teensy port 9
+
+double sp = 1200.000; //speed in RPM ~0-1800
+const int maxspeed = 1000;
+
+//calibration data
+int Amotordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910};
+int Bmotordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910};
+int Cmotordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910};
+
+int AneedRPM = 0;
+int BneedRPM = 0;
+int CneedRPM = 0;
+
+
+//kiwidrive
+double dirA = 0.00;
+double dirB = 0.00;
+double dirC = 0.00;
+double spdA = 0.00;
+double spdB = 0.00;
+double spdC = 0.00;
+double dirrad = 0.00;
+double pi = 3.1415926535897;
+
+//encoder
+double Arpm = 0.00;
+volatile long Aencount = 0;
+unsigned long ALtime = 0;
+unsigned long ACtime = 0;
+unsigned long APtime = 0;
+double Brpm = 0.00;
+volatile long Bencount = 0;
+unsigned long BLtime = 0;
+unsigned long BCtime = 0;
+unsigned long BPtime = 0;
+double Crpm = 0.00;
+volatile long Cencount = 0;
+unsigned long CLtime = 0;
+unsigned long CCtime = 0;
+unsigned long CPtime = 0;
+
+//softPWM
+volatile int PWMval1 = 0;   // PWM m1enA value 0-100
+volatile int PWMval2 = 0;   // PWM m2enA value 0-100
+volatile int PWMval3 = 0;   // PWM m3enA value 0-100
+
+
+Adafruit_MCP23X17 mcp2;
+
+void setup(){
+  if (!mcp2.begin_I2C(0x22)) {
+    Serial.println("Error initializing MCP23017 at 0x22.");
+    //while (1);
+  }
+  // set all the motor control pins
+  mcp2.pinMode(m1enA, OUTPUT);
+  mcp2.pinMode(m1enB, OUTPUT);
+  mcp2.pinMode(m2enA, OUTPUT);
+  mcp2.pinMode(m2enB, OUTPUT);
+  mcp2.pinMode(m3enA, OUTPUT);
+  mcp2.pinMode(m3enB, OUTPUT);
+  mcp2.pinMode(m1pwm1, OUTPUT);
+  mcp2.pinMode(m1pwm2, OUTPUT);
+  mcp2.pinMode(m2pwm1, OUTPUT);
+  mcp2.pinMode(m2pwm2, OUTPUT);
+  mcp2.pinMode(m3pwm1, OUTPUT);
+  mcp2.pinMode(m3pwm2, OUTPUT);
+
+  //set encoder pins
+  mcp2.pinMode(m1encA, INPUT);
+  mcp2.pinMode(m1encB, INPUT);
+  mcp2.pinMode(m2encA, INPUT);
+  mcp2.pinMode(m2encB, INPUT);
+  pinMode(m3encA, INPUT);
+  pinMode(m3encB, INPUT);
+  attachInterrupt(digitalPinToInterrupt(m1encA), AencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m1encB), AencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m2encA), BencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m2encB), BencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m3encA), CencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m3encB), CencoderISR, CHANGE);
+
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM1); 
+  Timer3.attachInterrupt(updatePWM2); 
+  Timer3.attachInterrupt(updatePWM3); 
+  
+}
+void updatePWM1() {
+  static int counter1 = 0;
+
+  if (counter1 < PWMval1) {
+    mcp2.digitalWrite(m1enA, HIGH);
+  } else {
+    mcp2.digitalWrite(m1enA, LOW);
+  }
+
+  counter1++;
+  if (counter1 >= 100) {
+    counter1 = 0;
+  }
+}
+void updatePWM2() {
+  static int counter2 = 0;
+
+  if (counter2 < PWMval2) {
+    mcp2.digitalWrite(m2enA, HIGH);
+  } else {
+    mcp2.digitalWrite(m2enA, LOW);
+  }
+
+  counter2++;
+  if (counter2 >= 100) {
+    counter2 = 0;
+  }
+}
+void updatePWM3() {
+  static int counter3 = 0;
+
+  if (counter3 < PWMval3) {
+    mcp2.digitalWrite(m3enA, HIGH);
+  } else {
+    mcp2.digitalWrite(m3enA, LOW);
+  }
+
+  counter3++;
+  if (counter3 >= 100) {
+    counter3 = 0;
+  }
+}
+void mcpsoftpwm(int pin, int vall){
+  //M1enA = 3   = GPA3
+  //M2enA = 7   = GPA7
+  //M3enA = 11  = GPB3
+  Serial.print("softPWM pin: ");
+  Serial.print(pin);
+  Serial.print(", value: ");
+  Serial.println(vall);
+  if (pin = 3){
+    PWMval1 = vall;
+    Serial.print("Motor 1 enA PWM set");
+  }
+  if (pin = 7){
+    PWMval2 = vall;
+    Serial.print("Motor 2 enA PWM set");
+  }
+  if (pin = 11){
+    PWMval3 = vall;
+    Serial.print("Motor 3 enA PWM set");
+  }
+  
+}
+void AencoderISR() {
+  Aencount++;
+}
+void BencoderISR() {
+  Bencount++;
+}
+void CencoderISR() {
+  Cencount++;
+}
+void Areadenc() {
+  ACtime = micros();
+  APtime = ACtime - ALtime;
+  ALtime = ACtime;
+  Arpm = (Aencount / 211.2) * (60000000.0 / APtime);
+  Serial.print("Motor A RPM: ");
+  Serial.print(Arpm);
+  Serial.print("    Sample period:");
+  Serial.print(APtime);
+  Serial.print("ms, encoder count:");
+  Serial.println(Aencount);
+  Aencount = 0;
+}
+void Breadenc() {
+  BCtime = micros();
+  BPtime = BCtime - BLtime;
+  BLtime = BCtime;
+  Brpm = (Bencount / 211.2) * (60000000.0 / BPtime);
+  Serial.print("Motor B RPM: ");
+  Serial.print(Brpm);
+  Serial.print("    Sample period: ");
+  Serial.print(BPtime);
+  Serial.print(" ms, encoder count: ");
+  Serial.println(Bencount);
+  Bencount = 0;
+}
+void Creadenc() {
+  CCtime = micros();
+  CPtime = CCtime - CLtime;
+  CLtime = CCtime;
+  Crpm = (Cencount / 211.2) * (60000000.0 / CPtime);
+  Serial.print("Motor C RPM: ");
+  Serial.print(Crpm);
+  Serial.print("    Sample period: ");
+  Serial.print(CPtime);
+  Serial.print(" ms, encoder count: ");
+  Serial.println(Cencount);
+  Cencount = 0;
+}
+void Aresetenc(){
+  ACtime = micros();
+  ALtime = ACtime;
+  Aencount = 0;
+}
+void Bresetenc(){
+  BCtime = micros();
+  BLtime = BCtime;
+  Bencount = 0;
+}
+void Cresetenc(){
+  CCtime = micros();
+  CLtime = CCtime;
+  Cencount = 0;
+}
+void Amove(double Rsped) {
+  double sped = nearest(Rsped, Amotordata, ArrayElementCount(Amotordata));
+  spdA = abs(sped) * 0.1;
+  mcpsoftpwm(m1enA, spdA);
+  mcp2.digitalWrite(m1enB, LOW);
+  if (sped < 0) {
+    Serial.print("Motor A back ");
+    mcp2.digitalWrite(m1pwm1, LOW);
+    mcp2.digitalWrite(m1pwm2, HIGH);
+  } else {
+    Serial.print("Motor A forward ");
+    mcp2.digitalWrite(m1pwm1, HIGH);
+    mcp2.digitalWrite(m1pwm2, LOW);
+  } 
+  Serial.println(spdA);
+}
+void Bmove(double Rsped) {
+  double sped = nearest(Rsped, Bmotordata, ArrayElementCount(Bmotordata));
+  spdB = abs(sped) * 0.1;
+  mcpsoftpwm(m2enA, spdB);
+  mcp2.digitalWrite(m2enB, LOW);
+  if (sped < 0) {
+    Serial.print("Motor B back ");
+    mcp2.digitalWrite(m2pwm1, LOW);
+    mcp2.digitalWrite(m2pwm2, HIGH);
+  } else {
+    Serial.print("Motor B forward ");
+    mcp2.digitalWrite(m2pwm1, HIGH);
+    mcp2.digitalWrite(m2pwm2, LOW);
+  } 
+  Serial.println(spdB);
+}
+void Cmove(double Rsped) {
+  double sped = nearest(Rsped, Cmotordata, ArrayElementCount(Cmotordata));
+  spdC = abs(sped) * 0.1;
+  mcpsoftpwm(m3enA, spdC);
+  mcp2.digitalWrite(m3enB, LOW);
+  if (sped < 0) {
+    Serial.print("Motor C back ");
+    mcp2.digitalWrite(m3pwm1, LOW);
+    mcp2.digitalWrite(m3pwm2, HIGH);
+  } else {
+    Serial.print("Motor C forward ");
+    mcp2.digitalWrite(m3pwm1, HIGH);
+    mcp2.digitalWrite(m3pwm2, LOW);
+  } 
+  Serial.println(spdC);
+}
+
+void moveohmi(int speed, int direction){
+  Amove(0);
+  Bmove(0);
+  Cmove(0);
+
+  double dirrad = (direction*71.0000) / 4068.0000;  
+  Serial.println(dirrad);
+  //vector ontb in x en y
+  double y = sin(dirrad);
+  double x = cos(dirrad);
+  Serial.println((String)"x:"+x+" y:"+y);
+  double spdeA = y*speed;
+  double spdeB = (-0.500*x - 0.8660054*y)*speed;
+  double spdeC = (-0.500*x + 0.8660054*y)*speed;
+  Amove(spdeA);
+  Bmove(spdeB);
+  Cmove(spdeC);
+  Serial.println();
+  Serial.println();
+}
+void rotate(int rodeg, int speed){
+  //use compas heading
+  //inacurate compas data
+}
+void demo(){
+  Amove(sp);
+  Bmove(sp);
+  Cmove(sp); 
+  Serial.println("seperate A B C rotation");
+  delay(1000);
+  
+  Amove(0);
+  Bmove(0);
+  Cmove(0);
+  Serial.println("seperate A B C stop");
+  delay(1000);
+
+  Amove(-sp);
+  Bmove(-sp);
+  Cmove(-sp);
+  Serial.println("seperate A B C -rotation");
+  delay(1000);
+
+  moveohmi(0, 0);
+  Serial.println("ohmi stop");
+  delay(1000);
+
+  rotate(90, sp);
+  Serial.println("rotation function 90deg");
+  rotate(90, sp);
+  Serial.println("rotation function -90deg");
+  delay(1000);
+
+  moveohmi(sp, 35);
+  Serial.println("ohmi move 35deg");
+  delay(1000);
+
+  moveohmi(sp, 240);
+  Serial.println("ohmi move 215deg");
+  delay(1000);
+
+  for(int i = 0; i<360; i++){
+    moveohmi(sp, i);
+    Serial.println(i);
+    delay(50);
+  }
+  Serial.println("ohmi move circle");
+  delay(1000);
+}
+void loop(){
+  demo();
+  delay(100);
+
+}
+int nearest(int x, int myArray[], int elements)
+{
+  //https://forum.arduino.cc/t/search-for-nearest-number-in-an-array-help/245843/5
+  int idx = 0; // by default near first element
+
+  int distance = abs(myArray[idx] - x);
+  for (int i = 1; i < elements; i++)
+  {
+    int d = abs(myArray[i] - x);
+    if (d < distance)
+    {
+      idx = i;
+      distance = d;
+    }
+  }
+  return idx;
+}