Figured out speed calculation, completed the plotter