Added things from RoboticStaff 1.9

And fixed some bugs.