Added atomic block

-Atomic block when copying sensor values
-Changed pd constans and variables to double
-Timer interrupt only sets a flag
-Main work is done i the main loop