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.
[sourcecode]#define VERBOSE (1)[/sourcecode]
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.
[sourcecode]
== HELLO WORLD ==
(-14.00,-21.50)-(14.00,30.00) F8.90 A5.00
== DRAWBOT – 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.
> 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.
>
[/sourcecode]
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
[sourcecode]> G00 X5 Y4;[/sourcecode]
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.