Set up new nodes
Added PD controller
Added messages