]> git.donarmstrong.com Git - kiibohd-controller.git/blobdiff - Scan/avr-capsense/scan_loop.c
Fixing the strobe count on the Kishsaver
[kiibohd-controller.git] / Scan / avr-capsense / scan_loop.c
index e7cfd2cf4cbd91c6953aff077bdaf2becd05b961..bec9d9d95e21eda598c718acf82333b92a5f263e 100644 (file)
 // ----- Defines -----
 
 // TODO dfj defines...needs commenting and maybe some cleaning...
-#define MAX_PRESS_DELTA_MV 470
+#define MAX_PRESS_DELTA_MV 380
 #define THRESHOLD_MV (MAX_PRESS_DELTA_MV >> 1)
 //(2560 / (0x3ff/2)) ~= 5
 #define MV_PER_ADC 5
-// 5
-
 #define THRESHOLD (THRESHOLD_MV / MV_PER_ADC)
 
-#define BUMP_DETECTION 0
-#define BUMP_THRESHOLD 0x50
-#define BUMP_REST_US 1200
-
 #define STROBE_SETTLE 1
 #define MUX_SETTLE 1
 
 #define PRESCALE_SHIFT (ADPS0)
 #define PRESCALE 3
 
-// TODO Remove this define when unnecessary -HaaTa
-#define STROBE_LINES 16
+// Max number of strobes supported by the hardware
+// Strobe lines are detected at startup, extra strobes cause anomalies like phantom keypresses
+#define MAX_STROBES 18
+
+// Number of consecutive samples required to pass debounce
+#define DEBOUNCE_THRESHOLD 5
 
 #define MUXES_COUNT 8
 #define MUXES_COUNT_XSHIFT 3
 
 #define WARMUP_LOOPS ( 1024 )
+#define WARMUP_STOP (WARMUP_LOOPS - 1)
 
 #define SAMPLES 10
 #define SAMPLE_OFFSET ((SAMPLES) - MUXES_COUNT)
 #define SAMPLE_CONTROL 3
 
-// TODO Figure out calculation or best way to determine at startup -HaaTa
-//#define DEFAULT_KEY_BASE 0xc8
-#define DEFAULT_KEY_BASE 0x95
+// Starting average for keys, per key will adjust during runtime
+// XXX - A better method is needed to choose this value (i.e. not experimental)
+//       The ideal average is not always found for weak keys if this is set too high...
+#define DEFAULT_KEY_BASE 0xB0
 
-#define KEY_COUNT ((STROBE_LINES) * (MUXES_COUNT))
+#define KEY_COUNT ((MAX_STROBES) * (MUXES_COUNT))
 
 #define RECOVERY_CONTROL 1
 #define RECOVERY_SOURCE  0
@@ -146,23 +146,17 @@ uint8_t ze_strober = 0;
 
 uint16_t samples [SAMPLES];
 
-uint16_t adc_mux_averages   [MUXES_COUNT];
-uint16_t adc_strobe_averages[STROBE_LINES];
+uint8_t cur_keymap[MAX_STROBES];
 
-uint8_t cur_keymap[STROBE_LINES];
-uint8_t usb_keymap[STROBE_LINES];
+uint8_t keymap_change;
 
-uint8_t usb_dirty;
-
-uint16_t threshold = 0x25; // HaaTa Hack -TODO
-//uint16_t threshold = 0x16; // HaaTa Hack -TODO
-//uint16_t threshold = THRESHOLD;
+uint16_t threshold = THRESHOLD;
 
 uint8_t column = 0;
 
 uint16_t keys_averages_acc[KEY_COUNT];
-uint16_t keys_averages[KEY_COUNT];
-uint16_t keys_averages_acc_count=0;
+uint16_t keys_averages    [KEY_COUNT];
+uint8_t  keys_debounce    [KEY_COUNT];
 
 uint8_t full_samples[KEY_COUNT];
 
@@ -175,8 +169,8 @@ uint8_t idle = 1;
 uint8_t error = 0;
 uint16_t error_data = 0;
 
-uint16_t mux_averages[MUXES_COUNT];
-uint16_t strobe_averages[STROBE_LINES];
+uint8_t total_strobes = MAX_STROBES;
+uint8_t strobe_map[MAX_STROBES];
 
 uint8_t dump_count = 0;
 
