From 44cf0c8f1ff86b72ed6adddd79403fa6b2878b75 Mon Sep 17 00:00:00 2001 From: EmanuelFeru Date: Thu, 19 Nov 2020 17:49:14 +0100 Subject: [PATCH] update Auto-calibration - implemented forced potentiometer type - extended the INPUTx_TYPE to 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect --- Inc/config.h | 43 ++++++++++++++++------------- Src/util.c | 78 +++++++++++++++++++++++++++------------------------- 2 files changed, 64 insertions(+), 57 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index c752b0f..1f9dc87 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -253,15 +253,15 @@ #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! #define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken #define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095) - #define INPUT1_MID 2048 // mid ADC1-value while poti at minimum-position (ADC1_MIN - ADC1_MAX) + #define INPUT1_MID 0 // mid ADC1-value while poti at minimum-position (ADC1_MIN - ADC1_MAX) #define INPUT1_MAX 4095 // max ADC1-value while poti at maximum-position (0 - 4095) #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT2_MIN 0 // min ADC2-value while poti at minimum-position (0 - 4095) - #define INPUT2_MID 2048 // mid ADC2-value while poti at minimum-position (ADC2_MIN - ADC2_MAX) + #define INPUT2_MID 0 // mid ADC2-value while poti at minimum-position (ADC2_MIN - ADC2_MAX) #define INPUT2_MAX 4095 // max ADC2-value while poti at maximum-position (0 - 4095) #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! @@ -281,13 +281,13 @@ #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT1_MIN -1000 // (-1000 - 0) #define INPUT1_MID 0 #define INPUT1_MAX 1000 // (0 - 1000) #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT2_MIN -1000 // (-1000 - 0) #define INPUT2_MID 0 #define INPUT2_MAX 1000 // (0 - 1000) @@ -309,13 +309,13 @@ */ #define CONTROL_NUNCHUK // use nunchuk as input. disable FEEDBACK_SERIAL_USART3, DEBUG_SERIAL_USART3! // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT1_MIN -1024 // (-1024 - 0) #define INPUT1_MID 0 #define INPUT1_MAX 1024 // (0 - 1024) #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT2_MIN -1024 // (-1024 - 0) #define INPUT2_MID 0 #define INPUT2_MAX 1024 // (0 - 1024) @@ -346,13 +346,13 @@ #endif #define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used. // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT1_MIN -1000 // (-1000 - 0) - #define INPUT1_MID 0 + #define INPUT1_MID 3 #define INPUT1_MAX 1000 // (0 - 1000) #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT2_MIN -1000 // (-1000 - 0) #define INPUT2_MID 0 #define INPUT2_MAX 1000 // (0 - 1000) @@ -378,13 +378,13 @@ #define DEBUG_SERIAL_USART3 // right sensor cable debug #endif // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT1_MIN -1000 // (-1000 - 0) #define INPUT1_MID 0 #define INPUT1_MAX 1000 // (0 - 1000) #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT2_MIN -1000 // (-1000 - 0) #define INPUT2_MID 0 #define INPUT2_MAX 1000 // (0 - 1000) @@ -419,13 +419,13 @@ // #define DEBUG_SERIAL_USART3 // right sensor cable debug // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT1_MIN -1000 // (-1000 - 0) #define INPUT1_MID 0 #define INPUT1_MAX 1000 // (0 - 1000) #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT2_MIN -1000 // (-1000 - 0) #define INPUT2_MID 0 #define INPUT2_MAX 1000 // (0 - 1000) @@ -443,13 +443,13 @@ #define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken #define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values - #define INPUT1_TYPE 1 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT1_MIN 1000 // min ADC1-value while poti at minimum-position (0 - 4095) #define INPUT1_MID 0 #define INPUT1_MAX 2500 // max ADC1-value while poti at maximum-position (0 - 4095) #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 1 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT2_MIN 500 // min ADC2-value while poti at minimum-position (0 - 4095) #define INPUT2_MID 0 #define INPUT2_MAX 2200 // max ADC2-value while poti at maximum-position (0 - 4095) @@ -527,13 +527,13 @@ #define DEBUG_SERIAL_USART3 // right sensor cable debug #endif // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 0 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_TYPE 0 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT1_MIN -1000 // (-1000 - 0) #define INPUT1_MID 0 #define INPUT1_MAX 1000 // (0 - 1000) #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define INPUT2_MIN -800 // (-1000 - 0) #define INPUT2_MID 0 #define INPUT2_MAX 700 // (0 - 1000) @@ -587,6 +587,11 @@ #ifndef STEER_COEFFICIENT #define STEER_COEFFICIENT DEFAULT_STEER_COEFFICIENT #endif +#ifdef CONTROL_ADC + #define INPUT_MARGIN 100 // Input margin applied on the raw ADC min and max to make sure the motor MIN and MAX values are reached even in the presence of noise +#else + #define INPUT_MARGIN 0 +#endif // ########################### END OF APPLY DEFAULT SETTING ############################ diff --git a/Src/util.c b/Src/util.c index 8c9177f..bd795f1 100644 --- a/Src/util.c +++ b/Src/util.c @@ -292,6 +292,9 @@ void Input_Init(void) { rtP_Left.n_max = n_max; rtP_Right.i_max = i_max; rtP_Right.n_max = n_max; + } else { // Else If Input type is 3 (auto), identify the input type based on the values from config.h + if (INPUT1_TYPE == 3) { INPUT1_TYP_CAL = checkInputType(INPUT1_MIN, INPUT1_MID, INPUT1_MAX); } + if (INPUT2_TYPE == 3) { INPUT2_TYP_CAL = checkInputType(INPUT2_MIN, INPUT2_MID, INPUT2_MAX); } } HAL_FLASH_Lock(); #endif @@ -453,22 +456,22 @@ int checkInputType(int16_t min, int16_t mid, int16_t max){ #endif HAL_Delay(10); - if ((min / threshold) == (max / threshold)) { - consoleLog("Input is ignored"); // MIN and MAX are close, disable input + if ((min / threshold) == (max / threshold) || (mid / threshold) == (max / threshold)) { type = 0; + consoleLog("Input is ignored"); // (MIN and MAX) OR (MID and MAX) are close, disable input } else { if ((min / threshold) == (mid / threshold)){ - consoleLog("Input is a normal pot"); // MIN and MID are close, it's a normal pot type = 1; - } else { + consoleLog("Input is a normal pot"); // MIN and MID are close, it's a normal pot + } else { + type = 2; consoleLog("Input is a mid-resting pot"); // it's a mid resting pot - type = 2; } HAL_Delay(10); #ifdef CONTROL_ADC - if ((min - ADC_PROTECT_THRESH) > 0 && (max + ADC_PROTECT_THRESH) < 4095) { + if ((min + INPUT_MARGIN - ADC_PROTECT_THRESH) > 0 && (max - INPUT_MARGIN + ADC_PROTECT_THRESH) < 4095) { consoleLog(" and protected"); - shortBeep(2); // Indicate protection by a beep + longBeep(2); // Indicate protection by a beep } #endif } @@ -494,11 +497,11 @@ void adcCalibLim(void) { return; } - #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) + #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) consoleLog("Input calibration started...\n"); readInput(); - // Inititalization: MIN = a high value, MAX = a low value + // Inititalization: MIN = a high value, MAX = a low value int32_t input1_fixdt = input1 << 16; int32_t input2_fixdt = input2 << 16; int16_t INPUT1_MIN_temp = MAX_int16_T; @@ -524,37 +527,39 @@ void adcCalibLim(void) { HAL_Delay(5); } - #ifdef CONTROL_ADC - int16_t input_margin = 100; - #else - int16_t input_margin = 0; - #endif - - INPUT1_MIN_CAL = INPUT1_MIN_temp + input_margin; - INPUT1_MID_CAL = INPUT1_MID_temp; - INPUT1_MAX_CAL = INPUT1_MAX_temp - input_margin; - INPUT1_TYP_CAL = checkInputType(INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL); + INPUT1_TYP_CAL = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp); + if (INPUT1_TYP_CAL == INPUT1_TYPE || INPUT1_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto) + INPUT1_MIN_CAL = INPUT1_MIN_temp + INPUT_MARGIN; + INPUT1_MID_CAL = INPUT1_MID_temp; + INPUT1_MAX_CAL = INPUT1_MAX_temp - INPUT_MARGIN; + consoleLog("Input1 OK\n"); HAL_Delay(10); + } else { + INPUT1_TYP_CAL = 0; // Disable input + consoleLog("Input1 Fail\n"); HAL_Delay(10); + } - INPUT2_MIN_CAL = INPUT2_MIN_temp + input_margin; - INPUT2_MID_CAL = INPUT2_MID_temp; - INPUT2_MAX_CAL = INPUT2_MAX_temp - input_margin; - INPUT2_TYP_CAL = checkInputType(INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL); - - inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown - consoleLog("Saved limits\n"); - HAL_Delay(10); + INPUT2_TYP_CAL = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp); + if (INPUT2_TYP_CAL == INPUT2_TYPE || INPUT2_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto) + INPUT2_MIN_CAL = INPUT2_MIN_temp + INPUT_MARGIN; + INPUT2_MID_CAL = INPUT2_MID_temp; + INPUT2_MAX_CAL = INPUT2_MAX_temp - INPUT_MARGIN; + consoleLog("Input2 OK\n"); HAL_Delay(10); + } else { + INPUT2_TYP_CAL = 0; // Disable input + consoleLog("Input2 Fail\n"); HAL_Delay(10); + } + + inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown + consoleLog("Limits: "); HAL_Delay(10); setScopeChannel(0, (int16_t)INPUT1_TYP_CAL); setScopeChannel(1, (int16_t)INPUT1_MIN_CAL); setScopeChannel(2, (int16_t)INPUT1_MID_CAL); setScopeChannel(3, (int16_t)INPUT1_MAX_CAL); - setScopeChannel(4, (int16_t)INPUT2_TYP_CAL); + setScopeChannel(4, (int16_t)INPUT2_TYP_CAL); setScopeChannel(5, (int16_t)INPUT2_MIN_CAL); setScopeChannel(6, (int16_t)INPUT2_MID_CAL); - setScopeChannel(7, (int16_t)INPUT2_MAX_CAL); + setScopeChannel(7, (int16_t)INPUT2_MAX_CAL); consoleScope(); - HAL_Delay(20); - consoleLog("OK\n"); - #endif } /* @@ -584,7 +589,7 @@ void updateCurSpdLim(void) { readInput(); filtLowPass32(input1, FILTER, &input1_fixdt); filtLowPass32(input2, FILTER, &input2_fixdt); - HAL_Delay(5); + HAL_Delay(5); } // Calculate scaling factors cur_factor = CLAMP((input1_fixdt - ((int16_t)INPUT1_MIN_CAL << 16)) / ((int16_t)INPUT1_MAX_CAL - (int16_t)INPUT1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A @@ -602,8 +607,7 @@ void updateCurSpdLim(void) { cur_spd_valid += 2; // Mark update to be saved in Flash at shutdown } - consoleLog("Saved limits\n"); - HAL_Delay(10); + consoleLog("Limits: "); HAL_Delay(10); setScopeChannel(0, (int16_t)cur_spd_valid); // 0 = No limit changed, 1 = Current limit changed, 2 = Speed limit changed, 3 = Both limits changed setScopeChannel(1, (int16_t)input1_fixdt); setScopeChannel(2, (int16_t)cur_factor); @@ -611,10 +615,8 @@ void updateCurSpdLim(void) { setScopeChannel(4, (int16_t)0); setScopeChannel(5, (int16_t)input2_fixdt); setScopeChannel(6, (int16_t)spd_factor); - setScopeChannel(7, (int16_t)rtP_Left.n_max); + setScopeChannel(7, (int16_t)rtP_Left.n_max); consoleScope(); - HAL_Delay(20); - consoleLog("OK\n"); #endif }