]> git.donarmstrong.com Git - kiibohd-controller.git/blobdiff - Scan/DPH/scan_loop.c
As per request of original author, updating license to MIT
[kiibohd-controller.git] / Scan / DPH / scan_loop.c
index bf939a74389012a42d8675fa7bb3fc106bde72d5..5d8d832a5cc034901e51d6597bee29a8154ccce0 100644 (file)
@@ -1,18 +1,23 @@
-/* Copyright (C) 2011-2013 by Joseph Makuch
- * Additions by Jacob Alexander (2013-2014)
+/* Copyright (C) 2011-2013 by Joseph Makuch (jmakuch+f@gmail.com)
+ * Additions by Jacob Alexander (2013-2014) (haata@kiibohd.com)
  *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 3.0 of the License, or (at your option) any later version.
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
  *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
  *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
  */
 
 // ----- Includes -----
@@ -23,6 +28,7 @@
 // Project Includes
 #include <cli.h>
 #include <led.h>
+#include <macro.h>
 #include <print.h>
 
 // Local Includes
 
 #define STROBE_SETTLE 1
 
-#define TEST_KEY_STROBE (0x05)
-#define TEST_KEY_MASK (1 << 0)
-
 #define ADHSM 7
 
-#define RIGHT_JUSTIFY 0
-#define LEFT_JUSTIFY (0xff)
-
-// set left or right justification here:
-#define JUSTIFY_ADC RIGHT_JUSTIFY
-#define ADLAR_MASK (1 << ADLAR)
-
-#ifdef JUSTIFY_ADC
-#define ADLAR_BITS ((ADLAR_MASK) & (JUSTIFY_ADC))
-#else // defaults to right justification.
+// Right justification of ADLAR
 #define ADLAR_BITS 0
-#endif
 
 // full muxmask
 #define FULL_MUX_MASK ((1 << MUX0) | (1 << MUX1) | (1 << MUX2) | (1 << MUX3) | (1 << MUX4))
 // Number of consecutive samples required to pass debounce
 #define DEBOUNCE_THRESHOLD 5
 
+// Scans to remain idle after all keys were release before starting averaging
+// XXX So this makes the initial keypresses fast,
+//      but it's still possible to lose a keypress if you press at the wrong time -HaaTa
+#define KEY_IDLE_SCANS 30000
+
+// Total number of muxes/sense lines available
 #define MUXES_COUNT 8
 #define MUXES_COUNT_XSHIFT 3
 
+// Number of warm-up loops before starting to scan keys
 #define WARMUP_LOOPS ( 1024 )
 #define WARMUP_STOP (WARMUP_LOOPS - 1)
 
 // mix in 1/4 of the current average to the running average. -> (@mux_mix = 2)
 #define MUX_MIX 2
 
-#define IDLE_COUNT_MASK 0xff
 #define IDLE_COUNT_SHIFT 8
 
 // av = (av << shift) - av + sample; av >>= shift
 
 // ----- Function Declarations -----
 
-void cliFunc_echo   ( char* args );
+// CLI Functions
+void cliFunc_avgDebug   ( char* args );
+void cliFunc_echo       ( char* args );
+void cliFunc_keyDebug   ( char* args );
+void cliFunc_pressDebug ( char* args );
+void cliFunc_problemKeys( char* args );
+void cliFunc_senseDebug ( char* args );
+
+// Debug Functions
+void dumpSenseTable();
+
+// High-level Capsense Functions
+void setup_ADC();
+void capsense_scan();
 
+// Capsense Sense Functions
+void testColumn  ( uint8_t strobe );
+void sampleColumn( uint8_t column );
 
+// Low-level Capsense Functions
+void strobe_w( uint8_t strobe_num );
+void recovery( uint8_t on );
 
-// ----- Variables -----
 
-// Buffer used to inform the macro processing module which keys have been detected as pressed
-volatile uint8_t KeyIndex_Buffer[KEYBOARD_BUFFER];
-volatile uint8_t KeyIndex_BufferUsed;
 
+// ----- Variables -----
 
 // Scan Module command dictionary
