diff --git a/pedalboard-footswitch.ino b/pedalboard-footswitch.ino
index d179479..953b488 100644
--- a/pedalboard-footswitch.ino
+++ b/pedalboard-footswitch.ino
@@ -1,3 +1,4 @@
+/* Pinout Config */
 #define SEL_A_IN    D2
 #define SEL_A_LED   D3
 #define SEL_B_IN    D4
@@ -10,6 +11,31 @@
 #define SUST_IN     D10
 #define EXPR_IN     A0
 
+/* MIDI codes */
+#define NOTE_OFF       0x8
+#define NOTE_ON        0x9
+#define AFTERTOUCH     0xA
+#define CONTROL_CHANGE 0xB
+#define PATCH_CHANGE   0xC
+#define CHANNEL_PRESSURE 0xD
+#define PITCH_BEND     0xE
+#define OTHER          0xF
+
+void MIDImessage(int cmd_id, int data1, int data2) {
+  int channel = 0;
+  int cmd_byte = (cmd_id << 4) | (channel & 0xf);
+  Serial.write(cmd_byte);//send command byte
+  Serial.write(data1);//send data byte #1
+  Serial.write(data2);//send data byte #2
+}
+
+void MIDImessage2(int cmd_id, int data1) {
+  int channel = 0;
+  int cmd_byte = (cmd_id << 4) | (channel & 0xf);
+  Serial.write(cmd_byte);//send command byte
+  Serial.write(data1);//send data byte #1
+}
+
 void setup() {
   pinMode( SEL_A_IN, INPUT_PULLUP );
   pinMode( SEL_A_LED, OUTPUT );
@@ -26,86 +52,107 @@ void setup() {
   Serial.begin(31250);
 }
 
-int last_expr_val = 0;
-int last_sust_val = 0;
-
-#define NOTE_OFF       0x8
-#define NOTE_ON        0x9
-#define AFTERTOUCH     0xA
-#define CONTROL_CHANGE 0xB
-#define PATCH_CHANGE   0xC
-#define CHANNEL_PRESSURE 0xD
-#define PITCH_BEND     0xE
-#define OTHER          0xF
-
-//send MIDI message
-void MIDImessage(int cmd_id, int data1, int data2) {
-  int channel = 0;
-  int cmd_byte = (cmd_id << 4) | (channel & 0xf);
-  Serial.write(cmd_byte);//send command byte
-  Serial.write(data1);//send data byte #1
-  Serial.write(data2);//send data byte #2
-}
+unsigned sust_stable_count = 0;
+unsigned last_sust_val = 0;
+float last_expr_val = 0.0;
+unsigned last_program_sel = -1;
 
 void loop() {
-  delay(100);
+  delay(1);
+
+
+  /* Sustain
+   */
 
   if( digitalRead( SUST_IN ) == LOW ) {
-    if( last_sust_val != 1 ) {
+    if( last_sust_val == 1 ) {
+      if( sust_stable_count == 10 ) {
+        // Sustain On
+        MIDImessage( CONTROL_CHANGE, 0x42, 0x7f );
+        sust_stable_count ++;
+      } else if ( sust_stable_count < 10 ) {
+        sust_stable_count ++;
+      }
+    } else {
+      // sustain pressed down
       last_sust_val = 1;
-      MIDImessage( CONTROL_CHANGE, 0x42, 0x7f );
+      sust_stable_count = 0;
     }
   } else {
-    if( last_sust_val != 0 ) {
+    if( last_sust_val == 0 ) {
+      if( sust_stable_count == 10 ) {
+        // Sustain Off
+        MIDImessage( CONTROL_CHANGE, 0x42, 0x00 );
+        sust_stable_count ++;
+      } else if ( sust_stable_count < 10 ) {
+        sust_stable_count ++;
+      }
+    } else {
+      // sustain released
       last_sust_val = 0;
-      MIDImessage( CONTROL_CHANGE, 0x42, 0x00 );
+      sust_stable_count = 0;
     }
   }
 
-  int expr_val = analogRead( EXPR_IN );
-  if( expr_val != last_expr_val )
-  {
-    last_expr_val = expr_val;
+  /* Expression
+   */
 
-    float expr_val_f = (float)(expr_val - 66) / 590.0;
-    if( expr_val_f < 0.0 ){
-      expr_val_f = 0.0;
-    }
-    if( expr_val_f > 1.0 ){
-      expr_val_f = 1.0;
-    }
-
-    int val_b = (int)(expr_val_f * 128.0);
-
-    MIDImessage( CONTROL_CHANGE, 0x0b, val_b );
+  int expr_val_in = analogRead( EXPR_IN );
+  float expr_val_f = (float)(expr_val_in - 66) / 590.0;
+  if( expr_val_f < 0.0 ){
+    expr_val_f = 0.0;
   }
+  if( expr_val_f > 1.0 ){
+    expr_val_f = 1.0;
+  }
+
+  if( fabs(last_expr_val - expr_val_f) > 0.0086 ) {
+    last_expr_val = expr_val_f;
+    unsigned expr_val = (unsigned)(expr_val_f * 128.0);
+    MIDImessage( CONTROL_CHANGE, 0x0b, expr_val );
+  }
+
+  /* Program Select
+   */
 
   if( digitalRead( SEL_A_IN ) == LOW ) {
-    MIDImessage( PATCH_CHANGE, 0x0, 0x0 );
-    digitalWrite( SEL_A_LED, HIGH );
-    digitalWrite( SEL_B_LED, LOW );
-    digitalWrite( SEL_C_LED, LOW );
-    digitalWrite( SEL_D_LED, LOW );
+    if( last_program_sel != 0 ) {
+      last_program_sel = 0;
+      MIDImessage2( PATCH_CHANGE, 0x0 );
+      digitalWrite( SEL_A_LED, HIGH );
+      digitalWrite( SEL_B_LED, LOW );
+      digitalWrite( SEL_C_LED, LOW );
+      digitalWrite( SEL_D_LED, LOW );
+    }
   }
   if( digitalRead( SEL_B_IN ) == LOW ) {
-    MIDImessage( PATCH_CHANGE, 0x1, 0x1 );
-    digitalWrite( SEL_A_LED, LOW );
-    digitalWrite( SEL_B_LED, HIGH );
-    digitalWrite( SEL_C_LED, LOW );
-    digitalWrite( SEL_D_LED, LOW );
+    if( last_program_sel != 1 ) {
+      last_program_sel = 1;
+      MIDImessage2( PATCH_CHANGE, 0x1 );
+      digitalWrite( SEL_A_LED, LOW );
+      digitalWrite( SEL_B_LED, HIGH );
+      digitalWrite( SEL_C_LED, LOW );
+      digitalWrite( SEL_D_LED, LOW );
+    }
   }
   if( digitalRead( SEL_C_IN ) == LOW ) {
-    MIDImessage( PATCH_CHANGE, 0x2, 0x2 );
-    digitalWrite( SEL_A_LED, LOW );
-    digitalWrite( SEL_B_LED, LOW );
-    digitalWrite( SEL_C_LED, HIGH );
-    digitalWrite( SEL_D_LED, LOW );
+    if( last_program_sel != 2 ) {
+      last_program_sel = 2;
+      MIDImessage2( PATCH_CHANGE, 0x2 );
+      digitalWrite( SEL_A_LED, LOW );
+      digitalWrite( SEL_B_LED, LOW );
+      digitalWrite( SEL_C_LED, HIGH );
+      digitalWrite( SEL_D_LED, LOW );
+    }
   }
   if( digitalRead( SEL_D_IN ) == LOW ) {
-    MIDImessage( PATCH_CHANGE, 0x3, 0x3 );
-    digitalWrite( SEL_A_LED, LOW );
-    digitalWrite( SEL_B_LED, LOW );
-    digitalWrite( SEL_C_LED, LOW );
-    digitalWrite( SEL_D_LED, HIGH );
+    if( last_program_sel != 3 ) {
+      last_program_sel = 3;
+      MIDImessage2( PATCH_CHANGE, 0x3 );
+      digitalWrite( SEL_A_LED, LOW );
+      digitalWrite( SEL_B_LED, LOW );
+      digitalWrite( SEL_C_LED, LOW );
+      digitalWrite( SEL_D_LED, HIGH );
+    }
   }
 }