summaryrefslogtreecommitdiff
path: root/CODE/debugprewk/CODE.ino
diff options
context:
space:
mode:
Diffstat (limited to 'CODE/debugprewk/CODE.ino')
-rw-r--r--CODE/debugprewk/CODE.ino992
1 files changed, 992 insertions, 0 deletions
diff --git a/CODE/debugprewk/CODE.ino b/CODE/debugprewk/CODE.ino
new file mode 100644
index 0000000..5b993c3
--- /dev/null
+++ b/CODE/debugprewk/CODE.ino
@@ -0,0 +1,992 @@
+#include <Wire.h> //https://www.arduino.cc/reference/en/language/functions/communication/wire/
+#include "Adafruit_TCS34725.h" //https://github.com/adafruit/Adafruit_TCS34725
+#include <Adafruit_MCP23X17.h> //https://github.com/adafruit/Adafruit-MCP23017-Arduino-Library/tree/master
+#include <LIS3MDL.h> //https://github.com/pololu/lis3mdl-arduino
+#include <LSM6.h> //https://github.com/pololu/lsm6-arduino
+#include <TimerThree.h> //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<int16_t> m_min = { +942, +3436, -3323 };
+LIS3MDL::vector<int16_t> 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<typename T> float computeHeading(LIS3MDL::vector<T> from) {
+ //from https://github.com/pololu/lis3mdl-arduino/tree/master/examples/HeadingWithLSM6
+ LIS3MDL::vector<int32_t> temp_m = { mag.m.x, mag.m.y, mag.m.z };
+
+ // copy acceleration readings from LSM6::vector into an LIS3MDL::vector
+ LIS3MDL::vector<int16_t> 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<float> E;
+ LIS3MDL::vector<float> 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<int>){ 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;
+}
generated by cgit v1.2.3 (git 2.46.0) at 2026-09-30 04:28:48 +0000