-char*       scanCLIDictName = "DPH Module Commands";
-CLIDictItem scanCLIDict[] = {
-       { "echo",    "Example command, echos the arguments.", cliFunc_echo },
+CLIDict_Entry( echo,        "Example command, echos the arguments." );
+CLIDict_Entry( avgDebug,    "Enables/Disables averaging results." NL "\t\tDisplays each average, starting from Key 0x00, ignoring 0 valued averages." );
+CLIDict_Entry( keyDebug,    "Enables/Disables long debug for each keypress." NL "\t\tkeycode - [strobe:mux] : sense val : threshold+delta=total : margin" );
+CLIDict_Entry( pressDebug,  "Enables/Disables short debug for each keypress." );
+CLIDict_Entry( problemKeys, "Display current list of problem keys," );
+CLIDict_Entry( senseDebug,  "Prints out the current sense table N times." NL "\t\tsense:max sense:delta" );
+
+CLIDict_Def( scanCLIDict, "DPH Module Commands" ) = {
+       CLIDict_Item( echo ),
+       CLIDict_Item( avgDebug ),
+       CLIDict_Item( keyDebug ),
+       CLIDict_Item( pressDebug ),
+       CLIDict_Item( problemKeys ),
+       CLIDict_Item( senseDebug ),
        { 0, 0, 0 } // Null entry for dictionary end
 };
 
 
-// TODO dfj variables...needs cleaning up and commenting
+// CLI Control Variables
+uint8_t enableAvgDebug   = 0;
+uint8_t enableKeyDebug   = 0;
+uint8_t enablePressDebug = 0;
+uint8_t senseDebugCount  = 3; // In order to get boot-time oddities
+
 
 // Variables used to calculate the starting sense value (averaging)
 uint32_t full_avg = 0;
@@ -152,70 +183,112 @@ uint8_t  high_count = 0;
 uint8_t   low_count = 0;
 
 
-uint8_t ze_strober = 0;
+uint16_t samples[MAX_STROBES][MUXES_COUNT];   // Overall table of cap sense ADC values
+uint16_t sampleMax[MAX_STROBES][MUXES_COUNT]; // Records the max seen ADC value
 
-uint16_t samples[MUXES_COUNT];
-
-uint8_t cur_keymap[MAX_STROBES];
-
-uint8_t keymap_change;
+uint8_t  key_activity = 0; // Increments for each detected key per each full scan of the keyboard, it is reset before each full scan
+uint16_t key_idle     = 0; // Defines how scans after all keys were released before starting averaging again
+uint8_t  key_release  = 0; // Indicates if going from key press state to release state (some keys pressed to no keys pressed)
 
 uint16_t threshold = THRESHOLD;
 
-uint8_t column = 0;
-
 uint16_t keys_averages_acc[KEY_COUNT];
 uint16_t keys_averages    [KEY_COUNT];
 uint8_t  keys_debounce    [KEY_COUNT]; // Contains debounce statistics
 uint8_t  keys_problem     [KEY_COUNT]; // Marks keys that should be ignored (determined by averaging at startup)
 
-uint8_t full_samples[KEY_COUNT];
-
 // TODO: change this to 'booting', then count down.
 uint16_t boot_count = 0;
 
-uint16_t idle_count = 0;
-uint8_t idle = 1;
-
-uint8_t error = 0;
-uint16_t error_data = 0;
-
 uint8_t total_strobes = MAX_STROBES;
 uint8_t strobe_map[MAX_STROBES];
 
-uint8_t dump_count = 0;
-
 
 
-// ----- Function Declarations -----
-
-void dump();
-
-void recovery( uint8_t on );
-
-int sampleColumn( uint8_t column );
+// ----- Functions -----
 
-void capsense_scan();
+// Initial setup for cap sense controller
+inline void Scan_setup()
+{
+       // Register Scan CLI dictionary
+       CLI_registerDictionary( scanCLIDict, scanCLIDictName );
 
-void setup_ADC();
+       // Scan for active strobes
+       // NOTE1: On IBM PCBs, each strobe line that is *NOT* used is connected to GND.
+       //       This means, the strobe GPIO can be set to Tri-State pull-up to detect which strobe lines are not used.
+       // NOTE2: This will *NOT* detect floating strobes.
+       // NOTE3: Rev 0.4, the strobe numbers are reversed, so D0 is actually strobe 0 and C7 is strobe 17
+       info_msg("Detecting Strobes...");
+
+       DDRC  = 0;
+       PORTC = C_MASK;
+       DDRD  = 0;
+       PORTD = D_MASK;
+       DDRE  = 0;
+       PORTE = E_MASK;
+
+       // Initially there are 0 strobes
+       total_strobes = 0;
+
+       // Iterate over each the strobes
+       for ( uint8_t strobe = 0; strobe < MAX_STROBES; strobe++ )
+       {
+               uint8_t detected = 0;
 
-void strobe_w( uint8_t strobe_num );
+               // If PIN is high, then strobe is *NOT* connected to GND and may be a strobe
+               switch ( strobe )
+               {
 
-uint8_t testColumn( uint8_t strobe );
+               // Strobe Mappings
+               //              Rev  Rev
+               //              0.2  0.4
+#ifndef REV0_4_DEBUG // XXX These pins should be reworked, and connect to GND on Rev 0.4
+               case 0:  // D0   0   n/c
+               case 1:  // D1   1   n/c
+#endif
+               case 2:  // D2   2   15
+               case 3:  // D3   3   14
+               case 4:  // D4   4   13
+               case 5:  // D5   5   12
+               case 6:  // D6   6   11
+               case 7:  // D7   7   10
+                       detected = PIND & (1 << strobe);
+                       break;
 
+               case 8:  // E0   8    9
+               case 9:  // E1   9    8
+                       detected = PINE & (1 << (strobe - 8));
+                       break;
 
+               case 10: // C0  10    7
+               case 11: // C1  11    6
+               case 12: // C2  12    5
+               case 13: // C3  13    4
+               case 14: // C4  14    3
+               case 15: // C5  15    2
+#ifndef REV0_2_DEBUG // XXX If not using the 18 pin connector on Rev 0.2, rework these pins to GND
+               case 16: // C6  16    1
+               case 17: // C7  17    0
+#endif
+                       detected = PINC & (1 << (strobe - 10));
+                       break;
 
-// ----- Functions -----
+               default:
+                       break;
+               }
 
-// Initial setup for cap sense controller
-inline void Scan_setup()
-{
-       // Register Scan CLI dictionary
-       CLI_registerDictionary( scanCLIDict, scanCLIDictName );
+               // Potential strobe line detected
+               if ( detected )
+               {
+                       strobe_map[total_strobes] = strobe;
+                       total_strobes++;
+               }
+       }
 
-       // TODO dfj code...needs cleanup + commenting...
-       setup_ADC();
+       printInt8( total_strobes );
+       print( " strobes found." NL );
 
+       // Setup Pins for Strobing
        DDRC  = C_MASK;
        PORTC = 0;
        DDRD  = D_MASK;
@@ -223,83 +296,8 @@ 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 KISHSAVER_OLD_STROBE
-//#define TERMINAL_6110668_OLD_STROBE
-//#define UNSAVER_OLD_STROBE
-#ifdef KISHSAVER_OLD_STROBE
-       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;
-       strobe_map[8] = 15; // Test point strobe (3 test points, sense 1, 4, 5)
-#elif defined(KISHSAVER_STROBE)
-       total_strobes = 9;
-
-       strobe_map[0] = 15; // Kishsaver doesn't use strobe 0 and 1
-       strobe_map[1] = 14;
-       strobe_map[2] = 13;
-       strobe_map[3] = 12;
-       strobe_map[4] = 11;
-       strobe_map[5] = 10;
-       strobe_map[6] = 9;
-       strobe_map[7] = 8;
-       strobe_map[8] = 2; // Test point strobe (3 test points, sense 1, 4, 5)
-#elif defined(TERMINAL_6110668_OLD_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_OLD_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 < total_strobes; ++i)
-       {
-               cur_keymap[i] = 0;
-       }
+       // Initialize ADC
+       setup_ADC();
 
        // Reset debounce table
        for ( int i = 0; i < KEY_COUNT; ++i )
@@ -321,33 +319,6 @@ 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
@@ -356,19 +327,17 @@ inline uint8_t Scan_loop()
 }
 
 
-// Signal KeyIndex_Buffer that it has been properly read
+// Signal from macro module that keys have been processed
 // NOTE: Only really required for implementing "tricks" in converters for odd protocols
-void Scan_finishedWithBuffer( uint8_t sentKeys )
+void Scan_finishedWithMacro( 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
+// Signal from output module that keys have been processed/sent
 // NOTE: Only really required for implementing "tricks" in converters for odd protocols
-void Scan_finishedWithUSBBuffer( uint8_t sentKeys )
+void Scan_finishedWithOutput( uint8_t sentKeys )
 {
        return;
 }
@@ -386,43 +355,29 @@ inline void capsense_scan()
 
        high_count = 0;
 
+       // Reset key activity, if there is no key activity, averages will accumulate for sense deltas, otherwise they will be reset
+       key_activity = 0;
+
        // Scan each of the mapped strobes in the matrix
        for ( uint8_t strober = 0; strober < total_strobes; ++strober )
        {
                uint8_t map_strobe = strobe_map[strober];
 
-               uint8_t tries = 1;
-               while ( tries++ && sampleColumn( map_strobe ) ) { tries &= 0x7; } // don't waste this one just because the last one was poop.
+               // Sample the ADCs for the given column/strobe
+               sampleColumn( map_strobe );
 
                // Only process sense data if warmup is finished
                if ( boot_count >= WARMUP_LOOPS )
                {
-                       column = testColumn( map_strobe );
-
-                       idle |= column; // if column has any pressed keys, then we are not idle.
-
-                       // TODO Is this needed anymore? Really only helps debug -HaaTa
-                       if( column != cur_keymap[map_strobe] && ( boot_count >= WARMUP_LOOPS ) )
-                       {
-                               cur_keymap[map_strobe] = column;
-                               keymap_change = 1;
-                       }
-
-                       idle |= keymap_change; // if any keys have changed inc. released, then we are not idle.
-               }
-
-               if ( error == 0x50 )
-               {
-                       error_data |= (((uint16_t)map_strobe) << 12);
+                       testColumn( map_strobe );
                }
 
                uint8_t strobe_line = map_strobe << MUXES_COUNT_XSHIFT;
-               for ( int i = 0; i < MUXES_COUNT; ++i )
+               for ( int mux = 0; mux < MUXES_COUNT; ++mux )
                {
                        // discard sketchy low bit, and meaningless high bits.
-                       uint8_t sample = samples[i] >> 1;
-                       full_samples[strobe_line + i] = sample;
-                       keys_averages_acc[strobe_line + i] += sample;
+                       uint8_t sample = samples[map_strobe][mux] >> 1;
+                       keys_averages_acc[strobe_line + mux] += sample;
                }
 
                // Accumulate 3 total averages (used for determining starting average during warmup)
@@ -433,9 +388,9 @@ inline void capsense_scan()
                //      low_avg - Average of all sampled lines below or equal to full_avg
                if ( boot_count < WARMUP_LOOPS )
                {
-                       for ( uint8_t i = 0; i < MUXES_COUNT; ++i )
+                       for ( uint8_t mux = 0; mux < MUXES_COUNT; ++mux )
                        {
-                               uint8_t sample = samples[i] >> 1;
+                               uint8_t sample = samples[map_strobe][mux] >> 1;
 
                                // Sample is high, add it to high avg
                                if ( sample > full_avg )
@@ -451,7 +406,8 @@ inline void capsense_scan()
                                }
 
                                // If sample is higher than previous high_avg, then mark as "problem key"
-                               keys_problem[strobe_line + i] = sample > high_avg ? sample : 0;
+                               // XXX Giving a bit more margin to pass (high_avg vs. high_avg + high_avg - full_avg) -HaaTa
+                               keys_problem[strobe_line + mux] = sample > high_avg + (high_avg - full_avg) ? sample : 0;
 
                                // Prepare for next average
                                cur_full_avg += sample;
@@ -474,23 +430,6 @@ inline void capsense_scan()
                }
        }
 
-#ifdef VERIFY_TEST_PAD
-       // verify test key is not down.
-       if ( ( cur_keymap[TEST_KEY_STROBE] & TEST_KEY_MASK ) )
-       {
-               error = 0x05;
-               error_data = cur_keymap[TEST_KEY_STROBE] << 8;
-               error_data += full_samples[TEST_KEY_STROBE * 8];
-       }
-#endif
-
-       /** aggregate if booting, or if idle;
-        * else, if not booting, check for dirty USB.
-        * */
-
-       idle_count++;
-       idle_count &= IDLE_COUNT_MASK;
-
        // Warm up voltage references
        if ( boot_count < WARMUP_LOOPS )
        {
@@ -512,10 +451,10 @@ inline void capsense_scan()
                        break;
                // Last loop
                case WARMUP_STOP:
-                       print("\n");
+                       print( NL );
                        info_msg("Warmup finished using ");
                        printInt16( WARMUP_LOOPS );
-                       print(" iterations\n");
+                       print(" iterations" NL );
 
                        // Display the final calculated averages of all the sensed strobes
                        info_msg("Full average (");
@@ -532,7 +471,10 @@ inline void capsense_scan()
                        printInt8( low_count );
                        print("): ");
                        printHex( low_avg );
-                       print("\n");
+
+                       print("  Rejection threshold: ");
+                       printHex( high_avg + (high_avg - full_avg) );
+                       print( NL );
 
                        // Display problem keys, and the sense value at the time
                        for ( uint8_t key = 0; key < KEY_COUNT; key++ )
@@ -543,34 +485,34 @@ inline void capsense_scan()
                                        printHex( key );
                                        print(" (");
                                        printHex( keys_problem[key] );
-                                       print(")\n");
+                                       print(")" NL );
                                }
                        }
 
-                       info_print("If problem keys were detected, and were being held down, they will be reset as soon as let go");
+                       info_print("If problem keys were detected, and were being held down, they will be reset as soon as let go.");
+                       info_print("Some keys have unusually high sense values, on the first press they should be re-enabled.");
                        break;
                }
        }
        else
        {
-               // Reset accumulators and idle flag/counter
-               if ( keymap_change )
-               {
-                       for ( uint8_t c = 0; c < KEY_COUNT; ++c ) { keys_averages_acc[c] = 0; }
-                       idle_count = 0;
-                       idle = 0;
-
-                       keymap_change = 0;
-               }
-
-               if ( !idle_count )
+               // No keypress, accumulate averages
+               if( !key_activity )
                {
-                       if( idle )
+                       // Only start averaging once the idle counter has counted down to 0
+                       if ( key_idle == 0 )
                        {
+                               // Average Debugging
+                               if ( enableAvgDebug )
+                               {
+                                       print("\033[1mAvg\033[0m: ");
+                               }
+
                                // aggregate
                                for ( uint8_t i = 0; i < KEY_COUNT; ++i )
                                {
-                                       uint16_t acc = keys_averages_acc[i] >> IDLE_COUNT_SHIFT;
+                                       uint16_t acc = keys_averages_acc[i];
+                                       //uint16_t acc = keys_averages_acc[i] >> IDLE_COUNT_SHIFT; // XXX This fixes things... -HaaTa
                                        uint32_t av = keys_averages[i];
 
                                        av = (av << KEYS_AVERAGES_MIX_SHIFT) - av + acc;
@@ -578,15 +520,45 @@ inline void capsense_scan()
 
                                        keys_averages[i] = av;
                                        keys_averages_acc[i] = 0;
+
+                                       // Average Debugging
+                                       if ( enableAvgDebug && av > 0 )
+                                       {
+                                               printHex( av );
+                                               print(" ");
+                                       }
                                }
-                       }
 
-                       if ( boot_count >= WARMUP_LOOPS )
+                               // Average Debugging
+                               if ( enableAvgDebug )
+                               {
+                                       print( NL );
+                               }
+
+                               // No key presses detected, set key_release indicator
+                               key_release = 1;
+                       }
+                       // Otherwise decrement the idle counter
+                       else
                        {
-                               dump();
+                               key_idle--;
                        }
                }