@@ -188,13 +182,14 @@ uint16_t db_threshold = 0;
 
 // ----- Function Declarations -----
 
-void dump    ( void );
-void dumpkeys( void );
+void dump( void );
 
 void recovery( uint8_t on );
 
 int sampleColumn( uint8_t column );
 
+void capsense_scan( void );
+
 void setup_ADC( void );
 
 void strobe_w( uint8_t strobe_num );
@@ -218,28 +213,86 @@ inline void scan_setup()
        DDRE  = E_MASK;
        PORTE = 0 ;
 
+       // Hardcoded strobes for debugging
+       // Strobes start at 0 and go to 17 (18), not all Model Fs use all of the available strobes
+       // The single row ribbon connector Model Fs only have a max of 16 strobes
+#define KISHSAVER_STROBE
+//#define TERMINAL_6110668_STROBE
+//#define UNSAVER_STROBE
+#ifdef KISHSAVER_STROBE
+       total_strobes = 8;
+       //total_strobes = 9;
+
+       strobe_map[0] = 2; // Kishsaver doesn't use strobe 0 and 1
+       strobe_map[1] = 3;
+       strobe_map[2] = 4;
+       strobe_map[3] = 5;
+       strobe_map[4] = 6;
+       strobe_map[5] = 7;
+       strobe_map[6] = 8;
+       strobe_map[7] = 9;
+       // XXX - Disabling for now, not sure how to deal with test points yet (without spamming the debug)
+       //strobe_map[9] = 15; // Test point strobe (3 test points, sense 1, 4, 5)
+#elif defined(TERMINAL_6110668_STROBE)
+       total_strobes = 16;
+
+       strobe_map[0] = 0;
+       strobe_map[1] = 1;
+       strobe_map[2] = 2;
+       strobe_map[3] = 3;
+       strobe_map[4] = 4;
+       strobe_map[5] = 5;
+       strobe_map[6] = 6;
+       strobe_map[7] = 7;
+       strobe_map[8] = 8;
+       strobe_map[9] = 9;
+       strobe_map[10] = 10;
+       strobe_map[11] = 11;
+       strobe_map[12] = 12;
+       strobe_map[13] = 13;
+       strobe_map[14] = 14;
+       strobe_map[15] = 15;
+#elif defined(UNSAVER_STROBE)
+       total_strobes = 14;
+
+       strobe_map[0] = 0;
+       strobe_map[1] = 1;
+       strobe_map[2] = 2;
+       strobe_map[3] = 3;
+       strobe_map[4] = 4;
+       strobe_map[5] = 5;
+       strobe_map[6] = 6;
+       strobe_map[7] = 7;
+       strobe_map[8] = 8;
+       strobe_map[9] = 9;
+       strobe_map[10] = 10;
+       strobe_map[11] = 11;
+       strobe_map[12] = 12;
+       strobe_map[13] = 13;
+#else
+       // Strobe detection
+       // TODO
+#endif
 
        // TODO all this code should probably be in scan_resetKeyboard
