Update code: servos calibrated, idle behaviour now working.