## How to test your motion code before breaking the hardware

There’s no such thing as too much testing. While I can’t prove a negative, I can prove that testing has always made my robots better, cheaper, and faster. Take, for example, moving my Drawbot. Of all the precompiler definitions in the code, this one is probably the most useful.

`#define VERBOSE (1)`

When I uncomment this line I get thousands of lines of serial output that tell me everything going on in my robot from moment to moment. When I was writing my motion interpolation code I used this to graph the movement of the Drawbot plotter long before I attached the stepper motors and tested it out. Here’s a small sample of what it looked like.

```== HELLO WORLD ==
(-14.00,-21.50)-(14.00,30.00) F8.90 A5.00
== <a href="/shop/drawing-robots/">Drawbot</a> - 2012 Feb 28 ==
All commands end with a semi-colon.
HELP; - display this message
WHERE; - display current virtual coordinates
LIMITS; - display maximum distance plotter can move
DEMO; - draw a test pattern
TELEPORT [Xx.xx] [Yx.xx]; - move the virtual plotter.
G00 [Xx.xx] [Yx.xx]; - draw line to X,Y
G01 [Xx.xx] [Yx.xx]; - draw line to X,Y
G02 Ix.xx Jx.xx [Xx.xx] [Yx.xx]; - draw a clockwise arc around I,J to X,Y.
G03 Ix.xx Jx.xx [Xx.xx] [Yx.xx]; - draw a counterclockwise arc around I,J to X,Y.
Fx.xx; - set feed rate (max speed). Can be done on the same line as a G command.
Zx.xx; - set pen height.  Can be done on the same line as a G command.
&gt; G00 X5 Y4;
x=5.00
y=4.00
posx=0.00
posy=0.00
start len1=1705
start len2=1705
end len1=2161
end len2=1835
dx=5.00
dy=4.00
len=6.40
time=2.26
A	0.00	11120	0.00	0.00	1705	1705	1705	1705
A	0.01	11127	0.00	0.00	1705	1705	1705	1705
A	0.01	11134	0.00	0.00	1705	1705	1705	1705
A	0.02	11142	0.00	0.00	1705	1705	1705	1705
A	0.03	11149	0.00	0.00	1705	1705	1705	1705
A	0.04	11156	0.00	0.00	1706	1705	1706	1705
A	0.04	11163	0.00	0.00	1706	1705	1706	1705
A	0.05	11170	0.00	0.00	1706	1705	1706	1705
A	0.06	11179	0.01	0.01	1706	1705	1706	1705
A	0.07	11186	0.01	0.01	1706	1705	1706	1705
A	0.07	11193	0.01	0.01	1706	1705	1706	1705
A	0.08	11200	0.01	0.01	1706	1705	1706	1705
A	0.09	11207	0.01	0.01	1707	1705	1707	1705
A	0.09	11214	0.02	0.01	1707	1706	1707	1706
A	0.10	11222	0.02	0.02	1707	1706	1707	1706
A	0.11	11230	0.02	0.02	1707	1706	1707	1706
A	0.12	11237	0.03	0.02	1708	1706	1708	1706
A	0.12	11244	0.03	0.02	1708	1706	1708	1706
A	0.13	11251	0.03	0.03	1708	1706	1708	1706
A	0.14	11258	0.04	0.03	1709	1706	1709	1706
A	0.15	11266	0.04	0.03	1709	1706	1709	1706
A	0.15	11273	0.05	0.04	1709	1706	1709	1706
A	0.16	11281	0.05	0.04	1710	1706	1710	1706
A	0.17	11288	0.06	0.04	1710	1706	1710	1706
A	0.17	11295	0.06	0.05	1711	1706	1711	1706
A	0.18	11302	0.06	0.05	1711	1706	1711	1706
A	0.19	11310	0.07	0.06	1712	1706	1712	1706
A	0.20	11317	0.08	0.06	1712	1706	1712	1706
A	0.20	11324	0.08	0.06	1713	1706	1713	1706
A	0.21	11332	0.09	0.07	1713	1706	1713	1706
A	0.22	11339	0.09	0.07	1714	1707	1714	1707
A	0.23	11346	0.10	0.08	1714	1707	1714	1707
A	0.23	11354	0.11	0.09	1715	1707	1715	1707
A	0.24	11361	0.11	0.09	1715	1707	1715	1707
A	0.25	11368	0.12	0.10	1716	1707	1716	1707
A	0.26	11375	0.13	0.10	1717	1707	1717	1707
A	0.26	11383	0.14	0.11	1717	1707	1717	1707
A	0.27	11390	0.14	0.11	1718	1707	1718	1707
A	0.28	11398	0.15	0.12	1719	1707	1719	1707
A	0.28	11405	0.16	0.13	1720	1707	1720	1707
A	0.29	11412	0.17	0.13	1720	1708	1720	1708
A	0.30	11419	0.17	0.14	1721	1708	1721	1708
A	0.31	11426	0.18	0.15	1722	1708	1722	1708
A	0.31	11433	0.19	0.15	1722	1708	1722	1708
A	0.32	11442	0.20	0.16	1723	1708	1723	1708
A	0.33	11449	0.21	0.17	1724	1708	1724	1708
A	0.34	11456	0.22	0.18	1725	1708	1725	1708
A	0.34	11463	0.23	0.18	1726	1709	1726	1709
A	0.35	11470	0.24	0.19	1727	1709	1727	1709
A	0.36	11478	0.25	0.20	1728	1709	1728	1709
A	0.36	11485	0.26	0.21	1729	1709	1729	1709
A	0.37	11493	0.27	0.22	1730	1709	1730	1709
A	0.38	11500	0.28	0.23	1731	1709	1731	1709
A	0.39	11507	0.29	0.23	1732	1709	1732	1709
A	0.39	11514	0.30	0.24	1733	1710	1733	1710
A	0.40	11522	0.32	0.25	1734	1710	1734	1710
A	0.41	11529	0.33	0.26	1735	1710	1735	1710
A	0.42	11536	0.34	0.27	1736	1710	1736	1710
A	0.42	11544	0.35	0.28	1737	1710	1737	1710
A	0.43	11551	0.36	0.29	1738	1711	1738	1711
A	0.44	11558	0.37	0.30	1739	1711	1739	1711
A	0.45	11566	0.39	0.31	1740	1711	1740	1711
A	0.45	11573	0.40	0.32	1741	1711	1741	1711
A	0.46	11580	0.41	0.33	1742	1711	1742	1711
A	0.47	11587	0.43	0.34	1744	1711	1744	1711
A	0.47	11595	0.44	0.35	1745	1712	1745	1712
A	0.48	11602	0.45	0.36	1746	1712	1746	1712
A	0.49	11610	0.47	0.37	1747	1712	1747	1712
A	0.50	11617	0.48	0.39	1749	1712	1749	1712
A	0.50	11624	0.50	0.40	1750	1713	1750	1713
A	0.51	11631	0.51	0.41	1751	1713	1751	1713
A	0.52	11638	0.52	0.42	1752	1713	1752	1713
A	0.53	11646	0.54	0.43	1754	1713	1754	1713
A	0.53	11654	0.56	0.45	1755	1714	1755	1714
A	0.54	11661	0.57	0.46	1757	1714	1757	1714
A	0.55	11668	0.59	0.47	1758	1714	1758	1714
A	0.55	11675	0.60	0.48	1759	1714	1759	1714
A	0.56	11682	0.62	0.49	1761	1715	1761	1715
A	0.57	11689	0.63	0.51	1762	1715	1762	1715
A	0.58	11697	0.65	0.52	1764	1715	1764	1715
A	0.59	11705	0.67	0.53	1765	1715	1765	1715
A	0.59	11712	0.68	0.55	1767	1716	1767	1716
A	0.60	11719	0.70	0.56	1768	1716	1768	1716
A	0.61	11726	0.72	0.57	1770	1716	1770	1716
A	0.61	11734	0.74	0.59	1771	1717	1771	1717
A	0.62	11741	0.75	0.60	1773	1717	1773	1717
A	0.63	11748	0.77	0.62	1775	1717	1775	1717
A	0.64	11756	0.79	0.63	1776	1718	1776	1718
A	0.64	11763	0.81	0.65	1778	1718	1778	1718
A	0.65	11770	0.82	0.66	1780	1718	1780	1718
A	0.66	11778	0.85	0.68	1781	1719	1781	1719
A	0.66	11785	0.86	0.69	1783	1719	1783	1719
A	0.67	11792	0.88	0.71	1785	1719	1785	1719
A	0.68	11799	0.90	0.72	1786	1719	1786	1719
A	0.69	11807	0.92	0.74	1788	1720	1788	1720
A	0.69	11814	0.94	0.75	1790	1720	1790	1720
A	0.70	11822	0.96	0.77	1792	1721	1792	1721
A	0.71	11829	0.98	0.79	1794	1721	1794	1721
A	0.72	11836	1.00	0.80	1795	1721	1795	1721
A	0.72	11843	1.02	0.82	1797	1722	1797	1722
A	0.73	11850	1.04	0.83	1799	1722	1799	1722
A	0.74	11858	1.06	0.85	1801	1723	1801	1723
A	0.75	11866	1.09	0.87	1803	1723	1803	1723
A	0.75	11873	1.11	0.89	1805	1723	1805	1723
A	0.76	11880	1.13	0.90	1807	1724	1807	1724
A	0.77	11887	1.15	0.92	1809	1724	1809	1724
A	0.77	11894	1.17	0.94	1811	1725	1811	1725
A	0.78	11901	1.19	0.95	1813	1725	1813	1725
A	0.79	11910	1.22	0.97	1815	1726	1815	1726
A	0.80	11917	1.24	0.99	1817	1726	1817	1726
A	0.80	11924	1.26	1.01	1819	1726	1819	1726
A	0.81	11931	1.28	1.03	1821	1727	1821	1727
A	0.82	11938	1.31	1.04	1823	1727	1823	1727
A	0.82	11945	1.33	1.06	1825	1728	1825	1728
A	0.83	11953	1.35	1.08	1827	1728	1827	1728
A	0.84	11960	1.38	1.10	1829	1729	1829	1729
A	0.85	11968	1.40	1.12	1832	1729	1832	1729
A	0.85	11975	1.43	1.14	1834	1730	1834	1730
A	0.86	11982	1.45	1.16	1836	1730	1836	1730
A	0.87	11990	1.48	1.18	1839	1731	1839	1731
A	0.88	11997	1.50	1.20	1841	1731	1841	1731
A	0.88	12004	1.53	1.22	1843	1732	1843	1732
A	0.89	12011	1.55	1.24	1845	1732	1845	1732
A	0.90	12019	1.58	1.26	1848	1733	1848	1733
A	0.91	12026	1.60	1.28	1850	1733	1850	1733
A	0.91	12034	1.63	1.30	1852	1734	1852	1734
A	0.92	12041	1.66	1.32	1855	1735	1855	1735
A	0.93	12048	1.68	1.34	1857	1735	1857	1735
A	0.93	12055	1.71	1.37	1859	1736	1859	1736
A	0.94	12062	1.73	1.39	1862	1736	1862	1736
A	0.95	12070	1.76	1.41	1864	1737	1864	1737
A	0.96	12078	1.79	1.43	1867	1738	1867	1738
A	0.97	12085	1.82	1.45	1869	1738	1869	1738
A	0.97	12092	1.84	1.48	1872	1739	1872	1739
A	0.98	12099	1.87	1.50	1874	1739	1874	1739
A	0.99	12106	1.90	1.52	1877	1740	1877	1740
A	0.99	12113	1.92	1.54	1879	1741	1879	1741
A	1.00	12122	1.96	1.57	1882	1742	1882	1742
A	1.01	12129	1.99	1.59	1885	1742	1885	1742
A	1.02	12136	2.02	1.61	1887	1743	1887	1743
A	1.02	12143	2.04	1.63	1890	1744	1890	1744
A	1.03	12150	2.07	1.66	1892	1744	1892	1744
A	1.04	12157	2.10	1.68	1895	1745	1895	1745
A	1.05	12165	2.13	1.71	1898	1746	1898	1746
A	1.05	12172	2.16	1.73	1901	1746	1901	1746
A	1.06	12180	2.19	1.75	1904	1747	1904	1747
A	1.07	12187	2.22	1.78	1906	1748	1906	1748
A	1.07	12194	2.25	1.80	1909	1749	1909	1749
A	1.08	12201	2.28	1.82	1912	1749	1912	1749
A	1.09	12209	2.32	1.85	1915	1750	1915	1750
A	1.10	12216	2.34	1.88	1917	1751	1917	1751
A	1.10	12223	2.38	1.90	1920	1752	1920	1752
A	1.11	12231	2.41	1.93	1923	1753	1923	1753
A	1.12	12238	2.44	1.95	1926	1753	1926	1753
A	1.13	12246	2.48	1.98	1929	1754	1929	1754
C	1.13	12253	2.51	2.00	1932	1755	1932	1755
C	1.14	12260	2.54	2.03	1935	1756	1935	1756
C	1.15	12267	2.57	2.05	1938	1757	1938	1757
C	1.15	12274	2.60	2.08	1940	1758	1940	1758
C	1.16	12282	2.63	2.11	1943	1758	1943	1758
C	1.17	12290	2.67	2.13	1947	1759	1947	1759
C	1.18	12297	2.70	2.16	1949	1760	1949	1760
C	1.18	12304	2.73	2.18	1952	1761	1952	1761
C	1.19	12311	2.76	2.20	1955	1762	1955	1762
C	1.20	12318	2.78	2.23	1957	1763	1957	1763
C	1.20	12325	2.81	2.25	1960	1763	1960	1763
C	1.21	12334	2.85	2.28	1963	1764	1963	1764
C	1.22	12341	2.88	2.30	1966	1765	1966	1765
C	1.23	12348	2.91	2.33	1969	1766	1969	1766
C	1.23	12355	2.94	2.35	1971	1767	1971	1767
C	1.24	12362	2.96	2.37	1974	1768	1974	1768
C	1.25	12369	2.99	2.39	1976	1768	1976	1768
C	1.26	12377	3.02	2.42	1979	1769	1979	1769
C	1.26	12385	3.05	2.44	1982	1770	1982	1770
C	1.27	12392	3.08	2.47	1985	1771	1985	1771
C	1.28	12399	3.11	2.49	1987	1772	1987	1772
C	1.29	12406	3.14	2.51	1989	1773	1989	1773
C	1.29	12413	3.16	2.53	1992	1773	1992	1773
C	1.30	12421	3.19	2.55	1995	1774	1995	1774
C	1.31	12428	3.22	2.57	1997	1775	1997	1775
C	1.31	12435	3.24	2.60	1999	1776	1999	1776
C	1.32	12443	3.27	2.62	2002	1777	2002	1777
C	1.33	12450	3.30	2.64	2004	1777	2004	1777
C	1.34	12457	3.32	2.66	2007	1778	2007	1778
C	1.34	12465	3.35	2.68	2009	1779	2009	1779
C	1.35	12472	3.38	2.70	2012	1780	2012	1780
C	1.36	12479	3.40	2.72	2014	1781	2014	1781
C	1.37	12486	3.43	2.74	2016	1781	2016	1781
C	1.37	12494	3.46	2.76	2019	1782	2019	1782
C	1.38	12502	3.48	2.79	2021	1783	2021	1783
C	1.39	12509	3.51	2.81	2023	1784	2023	1784
C	1.40	12516	3.53	2.83	2026	1784	2026	1784
C	1.40	12523	3.56	2.84	2028	1785	2028	1785
C	1.41	12530	3.58	2.86	2030	1786	2030	1786
C	1.42	12537	3.60	2.88	2032	1787	2032	1787
C	1.43	12546	3.63	2.91	2035	1788	2035	1788
C	1.43	12553	3.65	2.92	2037	1788	2037	1788
C	1.44	12560	3.68	2.94	2039	1789	2039	1789
C	1.45	12567	3.70	2.96	2041	1790	2041	1790
C	1.45	12574	3.72	2.98	2043	1790	2043	1790
C	1.46	12581	3.74	2.99	2045	1791	2045	1791
C	1.47	12589	3.77	3.01	2047	1792	2047	1792
C	1.48	12597	3.79	3.03	2050	1793	2050	1793
C	1.48	12604	3.81	3.05	2052	1793	2052	1793
C	1.49	12611	3.84	3.07	2054	1794	2054	1794
C	1.50	12618	3.86	3.09	2055	1795	2055	1795
C	1.51	12625	3.88	3.10	2057	1795	2057	1795
C	1.51	12633	3.90	3.12	2060	1796	2060	1796
C	1.52	12640	3.92	3.14	2061	1797	2061	1797
C	1.53	12648	3.94	3.16	2064	1798	2064	1798
C	1.53	12655	3.96	3.17	2065	1798	2065	1798
C	1.54	12662	3.98	3.19	2067	1799	2067	1799
C	1.55	12669	4.00	3.20	2069	1800	2069	1800
C	1.56	12677	4.03	3.22	2071	1800	2071	1800
C	1.56	12684	4.05	3.24	2073	1801	2073	1801
C	1.57	12691	4.06	3.25	2075	1802	2075	1802
C	1.58	12698	4.08	3.27	2076	1802	2076	1802
C	1.59	12706	4.10	3.28	2078	1803	2078	1803
C	1.59	12713	4.12	3.30	2080	1804	2080	1804
C	1.60	12721	4.14	3.31	2082	1804	2082	1804
C	1.61	12728	4.16	3.33	2083	1805	2083	1805
C	1.61	12735	4.18	3.34	2085	1806	2085	1806
C	1.62	12742	4.20	3.36	2087	1806	2087	1806
C	1.63	12749	4.21	3.37	2088	1807	2088	1807
C	1.64	12758	4.24	3.39	2090	1807	2090	1807
C	1.64	12765	4.25	3.40	2092	1808	2092	1808
C	1.65	12772	4.27	3.42	2093	1809	2093	1809
C	1.66	12779	4.29	3.43	2095	1809	2095	1809
C	1.67	12786	4.30	3.44	2096	1810	2096	1810
C	1.67	12793	4.32	3.46	2098	1810	2098	1810
C	1.68	12801	4.34	3.47	2100	1811	2100	1811
C	1.69	12809	4.36	3.48	2101	1812	2101	1812
C	1.70	12816	4.37	3.50	2103	1812	2103	1812
C	1.70	12823	4.39	3.51	2104	1813	2104	1813
C	1.71	12830	4.40	3.52	2106	1813	2106	1813
C	1.72	12837	4.42	3.53	2107	1814	2107	1814
C	1.72	12845	4.43	3.55	2108	1814	2108	1814
C	1.73	12852	4.45	3.56	2110	1815	2110	1815
C	1.74	12860	4.47	3.57	2111	1815	2111	1815
C	1.75	12867	4.48	3.58	2113	1816	2113	1816
C	1.75	12874	4.49	3.59	2114	1816	2114	1816
C	1.76	12881	4.51	3.61	2115	1817	2115	1817
C	1.77	12889	4.52	3.62	2117	1817	2117	1817
C	1.78	12896	4.54	3.63	2118	1818	2118	1818
C	1.78	12903	4.55	3.64	2119	1818	2119	1818
C	1.79	12910	4.56	3.65	2120	1819	2120	1819
C	1.80	12918	4.58	3.66	2122	1819	2122	1819
C	1.80	12925	4.59	3.67	2123	1820	2123	1820
C	1.81	12933	4.60	3.68	2124	1820	2124	1820
C	1.82	12940	4.62	3.69	2125	1821	2125	1821
C	1.83	12947	4.63	3.70	2126	1821	2126	1821
C	1.83	12954	4.64	3.71	2127	1822	2127	1822
C	1.84	12961	4.65	3.72	2128	1822	2128	1822
C	1.85	12969	4.66	3.73	2130	1823	2130	1823
C	1.86	12977	4.68	3.74	2131	1823	2131	1823
C	1.86	12984	4.69	3.75	2132	1823	2132	1823
C	1.87	12991	4.70	3.76	2133	1824	2133	1824
C	1.88	12998	4.71	3.77	2134	1824	2134	1824
C	1.89	13005	4.72	3.78	2135	1825	2135	1825
C	1.89	13012	4.73	3.78	2136	1825	2136	1825
C	1.90	13021	4.74	3.80	2137	1825	2137	1825
C	1.91	13028	4.75	3.80	2138	1826	2138	1826
C	1.91	13035	4.76	3.81	2139	1826	2139	1826
C	1.92	13042	4.77	3.82	2140	1826	2140	1826
C	1.93	13049	4.78	3.83	2140	1827	2140	1827
C	1.94	13057	4.79	3.83	2141	1827	2141	1827
C	1.94	13064	4.80	3.84	2142	1827	2142	1827
C	1.95	13072	4.81	3.85	2143	1828	2143	1828
C	1.96	13079	4.82	3.86	2144	1828	2144	1828
C	1.97	13086	4.83	3.86	2145	1828	2145	1828
C	1.97	13093	4.84	3.87	2145	1829	2145	1829
C	1.98	13101	4.84	3.88	2146	1829	2146	1829
C	1.99	13108	4.85	3.88	2147	1829	2147	1829
C	1.99	13115	4.86	3.89	2148	1830	2148	1830
C	2.00	13123	4.87	3.89	2148	1830	2148	1830
C	2.01	13130	4.87	3.90	2149	1830	2149	1830
C	2.02	13137	4.88	3.91	2150	1830	2150	1830
C	2.02	13145	4.89	3.91	2150	1831	2150	1831
C	2.03	13152	4.90	3.92	2151	1831	2151	1831
C	2.04	13159	4.90	3.92	2151	1831	2151	1831
C	2.05	13166	4.91	3.93	2152	1831	2152	1831
C	2.05	13173	4.91	3.93	2153	1832	2153	1832
C	2.06	13181	4.92	3.94	2153	1832	2153	1832
C	2.07	13189	4.93	3.94	2154	1832	2154	1832
C	2.08	13196	4.93	3.95	2154	1832	2154	1832
C	2.08	13203	4.94	3.95	2155	1833	2155	1833
C	2.09	13210	4.94	3.95	2155	1833	2155	1833
C	2.10	13217	4.95	3.96	2156	1833	2156	1833
C	2.10	13224	4.95	3.96	2156	1833	2156	1833
C	2.11	13233	4.96	3.96	2156	1833	2156	1833
C	2.12	13240	4.96	3.97	2157	1833	2157	1833
C	2.13	13247	4.96	3.97	2157	1834	2157	1834
C	2.13	13254	4.97	3.97	2158	1834	2158	1834
C	2.14	13261	4.97	3.98	2158	1834	2158	1834
C	2.15	13268	4.97	3.98	2158	1834	2158	1834
C	2.16	13276	4.98	3.98	2158	1834	2158	1834
C	2.16	13284	4.98	3.98	2159	1834	2159	1834
C	2.17	13291	4.98	3.99	2159	1834	2159	1834
C	2.18	13298	4.99	3.99	2159	1834	2159	1834
C	2.18	13305	4.99	3.99	2159	1834	2159	1834
C	2.19	13313	4.99	3.99	2160	1835	2160	1835
C	2.20	13320	4.99	3.99	2160	1835	2160	1835
C	2.21	13327	4.99	4.00	2160	1835	2160	1835
C	2.22	13335	5.00	4.00	2160	1835	2160	1835
C	2.22	13342	5.00	4.00	2160	1835	2160	1835
C	2.23	13349	5.00	4.00	2160	1835	2160	1835
C	2.24	13357	5.00	4.00	2160	1835	2160	1835
C	2.24	13364	5.00	4.00	2160	1835	2160	1835
C	2.25	13371	5.00	4.00	2160	1835	2160	1835
C	2.26	13378	5.00	4.00	2161	1835	2161	1835
D	2.26	13386	5.00	4.00	2161	1835	2161	1835
7309.89 microseconds/loop average
Done.
&gt;
```

The first column is the interpolation phase – speeding up, plateau, slowing down, and end.
The second column is time, in seconds. The third is time, in milliseconds.
The fourth & fifth are X & Y values of the plotter, respectively.
The fifth and sixth are the desired motor step positions. Steps * bobbin circumference = total string to the plotter. So whenever these numbers change the robot is supposed to reel string in or out to move the plotter.
The last values are the actual motor step positions. In early versions of the application sometimes the desired position would change by more than one step on each tick. Then the steppers would fall behind the simulation and things would get…ugly.

I load all this into into OpenCalc and graph the line data.

Remember the original command I sent was

`> G00 X5 Y4;`

Remember we’re looking to see X reach 5 and Y reach 4, exactly what we’ve got. I know that my motors are going what I expect them to go, without losing steps (m1=om1, m2=om2). In the old linear interpolation these lines would have been straight and the total time would have been much longer. Now I can accelerate up and down, leading to higher speeds over long distances. More to the point, I can plug in my steppers and expect them to move correctly with some confidence.