The robot should (finally) work with this commit (see desc if you care)

Got rid of all debug code. The whole reason stuff wasn’t working is
because one of the ports was incorrect (it was switched after
competition) so yeah. It all works now. Go team.