From 394e7c77e296a0dadb46aa503855d9b5452a3d06 Mon Sep 17 00:00:00 2001 From: MrUnzO Date: Sun, 24 Dec 2023 18:39:46 +0700 Subject: [PATCH] chore: resolve merge --- Arduino/Esp32/Main/Controller.cpp | 18 +- Arduino/Esp32/Main/Main.h | 2 +- Arduino/Esp32/Main/Main.ino | 429 ++++++++++++++++-------------- 3 files changed, 235 insertions(+), 214 deletions(-) diff --git a/Arduino/Esp32/Main/Controller.cpp b/Arduino/Esp32/Main/Controller.cpp index e916f5a0..b7736927 100644 --- a/Arduino/Esp32/Main/Controller.cpp +++ b/Arduino/Esp32/Main/Controller.cpp @@ -39,40 +39,34 @@ static const int16_t JOYSTICK_RANGE = JOYSTICK_MAX_VALUE - JOYSTICK_MIN_VALUE; char ssid[23]; uint64_t chipid = ESP.getEfuseMac(); // The chip ID is essentially its MAC address(length: 6 bytes). unsigned int chip = (unsigned int)(chipid >> 32); - std::string bluetoothName_lcl = "DiyFfbPedal_" + std::to_string( chip ); + std::string bluetoothName_lcl = "DiyActivePedal_" + std::to_string( chip ); BleGamepad bleGamepad(bluetoothName_lcl, bluetoothName_lcl, 100); void SetupController() { BleGamepadConfiguration bleGamepadConfig; - bleGamepadConfig.setControllerType(CONTROLLER_TYPE_MULTI_AXIS); // CONTROLLER_TYPE_JOYSTICK, CONTROLLER_TYPE_GAMEPAD (DEFAULT), CONTROLLER_TYPE_MULTI_AXIS + bleGamepadConfig.setControllerType(CONTROLLER_TYPE_GAMEPAD); // CONTROLLER_TYPE_JOYSTICK, CONTROLLER_TYPE_GAMEPAD (DEFAULT), CONTROLLER_TYPE_MULTI_AXIS bleGamepadConfig.setAxesMin(JOYSTICK_MIN_VALUE); // 0 --> int16_t - 16 bit signed integer - Can be in decimal or hexadecimal bleGamepadConfig.setAxesMax(JOYSTICK_MAX_VALUE); // 32767 --> int16_t - 16 bit signed integer - Can be in decimal or hexadecimal //bleGamepadConfig.setWhichSpecialButtons(false, false, false, false, false, false, false, false); - bleGamepadConfig.setWhichAxes(true, true, true, true, true, true, true, true); - //bleGamepadConfig.setWhichSimulationControls(true, true, true, true, true); // only brake active + //bleGamepadConfig.setWhichAxes(false, false, false, false, false, false, false, false); + bleGamepadConfig.setWhichSimulationControls(false, false, false, true, false); // only brake active bleGamepadConfig.setButtonCount(0); bleGamepadConfig.setHatSwitchCount(0); bleGamepadConfig.setAutoReport(false); bleGamepadConfig.setPid(chip); // product id bleGamepad.begin(&bleGamepadConfig); - - //bleGamepad.deviceManufacturer = "DiyFfbPedal"; - //bleGamepad.deviceName = chip; } bool IsControllerReady() { return bleGamepad.isConnected(); } void SetControllerOutputValue(int32_t value) { - //bleGamepad.setBrake(value); if (bleGamepad.isConnected() ) { - //bleGamepad.setAxes(value, 0, 0, 0, 0, 0, 0, 0); - bleGamepad.setX(value); - //bleGamepad.setSimulationControls(value, 0, 0, 0, 0); - //bleGamepad.setSliders(value,0); + // bleGamepad.setBrake(value); + bleGamepad.setAxes(0, 0, 0, 0, value, 0, 0, 0); bleGamepad.sendReport(); } else diff --git a/Arduino/Esp32/Main/Main.h b/Arduino/Esp32/Main/Main.h index 76288b14..9d68aa68 100644 --- a/Arduino/Esp32/Main/Main.h +++ b/Arduino/Esp32/Main/Main.h @@ -46,7 +46,7 @@ /********************************************************************/ /* Loadcell defines */ /********************************************************************/ -#define LOADCELL_WEIGHT_RATING_KG 300.0 +#define LOADCELL_WEIGHT_RATING_KG 200.0 #define LOADCELL_EXCITATION_V 5.0 #define LOADCELL_SENSITIVITY_MV_V 2.0 diff --git a/Arduino/Esp32/Main/Main.ino b/Arduino/Esp32/Main/Main.ino index b03c9018..6986102b 100644 --- a/Arduino/Esp32/Main/Main.ino +++ b/Arduino/Esp32/Main/Main.ino @@ -445,264 +445,252 @@ void pedalUpdateTask( void * pvParameters ) { for(;;){ + + if(!!isv57LifeSignal_b){ + // system identification mode + #ifdef ALLOW_SYSTEM_IDENTIFICATION + if (systemIdentificationMode_b == true) + { + measureStepResponse(stepper, &dap_calculationVariables_st, &dap_config_st, loadcell); + systemIdentificationMode_b = false; + } + #endif + - - // system identification mode - #ifdef ALLOW_SYSTEM_IDENTIFICATION - if (systemIdentificationMode_b == true) + // controll cycle time. Delay did not work with the multi tasking, thus this workaround was integrated + unsigned long now = micros(); + if (now - cycleTimeLastCall < PUT_TARGET_CYCLE_TIME_IN_US) // 100us = 10kHz { - measureStepResponse(stepper, &dap_calculationVariables_st, &dap_config_st, loadcell); - systemIdentificationMode_b = false; + // skip + continue; + } + { + // if target cycle time is reached, update last time + cycleTimeLastCall = now; } - #endif - - // controll cycle time. Delay did not work with the multi tasking, thus this workaround was integrated - unsigned long now = micros(); - if (now - cycleTimeLastCall < PUT_TARGET_CYCLE_TIME_IN_US) // 100us = 10kHz - { - // skip - continue; - } - { - // if target cycle time is reached, update last time - cycleTimeLastCall = now; - } + - + // print the execution time averaged over multiple cycles + #ifdef PRINT_CYCLETIME + static CycleTimer timerPU("PU cycle time"); + timerPU.Bump(); + #endif - // print the execution time averaged over multiple cycles - if (dap_config_st.payLoadPedalConfig_.debug_flags_0 & DEBUG_INFO_0_CYCLE_TIMER) - { - static CycleTimer timerPU("PU cycle time"); - timerPU.Bump(); - } - - // if a config update was received over serial, update the variables required for further computation - if (configUpdateAvailable == true) - { - if(semaphore_updateConfig!=NULL) + // if a config update was received over serial, update the variables required for further computation + if (configUpdateAvailable == true) { - - bool configWasUpdated_b = false; - // Take the semaphore and just update the config file, then release the semaphore - if(xSemaphoreTake(semaphore_updateConfig, (TickType_t)1)==pdTRUE) + if(semaphore_updateConfig!=NULL) { - Serial.println("Updating pedal config"); - configUpdateAvailable = false; - dap_config_st = dap_config_st_local; - configWasUpdated_b = true; - xSemaphoreGive(semaphore_updateConfig); - } - // update the calc params - if (true == configWasUpdated_b) + bool configWasUpdated_b = false; + // Take the semaphore and just update the config file, then release the semaphore + if(xSemaphoreTake(semaphore_updateConfig, (TickType_t)1)==pdTRUE) + { + Serial.println("Updating pedal config"); + configUpdateAvailable = false; + dap_config_st = dap_config_st_local; + configWasUpdated_b = true; + xSemaphoreGive(semaphore_updateConfig); + } + + // update the calc params + if (true == configWasUpdated_b) + { + Serial.println("Updating the calc params"); + configWasUpdated_b = false; + dap_config_st.storeConfigToEprom(dap_config_st); // store config to EEPROM + updatePedalCalcParameters(); // update the calc parameters + } + + } + else { - Serial.println("Updating the calc params"); - configWasUpdated_b = false; - dap_config_st.storeConfigToEprom(dap_config_st); // store config to EEPROM - updatePedalCalcParameters(); // update the calc parameters + semaphore_updateConfig = xSemaphoreCreateMutex(); + //Serial.println("semaphore_updateConfig == 0"); } - - } - else - { - semaphore_updateConfig = xSemaphoreCreateMutex(); - //Serial.println("semaphore_updateConfig == 0"); } - } - // 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) { + // 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) { - if (isv57LifeSignal_b && SENSORLESS_HOMING) - { - stepper->refindMinLimitSensorless(&isv57); - } - else - { - stepper->refindMinLimit(); + if (isv57LifeSignal_b && SENSORLESS_HOMING) + { + stepper->refindMinLimitSensorless(&isv57); + } + else + { + stepper->refindMinLimit(); + } + + resetPedalPosition = false; + resetServoEncoder = true; } - - resetPedalPosition = false; - resetServoEncoder = true; - } - //#define RECALIBRATE_POSITION - #ifdef RECALIBRATE_POSITION - stepper->checkLimitsAndResetIfNecessary(); - #endif + //#define RECALIBRATE_POSITION + #ifdef RECALIBRATE_POSITION + stepper->checkLimitsAndResetIfNecessary(); + #endif - // compute pedal oscillation, when ABS is active - float absForceOffset_fl32 = 0.0f; - #ifdef ABS_OSCILLATION - absForceOffset_fl32 = absOscillation.forceOffset(&dap_calculationVariables_st); - #endif + // compute pedal oscillation, when ABS is active + float absForceOffset_fl32 = 0.0f; + #ifdef ABS_OSCILLATION + absForceOffset_fl32 = absOscillation.forceOffset(&dap_calculationVariables_st); + #endif - // compute the pedal incline angle - //#define COMPUTE_PEDAL_INCLINE_ANGLE - #ifdef COMPUTE_PEDAL_INCLINE_ANGLE - float sledPosition = sledPositionInMM(stepper); - float pedalInclineAngle = pedalInclineAngleDeg(sledPosition, dap_config_st); + // compute the pedal incline angle + //#define COMPUTE_PEDAL_INCLINE_ANGLE + #ifdef COMPUTE_PEDAL_INCLINE_ANGLE + float sledPosition = sledPositionInMM(stepper); + float pedalInclineAngle = pedalInclineAngleDeg(sledPosition, dap_config_st); - // compute angular velocity & acceleration of incline angke - float pedalInclineAngle_Accel = pedalInclineAngleAccel(pedalInclineAngle); + // compute angular velocity & acceleration of incline angke + float pedalInclineAngle_Accel = pedalInclineAngleAccel(pedalInclineAngle); - //float legRotationalMoment = 0.0000001; - //float forceCorrection = pedalInclineAngle_Accel * legRotationalMoment; + //float legRotationalMoment = 0.0000001; + //float forceCorrection = pedalInclineAngle_Accel * legRotationalMoment; - //Serial.print(pedalInclineAngle_Accel); - //Serial.println(" "); + //Serial.print(pedalInclineAngle_Accel); + //Serial.println(" "); - #endif + #endif - // Get the loadcell reading - float loadcellReading = loadcell->getReadingKg(); + // Get the loadcell reading + float loadcellReading = loadcell->getReadingKg(); - // Do the loadcell signal filtering - float filteredReading = kalman->filteredValue(loadcellReading, 0, dap_config_st.payLoadPedalConfig_.kf_modelNoise); - float changeVelocity = kalman->changeVelocity(); + // Do the loadcell signal filtering + float filteredReading = kalman->filteredValue(loadcellReading, 0, dap_config_st.payLoadPedalConfig_.kf_modelNoise); + float changeVelocity = kalman->changeVelocity(); - // Apply FIR notch filter to reduce force oscillation caused by ABS - //#define APPLY_FIR_FILTER - #ifdef APPLY_FIR_FILTER - float filteredReading2 = firNotchFilter->filterValue(loadcellReading); - if (firCycleIncrementer > minCyclesForFirToInit) - { - filteredReading = filteredReading2; - } - else - { - firCycleIncrementer++; - } + // Apply FIR notch filter to reduce force oscillation caused by ABS + //#define APPLY_FIR_FILTER + #ifdef APPLY_FIR_FILTER + float filteredReading2 = firNotchFilter->filterValue(loadcellReading); + if (firCycleIncrementer > minCyclesForFirToInit) + { + filteredReading = filteredReading2; + } + else + { + firCycleIncrementer++; + } - firCycleIncrementer++; - /*if (firCycleIncrementer % 500 == 0) - { - firCycleIncrementer = 0; - Serial.print(filteredReading); - Serial.print(", "); - Serial.print(filteredReading2); - Serial.println(" "); - }*/ + firCycleIncrementer++; + /*if (firCycleIncrementer % 500 == 0) + { + firCycleIncrementer = 0; + Serial.print(filteredReading); + Serial.print(", "); + Serial.print(filteredReading2); + Serial.println(" "); + }*/ + + #endif - #endif - - //#define DEBUG_FILTER - if (dap_config_st.payLoadPedalConfig_.debug_flags_0 & DEBUG_INFO_0_LOADCELL_READING) - { - static RTDebugOutput rtDebugFilter({ "rawReading_g", "filtered_g"}); - rtDebugFilter.offerData({ loadcellReading * 1000, filteredReading * 1000}); - } - + //#define DEBUG_FILTER + #ifdef DEBUG_FILTER + static RTDebugOutput rtDebugFilter({ "rawReading_g", "filtered_g"}); + rtDebugFilter.offerData({ loadcellReading * 1000, filteredReading * 1000}); + #endif + - /*#ifdef ABS_OSCILLATION - filteredReading += forceAbsOffset; - #endif*/ + /*#ifdef ABS_OSCILLATION + filteredReading += forceAbsOffset; + #endif*/ - // use interpolation to determine local linearized spring stiffness - double stepperPosFraction = stepper->getCurrentPositionFraction(); - //double stepperPosFraction2 = stepper->getCurrentPositionFractionFromExternalPos( -(int32_t)(isv57.servo_pos_given_p + isv57.servo_pos_error_p - isv57.getZeroPos()) ); - //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); + // use interpolation to determine local linearized spring stiffness + double stepperPosFraction = stepper->getCurrentPositionFraction(); + //double stepperPosFraction2 = stepper->getCurrentPositionFractionFromExternalPos( -(int32_t)(isv57.servo_pos_given_p + isv57.servo_pos_error_p - isv57.getZeroPos()) ); + //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 - if (dap_config_st.payLoadPedalConfig_.debug_flags_0 & DEBUG_INFO_0_STEPPER_POS) - { - static RTDebugOutput rtDebugFilter({ "ESP_pos", "ESP_tar_pos", "ISV_pos", "frac1"}); - rtDebugFilter.offerData({ stepper->getCurrentPositionSteps(), Position_Next, -(int32_t)(isv57.servo_pos_given_p + isv57.servo_pos_error_p - isv57.getZeroPos()), (int32_t)(stepperPosFraction * 10000.)}); - } - - - //stepper->printStates(); - + //#define DEBUG_STEPPER_POS + #ifdef DEBUG_STEPPER_POS + static RTDebugOutput rtDebugFilter({ "ESP_pos", "ESP_tar_pos", "ISV_pos", "frac1", "frac2"}); + rtDebugFilter.offerData({ stepper->getCurrentPositionSteps(), Position_Next, -(int32_t)(isv57.servo_pos_given_p + isv57.servo_pos_error_p - isv57.getZeroPos()), (int32_t)(stepperPosFraction * 10000.), (int32_t)(stepperPosFraction2 * 10000.)}); + #endif - // add dampening - if (dap_calculationVariables_st.dampingPress > 0.0001) - { - // dampening is proportional to velocity --> D-gain for stability - Position_Next -= dap_calculationVariables_st.dampingPress * changeVelocity * dap_calculationVariables_st.springStiffnesssInv; - } + + //stepper->printStates(); + // add dampening + if (dap_calculationVariables_st.dampingPress > 0.0001) + { + // dampening is proportional to velocity --> D-gain for stability + Position_Next -= dap_calculationVariables_st.dampingPress * changeVelocity * dap_calculationVariables_st.springStiffnesssInv; + } + - - - // clip target position to configured target interval - Position_Next = (int32_t)constrain(Position_Next, dap_calculationVariables_st.stepperPosMin, dap_calculationVariables_st.stepperPosMax); + - // if pedal in min position, recalibrate position - #ifdef ISV_COMMUNICATION - // Take the semaphore and just update the config file, then release the semaphore - if(xSemaphoreTake(semaphore_resetServoPos, (TickType_t)1)==pdTRUE) - { - if (stepper->isAtMinPos()) + + // clip target position to configured target interval + Position_Next = (int32_t)constrain(Position_Next, dap_calculationVariables_st.stepperPosMin, dap_calculationVariables_st.stepperPosMax); + + // if pedal in min position, recalibrate position + #ifdef ISV_COMMUNICATION + // Take the semaphore and just update the config file, then release the semaphore + if(xSemaphoreTake(semaphore_resetServoPos, (TickType_t)1)==pdTRUE) { - stepper->correctPos(servo_offset_compensation_steps_i32); - servo_offset_compensation_steps_i32 = 0; + if (stepper->isAtMinPos()) + { + stepper->correctPos(servo_offset_compensation_steps_i32); + servo_offset_compensation_steps_i32 = 0; + } + xSemaphoreGive(semaphore_resetServoPos); } - xSemaphoreGive(semaphore_resetServoPos); - } - #endif + #endif - // get current stepper position right before sheduling a new move - //int32_t stepperPosCurrent = stepper->getCurrentPositionSteps(); - //int32_t stepperPosCurrent = stepper->getTargetPositionSteps(); - //int32_t movement = abs(stepperPosCurrent - Position_Next); - //if (movement > MIN_STEPS) - { - stepper->moveTo(Position_Next, false); - } + // get current stepper position right before sheduling a new move + //int32_t stepperPosCurrent = stepper->getCurrentPositionSteps(); + //int32_t stepperPosCurrent = stepper->getTargetPositionSteps(); + //int32_t movement = abs(stepperPosCurrent - Position_Next); + //if (movement > MIN_STEPS) + { + stepper->moveTo(Position_Next, false); + } - + - // compute controller output - if(semaphore_updateJoystick!=NULL) - { - if(xSemaphoreTake(semaphore_updateJoystick, (TickType_t)1)==pdTRUE) { - joystickNormalizedToInt32 = NormalizeControllerOutputValue(loadcellReading, dap_calculationVariables_st.Force_Min, dap_calculationVariables_st.Force_Max, dap_config_st.payLoadPedalConfig_.maxGameOutput); - //joystickNormalizedToInt32 = NormalizeControllerOutputValue(filteredReading, dap_calculationVariables_st.Force_Min, dap_calculationVariables_st.Force_Max, dap_config_st.payLoadPedalConfig_.maxGameOutput); - xSemaphoreGive(semaphore_updateJoystick); - if(Using_analog_output_ =1) - { - int dac_value=(int)(joystickNormalizedToInt32*255/10000); - dacWrite(D_O,dac_value); + // compute controller output + if(semaphore_updateJoystick!=NULL) + { + if(xSemaphoreTake(semaphore_updateJoystick, (TickType_t)1)==pdTRUE) { + joystickNormalizedToInt32 = NormalizeControllerOutputValue(loadcellReading, dap_calculationVariables_st.Force_Min, dap_calculationVariables_st.Force_Max, dap_config_st.payLoadPedalConfig_.maxGameOutput); + //joystickNormalizedToInt32 = NormalizeControllerOutputValue(filteredReading, dap_calculationVariables_st.Force_Min, dap_calculationVariables_st.Force_Max, dap_config_st.payLoadPedalConfig_.maxGameOutput); + xSemaphoreGive(semaphore_updateJoystick); } - if(dap_config_st.payLoadPedalConfig_.Simulate_ABS_trigger==1) - { - int32_t ABS_trigger_value=dap_config_st.payLoadPedalConfig_.Simulate_ABS_value*100; - if(joystickNormalizedToInt32 > ABS_trigger_value) - { - absOscillation.trigger(); - } - } } - } - else - { - semaphore_updateJoystick = xSemaphoreCreateMutex(); - //Serial.println("semaphore_updateJoystick == 0"); - } + else + { + semaphore_updateJoystick = xSemaphoreCreateMutex(); + //Serial.println("semaphore_updateJoystick == 0"); + } + + #ifdef PRINT_USED_STACK_SIZE + unsigned int temp2 = uxTaskGetStackHighWaterMark(nullptr); + Serial.print("PU task stack size="); Serial.println(temp2); + #endif - #ifdef PRINT_USED_STACK_SIZE - unsigned int temp2 = uxTaskGetStackHighWaterMark(nullptr); - Serial.print("PU task stack size="); Serial.println(temp2); - #endif + }else{ + delay(100); + } } } @@ -938,6 +926,8 @@ int64_t timeDiff = 0; #define TIME_SINCE_SERVO_POS_CHANGE_TO_DETECT_STANDSTILL_IN_MS 200 +#define SERVO_CHECKING_PERIOD 2000 +int64_t prevCheckTime = 0; uint64_t print_cycle_counter_u64 = 0; @@ -955,6 +945,13 @@ void servoCommunicationTask( void * pvParameters ) if (isv57LifeSignal_b) { + //Checking Servo Communication + int64_t nowCheckTime = millis(); + if(nowCheckTime - prevCheckTime > SERVO_CHECKING_PERIOD){ + isv57LifeSignal_b = isv57.checkCommunication(); + prevCheckTime = nowCheckTime; + } + delay(20); //isv57.readServoStates(); @@ -1082,6 +1079,36 @@ void servoCommunicationTask( void * pvParameters ) else { delay(1000); + +#ifdef ISV_COMMUNICATION + if(!isv57LifeSignal_b){ + Serial.println("Try to connecto to servo..."); + // check whether iSV57 is connected + bool servoCommuState = isv57.checkCommunication(); + + Serial.print("iSV57 communication state: "); + Serial.println(servoCommuState); + + if (servoCommuState && SENSORLESS_HOMING) + { + isv57.setupServoStateReading(); + // isv57.sendTunedServoParameters(); + } + delay(200); + if (servoCommuState && SENSORLESS_HOMING) + { + stepper->findMinMaxSensorless(&isv57); + delay(500); + isv57LifeSignal_b = true; + Serial.print("FindMinMax Done:"); + Serial.println(isv57LifeSignal_b); + } + + + } +#endif + + }