Replace dirty fixes with proper fixes

[nvbln]

PR #29 contains some much needed fixes, but they are not meant to be permanent. Instead the issue itself should be fixed rather than using workarounds. This issue is meant to track this.
The issues are the following:

• The communication duration with Stytra is highly variable and can take up to ~3s. Currently fixed by waiting for 5 seconds to make sure that communication finished.
• When saving the data, the queue with planes is not completely emptied. Therefore it can miss the last plane. Again, it is currently fixed by waiting for 5 seconds.
• The motor sometimes move double the z step size as it is triggered twice. This is currently solved by forcing the internal frame count to have a small number once the z motor is moved (such that it does not get triggered again).