22 int t1 = (int)
pulse_in(pinFeedback, 1);
23 if( (t1 > 1200) || (t1 < 25) )
return -8;
25 if(!_fb360c.servoCog) servo360_run();
27 if(servo360_findServoIndex(pinControl) != -1)
30 if(servo360_findServoIndex(pinFeedback) != -1)
33 if(pinControl >= 28 && pinFeedback >= 28)
36 if(_fb360c.devCount >= S360_DEVS_MAX)
39 while(
lockset(_fb360c.lock360));
43 for(
int p = 0; p < S360_DEVS_MAX; p++)
45 if( (_fs[p].pinCtrl == -1) && (_fs[p].pinFb == -1) )
55 _fs[p].pinCtrl = pinControl;
56 _fs[p].pinFb = pinFeedback;
58 _fs[p].unitsRev = S360_UNITS_REV;
60 _fs[p].KpV = S360_KPV;
61 _fs[p].KiV = S360_KIV;
62 _fs[p].KdV = S360_KDV;
63 _fs[p].iMax = S360_POS_INTGRL_MAX;
64 _fs[p].iMin = -S360_POS_INTGRL_MAX;
69 _fs[p].iMaxV = S360_VEL_INTGRL_MAX;
70 _fs[p].iMinV = -S360_VEL_INTGRL_MAX;
72 _fs[p].pw = S360_PW_CENTER;
74 _fs[p].speedLimit = S360_MAX_SPEED;
75 _fs[p].rampStep = S360_RAMP_STEP;
77 servo360_setPositiveDirection(p, S360_CCW_POS);
79 _fs[p].theta = servo360_getTheta(p);
80 _fs[p].thetaP = _fs[p].theta;
81 _fs[p].angleFixed = _fs[p].theta;
83 _fs[p].pvOffset = _fs[p].angleFixed;
85 _fs[p].angle = (_fs[p].angleSign) * (_fs[p].angleFixed - _fs[p].pvOffset);
86 _fs[p].angleCalc = _fs[p].angle;
87 _fs[p].angleP = _fs[p].angle;
89 _fs[p].angleMax = S360_A_MAX;
90 _fs[p].angleMin = -S360_A_MAX;
92 _fs[p].opMax = S360_MAX_SPEED;
95 _fs[p].coupleScale = 0;
96 _fs[p].accelerating = 0;
104 _fs[p].vmCcw = S360_VM_CCW;
105 _fs[p].vmCw = S360_VM_CW;
106 _fs[p].vbCcw = S360_VB_CCW;
107 _fs[p].vbCw = S360_VB_CW;