+               // Keypresses, reset accumulators
+               else if ( key_release )
+               {
+                       for ( uint8_t c = 0; c < KEY_COUNT; ++c ) { keys_averages_acc[c] = 0; }
 
+                       key_release = 0;
+               }
+
+               // If the debugging sense table is non-zero, display
+               if ( senseDebugCount > 0 )
+               {
+                       senseDebugCount--;
+                       print( NL );
+                       dumpSenseTable();
+               }
        }
 }
 
@@ -642,7 +614,7 @@ void recovery( uint8_t on )
                PORTD &= ~D_MASK;
                PORTE &= ~E_MASK;
 
-               DDRB  |= (1 << RECOVERY_SINK);   // SINK pull
+               DDRB  |= (1 << RECOVERY_SINK);   // SINK pull
                PORTB |= (1 << RECOVERY_CONTROL);
                PORTB |= (1 << RECOVERY_SOURCE); // SOURCE high
                DDRB  |= (1 << RECOVERY_SOURCE);
@@ -652,7 +624,7 @@ void recovery( uint8_t on )
                PORTB &= ~(1 << RECOVERY_CONTROL);
                DDRB  &= ~(1 << RECOVERY_SOURCE);
                PORTB &= ~(1 << RECOVERY_SOURCE); // SOURCE low
