vibrato and polyphony paratmeters

3 files changed, 249 insertions, 159 deletions

diff --git a/quantum/audio.c b/quantum/audio.c
index c92cb53731..8ea1bf6ff0 100644
--- a/quantum/audio.c
+++ b/quantum/audio.c
@@ -10,10 +10,13 @@
 
 #include "eeconfig.h"
 
+#include "vibrato_lut.h"
+
 #define PI 3.14159265
 
 #define CPU_PRESCALER 8
 
+// Largely untested PWM audio mode (doesn't sound as good)
 // #define PWM_AUDIO
 
 #ifdef PWM_AUDIO
@@ -67,10 +70,11 @@ bool note_resting = false;
 uint8_t current_note = 0;
 uint8_t rest_counter = 0;
 
-uint8_t vibrato_counter = 0;
-float vibrato_strength = 0;
+float vibrato_counter = 0;
+float vibrato_strength = .5;
+float vibrato_rate = 0.125;
 
-float polyphony_rate = 0;
+float polyphony_rate = .5;
 
 audio_config_t audio_config;
 
@@ -90,6 +94,81 @@ void audio_off(void) {
     eeconfig_write_audio(audio_config.raw);
 }
 
+// Vibrato rate functions
+
+void set_vibrato_rate(float rate) {
+    vibrato_rate = rate;
+}
+
+void increase_vibrato_rate(float change) {
+    vibrato_rate *= change;
+}
+
+void decrease_vibrato_rate(float change) {
+    vibrato_rate /= change;
+}
+
+#ifdef VIBRATO_STRENGTH_ENABLE
+
+void set_vibrato_strength(float strength) {
+    vibrato_strength = strength;
+}
+
+void increase_vibrato_strength(float change) {
+    vibrato_strength *= change;
+}
+
+void decrease_vibrato_strength(float change) {
+    vibrato_strength /= change;
+}
+
+#endif
+
+// Polyphony functions
+
+void set_polyphony_rate(float rate) {
+    polyphony_rate = rate;
+}
+
+void enable_polyphony() {
+    polyphony_rate = 5;
+}
+
+void disable_polyphony() {
+    polyphony_rate = 0;
+}
+
+void increase_polyphony_rate(float change) {
+    polyphony_rate *= change;
+}
+
+void decrease_polyphony_rate(float change) {
+    polyphony_rate /= change;
+}
+
+// Timbre function
+
+void set_timbre(float timbre) {
+    note_timbre = timbre;
+}
+
+// Tempo functions
+
+void set_tempo(float tempo) {
+    note_tempo = tempo;
+}
+
+void decrease_tempo(uint8_t tempo_change) {
+    note_tempo += (float) tempo_change;
+}
+
+void increase_tempo(uint8_t tempo_change) {
+    if (note_tempo - (float) tempo_change < 10) {
+        note_tempo = 10;
+    } else {
+        note_tempo -= (float) tempo_change;
+    }
+}
 
 void stop_all_notes() {
     voices = 0;
@@ -112,6 +191,7 @@ void stop_all_notes() {
 
 void stop_note(double freq) {
     if (note) {
+        cli();
         #ifdef PWM_AUDIO
             freq = freq / SAMPLE_RATE;
         #endif
@@ -125,6 +205,7 @@ void stop_note(double freq) {
                     volumes[j] = volumes[j+1];
                     volumes[j+1] = 0;
                 }
+                break;
             }
         }
         voices--;
@@ -144,6 +225,7 @@ void stop_note(double freq) {
             volume = 0;
             note = false;
         }
+        sei();
     }
 }
 
@@ -190,6 +272,16 @@ float mod(float a, int b)
     return r < 0 ? r + b : r;
 }
 
