Optimize runningtime:
-reorganize loops
-Moved Serial recive to mediumLoop
-Read PID setpoints from setpoint structs
-Update setpoints in medium loop

Refactoring:
-Use difined constants for mode and status enums
-Implement setpoint struct to hold setpoints
-Add StopWatch to benchmark code