Started with max speed calculation
max speed, next point logic and taskdiagramm
Merge branch 'origin/Teil_Stetter'