setting 1 complete need to do 2, 3, 4

6 files changed, 884 insertions, 130 deletions

diff --git a/CODE/CODE.ino b/CODE/CODE.ino
index 26fae53..32506fd 100644
--- a/CODE/CODE.ino
+++ b/CODE/CODE.ino
@@ -1,15 +1,20 @@
 #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/
 
-//25, 26, 27, 28 connect to extra pins for potential expention
+
+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
 
-//miltiplex1
+//IR sensors on multiplex1 0x20
 #define ir1  0   // GPA0
 #define ir2  1   // GPA1
 #define ir3  2   // GPA2
@@ -26,54 +31,187 @@
 #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
 
-//miltiplex2
-#define pwm5 0  // GPA0  Motor driver 3 pwm1
-#define pwm6 1  // GPA1  Motor driver 3 pwm2
-#define enB3 2  // GPA2  Motor driver 3 enB
-#define enA3 3  // GPA3  Motor driver 3 enA
-#define M1A  4   // GPA4  Motor 1 A
-#define M1B  5   // GPA5  Morot 1 B
-#define M2A  6   // GPA6  Motor 2 A
-#define M1B  7   // GPA7  Motpr 2 B
-#define pwm1 8   // GPB0  Motor driver 1 pwm1
-#define pwm2 9   // GPB1  Motor driver 1 pwm2
-#define enB1 10  // GPB2  Motor driver 1 enB
-#define enA1 11  // GPB3  Motor driver 1 enA
-#define pwm3 12  // GPB4  Motor driver 2 pwm1
-#define pwm4 13  // GPB5  Motor driver 2 pwm2
-#define enB2 14  // GPB6  Motor driver 2 enB
-#define enA2 15  // GPB7  Motor driver 2 enA
-
-//teensy
-#define M3A 24  //Motor 3 pin A
-#define M3B 9   //Motor 3 pin B
+//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 dir1 = 1;   // data IR1
-int dir2 = 1;   // data IR2
-int dir3 = 1;   // etc
-int dir4 = 1;   
-int dir5 = 1;    
-int dir6 = 1;   
-int dir7 = 1;   
-int dir8 = 1;   
-int dir9 = 1;     
-int dir10 = 1;  
-int dir11 = 1; 
-int dir12 = 1; 
-int dir13 = 1; 
-int dir14 = 1; 
-int dir15 = 1; 
-int dir16 = 1; 
+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
@@ -82,56 +220,53 @@ Adafruit_TCS34725 tcs = Adafruit_TCS34725();  // Initialise color sensorwith def
 
 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;
 
-template <typename T_ty> struct TypeInfo { static const char * name; };
-template <typename T_ty> const char * TypeInfo<T_ty>::name = "unknown";
-#define TYPE_NAME(var) TypeInfo< typeof(var) >::name
-#define MAKE_TYPE_INFO(type)  template <> const char * TypeInfo<type>::name = #type;
-MAKE_TYPE_INFO( int )
-MAKE_TYPE_INFO( float )
-MAKE_TYPE_INFO( short )
+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);
-
-  Serial1.begin(19200);
+  Serial.begin(9600);   //serial debug
+  Serial1.begin(19200); //connection to openMV
   
   //connect unconected pins to GND
