Updated Test firmware with new pins

Remapped pins to match Blue wires