-               DDRB  &= ~(1 << RECOVERY_SINK);   // SINK high-imp
+               DDRB  &= ~(1 << RECOVERY_SINK);   // SINK high-imp
        }
 }
 
@@ -721,7 +693,7 @@ inline uint16_t getADC(void)
 }
 
 
-int sampleColumn_8x( uint8_t column, uint16_t * buffer )
+void sampleColumn( uint8_t column )
 {
        // ensure all probe lines are driven low, and chill for recovery delay.
        ADCSRA |= (1 << ADEN) | (1 << ADSC); // enable and start conversions
@@ -751,7 +723,15 @@ int sampleColumn_8x( uint8_t column, uint16_t * buffer )
                SET_FULL_MUX( mux + 1 ); // our *next* sample will use this
 
                // retrieve current read.
-               buffer[mux] = getADC();
+               uint16_t readVal = getADC();
+               samples[column][mux] = readVal;
+
+               // Update max sense sample table
+               if ( readVal > sampleMax[column][mux] )
+               {
+                       sampleMax[column][mux] = readVal;
+               }
+
                mux++;
 
        } while ( mux < 8 );
@@ -770,22 +750,10 @@ int sampleColumn_8x( uint8_t column, uint16_t * buffer )
        PORTC &= ~C_MASK;
        PORTD &= ~D_MASK;
        PORTE &= ~E_MASK;
