Added DriveStraight command

DriveStraight uses a second PID to keep track of driving angle

Also renamed drive functions, to make them more accurate. (setPower
instead of setSpeed)