+float vibrato(float average_freq) {
+    #ifdef VIBRATO_STRENGTH_ENABLE
+        float vibrated_freq = average_freq * pow(VIBRATO_LUT[(int)vibrato_counter], vibrato_strength);
+    #else
+        float vibrated_freq = average_freq * VIBRATO_LUT[(int)vibrato_counter];
+    #endif
+    vibrato_counter = mod((vibrato_counter + vibrato_rate * (1.0 + 440.0/average_freq)), VIBRATO_LUT_LENGTH);
+    return vibrated_freq;
+}
+
 ISR(TIMER3_COMPA_vect) {
     if (note) {
         #ifdef PWM_AUDIO
@@ -241,34 +333,30 @@ ISR(TIMER3_COMPA_vect) {
             }
         #else
             if (voices > 0) {
-                if (false && polyphony_rate > 0) {                
+                if (polyphony_rate > 0) {                
                     if (voices > 1) {
                         voice_place %= voices;
-                        if (place++ > (frequencies[voice_place] / polyphony_rate / CPU_PRESCALER / voices)) {
+                        if (place++ > (frequencies[voice_place] / polyphony_rate / CPU_PRESCALER)) {
                             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);
+                        freq = vibrato(frequencies[voice_place]);
                     } 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);
-                        }
+                    if (frequency != 0 && frequency < frequencies[voices - 1] && frequency < frequencies[voices - 1] * pow(2, -440/frequencies[voices - 1]/12/2)) {
+                        frequency = frequency * pow(2, 440/frequency/12/2);
+                    } else if (frequency != 0 && frequency > frequencies[voices - 1] && frequency > frequencies[voices - 1] * pow(2, 440/frequencies[voices - 1]/12/2)) {
+                        frequency = frequency * pow(2, -440/frequency/12/2);
                     } 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);
+                    if (vibrato_strength > 0) {
+                        freq = vibrato(frequency);
                     } else {
                         freq = frequency;
                     } 
@@ -302,9 +390,8 @@ ISR(TIMER3_COMPA_vect) {
             if (note_frequency > 0) {
                 float freq;
 
-                if (false && vibrato_strength > 0) {
-                    freq = note_frequency * pow(VIBRATO_LUT[(int)vibrato_counter], vibrato_strength);
-                    vibrato_counter = mod((vibrato_counter + 1), VIBRATO_LUT_LENGTH);
+                if (vibrato_strength > 0) {
+                    freq = vibrato(note_frequency);
                 } else {
                     freq = note_frequency;
                 }
@@ -369,7 +456,7 @@ ISR(TIMER3_COMPA_vect) {
 void play_notes(float (*np)[][2], uint8_t n_count, bool n_repeat, float n_rest) {
 
 if (audio_config.enable) {
-
+    cli();
 	// Cancel note if a note is playing
     if (note)
         stop_all_notes();
@@ -398,7 +485,7 @@ if (audio_config.enable) {
         TIMSK3 |= _BV(OCIE3A);
         TCCR3A |= _BV(COM3A1);
     #endif
-
+    sei();
 }
 
 }
@@ -425,7 +512,7 @@ if (audio_config.enable) {
 void play_note(double freq, int vol) {
 
 if (audio_config.enable && voices < 8) {
-
+    cli();
     // Cancel notes if notes are playing
     if (notes)
         stop_all_notes();
@@ -445,36 +532,9 @@ if (audio_config.enable && voices < 8) {
         TIMSK3 |= _BV(OCIE3A);
         TCCR3A |= _BV(COM3A1);
     #endif
-
-}
-
-}
-
-void set_timbre(float timbre)
-{
-	note_timbre = timbre;
+    sei();
 }
 
-void set_tempo(float tempo)
-{
-	note_tempo = tempo;
-}
-
-void decrease_tempo(uint8_t tempo_change)
-{
-	note_tempo += (float) tempo_change;
-}
-
-void increase_tempo(uint8_t tempo_change)
-{
-	if (note_tempo - (float) tempo_change < 10)
-		{
-			note_tempo = 10;
-		}
-	else
-		{
-		note_tempo -= (float) tempo_change;
-		}
 }
 
 //------------------------------------------------------------------------------
diff --git a/quantum/audio.h b/quantum/audio.h
index f705341d7f..85756af9d4 100644
--- a/quantum/audio.h
+++ b/quantum/audio.h
@@ -8,6 +8,9 @@
 #ifndef AUDIO_H
 #define AUDIO_H
 
+// Enable vibrato strength/amplitude - slows down ISR too much
+// #define VIBRATO_STRENGTH_ENABLE
+
 typedef union {
     uint8_t raw;
     struct {
@@ -20,6 +23,34 @@ void audio_toggle(void);
 void audio_on(void);
 void audio_off(void);
 
+// Vibrato rate functions
+
+void set_vibrato_rate(float rate);
+void increase_vibrato_rate(float change);
+void decrease_vibrato_rate(float change);
+
+#ifdef VIBRATO_STRENGTH_ENABLE
+
+void set_vibrato_strength(float strength);
+void increase_vibrato_strength(float change);
+void decrease_vibrato_strength(float change);
+
+#endif
+
+// Polyphony functions
+
+void set_polyphony_rate(float rate);
+void enable_polyphony();
+void disable_polyphony();
+void increase_polyphony_rate(float change);
+void decrease_polyphony_rate(float change);
+
+void set_timbre(float timbre);
+void set_tempo(float tempo);
+
+void increase_tempo(uint8_t tempo_change);
+void decrease_tempo(uint8_t tempo_change);
+
 void play_sample(uint8_t * s, uint16_t l, bool r);
 void play_note(double freq, int vol);
 void stop_note(double freq);
@@ -27,11 +58,6 @@ void stop_all_notes(void);
 void init_notes(void);
 void play_notes(float (*np)[][2], uint8_t n_count, bool n_repeat, float n_rest);
 
-void set_timbre(float timbre);
-void set_tempo(float tempo);
-void increase_tempo(uint8_t tempo_change);
-void decrease_tempo(uint8_t tempo_change);
-
 #define SCALE (int []){ 0 + (12*0), 2 + (12*0), 4 + (12*0), 5 + (12*0), 7 + (12*0), 9 + (12*0), 11 + (12*0), \
 						0 + (12*1), 2 + (12*1), 4 + (12*1), 5 + (12*1), 7 + (12*1), 9 + (12*1), 11 + (12*1), \
 						0 + (12*2), 2 + (12*2), 4 + (12*2), 5 + (12*2), 7 + (12*2), 9 + (12*2), 11 + (12*2), \
@@ -47,108 +73,4 @@ void decrease_tempo(uint8_t tempo_change);
 void play_goodbye_tone(void);
 void play_startup_tone(void);
 
-#define VIBRATO_LUT (float []) { \
-1.00090714186239, \
-1.00181152169061, \
-1.00270955652027, \
-1.00359767896099, \
-1.00447235162891, \
-1.00533008160601, \
-1.00616743486158, \
-1.00698105056935, \
-1.00776765525194, \
-1.00852407668313, \
-1.0092472574777, \
-1.00993426829815, \
-1.01058232060837, \
-1.01118877890462, \
-1.01175117235612, \
-1.01226720578933, \
-1.01273476995269, \
-1.01315195100182, \
-1.0135170391489, \
-1.01382853642434, \
-1.01408516350345, \
-1.01428586555648, \
-1.0144298170856, \
-1.0145164257189, \
-1.01454533493752, \
-1.0145164257189, \
-1.0144298170856, \
-1.01428586555648, \
-1.01408516350345, \
-1.01382853642434, \
-1.0135170391489, \
-1.01315195100182, \
-1.01273476995269, \
-1.01226720578933, \
-1.01175117235612, \
-1.01118877890462, \
-1.01058232060837, \
-1.00993426829815, \
-1.0092472574777, \
-1.00852407668313, \
-1.00776765525194, \
-1.00698105056935, \
-1.00616743486158, \
-1.00533008160601, \
-1.00447235162891, \
-1.00359767896099, \
-1.00270955652027, \
-1.00181152169061, \
-1.00090714186239, \
-1, \
-0.999093680298157, \
-0.998191753986265, \
-0.997297765337276, \
-0.996415217934032, \
-0.995547561242821, \
-0.99469817754036, \
-0.993870369236874, \
-0.993067346634376, \
-0.992292216155724, \
-0.991547969076588, \
-0.990837470789065, \
-0.990163450622494, \
-0.989528492243954, \
-0.988935024658062, \
-0.988385313823004, \
-0.98788145489731, \
-0.987425365129624, \
-0.987018777401739, \
-0.986663234433381, \
-0.986360083655655, \
-0.986110472758728, \
-0.985915345918143, \
-0.985775440703176, \
-0.985691285669809, \
-0.985663198640188, \
-0.985691285669809, \
-0.985775440703176, \
-0.985915345918143, \
-0.986110472758728, \
-0.986360083655655, \
-0.986663234433381, \
-0.987018777401739, \
-0.987425365129624, \
-0.98788145489731, \
-0.988385313823004, \
-0.988935024658062, \
-0.989528492243954, \
-0.990163450622494, \
-0.990837470789065, \
-0.991547969076588, \
-0.992292216155724, \
-0.993067346634376, \
-0.993870369236874, \
-0.99469817754036, \
-0.99554756124282, \
-0.996415217934032, \
-0.997297765337276, \
-0.998191753986265, \
-0.999093680298157, \
-1, \
-}
-#define VIBRATO_LUT_LENGTH NOTE_ARRAY_SIZE(VIBRATO_LUT)
-
 #endif
\ No newline at end of file
diff --git a/quantum/vibrato_lut.h b/quantum/vibrato_lut.h
new file mode 100644
index 0000000000..4c267a626a
--- /dev/null
+++ b/quantum/vibrato_lut.h
@@ -0,0 +1,108 @@
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <avr/pgmspace.h>
+
+#define VIBRATO_LUT_LENGTH 100
+
+const float VIBRATO_LUT[VIBRATO_LUT_LENGTH] = { \
+1.00045346811453,
+1.00090535101508,
+1.00135386178926,
+1.00179722447259,
+1.00223368114872,
+1.0026614990145,
+1.00307897737994,
+1.00348445457284,
+1.00387631471807,
+1.00425299436105,
+1.00461298890553,
+1.00495485883603,
+1.00527723569589,
+1.00557882779254,
+1.00585842560279,
+1.00611490685176,
+1.00634724124066,
+1.00655449479987,
+1.00673583384565,
+1.00689052852052,
+1.00701795589922,
+1.00711760264454,
+1.0071890671992,
+1.00723206150266,
+1.0072464122237,
+1.00723206150266,
+1.0071890671992,
+1.00711760264454,
+1.00701795589922,
+1.00689052852052,
+1.00673583384565,
+1.00655449479987,
+1.00634724124066,
+1.00611490685176,
+1.00585842560279,
+1.00557882779254,
+1.00527723569589,
+1.00495485883603,
+1.00461298890553,
+1.00425299436105,
+1.00387631471807,
+1.00348445457284,
+1.00307897737994,
+1.0026614990145,
+1.00223368114872,
+1.00179722447259,
+1.00135386178926,
+1.00090535101508,
+1.00045346811453,
+1,
+0.999546737425598,
+0.999095467903976,
+0.998647968674285,
+0.998205999748565,
+0.99777129706302,
+0.997345565759612,
+0.996930473622346,
+0.996527644691494,
+0.996138653077835,
+0.99576501699778,
+0.995408193048995,
+0.995069570744927,
+0.994750467325326,
+0.994452122858643,
+0.994175695650927,
+0.993922257974591,
+0.99369279212925,
+0.993488186845591,
+0.993309234042139,
+0.993156625943589,
+0.993030952568311,
+0.99293269959154,
+0.992862246589715,
+0.992819865670409,
+0.992805720491269,
+0.992819865670409,
+0.992862246589715,
+0.99293269959154,
+0.993030952568311,
+0.993156625943589,
+0.993309234042139,
+0.993488186845591,
+0.99369279212925,
+0.993922257974591,
+0.994175695650927,
+0.994452122858643,
+0.994750467325326,
+0.995069570744927,
+0.995408193048995,
+0.99576501699778,
+0.996138653077835,
+0.996527644691494,
+0.996930473622346,
+0.997345565759612,
+0.99777129706302,
+0.998205999748565,
+0.998647968674285,
+0.999095467903976,
+0.999546737425598,
+1
+};
\ No newline at end of file