-
-       return 0;
-}
-
-
-int sampleColumn( uint8_t column )
-{
-       int rval = 0;
-
-       rval = sampleColumn_8x( column, samples );
-
-       return rval;
 }
 
 
-uint8_t testColumn( uint8_t strobe )
+void testColumn( uint8_t strobe )
 {
        uint16_t db_delta = 0;
        uint8_t  db_sample = 0;
@@ -803,28 +771,29 @@ uint8_t testColumn( uint8_t strobe )
                // Check if this is a bad key (e.g. test point, or non-existent key)
                if ( keys_problem[key] )
                {
-                       // If the sample value of the problem key goes below full_avg (overall initial average)
+                       // If the sample value of the problem key goes above initally recorded result + threshold
                        //  re-enable the key
-                       if ( (db_sample = samples[mux] >> 1) < full_avg )
+                       if ( (db_sample = samples[strobe][mux] >> 1) > keys_problem[key] + threshold )
+                       //if ( (db_sample = samples[strobe][mux] >> 1) < high_avg )
                        {
                                info_msg("Re-enabling problem key: ");
                                printHex( key );
-                               print("\n");
+                               print( NL );
 
                                keys_problem[key] = 0;
                        }
-                       // Otherwise, don't waste any more cycles processing the problem key
-                       else
-                       {
-                               continue;
-                       }
+
+                       // Do not waste any more cycles processing, regardless, a keypress cannot be detected
+                       continue;
                }
 
                // Keypress detected
                //  db_sample (uint8_t), discard meaningless high bit, and garbage low bit
-               if ( (db_sample = samples[mux] >> 1) > (db_threshold = threshold) + (db_delta = delta) )
+               if ( (db_sample = samples[strobe][mux] >> 1) > (db_threshold = threshold) + (db_delta = delta) )
                {
                        column |= bit;
+                       key_activity++; // No longer idle, stop averaging ADC data
+                       key_idle = KEY_IDLE_SCANS; // Reset idle count-down
 
                        // Only register keypresses once the warmup is complete, or not enough debounce info
                        if ( keys_debounce[key] <= DEBOUNCE_THRESHOLD )
@@ -833,28 +802,34 @@ uint8_t testColumn( uint8_t strobe )
                                // 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
-
-                                       // Only add the key to the buffer once
-                                       // NOTE: Buffer can easily handle multiple adds, just more efficient
-                                       //        and nicer debug messages :P
-                                       //Macro_bufferAdd( key );
+                                       // Debug message, pressDebug CLI
+                                       if ( enablePressDebug )
+                                       {
+                                               print("0x");
+                                               printHex_op( key, 2 );
+                                               print(" ");
+                                       }
+
+                                       // Initial Keypress
+                                       Macro_keyState( key, 0x01 );
                                }
 
                                keys_debounce[key]++;
 
-#define KEYSCAN_THRESHOLD_DEBUG
-#ifdef KEYSCAN_THRESHOLD_DEBUG
+                       }
+                       else if ( keys_debounce[key] >= DEBOUNCE_THRESHOLD )
+                       {
+                               // Held Key
+                               Macro_keyState( key, 0x02 );
+                       }
+
+                       // Long form key debugging
+                       if ( enableKeyDebug )
+                       {
                                // Debug message
                                // <key> [<strobe>:<mux>] : <sense val> : <delta + threshold> : <margin>
-                               dbug_msg("0x");
-                               printHex_op( key, 2 );
+                               dbug_msg("");
+                               printHex_op( key, 1 );
                                print(" [");
                                printInt8( strobe );
                                print(":");
@@ -869,30 +844,17 @@ uint8_t testColumn( uint8_t strobe )
                                printHex( db_threshold + db_delta ); // Sense compare
                                print(" : ");
                                printHex( db_sample - ( db_threshold + db_delta ) ); // Margin
-                               print("\n");
-#endif
+                               print( NL );
                        }
                }
                // Clear debounce entry if no keypress detected
                else
                {
-                       // 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;
-                                }
-                        }
-
+                       // Release Key
+                       if ( keys_debounce[key] >= DEBOUNCE_THRESHOLD )
+                       {
+                               Macro_keyState( key, 0x03 );
+                       }
 
                        // Clear debounce entry
                        keys_debounce[key] = 0;
