19 if(!_fb360c.servoCog) servo360_run();
20 int p = servo360_findServoIndex(pin);
23 while(
lockset(_fb360c.lock360));
30 if(_fs[p].csop == S360_POSITION)
33 _fs[p].angleCalc = _fs[p].sp;
35 else if(_fs[p].csop == S360_GOTO)
37 offset = _fs[p].angleTarget;
41 offset = _fs[p].angleCalc;
45 target = position * S360_UNITS_ENCODER / _fs[p].unitsRev;
47 _fs[p].angleTarget = target + offset;
51 if(_fs[p].csop != S360_GOTO)
53 _fs[p].csop = S360_GOTO;
57 _fs[p].approachFlag = 0;