Updated Firmware for Filter Wheel

Added comments and clean code.