@@ -900,94 +862,60 @@ uint8_t testColumn( uint8_t strobe )
 
                bit <<= 1;
        }
-       return column;
 }
 
 
-void dump(void) {
+void dumpSenseTable()
+{
+       // Initial table alignment, with base threshold used for every key
+       print("\033[1m");
+       printHex( threshold );
+       print("\033[0m       ");
 
-#ifdef DEBUG_FULL_SAMPLES_AVERAGES
-       // we don't want to debug-out during the measurements.
-       if ( !dump_count )
+       // Print out headers first
+       for ( uint8_t mux = 0; mux < MUXES_COUNT; ++mux )
        {
-               // Averages currently set per key
-               for ( int i = 0; i < KEY_COUNT; ++i )
-               {
-                       if ( !(i & 0x0f) )
-                       {
-                               print("\n");
-                       }
-                       else if ( !(i & 0x07) )
-                       {
-                               print("  ");
-                       }
-
-                       print(" ");
-                       printHex( keys_averages[i] );
-               }
-
-               print("\n");
-
-               // Previously read full ADC scans?
-               for ( int i = 0; i< KEY_COUNT; ++i)
-               {
-                       if ( !(i & 0x0f) )
-                       {
-                               print("\n");
-                       }
-                       else if ( !(i & 0x07) )
-                       {
-                               print("  ");
-                       }
-
-                       print(" ");
-                       printHex(full_samples[i]);
-               }
+               print("  Mux \033[1m");
+               printInt8( mux );
+               print("\033[0m  ");
        }
-#endif
-
-#ifdef DEBUG_STROBE_SAMPLES_AVERAGES
-       // Per strobe information
-       uint8_t cur_strober = ze_strober;
-       print("\n");
 
-       printHex(cur_strober);
+       print( NL );
 
-       // Previously read ADC scans on current strobe
-       print(" :");
-       for ( uint8_t i = 0; i < MUXES_COUNT; ++i )
+       // Display the full strobe/sense table
+       for ( uint8_t strober = 0; strober < total_strobes; ++strober )
        {
-               print(" ");
-               printHex(full_samples[(cur_strober << MUXES_COUNT_XSHIFT) + i]);
-       }
+               uint8_t strobe = strobe_map[strober];
 
-       // Averages current set on current strobe
-       print(" :");
+               // Display the strobe
+               print("Strobe \033[1m");
+               printHex( strobe );
+               print("\033[0m ");
 
-       for ( uint8_t i = 0; i < MUXES_COUNT; ++i )
-       {
-               print(" ");
-               printHex(keys_averages[(cur_strober << MUXES_COUNT_XSHIFT) + i]);
-       }
+               // For each mux, display sense:threshold:delta
+               for ( uint8_t mux = 0; mux < MUXES_COUNT; ++mux )
+               {
+                       uint8_t delta = keys_averages[(strobe << MUXES_COUNT_XSHIFT) + mux];
+                       uint8_t sample = samples[strobe][mux] >> 1;
+                       uint8_t max = sampleMax[strobe][mux] >> 1;
 
-#endif
+                       // Indicate if the key is being pressed by displaying green
+                       if ( sample > delta + threshold )
+                       {
+                               print("\033[1;32m");
+                       }
 
-#ifdef DEBUG_USB_KEYMAP
-       print("\n      ");
+                       printHex_op( sample, 2 );
+                       print(":");
+                       printHex_op( max, 2 );
+                       print(":");
+                       printHex_op( delta, 2 );
+                       print("\033[0m ");
+               }
 
-       // Current keymap values
-       for ( uint8_t i = 0; i < total_strobes; ++i )
-       {
-               printHex(cur_keymap[i]);
-               print(" ");
+               // New line for each strobe
+               print( NL );
        }
-#endif
-
-       ze_strober++;
-       ze_strober &= 0xf;
-
-       dump_count++;
-       dump_count &= 0x0f;
 }
 
 
@@ -1017,3 +945,96 @@ void cliFunc_echo( char* args )
        }
 }
 
