Angle working! ONLY FOR TERMINAL

the C code is ONLY FOR TERMINAL, don't use matlab yet.  When you rotate
it it's still a little funky, but eveyrthing else works. shouldn't be
too bad to figure out.