-       for (int i=0; i < STROBE_LINES; ++i) {
+       for ( int i = 0; i < total_strobes; ++i)
+       {
                cur_keymap[i] = 0;
-               usb_keymap[i] = 0;
-       }
-
-       for(int i=0; i < MUXES_COUNT; ++i) {
-               adc_mux_averages[i] = 0x20; // experimentally determined.
-       }
-       for(int i=0; i < STROBE_LINES; ++i) {
-               adc_strobe_averages[i] = 0x20; // yup.
        }
 
-       for(int i=0; i < KEY_COUNT; ++i) {
+       for ( int i = 0; i < KEY_COUNT; ++i )
+       {
                keys_averages[i] = DEFAULT_KEY_BASE;
                keys_averages_acc[i] = (DEFAULT_KEY_BASE);
+
+               // Reset debounce table
+               keys_debounce[i] = 0;
        }
 
        /** warm things up a bit before we start collecting data, taking real samples. */
-       for(uint8_t i = 0; i < STROBE_LINES; ++i) {
-               sampleColumn(i);
+       for ( uint8_t i = 0; i < total_strobes; ++i )
+       {
+               sampleColumn( strobe_map[i] );
        }
 
 
@@ -252,34 +305,120 @@ inline void scan_setup()
 // Main Detection Loop
 // This is where the important stuff happens
 inline uint8_t scan_loop()
+{
+       capsense_scan();
+
+       // Error case, should not occur in normal operation
+       if ( error )
+       {
+               erro_msg("Problem detected... ");
+
+               // Keymap scan debug
+               for ( uint8_t i = 0; i < total_strobes; ++i )
+               {
+                               printHex(cur_keymap[strobe_map[i]]);
+                               print(" ");
+               }
+
+               print(" : ");
+               printHex(error);
+               error = 0;
+               print(" : ");
+               printHex(error_data);
+               error_data = 0;
+
+               // Display keymaps and other debug information if warmup completede
+               if ( boot_count >= WARMUP_LOOPS )
+               {
+                       dump();
+               }
+       }
+
+
+       // Return non-zero if macro and USB processing should be delayed
+       // Macro processing will always run if returning 0
+       // USB processing only happens once the USB send timer expires, if it has not, scan_loop will be called
+       //  after the macro processing has been completed
+       return 0;
+}
+
+
+// Reset Keyboard
+void scan_resetKeyboard( void )
+{
+       // Empty buffer, now that keyboard has been reset
+       KeyIndex_BufferUsed = 0;
+}
+
+
+// Send data to keyboard
+// NOTE: Only used for converters, since the scan module shouldn't handle sending data in a controller
+uint8_t scan_sendData( uint8_t dataPayload )
+{
+       return 0;
+}
+
+
+// Reset/Hold keyboard
+// NOTE: Only used for converters, not needed for full controllers
+void scan_lockKeyboard( void )
+{
+}
+
+// NOTE: Only used for converters, not needed for full controllers
+void scan_unlockKeyboard( void )
+{
+}
+
+
+// Signal KeyIndex_Buffer that it has been properly read
+// NOTE: Only really required for implementing "tricks" in converters for odd protocols
+void scan_finishedWithBuffer( uint8_t sentKeys )
+{
+       // Convenient place to clear the KeyIndex_Buffer
+       KeyIndex_BufferUsed = 0;
+       return;
+}
+
+
+// Signal KeyIndex_Buffer that it has been properly read and sent out by the USB module
+// NOTE: Only really required for implementing "tricks" in converters for odd protocols
+void scan_finishedWithUSBBuffer( uint8_t sentKeys )
+{
+       return;
+}
+
+
+inline void capsense_scan()
 {
        // TODO dfj code...needs commenting + cleanup...
-       uint8_t strober = 0;
        uint32_t full_av_acc = 0;
 
-       for (strober = 0; strober < STROBE_LINES; ++strober)
+       for ( uint8_t strober = 0; strober < total_strobes; ++strober )
        {
+               uint8_t map_strobe = strobe_map[strober];
 
                uint8_t tries = 1;
-               while ( tries++ && sampleColumn( strober ) ) { tries &= 0x7; } // don't waste this one just because the last one was poop.
-               column = testColumn(strober);
+               while ( tries++ && sampleColumn( map_strobe ) ) { tries &= 0x7; } // don't waste this one just because the last one was poop.
+               column = testColumn( map_strobe );
 
                idle |= column; // if column has any pressed keys, then we are not idle.
 
-               if( column != cur_keymap[strober] && ( boot_count >= WARMUP_LOOPS ) )
+               // TODO Is this needed anymore? Really only helps debug -HaaTa
+               if( column != cur_keymap[map_strobe] && ( boot_count >= WARMUP_LOOPS ) )
                {
-                       cur_keymap[strober] = column;
-                       usb_dirty = 1;
+                       cur_keymap[map_strobe] = column;
+                       keymap_change = 1;
                }
 
-               idle |= usb_dirty; // if any keys have changed inc. released, then we are not idle.
+               idle |= keymap_change; // if any keys have changed inc. released, then we are not idle.
 
                if ( error == 0x50 )
                {
-                       error_data |= (((uint16_t)strober) << 12);
+                       error_data |= (((uint16_t)map_strobe) << 12);
                }
 
-               uint8_t strobe_line = strober << MUXES_COUNT_XSHIFT;
+               uint8_t strobe_line = map_strobe << MUXES_COUNT_XSHIFT;
                for ( int i = 0; i < MUXES_COUNT; ++i )
                {
                        // discard sketchy low bit, and meaningless high bits.
@@ -287,28 +426,11 @@ inline uint8_t scan_loop()
                        full_samples[strobe_line + i] = sample;
                        keys_averages_acc[strobe_line + i] += sample;
                }
-               keys_averages_acc_count++;
 
-               strobe_averages[strober] = 0;
                for ( uint8_t i = SAMPLE_OFFSET; i < ( SAMPLE_OFFSET + MUXES_COUNT ); ++i )
                {
                        full_av_acc += (samples[i]);
-#ifdef COLLECT_STROBE_AVERAGES
-                       mux_averages[i - SAMPLE_OFFSET] += samples[i];
-                       strobe_averages[strober] += samples[i];
-#endif
                }
-
-#ifdef COLLECT_STROBE_AVERAGES
-               adc_strobe_averages[strober] += strobe_averages[strober] >> 3;
-               adc_strobe_averages[strober] >>= 1;
-
-               /** test if we went negative. */
-               if ( ( adc_strobe_averages[strober] & 0xFF00 ) && ( boot_count >= WARMUP_LOOPS ) )
-               {
-                       error = 0xf; error_data = adc_strobe_averages[strober];
-               }
-#endif
        } // for strober
 
 #ifdef VERIFY_TEST_PAD
@@ -318,26 +440,6 @@ inline uint8_t scan_loop()
                error = 0x05;
                error_data = cur_keymap[TEST_KEY_STROBE] << 8;
                error_data += full_samples[TEST_KEY_STROBE * 8];
-               //threshold++;
-       }
-#endif
-
-#ifdef COLLECT_STROBE_AVERAGES
-       // calc mux averages.
-       if ( boot_count < WARMUP_LOOPS )
-       {
-               full_av += (full_av_acc >> (7));
-               full_av >>= 1;
-               full_av_acc = 0;
-
-               for ( int i = 0; i < MUXES_COUNT; ++i )
-               {
-                       adc_mux_averages[i] = (adc_mux_averages[i] << MUX_MIX) - adc_mux_averages[i];
-                       adc_mux_averages[i] += (mux_averages[i] >> 4);
-                       adc_mux_averages[i] >>= MUX_MIX;
-
-                       mux_averages[i] = 0;
-               }
        }
 #endif
 
@@ -348,28 +450,44 @@ inline uint8_t scan_loop()
        idle_count++;
        idle_count &= IDLE_COUNT_MASK;
 
+       // Warm up voltage references
        if ( boot_count < WARMUP_LOOPS )
        {
-               error = 0x0C;
-               error_data = boot_count;
                boot_count++;
+
+               switch ( boot_count )
+               {
+               // First loop
+               case 1:
+                       // Show msg at first iteration only
+                       info_msg("Warming up the voltage references");
+                       break;
+               // Middle iterations
+               case 300:
+               case 600:
+               case 900:
+               case 1200:
+                       print(".");
+                       break;
+               // Last loop
+               case WARMUP_STOP:
+                       print("\n");
+                       info_msg("Warmup finished using ");
+                       printInt16( WARMUP_LOOPS );
+                       print(" iterations\n");
+                       break;
+               }
        }
        else
        {
-               if ( usb_dirty )
+               // Reset accumulators and idle flag/counter
+               if ( keymap_change )
                {
-                       for ( int i = 0; i < STROBE_LINES; ++i )
-                       {
-                               usb_keymap[i] = cur_keymap[i];
-                       }
-
-                       dumpkeys();
-                       usb_dirty = 0;
-                       memset(((void *)keys_averages_acc), 0, (size_t)(KEY_COUNT * sizeof (uint16_t)));
-                       keys_averages_acc_count = 0;
+                       for ( uint8_t c = 0; c < KEY_COUNT; ++c ) { keys_averages_acc[c] = 0; }
                        idle_count = 0;
                        idle = 0;
-                       _delay_us(100);
+
+                       keymap_change = 0;
                }
 
                if ( !idle_count )
@@ -389,81 +507,14 @@ inline uint8_t scan_loop()
                                        keys_averages_acc[i] = 0;
                                }
                        }
-                       keys_averages_acc_count = 0;
 
                        if ( boot_count >= WARMUP_LOOPS )
                        {
                                dump();
                        }
-
-                       sampleColumn(0x0); // to resync us if we dumped a mess 'o text.
                }
 
        }
