Added first (pseudo-working) version of the controller. Needs to adjust phisical values to accepted quad vals.