add file and functions

* Added motorControl.c
*Added stabilityControl.c