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, 0 insertions, 992 deletions
diff --git a/CODE/debugprewk/CODE.ino b/CODE/debugprewk/CODE.ino
deleted file mode 100644
index 5b993c3..0000000
--- a/CODE/debugprewk/CODE.ino
+++ /dev/null
@@ -1,992 +0,0 @@
-#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 05:08:09 +0000