1 files changed, 43 insertions, 3 deletions

diff --git a/usb-i2c-sl018/limits.c b/usb-i2c-sl018/limits.c
index b65fe28..5807151 100644
--- a/usb-i2c-sl018/limits.c
+++ b/usb-i2c-sl018/limits.c
@@ -30,22 +30,62 @@
 #define LIMITS_OPEN 6
 #define LIMITS_CLOSE 7
 
+#define LIMITS_LP_MAX 255
+
 void init_limits(void)
 {
   LIMITS_DDR = LIMITS_DDR & ~(1<<LIMITS_OPEN | 1<<LIMITS_CLOSE);
   LIMITS_PORT |= (1<<LIMITS_OPEN | 1<<LIMITS_CLOSE);
 }
 
+uint8_t limits_get_close(uint8_t pin)
+{
+  static uint8_t last_state = 0;
+  static uint8_t lp_cnt = 0;
+
+  uint8_t state = pin & (1<<LIMITS_CLOSE);
+  if(state != last_state)
+    lp_cnt++;
+  else
+    lp_cnt += lp_cnt ? -1 : 0;
+
+  if(lp_cnt >= LIMITS_LP_MAX) {
+    last_state = state;
+    lp_cnt = 0;
+  }
+
+  return last_state;
+}
+
+uint8_t limits_get_open(uint8_t pin)
+{
+  static uint8_t last_state = 0;
+  static uint8_t lp_cnt = 0;
+
+  uint8_t state = pin & (1<<LIMITS_OPEN);
+  if(state != last_state)
+    lp_cnt++;
+  else
+    lp_cnt += lp_cnt ? -1 : 0;
+
+  if(lp_cnt >= LIMITS_LP_MAX) {
+    last_state = state;
+    lp_cnt = 0;
+  }
+
+  return last_state;
+}
+
 limits_t limits_get(void)
 {
   uint8_t tmp = LIMITS_PIN & (1<<LIMITS_OPEN | 1<<LIMITS_CLOSE);
-  if(!(tmp & 1<<LIMITS_OPEN)) {
-    if(tmp & 1<<LIMITS_CLOSE)
+  if(!limits_get_open(tmp)) {
+    if(limits_get_close(tmp))
       return open;
     else
       return both;
   }
-  else if(!(tmp & 1<<LIMITS_CLOSE))
+  else if(!limits_get_close(tmp))
     return close;
 
   return moving;