Create more boudary function
Create motor control function
Fix RC boundary
Fix optimiztion error