Safety controller

Merging the latest updates