Added second motor to control. Added PID control.