float Motor::getPosition() {
// Returns the position AFTER the gearbox in the range (-pi, pi). Since there is no
// calibraton routine, the output must start within (-pi,pi)/gearRatio of the zero.
// Additionally, this won't work if the output shaft can complete a full revolution.
// E.g. with a 2:1 gear ratio, the output shaft must start in the range (-pi/2, pi/2) of
// your desired output zero.
curPos = fmodf_mpi_pi(getRawPosition()-zero);
// curPos = getRawPosition()-zero*direction;
if (!isnan(prevPosition)) {
// if (curPos - prevPosition < -PI)
// unwrapOffset += 1;
// else if (curPos - prevPosition > PI)
// unwrapOffset -= 1;
// Handle unwrapping around +/- PI, does same thing as lines above, but without branching
if (fabsf(curPos - prevPosition) > PI) {
unwrapOffset += (curPos < prevPosition) - (curPos > prevPosition);
// If unwrapped values apart by too much, revert unwrapOffset and ignore curPos
// if (TWO_PI - fabsf(curPos - prevPosition) > posLimit) {
// unwrapOffset -= (curPos < prevPosition) - (curPos > prevPosition);
// curPos = prevPosition;
// }
}
// Separate posLimit check needed if not near +/- PI
// else if (fabsf(curPos - prevPosition) > posLimit)
// curPos = prevPosition;
}
prevPosition = curPos;
return fmodf_mpi_pi((TWO_PI*unwrapOffset + curPos) * direction / gearRatio);
}
float Motor::update() {
float pos = getPosition();
// Velocity calculation should happen independent of mode
float error = fmodf_mpi_pi(pos - setpoint);
float posCtrlVal = pd.update(error);
// In position mode, update the motor command
if (mode == POSITION_MODE)
val = posCtrlVal;
// In open-loop mode, val has been set and nothing else needs to be done
// Send command, but don't modify "val" (set by user)
correctedVal = mapVal(val + barrier.calculate(pos));
// send the command
sendOpenLoop(correctedVal);
// Return the command so that the slave can do the same thing
return correctedVal;
}
float DLPF::update(float val) {
float delta;
switch (type)
{
case DLPF_ANGRATE:
delta = fmodf_mpi_pi(val - oldVal);
vel = interp1(delta * freq, vel, smooth);
oldVal = val;
return vel;
case DLPF_RATE:
delta = val - oldVal;
vel = interp1(delta * freq, vel, smooth);
oldVal = val;
return vel;
case DLPF_INTEGRATE:
val = smooth * oldVal + val / freq;
return val;
case DLPF_SMOOTH:
default:
oldVal = interp1(val, oldVal, smooth);
return oldVal;
}
}
extern void circleMeanDiff(float a, float b, float *mean, float *diff)
{
float r = fmodf_mpi_pi(a - b);
*diff = 0.5 * r;
*mean = fmodf_mpi_pi(b + 0.5*r);
}
