Reworking code for use with PIC32MX795F512L