Fixed Software and Firmware Bugs

Modified the software and firmware to actually work in automatic mode.