Changed sampling time to 2ms, reset reaching gains. Note: it must be used with lower arm gains