Add motor control codes

LOL
Add elevator scheduling algorithm