+void cliFunc_avgDebug( char* args )
+{
+       print( NL );
+
+       // Args ignored, just toggling
+       if ( enableAvgDebug )
+       {
+               info_print("Cap Sense averaging debug disabled.");
+               enableAvgDebug = 0;
+       }
+       else
+       {
+               info_print("Cap Sense averaging debug enabled.");
+               enableAvgDebug = 1;
+       }
+}
+
+void cliFunc_keyDebug( char* args )
+{
+       print( NL );
+
+       // Args ignored, just toggling
+       if ( enableKeyDebug )
+       {
+               info_print("Cap Sense key long debug disabled - pre debounce.");
+               enableKeyDebug = 0;
+       }
+       else
+       {
+               info_print("Cap Sense key long debug enabled - pre debounce.");
+               enableKeyDebug = 1;
+       }
+}
+
+void cliFunc_pressDebug( char* args )
+{
+       print( NL );
+
+       // Args ignored, just toggling
+       if ( enablePressDebug )
+       {
+               info_print("Cap Sense key debug disabled - post debounce.");
+               enablePressDebug = 0;
+       }
+       else
+       {
+               info_print("Cap Sense key debug enabled - post debounce.");
+               enablePressDebug = 1;
+       }
+}
+
+void cliFunc_problemKeys( char* args )
+{
+       print( NL );
+
+       uint8_t count = 0;
+
+       // Args ignored, just displaying
+       // Display problem keys, and the sense value at the time
+       for ( uint8_t key = 0; key < KEY_COUNT; key++ )
+       {
+               if ( keys_problem[key] )
+               {
+                       if ( count++ == 0 )
+                       {
+                               warn_msg("Problem keys: ");
+                       }
+                       printHex( key );
+                       print(" (");
+                       printHex( keys_problem[key] );
+                       print(")   "  );
+               }
+       }
+}
+
+void cliFunc_senseDebug( char* args )
+{
+       // Parse code from argument
+       //  NOTE: Only first argument is used
+       char* arg1Ptr;
+       char* arg2Ptr;
+       CLI_argumentIsolation( args, &arg1Ptr, &arg2Ptr );
+
+       // Default to a single print
+       senseDebugCount = 1;
+
+       // If there was an argument, use that instead
+       if ( *arg1Ptr != '\0' )
+       {
+               senseDebugCount = numToInt( arg1Ptr );
+       }
+}
+