-
-
-       // Return non-zero if macro and USB processing should be delayed
-       // Macro processing will always run if returning 0
-       // USB processing only happens once the USB send timer expires, if it has not, scan_loop will be called
-       //  after the macro processing has been completed
-       return 0;
-}
-
-
-// Reset Keyboard
-void scan_resetKeyboard( void )
-{
-       // Empty buffer, now that keyboard has been reset
-       KeyIndex_BufferUsed = 0;
-}
-
-
-// Send data to keyboard
-// NOTE: Only used for converters, since the scan module shouldn't handle sending data in a controller
-uint8_t scan_sendData( uint8_t dataPayload )
-{
-       return 0;
-}
-
-
-// Reset/Hold keyboard
-// NOTE: Only used for converters, not needed for full controllers
-void scan_lockKeyboard( void )
-{
-}
-
-// NOTE: Only used for converters, not needed for full controllers
-void scan_unlockKeyboard( void )
-{
-}
-
-
-// Signal KeyIndex_Buffer that it has been properly read
-// NOTE: Only really required for implementing "tricks" in converters for odd protocols
-void scan_finishedWithBuffer( uint8_t sentKeys )
-{
-       // Convenient place to clear the KeyIndex_Buffer
-       KeyIndex_BufferUsed = 0;
-       return;
-}
-
-
-// Signal KeyIndex_Buffer that it has been properly read and sent out by the USB module
-// NOTE: Only really required for implementing "tricks" in converters for odd protocols
-void scan_finishedWithUSBBuffer( uint8_t sentKeys )
-{
-       return;
-}
-
-
-void _delay_loop( uint8_t __count )
-{
-       __asm__ volatile (
-               "1: dec %0" "\n\t"
-               "brne 1b"
-               : "=r" (__count)
-               : "0" (__count)
-       );
 }
 
 
