#include //https://www.arduino.cc/reference/en/language/functions/communication/wire/ #include "Adafruit_TCS34725.h" //https://github.com/adafruit/Adafruit_TCS34725 #include //https://github.com/adafruit/Adafruit-MCP23017-Arduino-Library/tree/master #include //https://github.com/pololu/lis3mdl-arduino #include //https://github.com/pololu/lsm6-arduino #include //https://www.arduino.cc/reference/en/libraries/timerthree/ const int minpulsbal = 1100; //minimum pulses needed to be able to say it has the ball in posesion double sp = 1200.000; //estemate needed speed in RPM ~0-1800 //i2c addr #define PCAADDR 0x70 //i2c address of the i2c multiplexer #define mcp1ADDR 0x20 //first portmultiplexer for ir sensors #define mcp2ADDR 0x22 //first portmultiplexer for motors //IR sensors on multiplex1 0x20 #define ir1 0 // GPA0 #define ir2 1 // GPA1 #define ir3 2 // GPA2 #define ir4 3 // GPA3 #define ir5 4 // GPA4 #define ir6 5 // GPA5 #define ir7 6 // GPA6 #define ir8 7 // GPA7 #define ir9 8 // GPB0 #define ir10 9 // GPB1 #define ir11 10 // GPB2 #define ir12 11 // GPB3 #define ir13 12 // GPB4 #define ir14 13 // GPB5 #define ir15 14 // GPB6 #define ir16 15 // GPB7 #define irBALL 25 // IR sensor to see if we have the ball, teensy pin NO 25 //motors on multiplex2 0x22 #define ArrayElementCount(needRPM) (sizeof(needRPM) / sizeof(needRPM[0])) //used to calculate array size const int maxspeed = 1000; //max speed in PWM value //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 }; //how much rpm the robot aims for int AneedRPM = 0; int BneedRPM = 0; int CneedRPM = 0; //kiwidrive values 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; #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 //encoder values 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 values 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 //settings #define dip1 2 //settings dip switch port 1 #define dip2 3 //settings dip switch port 2 #define dip3 4 //settings dip switch port 3 #define dip4 5 //settings dip switch port 4 #define buz 8 //buzzer //NOT USED CONNECT TO PULLUP #define P1 6 //pin 6 on Teensy #define P2 7 //pin 7 on Teensy #define P3 10 //pin 10 on Teensy #define P4 11 //pin 11 on Teensy #define P5 12 //pin 12 on Teensy #define P6 13 //pin 13 on Teensy #define P7 14 //pin 14 on Teensy #define P8 15 //pin 15 on Teensy #define P9 16 //pin 16 on Teensy #define P10 17 //pin 17 on Teensy #define P11 20 //pin 20 on Teensy #define P12 21 //pin 21 on Teensy #define P13 22 //pin 22 on Teensy #define P14 23 //pin 23 on Teensy #define P15 29 //pin 29 on Teensy #define P16 30 //pin 30 on Teensy #define P17 31 //pin 31 on Teensy #define P18 32 //pin 32 on Teensy #define P19 33 //pin 33 on Teensy #define P20 34 //pin 34 on Teensy #define P21 35 //pin 35 on Teensy #define P22 36 //pin 36 on Teensy #define P23 37 //pin 37 on Teensy #define P24 38 //pin 38 on Teensy #define P25 39 //pin 39 on Teensy #define P26 40 //pin 40 on Teensy #define P27 41 //pin 41 on Teensy //extra pins // #define EP1 25 //pin 41 on Teensy used for IR seeker #define EP2 26 //pin 41 on Teensy #define EP3 27 //pin 41 on Teensy #define EP4 28 //pin 41 on Teensy volatile long IRcount1 = 0; volatile long IRcount2 = 0; volatile long IRcount3 = 0; volatile long IRcount4 = 0; volatile long IRcount5 = 0; volatile long IRcount6 = 0; volatile long IRcount7 = 0; volatile long IRcount8 = 0; volatile long IRcount9 = 0; volatile long IRcount10 = 0; volatile long IRcount11 = 0; volatile long IRcount12 = 0; volatile long IRcount13 = 0; volatile long IRcount14 = 0; volatile long IRcount15 = 0; volatile long IRcount16 = 0; volatile long IRcountBALL = 0; int ballpos = 0; //location of ball in degrees int errorbuz = 0; //if there is an error someware it goes to 1 and the buzzer starts int ballinpos = 0; //is ball in our posesion. 0=no 1=yes int colsensdata[6] = { 99, 99, 99, 99, 99, 99 }; //data colorsensors int dip1val = 0; //value of dip, 0=LOW, 1=HIGH int dip2val = 0; int dip3val = 0; int dip4val = 0; int settingval = 1; Adafruit_MCP23X17 mcp1; // Instance for the first MCP23017 at address 0x20 Adafruit_MCP23X17 mcp2; // Instance for the second MCP23017 at address 0x22 Adafruit_TCS34725 tcs = Adafruit_TCS34725(); // Initialise color sensorwith default values (int time = 2.4ms, gain = 1x) String tlocd = ""; //temp data for goal location String locd = ""; //goal location in deg int gloc = 0; //goal location in deg int bool readloc = false; unsigned long datatime = 0; LIS3MDL mag; LSM6 imu; int front = 0; //enter calibration values LIS3MDL::vector m_min = { +942, +3436, -3323 }; LIS3MDL::vector m_max = { +1053, +3559, -3142 }; void setup() { Serial.begin(9600); //serial debug Serial1.begin(19200); //connection to openMV //connect unconected pins to GND pinMode(P1, INPUT_PULLUP); pinMode(P2, INPUT_PULLUP); pinMode(P3, INPUT_PULLUP); pinMode(P4, INPUT_PULLUP); pinMode(P5, INPUT_PULLUP); pinMode(P6, INPUT_PULLUP); pinMode(P7, INPUT_PULLUP); pinMode(P8, INPUT_PULLUP); pinMode(P9, INPUT_PULLUP); pinMode(P10, INPUT_PULLUP); pinMode(P11, INPUT_PULLUP); pinMode(P12, INPUT_PULLUP); pinMode(P13, INPUT_PULLUP); pinMode(P14, INPUT_PULLUP); pinMode(P15, INPUT_PULLUP); pinMode(P16, INPUT_PULLUP); pinMode(P17, INPUT_PULLUP); pinMode(P18, INPUT_PULLUP); pinMode(P19, INPUT_PULLUP); pinMode(P20, INPUT_PULLUP); pinMode(P21, INPUT_PULLUP); pinMode(P22, INPUT_PULLUP); pinMode(P23, INPUT_PULLUP); pinMode(P24, INPUT_PULLUP); pinMode(P25, INPUT_PULLUP); pinMode(P26, INPUT_PULLUP); pinMode(P27, INPUT_PULLUP); //connect unconected extrapins to GND pinMode(EP2, INPUT_PULLUP); pinMode(EP3, INPUT_PULLUP); pinMode(EP4, INPUT_PULLUP); // Initialize first MCP23017 with I2C address 0x20 if (!mcp1.begin_I2C(mcp1ADDR)) { Serial.println("(IR_sensors)Error initializing MCP23017 at 0x20."); } // Initialize second MCP23017 with I2C address 0x22 if (!mcp2.begin_I2C(mcp2ADDR)) { Serial.println("(Motors)Error initializing MCP23017 at 0x22."); } //magnetometer if (!mag.init()) { Serial.println("Error initializing LIS3MDL magnetometer."); } //IMU if (!imu.init()) { Serial.println("Error initializing LSM6 IMU."); } imu.enableDefault(); //get start heading to use as refreance point mag.read(); imu.read(); float heading = computeHeading(); front = heading; Serial.println(heading); // Configure pins for the IR seekers mcp1.pinMode(ir1, INPUT); mcp1.pinMode(ir2, INPUT); mcp1.pinMode(ir3, INPUT); mcp1.pinMode(ir4, INPUT); mcp1.pinMode(ir5, INPUT); mcp1.pinMode(ir6, INPUT); mcp1.pinMode(ir7, INPUT); mcp1.pinMode(ir8, INPUT); mcp1.pinMode(ir9, INPUT); mcp1.pinMode(ir10, INPUT); mcp1.pinMode(ir11, INPUT); mcp1.pinMode(ir12, INPUT); mcp1.pinMode(ir13, INPUT); mcp1.pinMode(ir14, INPUT); mcp1.pinMode(ir15, INPUT); mcp1.pinMode(ir16, INPUT); pinMode(irBALL, INPUT); // Configure pins for the motor 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); //IR seeker interupts attachInterrupt(digitalPinToInterrupt(ir1), IRtrigger1, CHANGE); attachInterrupt(digitalPinToInterrupt(ir2), IRtrigger2, CHANGE); attachInterrupt(digitalPinToInterrupt(ir3), IRtrigger3, CHANGE); attachInterrupt(digitalPinToInterrupt(ir4), IRtrigger4, CHANGE); attachInterrupt(digitalPinToInterrupt(ir5), IRtrigger5, CHANGE); attachInterrupt(digitalPinToInterrupt(ir6), IRtrigger6, CHANGE); attachInterrupt(digitalPinToInterrupt(ir7), IRtrigger7, CHANGE); attachInterrupt(digitalPinToInterrupt(ir8), IRtrigger8, CHANGE); attachInterrupt(digitalPinToInterrupt(ir9), IRtrigger9, CHANGE); attachInterrupt(digitalPinToInterrupt(ir10), IRtrigger10, CHANGE); attachInterrupt(digitalPinToInterrupt(ir11), IRtrigger11, CHANGE); attachInterrupt(digitalPinToInterrupt(ir12), IRtrigger12, CHANGE); attachInterrupt(digitalPinToInterrupt(ir13), IRtrigger13, CHANGE); attachInterrupt(digitalPinToInterrupt(ir14), IRtrigger14, CHANGE); attachInterrupt(digitalPinToInterrupt(ir15), IRtrigger15, CHANGE); attachInterrupt(digitalPinToInterrupt(ir16), IRtrigger16, CHANGE); attachInterrupt(digitalPinToInterrupt(irBALL), IRtrigger16, CHANGE); //encoder interupst 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); //PWM interupst Timer3.initialize(800); // 800 microseconds or 1.25kHz Timer3.attachInterrupt(updatePWM1); Timer3.attachInterrupt(updatePWM2); Timer3.attachInterrupt(updatePWM3); pinMode(buz, OUTPUT); Serial.println("SETUP COMPLETE GOING TO LOOP"); tone(buz, 1000); } 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 IRtrigger1() { IRcount1++; } void IRtrigger2() { IRcount2++; } void IRtrigger3() { IRcount3++; } void IRtrigger4() { IRcount4++; } void IRtrigger5() { IRcount5++; } void IRtrigger6() { IRcount6++; } void IRtrigger7() { IRcount7++; } void IRtrigger8() { IRcount8++; } void IRtrigger9() { IRcount9++; } void IRtrigger10() { IRcount10++; } void IRtrigger11() { IRcount11++; } void IRtrigger12() { IRcount12++; } void IRtrigger13() { IRcount13++; } void IRtrigger14() { IRcount14++; } void IRtrigger15() { IRcount15++; } void IRtrigger16() { IRcount16++; } void IRtriggerBALL() { IRcountBALL++; } void pcaselect(uint8_t i) { if (i > 7) return; Wire.beginTransmission(PCAADDR); Wire.write(1 << i); Wire.endTransmission(); } 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 ontbinden in x en y double y = sin(dirrad); double x = cos(dirrad); Serial.println((String) "x:" + x + " y:" + y); double spdeA = y * speed; //speed motor A = Y vector double spdeB = (-0.500 * x - 0.8660054 * y) * speed; //speed motor B = -1/2 X - sqrt(3)/2 Y double spdeC = (-0.500 * x + 0.8660054 * y) * speed; //speed motor C = -1/2 X + sqrt(3)/2 Y Amove(spdeA); Bmove(spdeB); Cmove(spdeC); Serial.println(); Serial.println(); } void rotate(int speed) { Amove(speed); Bmove(speed); Cmove(speed); } void compasrotate(int speed, int direction, int accuracy, int curheading) { //inacurate compas data //0deg is start heading offset(deg) int headmin = direction - accuracy; if (headmin < 0) { headmin = 360 - headmin; } int headmax = direction + accuracy; if (headmax > 360) { headmax = headmax - 360; } if (!(curheading >= headmin) && !(curheading <= headmax)) { if ((direction - curheading) > 180) { Amove(-speed); Bmove(-speed); Cmove(-speed); } else { Amove(speed); Bmove(speed); Cmove(speed); } } } void loop() { /* 1. it proceses/collects the sensor data 2. It prints the data for debugging 3. It do stuff with the data */ /* 1. collect data 2. do stuff with data --> 3 3. check if we are on any white lines: Yes: go oppiside direction for 250ms --> 1 No: --> 4 4. do we have the ball Yes: go towards goal --> 1 No: --> 5 5. Go towards the ball --> 1 */ Wire.begin(); //IR sensors data collection done in background, finding highest value and see if we have ball in posesion volatile long IRcounts[] = { IRcount1, IRcount2, IRcount3, IRcount4, IRcount5, IRcount6, IRcount7, IRcount8, IRcount9, IRcount10, IRcount11, IRcount12, IRcount13, IRcount14, IRcount15, IRcount16 }; long maxCount = IRcounts[0]; int highessensor = 1; for (int i = 0; i < 15; ++i) { if (IRcounts[i] > maxCount) { maxCount = IRcounts[i]; highessensor = i + 1; ballpos = (360 - (22.5 * i)); } } if (IRcountBALL > minpulsbal) { ballinpos = 1; } else { ballinpos = 0; } //Read color sensors for (uint8_t t = 0; t < 6; t++) { pcaselect(t); for (uint8_t addr = 0; addr <= 127; addr++) { if (addr == PCAADDR) continue; Wire.beginTransmission(addr); if (!Wire.endTransmission()) { if (tcs.begin()) { uint16_t r, g, b, c, colorTemp, lux; tcs.getRawData(&r, &g, &b, &c); colorTemp = tcs.calculateColorTemperature_dn40(r, g, b, c); lux = tcs.calculateLux(r, g, b); if ((colorTemp) == 5201) { colsensdata[t] = 0; } else if ((g) >= (2 * (r)) && (g) >= 12) { colsensdata[t] = 1; } else if ((colorTemp) >= 6750) { colsensdata[t] = 2; } else if ((colorTemp) < 6750) { colsensdata[t] = 3; } else { Serial.println(colorTemp); colsensdata[t] = 98; } } else { Serial.println("No TCS34725 found ... check your connections"); colsensdata[t] = 97; } } } } //read camera if (Serial1.available()) { datatime = millis(); byte byteRead = Serial1.read(); if (byteRead == 's') { tlocd = ""; readloc = true; } else if (byteRead == 'e') { readloc = false; locd = tlocd; } else if (readloc == true) { tlocd += (char)byteRead; } } else { if (millis() - datatime > 5000) { locd = "999"; } } int gloc = locd.toInt(); //read and calculate compas mag.read(); imu.read(); float heading = computeHeading(); int offset = heading - front; if (offset < 0) { offset = (offset + 360); } //read dip settings dip1val = digitalRead(dip1); dip2val = digitalRead(dip2); dip3val = digitalRead(dip3); dip4val = digitalRead(dip4); //printing sensor data //IR Serial.println("IR data: "); Serial.print("ir 1 changes: "); Serial.print(IRcount1); Serial.print(", ir 2 changes: "); Serial.print(IRcount2); Serial.print(", ir 3 changes: "); Serial.print(IRcount3); Serial.print(", ir 4 changes: "); Serial.print(IRcount4); Serial.print(", ir 5 changes: "); Serial.print(IRcount5); Serial.print(", ir 6 changes: "); Serial.print(IRcount6); Serial.print(", ir 7 changes: "); Serial.print(IRcount7); Serial.print(", ir 8 changes: "); Serial.print(IRcount8); Serial.print(", ir 9 changes: "); Serial.print(IRcount9); Serial.print(", ir 10 changes: "); Serial.print(IRcount10); Serial.print(", ir 11 changes: "); Serial.print(IRcount11); Serial.print(", ir 12 changes: "); Serial.print(IRcount12); Serial.print(", ir 13 changes: "); Serial.print(IRcount13); Serial.print(", ir 14 changes: "); Serial.print(IRcount14); Serial.print(", ir 15 changes: "); Serial.print(IRcount15); Serial.print(", ir 16 changes: "); Serial.println(IRcount16); Serial.print(", ball in posesion ir changes: "); Serial.println(IRcountBALL); Serial.print("Ball is at sensor: "); Serial.print(highessensor); Serial.print(", with value of "); Serial.print(maxCount); Serial.print(", withs it also at"); Serial.print(ballpos); Serial.print(" degrees"); if (ballinpos == 1) { Serial.print("Ball is in our posesion with: "); Serial.print(IRcountBALL); Serial.println(" pulses."); } else { Serial.println("ball is not in our posesion"); } //color Serial.print("sensor data = "); // 0=air, 1=green, 2=black, 3=white, 90=error for (int i = 0; i < 6; i++) { Serial.print(colsensdata[i]); Serial.print(", "); } Serial.println(); //camera Serial.print("location of the goal is: "); if (gloc == 999) { Serial.println("unknown, there is no working connection with the camera"); } else { Serial.println(gloc); } //compas Serial.print("heading: "); Serial.print(heading); Serial.print(", goal location: "); Serial.print(front); Serial.print(", offset: "); //dip if ((dip1val = 0) && (dip1val = 1)) { settingval = 1; } else if ((dip1val = 1) && (dip1val = 1)) { settingval = 2; } else if ((dip1val = 0) && (dip1val = 0)) { settingval = 3; } else if ((dip1val = 1) && (dip1val = 0)) { settingval = 4; } /* dip 1-4 settings: dip 1 = rotation 1=enables 0 = disables dip 2 = buzzer err/debug (buzz only if err = 1 -debug buzz = 0) dip 3 = on/off 1=on 0 = off dip 4 = camera 1=enables 0 = disables settings: 1. rotation OFF camera ON 2. rotation ON camera ON 3. rotation OFF camera OFF 4. rotation ON camera OFF /*dip3val /* data we have: rotation since start: offset(deg) if we have an ball in posesion ballinpos = 1, else ballinpos = 0 location of the ball on the field: ballpos(deg) colorsensordata list: colsensdata[6]. 0=air, 1=green, 2=black, 3=white, 90=error goal location: gloc(deg) */ //select with version of the code using dip switches //do stuf with data version 1: rotation disables and camera enabled if (dip3val = 1) { if (settingval = 1) { //check if we arent on any white lines for (int i = 0; i < 6; i++) { if (colsensdata[i] = 3) { Serial.println((String) "detected white collor on sensor:" + i + "going back " + (360 - (i * 60)) + "degrees"); moveohmi(sp, (i * 60)); delay(250); return; //restart loop } } //if we have the bal go towards the goal if (ballinpos = 1) { moveohmi(sp, 0); } else { //go towards ball moveohmi(sp, ballpos); } } //do stuf with data version 2: rotation enabled and camera enabled N if (settingval = 2) { //check if we arent on any white lines for (int i = 0; i < 6; i++) { if (colsensdata[i] = 3) { Serial.println((String) "detected white collor on sensor:" + i + "going back " + (360 - (i * 60)) + "degrees"); moveohmi(sp, (i * 60)); delay(250); return; //restart loop } if (ballinpos = 1) { moveohmi(sp, gloc); } else { //go towards ball moveohmi(sp, ballpos); } } } //do stuf with data version 3: rotation disabled and camera disabled (use compas to detect goal) if (settingval = 3) { //check if we arent on any white lines for (int i = 0; i < 6; i++) { if (colsensdata[i] = 3) { Serial.println((String) "detected white collor on sensor:" + i + "going back " + (360 - (i * 60)) + "degrees"); moveohmi(sp, (i * 60)); delay(250); return; //restart loop } } } //do stuf with data version 4: rotation enabled and camera disabled (use compas to detect goal) if (settingval = 4) { //check if we arent on any white lines for (int i = 0; i < 6; i++) { if (colsensdata[i] = 3) { Serial.println((String) "detected white collor on sensor:" + i + "going back " + (360 - (i * 60)) + "degrees"); moveohmi(sp, (i * 60)); delay(250); return; //restart loop } } } } else { Serial.println("skipping loop robot is off"); } if (errorbuz = 1) { tone(buz, 2000); } Serial.println(); Serial.println(); Serial.println(); } template float computeHeading(LIS3MDL::vector from) { //from https://github.com/pololu/lis3mdl-arduino/tree/master/examples/HeadingWithLSM6 LIS3MDL::vector temp_m = { mag.m.x, mag.m.y, mag.m.z }; // copy acceleration readings from LSM6::vector into an LIS3MDL::vector LIS3MDL::vector a = { imu.a.x, imu.a.y, imu.a.z }; // subtract offset (average of min and max) from magnetometer readings temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2; temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2; temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2; // compute E and N LIS3MDL::vector E; LIS3MDL::vector N; LIS3MDL::vector_cross(&temp_m, &a, &E); LIS3MDL::vector_normalize(&E); LIS3MDL::vector_cross(&a, &E, &N); LIS3MDL::vector_normalize(&N); // compute heading float heading = atan2(LIS3MDL::vector_dot(&E, &from), LIS3MDL::vector_dot(&N, &from)) * 180 / PI; if (heading < 0) heading += 360; return heading; } /* Returns the angular difference in the horizontal plane between a default vector (the +X axis) and north, in degrees. */ float computeHeading() { //from https://github.com/pololu/lis3mdl-arduino/tree/master/examples/HeadingWithLSM6 return computeHeading((LIS3MDL::vector){ 1, 0, 0 }); } 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; }