Modified for PIC32MX695

Might not work for 320F now. (I didn't check)

Connor