[Zalumaskov]

I am basically trying to get the robot to keep note of its own (x,y) position then later go to a set (x,y).

To do the first part I think I must get some trig functions imported to do what
//pos_x += sin_int(heading) * dist;
//pos_y += cos_int(heading) * dist;
are supposed to do in robot_position()

Now to go to get to a set (x,y) I guess I must complete cmd_goto_waypoint(void). Can anyone help me with that? I lack PID knowledge.