Added 3 * sigma to loadcell reading, minimum travel distance before t…

…riggering max endstop
ChrGri · Nov 12, 2023 · c857bca · c857bca
1 parent e47c939
commit c857bca
Show file tree

Hide file tree

Showing 8 changed files with 16,295 additions and 16,254 deletions.
diff --git a/.DS_Store b/.DS_Store
diff --git a/Arduino/.DS_Store b/Arduino/.DS_Store
diff --git a/Arduino/Esp32/Main/LoadCell.cpp b/Arduino/Esp32/Main/LoadCell.cpp
@@ -63,7 +63,9 @@ LoadCell_ADS1256::LoadCell_ADS1256(uint8_t channel0, uint8_t channel1)
 float LoadCell_ADS1256::getReadingKg() const {
   ADS1256& adc = ADC();
   adc.waitDRDY();        // wait for DRDY to go low before next register read
-  return adc.readCurrentChannel() - _zeroPoint;
+
+  // correct bias, assume AWGN --> 3 * sigma is 99.9 %
+  return adc.readCurrentChannel() - ( _zeroPoint + 3.0 * _standardDeviationEstimate );
 }
 
 void LoadCell_ADS1256::setZeroPoint() {
@@ -86,6 +88,7 @@ void LoadCell_ADS1256::setZeroPoint() {
 void LoadCell_ADS1256::estimateVariance() {
   ADS1256& adc = ADC();
 
+
   Serial.println("ADC: Identify loadcell variance");
   float varNormalizer = 1. / (float)(NUMBER_OF_SAMPLES_FOR_LOADCELL_OFFFSET_ESTIMATION - 1);
   float varEstimate = 0.0f;
@@ -95,11 +98,13 @@ void LoadCell_ADS1256::estimateVariance() {
     varEstimate += sq(loadcellReading) * varNormalizer;
   }
 
+  _standardDeviationEstimate = sqrt(varEstimate);
+
   Serial.println("Variance est.:");
   Serial.println(varEstimate);
 
   Serial.println("Stddev est.:");
-  Serial.println(sqrt(varEstimate));
+  Serial.println(_standardDeviationEstimate);
 
   // make sure estimate is nonzero
   if (varEstimate < LOADCELL_VARIANCE_MIN) { 
@@ -108,4 +113,7 @@ void LoadCell_ADS1256::estimateVariance() {
   varEstimate *= 9;
 
   _varianceEstimate = varEstimate;
+
+
+
 }
diff --git a/Arduino/Esp32/Main/LoadCell.h b/Arduino/Esp32/Main/LoadCell.h
@@ -5,8 +5,9 @@
 
 class LoadCell_ADS1256 {
 private:
-  float _zeroPoint;
-  float _varianceEstimate;
+  float _zeroPoint = 0.0;
+  float _varianceEstimate = 0.0;
+  float _standardDeviationEstimate = 0.0;
 
 public:
   LoadCell_ADS1256(uint8_t channel0=0, uint8_t channel1=1);

diff --git a/Arduino/Esp32/Main/StepperWithLimits.cpp b/Arduino/Esp32/Main/StepperWithLimits.cpp
@@ -3,9 +3,13 @@
 
 
 #define STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT 20
+#define MIN_POS_MAX_ENDSTOP STEPS_PER_MOTOR_REVOLUTION * 3 // servo has to drive minimum N steps before it allows the detection of the max endstop
+
 static const uint8_t LIMIT_TRIGGER_VALUE = LOW;                                   // does endstop trigger high or low
 static const int32_t ENDSTOP_MOVEMENT = STEPS_PER_MOTOR_REVOLUTION / 100;         // how much to move between trigger checks
 static const int32_t ENDSTOP_MOVEMENT_SENSORLESS = ENDSTOP_MOVEMENT * 5;
+
+
 FastAccelStepperEngine& stepperEngine() {
   static FastAccelStepperEngine myEngine = FastAccelStepperEngine();   // this is a factory and manager for all stepper instances
 
@@ -81,17 +85,23 @@ void StepperWithLimits::findMinMaxSensorless(isv57communication * isv57)
   _limitMin = 0;
 
   // wait N ms to let the endPosDetected become 0 again
-  delay(300);
+  //delay(300);
 
   // read servo states again
-  isv57->readServoStates();
-  endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT;
+  //isv57->readServoStates();
+  endPosDetected = 0;//abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT;
 
   setPosition = _stepper->getCurrentPosition();
-  while(!endPosDetected){
+  while (!endPosDetected) {
     delay(10);
     isv57->readServoStates();
-    endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT;
+
+    // only trigger when difference is significant
+    if (setPosition > MIN_POS_MAX_ENDSTOP)
+    {
+      endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT;  
+    }
+
 
     setPosition = setPosition + ENDSTOP_MOVEMENT_SENSORLESS;
     _stepper->moveTo(setPosition, true);

diff --git a/Arduino/Esp32/bin/Main.ino.bin b/Arduino/Esp32/bin/Main.ino.bin
diff --git a/Arduino/Esp32/bin/Main.ino.elf b/Arduino/Esp32/bin/Main.ino.elf
diff --git a/Arduino/Esp32/bin/Main.ino.map b/Arduino/Esp32/bin/Main.ino.map