issue 15 should be fixed

2 files changed, 221 insertions, 979 deletions

diff --git a/CODE/CODE.ino b/CODE/CODE.ino
index 5b993c3..8cd2ef7 100644
--- a/CODE/CODE.ino
+++ b/CODE/CODE.ino
@@ -1,992 +1,231 @@
-#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 };
+int incomingbyte1 = 0;  //the byte that it is reciefing
+bool iswriting1 = false;  //true if writing else fals
+String tlocdat1 = "";  //temporary location data
+String locdat1 = "";   //location data
+int nir1 = "-99"; // which ir sensor it is
+int pir1 = "-99"; // amount of pulses it had
+
+int incomingbyte2 = 0;  //same but for seeed xiao N 2
+bool iswriting2 = false;
+String tlocdat2 = ""; 
+String locdat2 = ""; 
+int nir2 = "-99";
+int pir2 = "-99";
+
+float degwhere = -99.0; // where the ball is in degrees
+
+int led = 13;
+
+int m1enA = 6;
+int m1enB = 5;
+int m1pwm1 = 4;
+int m1pwm2 = 3;
+int m1encoA = 32;
+int m1encoB = 31;
+
+int m2enA = 12;
+int m2enB = 11;
+int m2pwm1 = 9;
+int m2pwm2 = 10;
+int m2encoA = 30;
+int m2encoB = 29;
+
+int m3enA = 27;
+int m3enB = 26;
+int m3pwm1 = 24;
+int m3pwm2 = 25;
+int m3encoA = 28;
+int m3encoB = 23;
+
+int softpwr = 2;
 
 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);
-    }
+  pinMode(m1enA, OUTPUT);
+  pinMode(m1enB, OUTPUT);
+  pinMode(m1pwm1, OUTPUT);
+  pinMode(m1pwm2, OUTPUT);
+  
+  pinMode(m2enA, OUTPUT);
+  pinMode(m2enB, OUTPUT);
+  pinMode(m2pwm1, OUTPUT);
+  pinMode(m2pwm2, OUTPUT);
+  
+  pinMode(m3enA, OUTPUT);
+  pinMode(m3enB, OUTPUT);
+  pinMode(m3pwm1, OUTPUT);
+  pinMode(m3pwm2, OUTPUT);
+
+  pinMode(softpwr, INPUT_PULLUP);
+
+  Serial.begin(9600);
+  Serial1.begin(9600);
+  Serial2.begin(9600);
+}
+
+void drive(int speed, int direction){
+ //
+ // Direction............|.Motor 1.|.Motor 2.|.Motor 3.
+ // 0deg(forward)........|....0....|....-....|....+....
+ // 60deg................|....+....|....-....|....0....    
+ // 120deg...............|....+....|....0....|....-....
+ // 180 deg (backward)...|....0....|....+....|....-.... 
+ // 240 deg..............|....-....|....+....|....0....
+ // 300 deg..............|....-....|....0....|....+....
+ //
+
+  digitalWrite(m1enA, HIGH);
+  digitalWrite(m2enA, HIGH);
+  digitalWrite(m3enA, HIGH);
+  digitalWrite(m1enB, LOW);
+  digitalWrite(m2enB, LOW);
+  digitalWrite(m3enB, LOW);
+  Serial.println("debug2");
+  Serial.println(direction);
+  if(direction>330 || direction<=30){
+    //0 deg
+    digitalWrite(m1pwm1, LOW);
+    digitalWrite(m1pwm2, HIGH);
+
+    digitalWrite(m2pwm1, LOW);
+    digitalWrite(m2pwm2, LOW);
+
+    digitalWrite(m3pwm1, LOW);
+    digitalWrite(m3pwm2, HIGH);
+    Serial.println("0 deg"); 
+  }else if (direction>30 && direction <= 90){
+    //60 deg
+    Serial.println("60deg");
+    digitalWrite(m1pwm1, LOW);
+    digitalWrite(m1pwm2, LOW);
+
+    digitalWrite(m2pwm1, HIGH);
+    digitalWrite(m2pwm2, LOW);
+    
+    digitalWrite(m3pwm1, LOW);
+    digitalWrite(m3pwm2, HIGH);
+  }else if (direction>90 && direction <= 150){
+    //120 deg
+    Serial.println("120 deg");
+    digitalWrite(m1pwm1, HIGH);
+    digitalWrite(m1pwm2, LOW);
+    digitalWrite(m2pwm1, HIGH);
+    digitalWrite(m2pwm2, LOW);
+    digitalWrite(m3pwm1, LOW);
+    digitalWrite(m3pwm2, LOW);
+  }else if (direction > 150 && direction <= 210) {
+    //180 degrees
+    Serial.println("on 180");
+    digitalWrite(m1pwm1, HIGH);
+    digitalWrite(m1pwm2, LOW);
+
+    digitalWrite(m2pwm1, LOW);
+    digitalWrite(m2pwm2, LOW);
+    
+    digitalWrite(m3pwm1, HIGH);
+    digitalWrite(m3pwm2, LOW);
+  }else if (direction > 210 && direction <= 270) {
+    //240 degrees
+    digitalWrite(m1pwm1, LOW);
+    digitalWrite(m1pwm2, LOW);
+
+    digitalWrite(m2pwm1, LOW);
+    digitalWrite(m2pwm2, HIGH);
+
+    digitalWrite(m3pwm1, HIGH);
+    digitalWrite(m3pwm2, LOW);
+  }else if (direction > 270 && direction <= 330) {
+    //300 degrees
+    digitalWrite(m1pwm1, LOW);
+    digitalWrite(m1pwm2, HIGH);
+    digitalWrite(m2pwm1, LOW);
+    digitalWrite(m2pwm2, HIGH);
+    Serial.println("300deg");
+    digitalWrite(m3pwm1, LOW);
+    digitalWrite(m3pwm2, LOW);
+  }else{
+    Serial.println("how?");  
   }
 }
 
 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;
