Wait 300ms after min position was triggered so sensor states can recover

ChrGri · Nov 7, 2023 · 0ec5b3f · 0ec5b3f
1 parent 1150bbd
commit 0ec5b3f
Show file tree

Hide file tree

Showing 6 changed files with 117 additions and 19 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/Modbus.cpp b/Arduino/Esp32/Main/Modbus.cpp
@@ -70,6 +70,50 @@ int Modbus::discreteInputRead(int id, int address)
    }
 }
 
+
+// check target values at register address. If target value was already present, return 0. If target value has to be set, return 1.
+bool Modbus::checkAndReplaceParameter(uint16_t slaveId_local_u16, uint16_t parameterAdress, long value) {
+
+  bool retValue_b = false;
+
+  // check if value at address is already target value
+  byte  raw2[2];
+  uint8_t len;
+  int16_t regArray[4];
+
+  // read the four registers simultaneously
+  if(requestFrom(slaveId_local_u16, 0x03, parameterAdress,  2) > 0)
+  {
+    RxRaw(raw2,  len);
+    regArray[0] = uint16(0);
+  }
+
+  // write to public variables
+  int16_t returnValue = regArray[0];
+
+
+
+  // if value is not target value --> overwrite value
+  if(returnValue!= value)
+  {
+    Serial.print("Parameter adresse: ");
+    Serial.print(parameterAdress);
+    Serial.print(",    actual:");
+    Serial.print(returnValue);
+    Serial.print(",    target:");
+    Serial.println(value);
+
+
+
+    holdingRegisterWrite(slaveId_local_u16, parameterAdress, value); // deactivate auto gain
+    delay(50);
+    retValue_b = true;
+  }
+
+  return retValue_b;
+}
+
+
 long Modbus::holdingRegisterRead(int address)
 {
   return holdingRegisterRead(SlaveID, address, 1);

diff --git a/Arduino/Esp32/Main/Modbus.h b/Arduino/Esp32/Main/Modbus.h
@@ -62,6 +62,7 @@ class Modbus
     //int requestFrom(int type, int address, int nb, byte *ret,int len);
     int requestFrom(int slaveId, int type, int address,int nb);
     //  ~Modbus();
+    bool checkAndReplaceParameter(uint16_t slaveId_local_u16, uint16_t parameterAdress, long value);
 
 
     // Read Coil Register       0x01

diff --git a/Arduino/Esp32/Main/StepperWithLimits.cpp b/Arduino/Esp32/Main/StepperWithLimits.cpp
@@ -80,6 +80,10 @@ void StepperWithLimits::findMinMaxSensorless(isv57communication * isv57)
   _stepper->moveTo(0);
   _limitMin = 0;
 
+  // wait N ms to let the endPosDetected become 0 again
+  delay(300);
+
+  // read servo states again
   isv57->readServoStates();
   endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT;
 

diff --git a/Arduino/Esp32/Main/isv57communication.cpp b/Arduino/Esp32/Main/isv57communication.cpp
@@ -12,6 +12,8 @@ isv57communication::isv57communication()
 }
 
 
+
+
 // send tuned servo parameters
 void isv57communication::setupServoStateReading() {
 
@@ -36,32 +38,79 @@ void isv57communication::setupServoStateReading() {
 // send tuned servo parameters
 void isv57communication::sendTunedServoParameters() {
 
+  bool retValue_b = false;
+
   // servo config update
-  modbus.holdingRegisterWrite(slaveId, pr_0_00+2, 0); // deactivate auto gain
-  delay(50);
-  modbus.holdingRegisterWrite(slaveId, pr_0_00+3, 10); // machine stiffness
-  delay(50);
-  modbus.holdingRegisterWrite(slaveId, pr_0_00+4, 80); // ratio of inertia
-  delay(50);
-  modbus.holdingRegisterWrite(slaveId, pr_0_00+8, 1600); // microsteps
-  delay(50);
-  modbus.holdingRegisterWrite(slaveId, pr_0_00+14, 500); // position deviation setup
-  delay(50);
-  modbus.holdingRegisterWrite(slaveId, pr_1_00+0, 400); // 1st position gain
-  delay(50);
-  modbus.holdingRegisterWrite(slaveId, pr_1_00+1, 300); // 1st velocity loop gain
-  delay(50);
-  modbus.holdingRegisterWrite(slaveId, pr_1_00+15, 0); // control switching mode
-  delay(50);
-  modbus.holdingRegisterWrite(slaveId, pr_5_00+20, 1); // encoder output resolution
-  delay(50);
+  //modbus.holdingRegisterWrite(slaveId, pr_0_00+2, 0); // deactivate auto gain
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_0_00+2, 0);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_0_00+3, 10); // machine stiffness
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_0_00+3, 10);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_0_00+4, 80); // ratio of inertia
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_0_00+4, 80);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_0_00+8, 1600); // microsteps
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_0_00+8, 1600);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_0_00+13, 300); // 1st torque limit
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_0_00+13, 300);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_0_00+14, 500); // position deviation setup
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_0_00+14, 500);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+0, 500); // 1st position gain
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+0, 600);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+1, 300); // 1st velocity loop gain
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+1, 300);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+2, 300); // 1st time constant of velocity loop
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+2, 500);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+3, 15); // 1st filter of velocity detection
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+3, 15);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+4, 150); // 1st torque filter
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+4, 150);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+10, 150); // velocity feed forward gain
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+10, 0);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+11, 6000); // velocity feed forward filter
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+11, 6000);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+12, 0); // torque feed forward gain
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+12, 0);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+13, 0); // torque feed forward filter
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+13, 0);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+15, 0); // control switching mode
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+15, 0);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_1_00+37, 0); // special function register
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_1_00+37, 1048);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_3_00+24, 5000); // maximum rpm
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_3_00+24, 5000);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_5_00+13, 1); // overspeed level
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_5_00+13, 5000);
+
+  //modbus.holdingRegisterWrite(slaveId, pr_5_00+20, 1); // encoder output resolution
+  retValue_b |= modbus.checkAndReplaceParameter(slaveId, pr_5_00+20, 1);
+
+
+
 
 
 
 
   // store the settings to servos NVM
-  if (0)
+  if (retValue_b)
   {
+    Serial.println("Servo registered in NVM have been updated! Please power cycle the servo and the ESP!");
     modbus.holdingRegisterWrite(slaveId, 0x019A, 0x5555); // store the settings to servos NVM
     delay(2000);
   }