19 if(!_fb360c.servoCog) servo360_run();
20 int p = servo360_findServoIndex(pin);
23 speed = speed * S360_UNITS_ENCODER / _fs[p].unitsRev;
25 if(speed != _fs[p].speedReq)
27 while(
lockset(_fb360c.lock360));
29 if( (_fs[p].csop == S360_POSITION) || (_fs[p].csop == S360_GOTO))
31 _fs[p].angleCalc = _fs[p].angleTarget;
34 if(speed > _fs[p].speedLimit)
35 _fs[p].speedReq = _fs[p].speedLimit;
36 else if(speed < -_fs[p].speedLimit)
37 _fs[p].speedReq = -_fs[p].speedLimit;
39 _fs[p].speedReq = speed;
41 _fs[p].csop = S360_SPEED;
47 _fs[p].derivative = 0;