- if (frequency > 0) {
- // ICR3 = (int)(((double)F_CPU) / frequency); // Set max to the period
- // OCR3A = (int)(((double)F_CPU) / frequency) >> 1; // Set compare to half the period
- if (place > 10) {
- voice_place = (voice_place + 1) % voices;
- place = 0.0;
+ if (voices > 0) {
+ if (false && polyphony_rate > 0) {
+ if (voices > 1) {
+ voice_place %= voices;
+ if (place++ > (frequencies[voice_place] / polyphony_rate / CPU_PRESCALER / voices)) {
+ voice_place = (voice_place + 1) % voices;
+ place = 0.0;
+ }
+ }
+ if (vibrato_strength > 0) {
+ freq = frequencies[voice_place] * pow(VIBRATO_LUT[(int)vibrato_counter], vibrato_strength);
+ vibrato_counter = mod((vibrato_counter + 1), VIBRATO_LUT_LENGTH);
+ } else {
+ freq = frequencies[voice_place];
+ }
+ } else {
+ if (frequency != 0) {
+ if (frequency < frequencies[voices - 1]) {
+ frequency = frequency * pow(2, 440/frequencies[voices - 1]/12/4);
+ } else if (frequency > frequencies[voices - 1]) {
+ frequency = frequency * pow(2, -440/frequencies[voices - 1]/12/4);
+ }
+ } else {
+ frequency = frequencies[voices - 1];
+ }
+
+ if (false && vibrato_strength > 0) {
+ freq = frequency * pow(VIBRATO_LUT[(int)vibrato_counter], vibrato_strength);
+ vibrato_counter = mod((vibrato_counter + 1 + 440/frequencies[voices - 1]), VIBRATO_LUT_LENGTH);
+ } else {
+ freq = frequency;
+ }