@@ -554,190 +605,35 @@ void strobe_w( uint8_t strobe_num )
        PORTD &= ~(D_MASK);
        PORTE &= ~(E_MASK);
 
-#ifdef SHORT_C
-       //strobe_num = 15 - strobe_num;
-#endif
-       /*
-       printHex( strobe_num );
-       print(" ");
-       strobe_num = 9 - strobe_num;
-       printHex( strobe_num );
-       print("\n");
-       */
-
-       switch(strobe_num) {
-
-       // XXX Kishsaver strobe (note that D0, D1 are not used)
-       case 0: PORTD |= (1 << 0); break;
-       case 1: PORTD |= (1 << 1); break;
-       case 2: PORTD |= (1 << 2); break;
-       case 3: PORTD |= (1 << 3); break;
-       case 4: PORTD |= (1 << 4); break;
-       case 5: PORTD |= (1 << 5); break;
-
-       // TODO REMOVEME
-       case 6: PORTD |= (1 << 6); break;
-       case 7: PORTD |= (1 << 7); break;
-       case 8: PORTE |= (1 << 0); break;
-       case 9: PORTE |= (1 << 1); break;
-       //case 15: PORTC |= (1 << 5); break; // Test strobe on kishsaver
-
-#if 0
-       // XXX Kishsaver strobe (note that D0, D1 are not used)
-       case 0: PORTD |= (1 << 2); break;
-       case 1: PORTD |= (1 << 3); break;
-       case 2: PORTD |= (1 << 4); break;
-       case 3: PORTD |= (1 << 5); break;
-
-       // TODO REMOVEME
-       case 4: PORTD |= (1 << 6); break;
-       case 5: PORTD |= (1 << 7); break;
-       case 6: PORTE |= (1 << 0); break;
-       case 7: PORTE |= (1 << 1); break;
-       case 15: PORTC |= (1 << 5); break; // Test strobe on kishsaver
-#endif
-/*
-#ifdef ALL_D
-
-       case 6: PORTD |= (1 << 6); break;
-       case 7: PORTD |= (1 << 7); break;
-
-       case 8:  PORTC |= (1 << 0); break;
-       case 9:  PORTC |= (1 << 1); break;
-       case 10: PORTC |= (1 << 2); break;
-       case 11: PORTC |= (1 << 3); break;
-       case 12: PORTC |= (1 << 4); break;
-       case 13: PORTC |= (1 << 5); break;
-       case 14: PORTC |= (1 << 6); break;
-       case 15: PORTC |= (1 << 7); break;
-
-       case 16: PORTE |= (1 << 0); break;
-       case 17: PORTE |= (1 << 1); break;
-
-#else
-#ifdef SHORT_D
-
-       case 6: PORTE |= (1 << 0); break;
-       case 7: PORTE |= (1 << 1); break;
-
-       case 8:  PORTC |= (1 << 0); break;
-       case 9:  PORTC |= (1 << 1); break;
-       case 10: PORTC |= (1 << 2); break;
-       case 11: PORTC |= (1 << 3); break;
-       case 12: PORTC |= (1 << 4); break;
-       case 13: PORTC |= (1 << 5); break;
-       case 14: PORTC |= (1 << 6); break;
-       case 15: PORTC |= (1 << 7); break;
-
-#else
-#ifdef SHORT_C
+       // Strobe table
+       // Not all strobes are used depending on which are detected
+       switch ( strobe_num )
+       {
 
-       case 6: PORTD |= (1 << 6); break;
-       case 7: PORTD |= (1 << 7); break;
+       case 0:  PORTD |= (1 << 0); break;
+       case 1:  PORTD |= (1 << 1); break;
+       case 2:  PORTD |= (1 << 2); break;
+       case 3:  PORTD |= (1 << 3); break;
+       case 4:  PORTD |= (1 << 4); break;
+       case 5:  PORTD |= (1 << 5); break;
+       case 6:  PORTD |= (1 << 6); break;
+       case 7:  PORTD |= (1 << 7); break;
 
-       case 8: PORTE |= (1 << 0); break;
-       case 9: PORTE |= (1 << 1); break;
+       case 8:  PORTE |= (1 << 0); break;
+       case 9:  PORTE |= (1 << 1); break;
 
-       case 10:  PORTC |= (1 << 0); break;
-       case 11:  PORTC |= (1 << 1); break;
+       case 10: PORTC |= (1 << 0); break;
+       case 11: PORTC |= (1 << 1); break;
        case 12: PORTC |= (1 << 2); break;
        case 13: PORTC |= (1 << 3); break;
        case 14: PORTC |= (1 << 4); break;
        case 15: PORTC |= (1 << 5); break;
-
        case 16: PORTC |= (1 << 6); break;
        case 17: PORTC |= (1 << 7); break;
 
-#endif
-#endif
-#endif
-*/
-
        default:
                break;
        }
