bool sRprop::step(arr& w, const arr& grad, uint *singleI) {
if(!stepSize.N) { //initialize
stepSize.resize(w.N);
lastGrad.resize(w.N);
lastGrad.setZero();
stepSize = delta0;
}
CHECK(grad.N==stepSize.N, "Rprop: gradient dimensionality changed!");
CHECK(w.N==stepSize.N , "Rprop: parameter dimensionality changed!");
uint i=0, I=w.N;
if(singleI) {
i=*(singleI);
I=i+1;
}
for(; i<I; i++) {
if(grad.elem(i) * lastGrad(i) > 0) { //same direction as last time
if(rMax) dMax=fabs(rMax*w.elem(i));
stepSize(i) = _mymin(dMax, incr * stepSize(i)); //increase step size
w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction
lastGrad(i) = grad.elem(i); //memorize gradient
} else if(grad.elem(i) * lastGrad(i) < 0) { //change of direction
stepSize(i) = _mymax(dMin, decr * stepSize(i)); //decrease step size
w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction (undo half the step)
lastGrad(i) = 0; //memorize to continue below next time
} else { //after change of direcion
w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction
lastGrad(i) = grad.elem(i); //memorize gradient
}
}
return stepSize.max() < incr*dMin;
}
int YARPMEIDeviceDriver::getPosition(void *cmd)
{
int16 rc = 0;
SingleAxisParameters *tmp = (SingleAxisParameters *) cmd;
const int axis = tmp->axis;
///rc = get_position(axis, (double *)tmp->parameters);
double tmpd;
tmpd = double(dsp_encoder (axis));
/// computing the actual encoder value.
if (_sgn(tmpd) < 0 && _16bit_oldpos[axis] > 16384.0 && _winding[axis] == 0)
{
_winding[axis] = 1;
}
else
if (_sgn(tmpd) > 0 && _16bit_oldpos[axis] < -16384.0 && _winding[axis] == 1)
{
_winding[axis] = 0;
}
else
if (_sgn(tmpd) > 0 && _16bit_oldpos[axis] < -16384.0 && _winding[axis] == 0)
{
_winding[axis] = -1;
}
else
if (_sgn(tmpd) < 0 && _16bit_oldpos[axis] > 16384.0 && _winding[axis] == -1)
{
_winding[axis] = 0;
}
_16bit_oldpos[axis] = tmpd;
switch (_winding[axis])
{
case 1:
tmpd = 65535.0 + tmpd - _position_zero[axis];
break;
case -1:
tmpd = -65536.0 + tmpd - _position_zero[axis];
break;
case 0:
tmpd -= _position_zero[axis];
break;
}
*((double *)tmp->parameters) = tmpd;
return rc;
}
int YARPMEIDeviceDriver::getPositions(void *j)
{
///long rc = 0;
double *out = (double *) j;
for(int i = 0; i < _njoints; i++)
{
/// direct reading from the DSP (fast!).
out[i] = double(dsp_encoder (i));
/// computing the actual encoder value.
if (_sgn(out[i]) < 0 && _16bit_oldpos[i] > 16384.0 && _winding[i] == 0)
{
_winding[i] = 1;
}
else
if (_sgn(out[i]) > 0 && _16bit_oldpos[i] < -16384.0 && _winding[i] == 1)
{
_winding[i] = 0;
}
else
if (_sgn(out[i]) > 0 && _16bit_oldpos[i] < -16384.0 && _winding[i] == 0)
{
_winding[i] = -1;
}
else
if (_sgn(out[i]) < 0 && _16bit_oldpos[i] > 16384.0 && _winding[i] == -1)
{
_winding[i] = 0;
}
_16bit_oldpos[i] = out[i];
switch (_winding[i])
{
case 1:
out[i] = 65535.0 + out[i] - _position_zero[i];
break;
case -1:
out[i] = -65536.0 + out[i] - _position_zero[i];
break;
case 0:
out[i] -= _position_zero[i];
break;
}
}
return 0;
}
int __cdecl rects<_t, _rt, _it>::sort_compare(const void *a, const void *b) {
int dx = __d_dx(_sort_direction);
int dy = __d_dy(_sort_direction);
rect_type *ra = (rect_type *)a;
rect_type *rb = (rect_type *)b;
int r = 0;
if (_sort_x_prior) {
r = _sgn(ra->_x0, rb->_x0) * dx;
if (r == 0) {
r = _sgn(ra->_y0, rb->_y0) * dy;
}
} else {
r = _sgn(ra->_y0, rb->_y0) * dy;
if (r == 0) {
r = _sgn(ra->_x0, rb->_x0) * dx;
}
}
return r;
}