UPDATE FOLDER STUCTURE

MOET APARTE_PCB NOG VERWIJDERTEN WINDOWS HEEFT BEST VERGRENDELD

1 files changed, 0 insertions, 386 deletions

diff --git a/debug/motordriver/motordriver.ino b/debug/motordriver/motordriver.ino
deleted file mode 100644
index 2f1c225..0000000
--- a/debug/motordriver/motordriver.ino
+++ /dev/null
@@ -1,386 +0,0 @@
-#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;
-}