-
-
-#if 0 // New code from dfj -> still needs redoing for kishsaver and autodetection of strobes
-#ifdef SHORT_C
-       strobe_num = 15 - strobe_num;
-#endif
-
-#ifdef SINGLE_COLUMN_TEST
-       strobe_num = 5;
-#endif
-
-       switch(strobe_num) {
-
-       case 0: PORTD |= (1 << 0); DDRD &= ~(1 << 0); break;
-       case 1: PORTD |= (1 << 1); DDRD &= ~(1 << 1); break;
-       case 2: PORTD |= (1 << 2); DDRD &= ~(1 << 2); break;
-       case 3: PORTD |= (1 << 3); DDRD &= ~(1 << 3); break;
-       case 4: PORTD |= (1 << 4); DDRD &= ~(1 << 4); break;
-       case 5: PORTD |= (1 << 5); DDRD &= ~(1 << 5); break;
-
-#ifdef ALL_D
-
-       case 6: PORTD |= (1 << 6); break;
-       case 7: PORTD |= (1 << 7); break;
-
-       case 8:  PORTC |= (1 << 0); break;
-       case 9:  PORTC |= (1 << 1); break;
-       case 10: PORTC |= (1 << 2); break;
-       case 11: PORTC |= (1 << 3); break;
-       case 12: PORTC |= (1 << 4); break;
-       case 13: PORTC |= (1 << 5); break;
-       case 14: PORTC |= (1 << 6); break;
-       case 15: PORTC |= (1 << 7); break;
-
-       case 16: PORTE |= (1 << 0); break;
-       case 17: PORTE |= (1 << 1); break;
-
-#else
-#ifdef SHORT_D
-
-       case 6: PORTE |= (1 << 0); break;
-       case 7: PORTE |= (1 << 1); break;
-
-       case 8:  PORTC |= (1 << 0); break;
-       case 9:  PORTC |= (1 << 1); break;
-       case 10: PORTC |= (1 << 2); break;
-       case 11: PORTC |= (1 << 3); break;
-       case 12: PORTC |= (1 << 4); break;
-       case 13: PORTC |= (1 << 5); break;
-       case 14: PORTC |= (1 << 6); break;
-       case 15: PORTC |= (1 << 7); break;
-
-#else
-#ifdef SHORT_C
-
-       case 6: PORTD |= (1 << 6); DDRD &= ~(1 << 6); break;
-       case 7: PORTD |= (1 << 7); DDRD &= ~(1 << 7); break;
-
-       case 8: PORTE |= (1 << 0); DDRE &= ~(1 << 0); break;
-       case 9: PORTE |= (1 << 1); DDRE &= ~(1 << 1); break;
-
-       case 10:  PORTC |= (1 << 0); DDRC &= ~(1 << 0); break;
-       case 11:  PORTC |= (1 << 1); DDRC &= ~(1 << 1); break;
-       case 12: PORTC |= (1 << 2); DDRC &= ~(1 << 2); break;
-       case 13: PORTC |= (1 << 3); DDRC &= ~(1 << 3); break;
-       case 14: PORTC |= (1 << 4); DDRC &= ~(1 << 4); break;
-       case 15: PORTC |= (1 << 5); DDRC &= ~(1 << 5); break;
-
-       case 16: PORTC |= (1 << 6); DDRC &= ~(1 << 6); break;
-       case 17: PORTC |= (1 << 7); DDRC &= ~(1 << 7); break;
-
-#endif
-#endif
-#endif
-
-       default:
-               break;
-       }
-
-#endif
-
-
 }
 
 
