From 50437833ddfe39cf443f2c319125b79c0d4b081e Mon Sep 17 00:00:00 2001 From: ChrGri Date: Thu, 5 Oct 2023 13:52:48 +0200 Subject: [PATCH] Sensorless homing, faster communication, auto check iSV57 communication - Faster communication between iSV57 and ESP - Sensorless homing can be activated - automatically check whether communication with iSV57 can be established, otherwise continue without --- Arduino/Esp32/Main/.theia/launch.json | 6 + Arduino/Esp32/Main/Main.ino | 281 +++++++++++++--------- Arduino/Esp32/Main/StepperWithLimits.cpp | 105 +++++++- Arduino/Esp32/Main/StepperWithLimits.h | 4 +- Arduino/Esp32/Main/isv57communication.cpp | 36 ++- Arduino/Esp32/Main/isv57communication.h | 6 + 6 files changed, 303 insertions(+), 135 deletions(-) create mode 100644 Arduino/Esp32/Main/.theia/launch.json diff --git a/Arduino/Esp32/Main/.theia/launch.json b/Arduino/Esp32/Main/.theia/launch.json new file mode 100644 index 00000000..a2ea02c4 --- /dev/null +++ b/Arduino/Esp32/Main/.theia/launch.json @@ -0,0 +1,6 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + "version": "0.2.0", + "configurations": [] +} diff --git a/Arduino/Esp32/Main/Main.ino b/Arduino/Esp32/Main/Main.ino index 489e7fd3..a2623fc7 100644 --- a/Arduino/Esp32/Main/Main.ino +++ b/Arduino/Esp32/Main/Main.ino @@ -1,9 +1,10 @@ #define ESTIMATE_LOADCELL_VARIANCE -//#define ISV_COMMUNICATION +#define ISV_COMMUNICATION //#define PRINT_SERVO_STATES - +#define SENSORLESS_HOMING false//true bool resetServoEncoder = true; +bool isv57LifeSignal_b = false; #ifdef ISV_COMMUNICATION #include "isv57communication.h" int32_t servo_offset_compensation_steps_i32 = 0; @@ -115,8 +116,8 @@ bool resetPedalPosition = false; #if CONFIG_IDF_TARGET_ESP32S2 #define minPin 11 #define maxPin 10 - #define dirPinStepper 8//17//8//12//3 - #define stepPinStepper 9//16//9//13//2 // step pin must be pin 9 + #define dirPinStepper 8 + #define stepPinStepper 9 // step pin must be pin 9 #elif CONFIG_IDF_TARGET_ESP32 #define minPin 34 #define maxPin 35 @@ -207,14 +208,34 @@ void setup() Serial.begin(921600); Serial.setTimeout(5); - - - + Serial.println(" "); + Serial.println(" "); + Serial.println(" "); // init controller SetupController(); + delay(2000); + + +// check whether iSV57 communication can be established +// and in case, (a) send tuned servo parameters and (b) prepare the servo for signal read +#ifdef ISV_COMMUNICATION + // check whether iSV57 is connected + isv57LifeSignal_b = isv57.checkCommunication(); + + Serial.print("iSV57 communication state: "); + Serial.println(isv57LifeSignal_b); + + if (isv57LifeSignal_b) + { + isv57.setupServoStateReading(); + isv57.sendTunedServoParameters(); + } + delay(200); +#endif + + - delay(1000); stepper = new StepperWithLimits(stepPinStepper, dirPinStepper, minPin, maxPin); loadcell = new LoadCell_ADS1256(); @@ -225,7 +246,17 @@ void setup() #endif // find the min & max endstops - stepper->findMinMaxEndstops(); + + if (isv57LifeSignal_b && SENSORLESS_HOMING) + { + stepper->findMinMaxSensorless(&isv57); + } + else + { + stepper->findMinMaxEndstops(); + } + + Serial.print("Min Position is "); Serial.println(stepper->getLimitMin()); Serial.print("Max Position is "); Serial.println(stepper->getLimitMax()); @@ -312,24 +343,23 @@ void setup() 1); delay(500); + #ifdef ISV_COMMUNICATION + + xTaskCreatePinnedToCore( + servoCommunicationTask, + "servoCommunicationTask", + 10000, + //STACK_SIZE_FOR_TASK_2, + NULL, + 1, + &Task2, + 1); + delay(500); +#endif + -#ifdef ISV_COMMUNICATION - - isv57.setupServoStateReading(); - isv57.sendTunedServoParameters(); - xTaskCreatePinnedToCore( - servoCommunicationTask, - "servoCommunicationTask", - 10000, - //STACK_SIZE_FOR_TASK_2, - NULL, - 1, - &Task2, - 1); - delay(500); -#endif @@ -348,7 +378,6 @@ void setup() /**********************************************************************************************/ void updatePedalCalcParameters() { - dap_calculationVariables_st.updateFromConfig(dap_config_st); dap_calculationVariables_st.updateEndstops(stepper->getLimitMin(), stepper->getLimitMax()); stepper->updatePedalMinMaxPos(dap_config_st.payLoadPedalConfig_.pedalStartPosition, dap_config_st.payLoadPedalConfig_.pedalEndPosition); @@ -360,12 +389,6 @@ void updatePedalCalcParameters() // equalize pedal config for both tasks dap_config_st_local = dap_config_st; - - - - - - } @@ -473,7 +496,16 @@ void pedalUpdateTask( void * pvParameters ) // if reset pedal position was requested, reset pedal now // This function is implemented, so that in case of lost steps, the user can request a reset of the pedal psotion if (resetPedalPosition) { - stepper->refindMinLimit(); + + if (isv57LifeSignal_b && SENSORLESS_HOMING) + { + stepper->refindMinLimitSensorless(&isv57); + } + else + { + stepper->refindMinLimit(); + } + resetPedalPosition = false; resetServoEncoder = true; } @@ -561,6 +593,11 @@ void pedalUpdateTask( void * pvParameters ) //int32_t Position_Next = MoveByInterpolatedStrategy(filteredReading, stepperPosFraction, &forceCurve, &dap_calculationVariables_st, &dap_config_st); int32_t Position_Next = MoveByPidStrategy(filteredReading, stepperPosFraction, stepper, &forceCurve, &dap_calculationVariables_st, &dap_config_st, absForceOffset_fl32); + //#define DEBUG_STEPPER_POS + #ifdef DEBUG_STEPPER_POS + static RTDebugOutput rtDebugFilter({ "ESP_pos", "ESP_tar_pos", "ISV_pos"}); + rtDebugFilter.offerData({ stepper->getCurrentPositionSteps(), Position_Next, -(int32_t)isv57.servo_pos_given_p}); + #endif //stepper->printStates(); @@ -595,6 +632,8 @@ void pedalUpdateTask( void * pvParameters ) } #endif + + // get current stepper position right before sheduling a new move //int32_t stepperPosCurrent = stepper->getCurrentPositionSteps(); //int32_t stepperPosCurrent = stepper->getTargetPositionSteps(); @@ -665,9 +704,6 @@ void serialCommunicationTask( void * pvParameters ) for(;;){ - - - // average cycle time averaged over multiple cycles #ifdef PRINT_CYCLETIME static CycleTimer timerSC("SC cycle time"); @@ -789,112 +825,131 @@ void serialCommunicationTask( void * pvParameters ) #ifdef ISV_COMMUNICATION -#define SERVO_HIST_SIZE 4 -int16_t servo_pos_hist_ai16[SERVO_HIST_SIZE]; -uint8_t servo_hist_pos_index = 0; +int16_t servoPos_last_i16 = 0; +int64_t timeSinceLastServoPosChange_l = 0; +int64_t timeNow_l = 0; +int64_t timeDiff = 0; + +#define TIME_SINCE_SERVO_POS_CHANGE_TO_DETECT_STANDSTILL_IN_MS 200 + void servoCommunicationTask( void * pvParameters ) { + for(;;){ - isv57.readServoStates(); - - int32_t servo_offset_compensation_steps_local_i32 = 0; - - // condition 1: servo must be at halt - // condition 2: the esp accel lib must be at halt - bool cond_1 = true; - bool cond_2 = false; - - servo_pos_hist_ai16[servo_hist_pos_index] = isv57.servo_pos_given_p; - servo_hist_pos_index++; - if(SERVO_HIST_SIZE <= servo_hist_pos_index) + if (isv57LifeSignal_b) { - servo_hist_pos_index = 0; - } - - // check the servo hist ringbuffer - for (uint8_t idx = 1; idx < SERVO_HIST_SIZE; idx++) - { - if(servo_pos_hist_ai16[idx] != servo_pos_hist_ai16[0]) - { - cond_1 = false; - } - } - cond_2 = stepper->isAtMinPos(); + delay(20); + - // calculate zero position offset - if (cond_1 && cond_2) - { + int32_t servo_offset_compensation_steps_local_i32 = 0; - // reset encoder position, when pedal is at min position - if (resetServoEncoder == true) - { - isv57.setZeroPos(); - resetServoEncoder = false; - } + + // condition 1: servo must be at halt + // condition 2: the esp accel lib must be at halt + bool cond_1 = false;; + bool cond_2 = false; - // calculate encoder offset - // movement to the back will reduce encoder value - servo_offset_compensation_steps_local_i32 = (int32_t)isv57.getZeroPos() - (int32_t)isv57.servo_pos_given_p; - // when pedal has moved to the back due to step losses --> offset will be positive + // check whether target position from ESP hasn't changed and is at min endstop position + cond_2 = stepper->isAtMinPos(); - // since the encoder positions are defined in int16 space, they wrap at multiturn - // to correct overflow, we apply modulo to take smallest possible deviation - if (servo_offset_compensation_steps_local_i32 > pow(2,15)-1) - { - servo_offset_compensation_steps_local_i32 -= pow(2,16); - } + if (cond_2 == true) + { + isv57.readServoStates(); + int16_t servoPos_now_i16 = isv57.servo_pos_given_p; + timeNow_l = millis(); - if (servo_offset_compensation_steps_local_i32 < -pow(2,15)) - { - servo_offset_compensation_steps_local_i32 += pow(2,16); - } - } + // check whether servo position has changed, in case, update the halt detection variable + if (servoPos_last_i16 != servoPos_now_i16) + { + servoPos_last_i16 = servoPos_now_i16; + timeSinceLastServoPosChange_l = timeNow_l; + } + // compute the time difference since last servo position change + timeDiff = timeNow_l - timeSinceLastServoPosChange_l; + // if time between last servo position is larger than a threshold, detect servo standstill + if ( (timeDiff > TIME_SINCE_SERVO_POS_CHANGE_TO_DETECT_STANDSTILL_IN_MS) + && (timeNow_l > 0) ) + { + cond_1 = true; + } + else + { + cond_1 = false; + } + } - if(semaphore_resetServoPos!=NULL) - { + - // Take the semaphore and just update the config file, then release the semaphore - if(xSemaphoreTake(semaphore_resetServoPos, (TickType_t)1)==pdTRUE) + // calculate zero position offset + if (cond_1 && cond_2) { - servo_offset_compensation_steps_i32 = servo_offset_compensation_steps_local_i32; - xSemaphoreGive(semaphore_resetServoPos); + + // reset encoder position, when pedal is at min position + if (resetServoEncoder == true) + { + isv57.setZeroPos(); + resetServoEncoder = false; + } + + // calculate encoder offset + // movement to the back will reduce encoder value + servo_offset_compensation_steps_local_i32 = (int32_t)isv57.getZeroPos() - (int32_t)isv57.servo_pos_given_p; + // when pedal has moved to the back due to step losses --> offset will be positive + + // since the encoder positions are defined in int16 space, they wrap at multiturn + // to correct overflow, we apply modulo to take smallest possible deviation + if (servo_offset_compensation_steps_local_i32 > pow(2,15)-1) + { + servo_offset_compensation_steps_local_i32 -= pow(2,16); + } + + if (servo_offset_compensation_steps_local_i32 < -pow(2,15)) + { + servo_offset_compensation_steps_local_i32 += pow(2,16); + } } - } - else - { - semaphore_resetServoPos = xSemaphoreCreateMutex(); - Serial.println("semaphore_resetServoPos == 0"); - } - #ifdef PRINT_SERVO_STATES - static RTDebugOutput rtDebugFilter({ "pos_p", "pos_error_p", "curr_per"}); - rtDebugFilter.offerData({ isv57.servo_pos_given_p, isv57.servo_pos_error_p, isv57.servo_current_percent}); + if(semaphore_resetServoPos!=NULL) + { + // Take the semaphore and just update the config file, then release the semaphore + if(xSemaphoreTake(semaphore_resetServoPos, (TickType_t)1)==pdTRUE) + { + servo_offset_compensation_steps_i32 = servo_offset_compensation_steps_local_i32; + xSemaphoreGive(semaphore_resetServoPos); + } - /*Serial.print("resetServoEncoder: "); - Serial.print(resetServoEncoder); + } + else + { + semaphore_resetServoPos = xSemaphoreCreateMutex(); + Serial.println("semaphore_resetServoPos == 0"); + } - Serial.print(", Offset cond 1: "); - Serial.print(cond_1); - - Serial.print(", Offset cond 2: "); - Serial.print(cond_2);*/ - //Serial.print(", Offset: "); - //Serial.println(servo_offset_compensation_steps_i32); - #endif + #ifdef PRINT_SERVO_STATES + static RTDebugOutput rtDebugFilter({ "pos_p", "pos_error_p", "curr_per", "offset"}); + rtDebugFilter.offerData({ isv57.servo_pos_given_p, isv57.servo_pos_error_p, isv57.servo_current_percent, servo_offset_compensation_steps_i32}); + #endif + + + + + } + else + { + delay(1000); + } - - } } diff --git a/Arduino/Esp32/Main/StepperWithLimits.cpp b/Arduino/Esp32/Main/StepperWithLimits.cpp index 381072be..70882d20 100644 --- a/Arduino/Esp32/Main/StepperWithLimits.cpp +++ b/Arduino/Esp32/Main/StepperWithLimits.cpp @@ -1,10 +1,11 @@ #include "StepperWithLimits.h" - #include "RTDebugOutput.h" + +#define STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT 20 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 @@ -42,6 +43,68 @@ StepperWithLimits::StepperWithLimits(uint8_t pinStep, uint8_t pinDirection, uint } } + +void StepperWithLimits::findMinMaxSensorless(isv57communication * isv57) +{ + + if (! hasValidStepper()) return; + + // obtain servo states + isv57->readServoStates(); + bool endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT; + + + int32_t setPosition = _stepper->getCurrentPosition(); + while(!endPosDetected){ + delay(10); + isv57->readServoStates(); + endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT; + + setPosition = setPosition - ENDSTOP_MOVEMENT_SENSORLESS; + _stepper->moveTo(setPosition, true); + + //Serial.print("Min_DetValue: "); + //Serial.println(isv57->servo_current_percent); + } + + // move away from min position to reduce servos current reading + _stepper->forceStop(); + setPosition = setPosition + 5 * ENDSTOP_MOVEMENT_SENSORLESS; + _stepper->moveTo(setPosition, true); + _stepper->forceStopAndNewPosition(0); + _stepper->moveTo(0); + _limitMin = 0; + + isv57->readServoStates(); + endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT; + + setPosition = _stepper->getCurrentPosition(); + while(!endPosDetected){ + delay(10); + isv57->readServoStates(); + endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT; + + setPosition = setPosition + ENDSTOP_MOVEMENT_SENSORLESS; + _stepper->moveTo(setPosition, true); + + //Serial.print("Max_DetValue: "); + //Serial.println(isv57->servo_current_percent); + } + + //_stepper->forceStop(); + //setPosition = setPosition - 5 * ENDSTOP_MOVEMENT; + + _limitMax = _stepper->getCurrentPosition(); + + _stepper->moveTo(_posMin, true); +#if defined(SUPPORT_ESP32_PULSE_COUNTER) + _stepper->clearPulseCounter(); +#endif + + +} + + void StepperWithLimits::findMinMaxEndstops() { if (! hasValidStepper()) return; @@ -50,6 +113,7 @@ void StepperWithLimits::findMinMaxEndstops() { setPosition = setPosition - ENDSTOP_MOVEMENT; _stepper->moveTo(setPosition, true); } + _stepper->forceStopAndNewPosition(0); _stepper->moveTo(0); @@ -84,6 +148,33 @@ void StepperWithLimits::refindMinLimit() { _stepper->forceStopAndNewPosition(_limitMin); } +void StepperWithLimits::refindMinLimitSensorless(isv57communication * isv57) { + + // obtain servo states + isv57->readServoStates(); + bool endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT; + + + int32_t setPosition = _stepper->getCurrentPosition(); + while(!endPosDetected){ + delay(10); + isv57->readServoStates(); + endPosDetected = abs( isv57->servo_current_percent) > STEPPER_WITH_LIMITS_SENSORLESS_CURRENT_THRESHOLD_IN_PERCENT; + + setPosition = setPosition - ENDSTOP_MOVEMENT_SENSORLESS; + _stepper->moveTo(setPosition, true); + + //Serial.print("Min_DetValue: "); + //Serial.println(isv57->servo_current_percent); + } + + // move away from min position to reduce servos current reading + _stepper->forceStop(); + setPosition = setPosition + 5 * ENDSTOP_MOVEMENT_SENSORLESS; + _stepper->moveTo(setPosition, true); + _stepper->forceStopAndNewPosition(_limitMin); +} + void StepperWithLimits::checkLimitsAndResetIfNecessary() { // in case the stepper loses its position and therefore an endstop is triggered reset position if (LIMIT_TRIGGER_VALUE == digitalRead(_pinMin)) { @@ -103,6 +194,8 @@ int8_t StepperWithLimits::moveTo(int32_t position, bool blocking) { int32_t StepperWithLimits::getCurrentPositionSteps() const { return _stepper->getCurrentPosition() - _posMin; } + + double StepperWithLimits::getCurrentPositionFraction() const { return double(getCurrentPositionSteps()) / getTravelSteps(); } @@ -143,12 +236,8 @@ bool StepperWithLimits::isAtMinPos() } bool StepperWithLimits::correctPos(int32_t posOffset) { - // how many revolutions - float pulsesPerRev = 0.0001f; - float revOffset = (float)posOffset * pulsesPerRev; - - // convert revolutions to steps - int32_t stepOffset = revOffset * (float)STEPS_PER_MOTOR_REVOLUTION; + // + int32_t stepOffset =(int32_t)constrain(posOffset, -10, 10); // correct pos _stepper->setCurrentPosition(_stepper->getCurrentPosition() + stepOffset); diff --git a/Arduino/Esp32/Main/StepperWithLimits.h b/Arduino/Esp32/Main/StepperWithLimits.h index 6f01750f..cbc01096 100644 --- a/Arduino/Esp32/Main/StepperWithLimits.h +++ b/Arduino/Esp32/Main/StepperWithLimits.h @@ -1,4 +1,5 @@ #include +#include "isv57communication.h" // these are physical properties of the stepper @@ -26,7 +27,8 @@ class StepperWithLimits { void updatePedalMinMaxPos(uint8_t pedalStartPosPct, uint8_t pedalEndPosPct); bool isAtMinPos(); bool correctPos(int32_t posOffset); - + void findMinMaxSensorless(isv57communication * isv57); + void refindMinLimitSensorless(isv57communication * isv57); public: int8_t moveTo(int32_t position, bool blocking = false); void printStates(); diff --git a/Arduino/Esp32/Main/isv57communication.cpp b/Arduino/Esp32/Main/isv57communication.cpp index 9aa62366..001cbcbb 100644 --- a/Arduino/Esp32/Main/isv57communication.cpp +++ b/Arduino/Esp32/Main/isv57communication.cpp @@ -53,7 +53,7 @@ void isv57communication::sendTunedServoParameters() { delay(50); modbus.holdingRegisterWrite(slaveId, pr_1_00+15, 0); // control switching mode delay(50); - modbus.holdingRegisterWrite(slaveId, pr_5_00+20, 2); // encoder output resolution + modbus.holdingRegisterWrite(slaveId, pr_5_00+20, 1); // encoder output resolution delay(50); @@ -71,6 +71,21 @@ void isv57communication::sendTunedServoParameters() { } + +bool isv57communication::checkCommunication() +{ + if(modbus.requestFrom(slaveId, 0x03, 0x0000, 2) > 0) + { + return true; + } + else + { + return false; + } +} + + + void isv57communication::setZeroPos() { zeroPos = servo_pos_given_p; @@ -85,26 +100,21 @@ int16_t isv57communication::getZeroPos() // read servo states void isv57communication::readServoStates() { - // read the four registers - for (uint8_t regIdx = 0; regIdx < 4; regIdx++) + // read the four registers simultaneously + if(modbus.requestFrom(slaveId, 0x03, ref_cyclic_read_0, 8) > 0) { - regArray[regIdx] = modbus.holdingRegisterRead(slaveId, ref_cyclic_read_0 + regIdx, 2); - - if(modbus.requestFrom(slaveId, 0x03, ref_cyclic_read_0 + regIdx, 1) > 0) - { - modbus.RxRaw(raw, len); - regArray[regIdx] = modbus.uint16(0); + modbus.RxRaw(raw, len); + for (uint8_t regIdx = 0; regIdx < 4; regIdx++) + { + regArray[regIdx] = modbus.uint16(regIdx); } - delay(5); } - + // write to public variables servo_pos_given_p = regArray[0]; servo_pos_error_p = regArray[1]; servo_current_percent = regArray[2]; - - // print registers if (0) { diff --git a/Arduino/Esp32/Main/isv57communication.h b/Arduino/Esp32/Main/isv57communication.h index f68f3949..42c69697 100644 --- a/Arduino/Esp32/Main/isv57communication.h +++ b/Arduino/Esp32/Main/isv57communication.h @@ -1,3 +1,6 @@ +#ifndef ISV57_COMMUNICATION_H +#define ISV57_COMMUNICATION_H + //#include #include "Modbus.h" @@ -49,6 +52,7 @@ class isv57communication { void setupServoStateReading(); void sendTunedServoParameters(); void readServoStates(); + bool checkCommunication(); void setZeroPos(); int16_t getZeroPos(); @@ -66,3 +70,5 @@ class isv57communication { //Modbus modbus; }; + +#endif