diff options
Diffstat (limited to 'CODE/debugprewk/debug/motordriver/motordriver.ino')
| -rw-r--r-- | CODE/debugprewk/debug/motordriver/motordriver.ino | 386 |
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; +} |