Fixed some bugs

Fixed some bugs:

I think the position of countours sometimes being [0,0] was somehow
fixed by defining the variable in a separate line, will have to look
that up

I did not properly update and copy the variable of step velocity and
new_velocity.