Fix camera to move properly and add printf for logging

Signed-off-by: Dariusz Kluska <darkenk@gmail.com>