Jelajahi Sumber

better limit handlingg

timer to limit updates (and debouncing needs)
buffer to prevent false detections
titi 5 tahun lalu
induk
melakukan
8592e02bdd
2 mengubah file dengan 84 tambahan dan 66 penghapusan
  1. 77 61
      Atm_AccelStepper.cpp
  2. 7 5
      Atm_AccelStepper.h

+ 77 - 61
Atm_AccelStepper.cpp

@@ -24,6 +24,8 @@ Atm_AccelStepper& Atm_AccelStepper::begin(int step_pin, int dir_pin) {
   stepper->setAcceleration(acceleration);
   idle_timer.set(ATM_TIMER_OFF);
   position_timer.set(POSITION_SEND_TIMER);
+  limits_timer.set(LIMIT_UPDATE_RATE);
+  limits_timer.setFromNow(this, LIMIT_UPDATE_RATE);
 
   return *this;
 }
@@ -33,7 +35,7 @@ Atm_AccelStepper& Atm_AccelStepper::begin(int step_pin, int dir_pin) {
  */
 
 int Atm_AccelStepper::event( int id ) {
-  bool tempState ;
+  //bool tempState ;
 
   switch ( id ) {
     case EVT_DISABLE:
@@ -49,61 +51,71 @@ int Atm_AccelStepper::event( int id ) {
     case EVT_EMERGENCY_STOP:
       return 0;
     case EVT_ON_LIMIT_LOW:
-      switch(_limitLow_Mode) {
-
-        case 0:
-          break;
-        case 1: //digital INPUT
-          // Serial.println("digital");
-          limitLow_State_raw = digitalRead(_limitLow_Pin);
-          limitLow_State_raw = _limitLow_Reversed ? !limitLow_State_raw : limitLow_State_raw;
-          break;
-        case 2:
-          int analogTemp = analogRead(_limitLow_Pin);
-          limitLow_State_raw = (_limitLow_Thresholds[0] < analogTemp) && (analogTemp < _limitLow_Thresholds[1]);
-          limitLow_State_raw = _limitLow_Reversed ? !limitLow_State_raw : limitLow_State_raw;
-          // if(limitLow_State){
-	        //   delay(3);
-	        //   analogTemp = analogRead(_limitLow_Pin);
-        	//   limitLow_State = (_limitLow_Thresholds[0] < analogTemp) && (analogTemp < _limitLow_Thresholds[1]);
-        	//   limitLow_State = _limitLow_Reversed ? !limitLow_State : limitLow_State;
-		      //   }
-          break;
+      if (limits_timer.expired(this)){
+        switch(_limitLow_Mode) {
+          case 0:
+            break;
+          case 1: //digital INPUT
+            // Serial.println("digital");
+            limitLow_State_raw = digitalRead(_limitLow_Pin);
+            limitLow_State_raw = _limitLow_Reversed ? !limitLow_State_raw : limitLow_State_raw;
+            break;
+          case 2:
+            int analogTemp = analogRead(_limitLow_Pin);
+            limitLow_State_raw = (_limitLow_Thresholds[0] < analogTemp) && (analogTemp < _limitLow_Thresholds[1]);
+            limitLow_State_raw = _limitLow_Reversed ? !limitLow_State_raw : limitLow_State_raw;
+            // if(limitLow_State){
+  	        //   delay(3);
+  	        //   analogTemp = analogRead(_limitLow_Pin);
+          	//   limitLow_State = (_limitLow_Thresholds[0] < analogTemp) && (analogTemp < _limitLow_Thresholds[1]);
+          	//   limitLow_State = _limitLow_Reversed ? !limitLow_State : limitLow_State;
+  		      //   }
+            break;
+          }
+          limitLow_State = limitLow_avg(limitLow_State_raw);
+          changed = limitLow_State != limitLow_State_prev ? 1 : 0 ;
+          limitLow_State_prev = limitLow_State ;
+          //Serial.println(limitLow_State);
+          if (changed){push( connectors, ON_ONLIMITLOW, 0, limitLow_State, 0 );}
+          // Serial.println("in limit");
+
+          return changed ;
         }
-        limitLow_State = limitLow_avg(limitLow_State_raw);
-        changed = limitLow_State != limitLow_State_prev ? 1 : 0 ;
-        limitLow_State_prev = limitLow_State ;
-        //Serial.println(limitLow_State);
-        if (changed){push( connectors, ON_ONLIMITLOW, 0, limitLow_State, 0 );}
-        return changed ;
+        else{return 0;}
+
 
 
     case EVT_ON_LIMIT_HIGH:
-      switch(_limitHigh_Mode) {
-        case 0:
-          break;
-        case 1: //digital INPUT
-          limitHigh_State_raw = digitalRead(_limitHigh_Pin);
-          limitHigh_State_raw = _limitHigh_Reversed ? !limitHigh_State_raw : limitHigh_State_raw;
-          break;
-        case 2:
-        //Serial.println("analog");
-          int analogTemp = analogRead(_limitHigh_Pin);
-          limitHigh_State_raw = (_limitHigh_Thresholds[0] < analogTemp) && (analogTemp < _limitHigh_Thresholds[1]);
-          limitHigh_State_raw = _limitHigh_Reversed ? !limitHigh_State_raw : limitHigh_State_raw;
-		      // if(limitHigh_State){
-		      //     delay(3);
-			    //     analogTemp = analogRead(_limitHigh_Pin);
-          // 	  limitHigh_State = (_limitHigh_Thresholds[0] < analogTemp) && (analogTemp < _limitHigh_Thresholds[1]);
-          // 	  limitHigh_State = _limitHigh_Reversed ? !limitHigh_State : limitHigh_State;
-		      //     }
-          break;
-        }
-        limitHigh_State = limitHigh_avg(limitHigh_State_raw);
-        changed = limitHigh_State != limitHigh_State_prev ? 1 : 0;
-        limitHigh_State_prev = limitHigh_State;
-        if (changed){push( connectors, ON_ONLIMITHIGH, 0, limitHigh_State, 0 );}
-        return changed;
+      if (limits_timer.expired(this)){
+        switch(_limitHigh_Mode) {
+          case 0:
+            break;
+          case 1: //digital INPUT
+            limitHigh_State_raw = digitalRead(_limitHigh_Pin);
+            limitHigh_State_raw = _limitHigh_Reversed ? !limitHigh_State_raw : limitHigh_State_raw;
+            break;
+          case 2:
+          //Serial.println("analog");
+            int analogTemp = analogRead(_limitHigh_Pin);
+            limitHigh_State_raw = (_limitHigh_Thresholds[0] < analogTemp) && (analogTemp < _limitHigh_Thresholds[1]);
+            limitHigh_State_raw = _limitHigh_Reversed ? !limitHigh_State_raw : limitHigh_State_raw;
+  		      // if(limitHigh_State){
+  		      //     delay(3);
+  			    //     analogTemp = analogRead(_limitHigh_Pin);
+            // 	  limitHigh_State = (_limitHigh_Thresholds[0] < analogTemp) && (analogTemp < _limitHigh_Thresholds[1]);
+            // 	  limitHigh_State = _limitHigh_Reversed ? !limitHigh_State : limitHigh_State;
+  		      //     }
+            break;
+          }
+          limitHigh_State = limitHigh_avg(limitHigh_State_raw);
+          changed = limitHigh_State != limitHigh_State_prev ? 1 : 0;
+          limitHigh_State_prev = limitHigh_State;
+          if (changed){push( connectors, ON_ONLIMITHIGH, 0, limitHigh_State, 0 );}
+          limits_timer.setFromNow(this, LIMIT_UPDATE_RATE);
+          return changed ;
+       }
+
+        else{return 0;}
 
     case EVT_ON_TARGET:
       return runMode ? 0 : _currentStep == _targetStep;
@@ -133,6 +145,7 @@ void Atm_AccelStepper::action( int id ) {
       push(connectors, ON_CHANGESTATE, 0,  state(), 0);
       enabled = _enableReversed ? HIGH : LOW;
       digitalWrite(_enablePin, enabled);
+      limits_timer.setFromNow(this, LIMIT_UPDATE_RATE);
       return;
     case ENT_ENABLED:
       _isHoming = 0 ;
@@ -145,7 +158,7 @@ void Atm_AccelStepper::action( int id ) {
       limitLow_State = 0;
       limitHigh_State = 0;
       digitalWrite(_enablePin, enabled);
-
+      limits_timer.setFromNow(this, LIMIT_UPDATE_RATE);
 
       return;
     case ENT_RUNNING:
@@ -159,6 +172,7 @@ void Atm_AccelStepper::action( int id ) {
       //stepper_update();
       //push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
       position_timer.setFromNow(this, POSITION_SEND_TIMER);
+      limits_timer.setFromNow(this, LIMIT_UPDATE_RATE);
       stepper_update();
       return;
     case LP_RUNNING:
@@ -212,6 +226,7 @@ void Atm_AccelStepper::action( int id ) {
       //_isHoming = 1 ;
       stepper->setSpeed(-1*homing_speed);
       position_timer.setFromNow(this, POSITION_SEND_TIMER);
+      limits_timer.setFromNow(this, LIMIT_UPDATE_RATE);
       return;
     case LP_HOMING_LOW:
       stepper_update();
@@ -224,7 +239,7 @@ void Atm_AccelStepper::action( int id ) {
           trigger(EVT_EMERGENCY_STOP);
           push(connectors, ON_ONHOMINGLOW, 0,  _currentStep, homingLow_done);
         }
-        else if (changed && limitHigh_State){
+        else if (changed && limitHigh_State ){
           homingLow_done = 0 ;
           runMode = 0;
           trigger(EVT_EMERGENCY_STOP);
@@ -259,6 +274,7 @@ void Atm_AccelStepper::action( int id ) {
       //_isHoming = 2 ;
       stepper->setSpeed(homing_speed);
       position_timer.setFromNow(this, POSITION_SEND_TIMER);
+      limits_timer.setFromNow(this, LIMIT_UPDATE_RATE);
       return;
     case LP_HOMING_HIGH:
       stepper_update();
@@ -273,7 +289,7 @@ void Atm_AccelStepper::action( int id ) {
           push(connectors, ON_ONHOMINGHIGH, 0,  _currentStep, homingHigh_done);
           //Serial.println("homing high done");
         }
-        else if (changed && limitLow_State){
+        else if (changed && limitLow_State ){
           homingHigh_done = 0;
           runMode = 0 ;
           trigger(EVT_EMERGENCY_STOP);
@@ -489,11 +505,11 @@ int Atm_AccelStepper::limitLow_avg(bool limistate){
   } else {
     limitLow_buf_head++;
   }
-  // for (int i =0; i<limit_buf_size; i++){
-  //   Serial.print(limitLow_state_buf[i]);
-  //   Serial.print(" ");
-  // }
-  // Serial.println();
+  for (int i =0; i<limit_buf_size; i++){
+    Serial.print(limitLow_state_buf[i]);
+    Serial.print(" ");
+  }
+  Serial.println();
   return limitLow_state_total > limit_buf_size / 2; //all values should agree
 }
 
@@ -506,7 +522,7 @@ int Atm_AccelStepper::limitHigh_avg(bool limistate){
     limitHigh_buf_head++;
   }
 
-  return limitHigh_state_total < limit_buf_size / 2; //all values should agree
+  return limitHigh_state_total > limit_buf_size / 2; //all values should agree
 }
 
 // Atm_AccelStepper& Atm_AccelStepper::rotationReversed(bool reversed){

+ 7 - 5
Atm_AccelStepper.h

@@ -77,18 +77,18 @@ class Atm_AccelStepper: public Machine {
   Atm_AccelStepper& limitHigh_isHard(bool hardlimit = 1);
   Atm_AccelStepper& limitHigh_setThresholds (int threshold_low=510, int threshold_high = 1024);
   bool limitLow_State_raw;
-  bool limitLow_State;
-  bool limitHigh_State;
+  bool limitLow_State = 0;
+  bool limitHigh_State = 0;
   bool limitHigh_State_raw;
   bool limitLow_State_prev;
   bool limitHigh_State_prev;
-  const static int limit_buf_size = 20 ;
+  const static int limit_buf_size = 16 ;
   bool limitLow_state_buf [limit_buf_size] ;
-  int limitLow_state_total ;
+  int limitLow_state_total = 0 ;
   int limitLow_buf_head ;
   int limitLow_avg(bool limitState);
   bool limitHigh_state_buf [limit_buf_size] ;
-  int limitHigh_state_total ;
+  int limitHigh_state_total = 0 ;
   int limitHigh_buf_head ;
   int limitHigh_avg(bool limitState);
   bool changed = 0 ;
@@ -132,6 +132,8 @@ class Atm_AccelStepper: public Machine {
   int _limitHigh_Thresholds[2] ; //analog value  range for two analog limits
   bool _limitHigh_Hard = 0 ;
   int _isHoming = 0 ;
+  atm_timer_millis limits_timer ;
+  int LIMIT_UPDATE_RATE = 5 ;
   void updateLimitSwitch();
 
 };