Added makeTriangles. It prints the triangles for the forward facing sonar based on the vehicles NAV_X, NAV_Y and HEADING. Need to get it working with real data and also make it work with colormap. Initial testing shows it is working at a basic level already.