AIPU Update

Major update to AIPU code, STM, and robot servo structure