Re-wrote microcontroller functions

Simplified so it is less messy