Added enterthematrix.c

This is for running tests on matrix stuff tomorrow. I "hope" I got the
motor config right :)