void Encoder_runSM() {
if (accumulatorIndex < ACCUMULATOR_LENGTH) {
accumulateAngle(currentReadAddress, currentWriteAddress);
accumulatorIndex++;
}
else {
// calculate and switch encoder choice, resetting index
calculateAngle();
}
}
float calculateAngle(float zeroAngle,int READ_ADDRESS,int WRITE_ADDRESS){
int count;
angleAccumulator = 0;
for(count = 0; count < ACCUMULATOR_LENGTH; count++){
accumulateAngle(READ_ADDRESS,WRITE_ADDRESS);
}
float finalAngle = angleAccumulator/ACCUMULATOR_LENGTH;
// TODO remove magick numbers
if(useZeroAngle){
if(finalAngle >= zeroAngle)
finalAngle = finalAngle - zeroAngle;
else
finalAngle = 360 - (zeroAngle - finalAngle);
if(finalAngle > 359.98)
finalAngle = 0;
}
return finalAngle;
}