-  pinMode(6, INPUT_PULLUP);
-  pinMode(7, INPUT_PULLUP);
-  pinMode(10, INPUT_PULLUP);
-  pinMode(11, INPUT_PULLUP);
-  pinMode(12, INPUT_PULLUP);
-  pinMode(29, INPUT_PULLUP);
-  pinMode(30, INPUT_PULLUP);
-  pinMode(31, INPUT_PULLUP);
-  pinMode(32, INPUT_PULLUP);
-  pinMode(33, INPUT_PULLUP);
-  pinMode(34, INPUT_PULLUP);
-  pinMode(35, INPUT_PULLUP);
-  pinMode(36, INPUT_PULLUP);
-  pinMode(37, INPUT_PULLUP);
-  pinMode(38, INPUT_PULLUP);
-  pinMode(39, INPUT_PULLUP);
-  pinMode(40, INPUT_PULLUP);
-  pinMode(41, INPUT_PULLUP);
-  pinMode(13, INPUT_PULLUP);
-  pinMode(14, INPUT_PULLUP);
-  pinMode(15, INPUT_PULLUP);
-  pinMode(16, INPUT_PULLUP);
-  pinMode(17, INPUT_PULLUP);
-  pinMode(20, INPUT_PULLUP);
-  pinMode(21, INPUT_PULLUP);
-  pinMode(22, INPUT_PULLUP);
-  pinMode(23, INPUT_PULLUP);
-
-  //extrapins
-  pinMode(25, INPUT_PULLUP);
-  pinMode(26, INPUT_PULLUP);
-  pinMode(27, INPUT_PULLUP);
-  pinMode(28, INPUT_PULLUP);
+  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)) {
@@ -142,8 +277,23 @@ void setup() {
   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 first MCP23017 (0x20)
+  // Configure pins for the IR seekers
   mcp1.pinMode(ir1, INPUT);
   mcp1.pinMode(ir2, INPUT);
   mcp1.pinMode(ir3, INPUT);
@@ -160,12 +310,185 @@ void setup() {
   mcp1.pinMode(ir14, INPUT);
   mcp1.pinMode(ir15, INPUT);
   mcp1.pinMode(ir16, INPUT);
+  pinMode(irBALL, INPUT);
 
-  // Configure pins for the second MCP23017 (0x22)
-  mcp1.pinMode(ir16, 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); 
+  
   Serial.println("SETUP COMPLETE GOING TO LOOP");
 }
+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;
@@ -175,26 +498,193 @@ void pcaselect(uint8_t 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();
-  // Read IR sensors
-  dir1 = mcp1.digitalRead(ir1);
-  dir2 = mcp1.digitalRead(ir2);
-  dir3 = mcp1.digitalRead(ir3);
-  dir4 = mcp1.digitalRead(ir4);
-  dir5 = mcp1.digitalRead(ir5);
-  dir6 = mcp1.digitalRead(ir6);
-  dir7 = mcp1.digitalRead(ir7);
-  dir8 = mcp1.digitalRead(ir8);
-  dir9 = mcp1.digitalRead(ir9);
-  dir10 = mcp1.digitalRead(ir10);
-  dir11 = mcp1.digitalRead(ir11);
-  dir12 = mcp1.digitalRead(ir12);
-  dir13 = mcp1.digitalRead(ir13);
-  dir14 = mcp1.digitalRead(ir14);
-  dir15 = mcp1.digitalRead(ir15);
-  dir16 = mcp1.digitalRead(ir16);
+  //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);
@@ -221,7 +711,6 @@ void loop() {
           }
           else{
             Serial.println(colorTemp);
-            Serial.println(TYPE_NAME((colorTemp, DEC)));
             colsensdata[t] = 98;
           }
         } else {
@@ -254,22 +743,68 @@ void loop() {
       locd = "999";
     }
   }
-  //read compas
-
+  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
 
   //printing sensor data
   //IR
-  Serial.print("IR data: ");
-  Serial.print(dir1); Serial.print(", "); Serial.print(dir2);
-  Serial.print(", "); Serial.print(dir3); Serial.print(", "); Serial.print(dir4);
-  Serial.print(", "); Serial.print(dir5); Serial.print(", "); Serial.print(dir6);
-  Serial.print(", "); Serial.print(dir7); Serial.print(", "); Serial.print(dir8);
-  Serial.print(", "); Serial.print(dir9); Serial.print(", "); Serial.print(dir10);
-  Serial.print(", "); Serial.print(dir11); Serial.print(", "); Serial.print(dir12);
-  Serial.print(", "); Serial.print(dir13); Serial.print(", "); Serial.print(dir14);
-  Serial.print(", "); Serial.print(dir15); Serial.print(", "); Serial.print(dir16);
-  Serial.println();
-  delay(250);
+  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++){
@@ -279,28 +814,166 @@ void loop() {
   Serial.println();
   //camera
   Serial.print("location of the goal is: ");
-  if(locd == 999){
+  if(gloc == 999){
     Serial.println("unknown, there is no working connection with the camera");
   }
   else{
-    Serial.println(locd);
+    Serial.println(gloc);
   }
   //compas
-  //dip settings
+  Serial.print("heading: ");
+  Serial.print(heading);
+  Serial.print(", goal location: ");
+  Serial.print(front);
+  Serial.print(", offset: ");
+  /*
+  dip 1-4 settings: 1=enables 0 = disables
+  dip 1 = rotation
+  dip 2 = buzzer
+  dip 3 = ?
+  dip 4 = camera
+  settings:
+    1.  rotation OFF
+        camera ON
+    2.  rotation ON
+        camera ON
+    3.  rotation OFF
+        camera OFF
+    4.  rotation ON
+        camera OFF
+  /*
 
 
 
-  //do stuf with data
-  /**
-  first move if line is detected with color sens
-  is ball in posesion:
-    yes: go to goal using camera location
-    no: move to ball using IR data
-     * 
-   */
+  
+  /*
+  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(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, gloc);
+    }else{    //go towards ball
+      moveohmi(sp, ballpos);
+    }
+  }
+  //do stuf with data version 2: rotation enabled and camera enabled
+  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
+      }
+    }
+  }
+  //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
+      }
+    }
+  }
+  
+
+
+  
+  
+
 
   Serial.println();
   Serial.println();
   Serial.println();
-  delay(1);
 }
+
+
+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;
+}
+
diff --git a/CODE/todo.txt b/CODE/todo.txt
new file mode 100644
index 0000000..f1e62cd
--- /dev/null
+++ b/CODE/todo.txt
@@ -0,0 +1,8 @@
+compas inaccurate
+dip settings
+buzzer
+calibrate color sensors
+
+do stuf with data
+    other versions
+        2, 3, 4
diff --git a/debug/IMU_comp/Calibrate/Calibrate.ino b/debug/IMU_comp/Calibrate/Calibrate.ino
new file mode 100644
index 0000000..c5db31c
--- /dev/null
+++ b/debug/IMU_comp/Calibrate/Calibrate.ino
@@ -0,0 +1,42 @@
+//example from https://github.com/pololu/lis3mdl-arduino
+#include <Wire.h>
+#include <LIS3MDL.h>
+
+LIS3MDL mag;
+LIS3MDL::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32768, -32768, -32768};
+
+char report[80];
+
+void setup()
+{
+  Serial.begin(9600);
+  Wire.begin();
+
+  if (!mag.init())
+  {
+    Serial.println("Failed to detect and initialize magnetometer!");
+    while (1);
+  }
+
+  mag.enableDefault();
+}
+
+void loop()
+{
+  mag.read();
+
+  running_min.x = min(running_min.x, mag.m.x);
+  running_min.y = min(running_min.y, mag.m.y);
+  running_min.z = min(running_min.z, mag.m.z);
+
+  running_max.x = max(running_max.x, mag.m.x);
+  running_max.y = max(running_max.y, mag.m.y);
+  running_max.z = max(running_max.z, mag.m.z);
+
+  snprintf(report, sizeof(report), "min: {%+6d, %+6d, %+6d}   max: {%+6d, %+6d, %+6d}",
+    running_min.x, running_min.y, running_min.z,
+    running_max.x, running_max.y, running_max.z);
+  Serial.println(report);
+
+  delay(100);
+}
diff --git a/debug/IMU_comp/IMU_comp.ino b/debug/IMU_comp/IMU_comp.ino
index fc35244..c4578cb 100644
--- a/debug/IMU_comp/IMU_comp.ino
+++ b/debug/IMU_comp/IMU_comp.ino
@@ -65,6 +65,9 @@ void loop()
 
   float heading = computeHeading();
   int offset = front - heading;
+  if(offset<0){
+    offset=360+offset;
+  }
   
   Serial.print("heading: ");
   Serial.print(heading);
diff --git a/debug/IMU_comp/calcoffset/calcoffset.ino b/debug/IMU_comp/calcoffset/calcoffset.ino
new file mode 100644
index 0000000..fc5f752
--- /dev/null
+++ b/debug/IMU_comp/calcoffset/calcoffset.ino
@@ -0,0 +1,27 @@
+void setup() {
+  randomSeed(analogRead(0));
+  Serial.begin(9600);
+
+}
+
+void loop() {
+  int front = random(0, 360);
+  int heading = random(0, 360);
+  Serial.print("Front: ");
+  Serial.println(front);
+  Serial.print("heading: ");
+  Serial.println(heading);
+
+  int offset = heading - front;
+  if(offset<0){
+    offset=(offset+360);
+  }
+  Serial.print("offset: ");
+  Serial.println(offset);
+  
+  Serial.println();
+  Serial.println();
+
+  delay(5000);
+
+}
diff --git a/debug/motordriver/motordriver.ino b/debug/motordriver/motordriver.ino
index 99cbd44..2f1c225 100644
--- a/debug/motordriver/motordriver.ino
+++ b/debug/motordriver/motordriver.ino
@@ -1,6 +1,6 @@
 #include <Adafruit_MCP23X17.h>
 #include <TimerThree.h>
-//moveohmi = https://robotics.stackexchange.com/questions/7829/how-to-program-a-three-wheel-omni
+//moveohmi = https://stackoverflow.com/questions/3748037/how-to-control-a-kiwi-drive-robot
 //https://www.pololu.com/product/2997
 //https://www.pololu.com/product/4861
 //https://ww1.microchip.com/downloads/en/devicedoc/20001952c.pdf
@@ -33,10 +33,11 @@
 double sp = 1200.000; //speed in RPM ~0-1800
 const int maxspeed = 1000;
 
-//calibration
+//calibration data
 int Amotordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910};
 int Bmotordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910};
 int Cmotordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910};
+
 int AneedRPM = 0;
 int BneedRPM = 0;
 int CneedRPM = 0;