@@ -799,8 +695,8 @@ int sampleColumn_8x( uint8_t column, uint16_t * buffer )
                mux++;
 
        } while (mux < 8);
-
 #endif
+
        hold_sample(OFF);
        recovery(ON);
 
@@ -826,21 +722,6 @@ int sampleColumn( uint8_t column )
 
        rval = sampleColumn_8x( column, samples + SAMPLE_OFFSET );
 
-#if (BUMP_DETECTION)
-       for ( uint8_t i = 0; i < 8; ++i )
-       {
-               if ( samples[SAMPLE_OFFSET + i] - adc_mux_averages[i] > BUMP_THRESHOLD )
-               {
-                       // was a hump
-                       _delay_us(BUMP_REST_US);
-                       rval++;
-                       error = 0x50;
-                       error_data = samples[SAMPLE_OFFSET +i]; //  | ((uint16_t)i << 8);
-                       return rval;
-               }
-       }
-#endif
-
        return rval;
 }
 
@@ -849,87 +730,92 @@ uint8_t testColumn( uint8_t strobe )
 {
        uint8_t column = 0;
        uint8_t bit = 1;
-       for ( uint8_t i = 0; i < MUXES_COUNT; ++i )
+       for ( uint8_t mux = 0; mux < MUXES_COUNT; ++mux )
        {
-               uint16_t delta = keys_averages[(strobe << MUXES_COUNT_XSHIFT) + i];
+               uint16_t delta = keys_averages[(strobe << MUXES_COUNT_XSHIFT) + mux];
 
-               if ( (db_sample = samples[SAMPLE_OFFSET + i] >> 1) > (db_threshold = threshold) + (db_delta = delta) )
+               uint8_t key = (strobe << MUXES_COUNT_XSHIFT) + mux;
+
+               // Keypress detected
+               if ( (db_sample = samples[SAMPLE_OFFSET + mux] >> 1) > (db_threshold = threshold) + (db_delta = delta) )
                {
                        column |= bit;
-               }
 
-#ifdef THRESHOLD_VERIFICATION
-               if ( db_sample > 0xA0 )
-               {
-                       printHex( db_sample );
-                       print(" : ");
-                       printHex( db_threshold );
-                       print(" : ");
-                       printHex( db_delta );
-                       print(" :: ");
-                       printHex( column );
-                       print(" : ");
-                       printHex( strobe );
-                       print(NL);
-               }
+                       // Only register keypresses once the warmup is complete, or not enough debounce info
+                       if ( boot_count >= WARMUP_LOOPS && keys_debounce[key] <= DEBOUNCE_THRESHOLD )
+                       {
+                               // Add to the Macro processing buffer if debounce criteria met
+                               // Automatically handles converting to a USB code and sending off to the PC
+                               if ( keys_debounce[key] == DEBOUNCE_THRESHOLD )
+                               {
+#define KEYSCAN_DEBOUNCE_DEBUG
+#ifdef KEYSCAN_DEBOUNCE_DEBUG
+                                       // Debug message
+                                       print("0x");
+                                       printHex_op( key, 2 );
+                                       print(" ");
 #endif
 
-               bit <<= 1;
-       }
-       return column;
-}
-
-
-void dumpkeys()
-{
-       if ( error )
-       {
-               erro_print("Problem detected...");
+                                       // Only add the key to the buffer once
+                                       // NOTE: Buffer can easily handle multiple adds, just more efficient
+                                       //        and nicer debug messages :P
+                                       //bufferAdd( key );
+                               }
 
-               if ( boot_count >= WARMUP_LOOPS )
-               {
-                       dump();
+                               keys_debounce[key]++;
+
+//#define KEYSCAN_THRESHOLD_DEBUG
+#ifdef KEYSCAN_THRESHOLD_DEBUG
+                               // Debug message
+                               // <key> [<strobe>:<mux>] : <sense val> : <delta + threshold> : <margin>
+                               dbug_msg("0x");
+                               printHex_op( key, 2 );
+                               print(" [");
+                               printInt8( strobe );
+                               print(":");
+                               printInt8( mux );
+                               print("] : ");
+                               printHex( db_sample ); // Sense
+                               print(" : ");
+                               printHex( db_threshold );
+                               print("+");
+                               printHex( db_delta );
+                               print("=");
+                               printHex( db_threshold + db_delta ); // Sense compare
+                               print(" : ");
+                               printHex( db_sample - ( db_threshold + db_delta ) ); // Margin
+                               print("\n");
+#endif
+                       }
                }
-
-               // Key scan debug
-               for ( uint8_t i = 0; i < STROBE_LINES; ++i )
+               // Clear debounce entry if no keypress detected
+               else
                {
-                               printHex(usb_keymap[i]);
-                               print(" ");
+                       // If the key was previously pressed, remove from the buffer
+                       for ( uint8_t c = 0; c < KeyIndex_BufferUsed; c++ )
+                        {
+                                // Key to release found
+                                if ( KeyIndex_Buffer[c] == key )
+                                {
+                                        // Shift keys from c position
+                                        for ( uint8_t k = c; k < KeyIndex_BufferUsed - 1; k++ )
+                                                KeyIndex_Buffer[k] = KeyIndex_Buffer[k + 1];
+
+                                        // Decrement Buffer
+                                        KeyIndex_BufferUsed--;
+
+                                        break;
+                                }
+                        }
+
+
+                       // Clear debounce entry
+                       keys_debounce[key] = 0;
                }
 
-               print(" : ");
-               printHex(error);
-               error = 0;
-               print(" : ");
-               printHex(error_data);
-               error_data = 0;
-               print(" : " NL);
-       }
-
-       // XXX Will be cleaned up eventually, but this will do for now :P -HaaTa
-       for ( uint8_t i = 0; i < STROBE_LINES; ++i )
-       {
-               for ( uint8_t j = 0; j < MUXES_COUNT; ++j )
-               {
-                       if ( usb_keymap[i] & (1 << j) )
-                       {
-                               uint8_t key = (i << MUXES_COUNT_XSHIFT) + j;
-
-                               // Add to the Macro processing buffer
-                               // Automatically handles converting to a USB code and sending off to the PC
-                               //bufferAdd( key );
-
-                               if ( usb_dirty )
-                               {
-                                       printHex( key );
-                                       print("\n");
-                               }
-                       }
-               }
+               bit <<= 1;
        }
-
-       usb_keyboard_send();
+       return column;
 }
 
 
@@ -1016,7 +902,7 @@ void dump(void) {
        print("\n      ");
 
        // Current keymap values
-       for ( uint8_t i = 0; i < STROBE_LINES; ++i )
+       for ( uint8_t i = 0; i < total_strobes; ++i )
        {
                printHex(cur_keymap[i]);
                print(" ");