Converting code from Arduino to PIC