Code created using Kevin Anderson's PIC Vex libraries