Diy hexapod, All of these calculations run at 50Hz