Midterm 1 working state. Hardcoded coordinates, bugfixes in cmd_vel