-        }
+  int sprst = digitalRead(softpwr);  //softpower status
+  if(sprst == LOW){
+    if (Serial1.available() > 0) {
+      incomingbyte1 = Serial1.read();
+      //Serial.print(char(incomingbyte1));
+      if (incomingbyte1 == 's'){
+        iswriting1 = true;
+        tlocdat1 = "";
+      }else if(incomingbyte1 == 'e'){
+        iswriting1 = false;
+        locdat1 = tlocdat1;        
+      }else if(iswriting1 == true){
+        //Serial.print(char(incomingbyte1));
+        tlocdat1 += (char)incomingbyte1;  
       }
     }
-  }
-  //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
-        }
+    if (Serial2.available() > 0) {
+      incomingbyte2 = Serial2.read();
+      //Serial.print(char(incomingbyte2));
+      if (incomingbyte2 == 's'){
+        iswriting2 = true;
+        tlocdat2 = "";
+      }else if(incomingbyte1 == 'e'){
+        iswriting2 = false;
+        locdat2 = tlocdat1;        
+      }else if(iswriting2 == true){
+        //Serial.print(char(incomingbyte1));
+        tlocdat2 += (char)incomingbyte1;  
       }
     }
-  } 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;
+    //Serial.println(locdat);
+    sscanf(locdat1.c_str(), "%d, %d", &nir1, &pir1);
+    sscanf(locdat2.c_str(), "%d, %d", &nir2, &pir2);
+    
+    
+    if(pir1 > pir2){
+       if(nir1 = 1){    //not good looking code because posbile wrong wireing
+        degwhere = 0;      
+       }else if(nir1 = 2){
+        degwhere = 22.5;
+       }else if(nir1 = 3){
+        degwhere = 45;
+       }else if(nir1 = 4){
+        degwhere = 67.5;
+       }else if(nir1 = 5){
+        degwhere = 90;
+       }else if(nir1 = 6){
+        degwhere = 112.5;
+       }else if(nir1 = 7){
+        degwhere = 135;
+       }else if(nir1 = 8){
+        degwhere = 157.5;
+       }
+    }else{     
+       if(nir2 = 1){
+        degwhere = 180;
+       }else if(nir2 = 2){
+        degwhere = 202.5;
+       }else if(nir2 = 3){
+        degwhere = 225;
+       }else if(nir2 = 4){
+        degwhere = 247.5;
+       }else if(nir2 = 5){
+        degwhere = 270;
+       }else if(nir2 = 6){
+        degwhere = 292.5;
+       }else if(nir2 = 7){
+        degwhere = 315; 
+       }else if(nir2 = 8){
+        degwhere = 337.5; 
+       }
     }
+    Serial.print(degwhere);
+    Serial.println(" degrees.");
+    drive(255, degwhere);
+    delay(1);
+  }else{
+    Serial.println("softPWR is off, waiting")
+    delay(100);  
   }
-  return idx;
 }
diff --git a/CODE/debug/IR/teensydeco/teensydeco.ino b/CODE/debug/IR/teensydeco/teensydeco.ino
index 87828be..2d06af9 100644
--- a/CODE/debug/IR/teensydeco/teensydeco.ino
+++ b/CODE/debug/IR/teensydeco/teensydeco.ino
@@ -97,5 +97,8 @@ void loop() {
   Serial.print(", with ");
   Serial.print(pir1);
   Serial.println(" pulses.");
+  Serial.print("That is ");
+  Serial.print(degwhere);
+  Serial.println(" degrees.");
   delay(10);
 }