# Path Planning
The current path planning makes it that there is an infinite acceleration required in the corners.
We need something that can process setpoints in a way that it can follow those.

## General Idea.
So if we want to draw a box. 4 setpoints for each corner should be sufficient.
This is a simple model detail. We just take the four setpoint and give the system some time to get there.

## Results
The implementation was fairly simple, The arm just does a joint move to the next setpoint.
The control of the motor is still a bit yanky. 
The next step is probably a better implementation of the motors.