Add debug messages

  - Commanded velocity computed from position difference