Algodoo

A little control circuit I wanted code and try out.

The little green drone always trys to follow the red cross.
You can simply move the cross around with your mouse to control the movement of the drone.

It's a non-linear cascaded control system.
The outer loop simply calculates the necessary acceleration in x- and y-direction from the positional difference through a PID-controller.
From those the angle and geometric sum are derived.
The angle is subject of the inner control loop, which calculates an angular acceleration to steer the drone.
It's applied to the thrusters however in opposite directions to rotate the drone.
The geometric sum of the total force is split in half and applied to both thrusters.
It is however also multiplied with the cosine of the angular difference to prevent the drone from accelerating into the wrong direction.

The controller and its parameters can be found in the drone's script menu.
The parameters were found by trial-and-error.
The integral parts are set to zero in this case, the systematic error due to gravity is cancelled by a direct offset.
It can be used for unknown or inhomogeneous external forces.But in this case I left it out to reduce overshoot.