20 servo360_cog_t _fb360c;
21 servo360_t _fs[S360_DEVS_MAX];
31 void servo360_run(
void)
35 _fb360c.servoCog =
cog_run(servo360_mainLoop, 512);
36 _fb360c.cntPrev =
CNT;
41 void servo360_end(
void)
48 void servo360_setup(
void)
50 for(
int p = 0; p < S360_DEVS_MAX; p++)
61 _fb360c.dt360 =
CLKFREQ / S360_FREQ_CTRL_SIG;
63 _fb360c.dt360a[0] = 16 * (
CLKFREQ / 1000);
64 _fb360c.dt360a[1] = 18 * (
CLKFREQ / 1000);
68 void servo360_mainLoop()
79 for(
int p = 0; p < S360_DEVS_MAX; p++)
81 if(_fs[p].pinCtrl != -1 && _fs[p].pinFb != -1)
85 servo360_checkAngle(p);
90 while(
lockset(_fb360c.lock360));
91 for(
int p = 0; p < S360_DEVS_MAX; p++)
93 if(_fs[p].pinCtrl != -1 && _fs[p].pinFb != -1)
98 servo360_outputSelector(p);
105 for(
int p = 0; p < S360_DEVS_MAX; p++)
107 if((_fs[p].couple != -1) && (_fs[p].feedback))
109 if(_fs[p].speedTarget > 0) _fs[p].stepDir = 1;
110 else if(_fs[p].speedTarget < 0) _fs[p].stepDir = -1;
111 else _fs[p].stepDir = 0;
113 _fs[p].lag = _fs[p].stepDir * _fs[p].angleError;
115 if(_fs[_fs[p].couple].speedTarget > 0) _fs[_fs[p].couple].stepDir = 1;
116 else if(_fs[_fs[p].couple].speedTarget < 0) _fs[_fs[p].couple].stepDir = -1;
117 else _fs[_fs[p].couple].stepDir = 0;
119 _fs[_fs[p].couple].lag = _fs[_fs[p].couple].stepDir * _fs[_fs[p].couple].angleError;
121 if(_fs[_fs[p].couple].lag > _fs[p].lag)
123 int compensate = _fs[_fs[p].couple].lag - _fs[p].lag;
124 compensate = _fs[p].coupleScale * compensate / S360_SCALE_DEN_COUPLE;
127 if(compensate > 500) compensate = 500;
130 if(_fs[p].speedOut > 0) _fs[p].speedOut -= compensate;
131 if(_fs[p].speedOut < 0) _fs[p].speedOut += compensate;
133 else if(_fs[p].lag > _fs[_fs[p].couple].lag)
135 int compensate = _fs[p].lag - _fs[_fs[p].couple].lag;
136 compensate = _fs[p].coupleScale * compensate / S360_SCALE_DEN_COUPLE;
139 if(compensate > 500) compensate = 500;
141 if(_fs[_fs[p].couple].speedOut > 0) _fs[_fs[p].couple].speedOut -= compensate;
142 if(_fs[_fs[p].couple].speedOut < 0) _fs[_fs[p].couple].speedOut += compensate;
152 target[0] = _fb360c.t360 + _fb360c.dt360a[0];
153 target[1] = _fb360c.t360 + _fb360c.dt360a[1];
155 for(
int p = 0; p < S360_DEVS_MAX; p++)
161 servo360_servoPulse(p - 1, p);
165 _fb360c.t360 += _fb360c.dt360;
170 void servo360_servoPulse(
int p,
int q)
172 int pinA = _fs[p].pinCtrl;
173 int pinB = _fs[q].pinCtrl;
176 if(pinA != -1 && _fs[p].dc != -1 && _fs[p].enable)
178 if(_fs[p].speedOut > S360_PWMAX) _fs[p].speedOut = S360_PWMAX;
179 if(_fs[p].speedOut < S360_PWMIN) _fs[p].speedOut = S360_PWMIN;
184 CTRA = (4 << 26) | pinA;
186 PHSA = -(15000 + _fs[p].speedOut) * (
CLKFREQ/10000000);
189 if(pinB != -1 && _fs[q].dc != -1 && _fs[q].enable)
191 if(_fs[q].speedOut > S360_PWMAX) _fs[q].speedOut = S360_PWMAX;
192 if(_fs[q].speedOut < S360_PWMIN) _fs[q].speedOut = S360_PWMIN;
197 CTRB = (4 << 26) | pinB;
199 PHSB = -(15000 + _fs[q].speedOut) * (
CLKFREQ/10000000);
202 if(pinA != -1 && _fs[p].dc != -1)
210 if(pinB != -1 && _fs[q].dc != -1)
221 void servo360_waitServoCtrllEdgeNeg(
int p)
223 int mask = 1 << _fs[p].pinCtrl;
226 while(!(
INA & mask));
232 void servo360_checkAngle(
int p)
234 _fs[p].thetaP = _fs[p].theta;
235 _fs[p].angleFixedP = _fs[p].angleFixed;
236 _fs[p].angleP = _fs[p].angle;
237 _fs[p].dcp = _fs[p].dc;
239 _fs[p].theta = servo360_getTheta(p);
241 _fs[p].turns += servo360_crossing(_fs[p].theta, _fs[p].thetaP, S360_UNITS_ENCODER);
243 if(_fs[p].turns >= 0)
245 _fs[p].angleFixed = (_fs[p].turns * S360_UNITS_ENCODER) + _fs[p].theta;
247 else if(_fs[p].turns < 0)
249 _fs[p].angleFixed = (S360_UNITS_ENCODER * (_fs[p].turns + 1)) + (_fs[p].theta - S360_UNITS_ENCODER);
252 _fs[p].angle = _fs[p].angleFixed - _fs[p].pvOffset;
256 void servo360_outputSelector(
int p)
258 if(_fs[p].csop == S360_POSITION)
260 int output = servo360_pidA(p);
261 _fs[p].pw = servo360_upsToPulseFromTransferFunction(output, p);
262 _fs[p].speedOut = _fs[p].pw - 15000;
264 else if(_fs[p].csop == S360_SPEED)
266 servo360_speedControl(p);
267 _fs[p].speedOut = _fs[p].opPidV;
269 else if(_fs[p].csop == S360_GOTO)
271 _fs[p].ticksDiff = _fs[p].angleTarget - _fs[p].angle;
274 _fs[p].ticksGuard = ( _fs[p].speedReq * abs(_fs[p].speedReq) ) / (100 * _fs[p].rampStep);
277 _fs[p].ticksGuard += (S360_LATENCY * _fs[p].speedReq / 50);
280 if((_fs[p].ticksDiff < 0) && (_fs[p].ticksDiff < _fs[p].ticksGuard) && (_fs[p].approachFlag == 0))
282 _fs[p].speedReq = -_fs[p].speedLimit;
285 _fs[p].approachFlag = 0;
287 else if((_fs[p].ticksDiff > 0) && (_fs[p].ticksDiff > _fs[p].ticksGuard) && (_fs[p].approachFlag == 0))
289 _fs[p].speedReq = _fs[p].speedLimit;
292 _fs[p].approachFlag = 0;
294 else if((_fs[p].ticksDiff > 0) && (_fs[p].ticksDiff <= _fs[p].ticksGuard) && (_fs[p].approachFlag == 0))
298 _fs[p].approachFlag = 1;
302 else if((_fs[p].ticksDiff < 0) && (_fs[p].ticksDiff >= _fs[p].ticksGuard) && (_fs[p].approachFlag == 0))
306 _fs[p].approachFlag = 1;
316 servo360_speedControl(p);
317 _fs[p].speedOut = _fs[p].opPidV;
322 (abs(_fs[p].ticksDiff) < (_fs[p].rampStep / (_fs[p].unitsRev * 50)))
323 || (_fs[p].approachFlag == 1 && _fs[p].speedMeasured == 0)
327 _fs[p].speedTarget = 0;
328 _fs[p].sp = _fs[p].angleTarget;
329 _fs[p].csop = S360_POSITION;
330 _fs[p].approachFlag = 0;
335 else if(_fs[p].csop == S360_MONITOR)
342 int servo360_dutyCycle(
int p,
int scale)
347 int pin = _fs[p].pinFb;
348 CTRA = (1000 << 26) | pin;
349 CTRB = (1100 << 26) | pin;
356 while(
INA & mask)
if(
CNT - t > dt)
break;
358 while(!(
INA & mask))
if(
CNT - t > dt)
break;
360 while(
INA & mask)
if(
CNT - t > dt)
break;
362 while(!(
INA & mask))
if(
CNT - t > dt)
break;
368 int dc = (phsa * scale) / (phsa + phsb);
381 int servo360_getTheta(
int p)
383 _fs[p].dc = servo360_dutyCycle(p, S360_M);
385 if(_fs[p].angleSign == S360_CCW_POS)
387 _fs[p].theta = (S360_ENC_RES - 1) - (_fs[p].dc + S360_B);
389 else if(_fs[p].angleSign == S360_CCW_NEG)
391 _fs[p].theta = _fs[p].dc + S360_B;
394 _fs[p].theta &= 0xFFF;
399 int servo360_crossing(
int current,
int previous,
int units)
401 int angleLow = units/3;
402 int angleHigh = angleLow * 2;
403 if(previous > angleHigh && current < angleLow)
407 else if(previous < angleLow && current > angleHigh)
418 void servo360_setPositiveDirection(
int p,
int direction)
420 _fs[p].angleSign = direction;
425 int servo360_pidA(
int p)
430 _fs[p].speedTarget = 0;
431 _fs[p].angleError = 0;
434 _fs[p].integralV = 0;
435 _fs[p].derivativeV = 0;
446 _fs[p].er = _fs[p].sp - _fs[p].angle;
448 _fs[p].integral += _fs[p].er;
450 _fs[p].derivative = _fs[p].er - _fs[p].erP;
453 if(_fs[p].integral > _fs[p].iMax) _fs[p].integral = _fs[p].iMax;
454 if(_fs[p].integral < _fs[p].iMin) _fs[p].integral = _fs[p].iMin;
457 _fs[p].p = (_fs[p].Kp * _fs[p].er) / S360_SCALE_DEN_A;
458 _fs[p].i = (_fs[p].Ki * _fs[p].integral) / S360_SCALE_DEN_A;
459 _fs[p].d = (_fs[p].Kd * _fs[p].derivative) / S360_SCALE_DEN_A;
462 _fs[p].op = (_fs[p].p + _fs[p].i + _fs[p].d);
469 if(_fs[p].op > _fs[p].opMax) _fs[p].op = _fs[p].opMax;
470 if(_fs[p].op < -_fs[p].opMax) _fs[p].op = -_fs[p].opMax;
472 _fs[p].erP = _fs[p].er;
482 int servo360_pidV(
int p)
487 int opMax = _fs[p].speedLimit;
489 _fs[p].speedMeasured = (_fs[p].angle - _fs[p].angleP) * 50;
497 _fs[p].angleDeltaCalc = _fs[p].speedTarget / S360_CS_HZ;
498 _fs[p].angleCalc += _fs[p].angleDeltaCalc;
535 int angleErrorPrev = _fs[p].angleError;
537 _fs[p].angleError = _fs[p].angleCalc - _fs[p].angle;
539 if(abs(_fs[p].angleError) >= S360_UNITS_ENCODER/2)
541 if(abs(_fs[p].angleError) > abs(angleErrorPrev))
543 _fs[p].angleCalc -= _fs[p].angleDeltaCalc;
544 _fs[p].angleError = angleErrorPrev;
551 if(abs(_fs[p].angleError) < S360_UNITS_ENCODER/2)
553 _fs[p].erDist = _fs[p].angleError;
554 _fs[p].integralV += _fs[p].erDist;
555 _fs[p].derivativeV = _fs[p].erDist - _fs[p].erDistP;
558 if(_fs[p].integralV > _fs[p].iMaxV) _fs[p].integralV = _fs[p].iMaxV;
559 if(_fs[p].integralV < _fs[p].iMinV) _fs[p].integralV = _fs[p].iMinV;
561 _fs[p].pV = (_fs[p].KpV * _fs[p].erDist) / S360_SCALE_DEN_V;
562 _fs[p].iV = (_fs[p].KiV * _fs[p].integralV) / S360_SCALE_DEN_V;
563 _fs[p].dV = (_fs[p].KdV * _fs[p].derivativeV) / S360_SCALE_DEN_V;
565 _fs[p].opV = _fs[p].pV + _fs[p].iV + _fs[p].dV;
567 if(_fs[p].opV > opMax) _fs[p].opV = opMax;
568 if(_fs[p].opV < -opMax) _fs[p].opV = -opMax;
570 _fs[p].erDistP = _fs[p].erDist;
608 int servo360_upsToPulseFromTransferFunction(
int unitsPerSec,
int p)
628 y = (m *
x / 1000) + b;
634 void servo360_speedControl(
int p)
643 _fs[p].derivative = 0;
654 if(_fs[p].speedTarget != _fs[p].speedReq)
656 int speedDifference = _fs[p].speedReq - _fs[p].speedTarget;
657 if(abs(_fs[p].angleError) < S360_UNITS_ENCODER/2)
659 if( abs(speedDifference) > _fs[p].rampStep)
661 _fs[p].accelerating = 1;
662 if(speedDifference > 0)
664 _fs[p].speedTarget += _fs[p].rampStep;
666 else if(speedDifference < 0)
668 _fs[p].speedTarget -= _fs[p].rampStep;
673 _fs[p].accelerating = 0;
674 _fs[p].speedTarget = _fs[p].speedReq;
678 _fs[p].pw = servo360_upsToPulseFromTransferFunction(_fs[p].speedTarget, p);
679 _fs[p].drive = _fs[p].pw - 15000;
680 _fs[p].opPidV = _fs[p].drive + servo360_pidV(p);
684 _fs[p].opPidV = _fs[p].drive + servo360_pidV(p);
686 _fs[p].speedTargetP = _fs[p].speedTarget;