diff --git a/Inc/comms.h b/Inc/comms.h index b723283..6bdb730 100644 --- a/Inc/comms.h +++ b/Inc/comms.h @@ -28,7 +28,6 @@ void setScopeChannel(uint8_t ch, int16_t val); void consoleScope(void); void consoleLog(char *message); -void print( const char * format, ... ); #endif diff --git a/Inc/config.h b/Inc/config.h index 449c9fb..a287a96 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -434,6 +434,7 @@ #define USART2_BAUD 115200 #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + // #define DEBUG_SERIAL_USART3 // right sensor cable debug #endif // ############################# END OF VARIANT_IBUS SETTINGS ############################ diff --git a/Inc/util.h b/Inc/util.h index cca2489..ee2cba6 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -67,9 +67,10 @@ void shortBeepMany(uint8_t cnt, int8_t dir); void longBeep(uint8_t freq); void calcAvgSpeed(void); void adcCalibLim(void); +int checkInputType(int16_t min, int16_t mid, int16_t max); void updateCurSpdLim(void); void saveConfig(void); -int addDeadBand(int16_t u, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max); +int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max); void standstillHold(int16_t *speedCmd); void electricBrake(uint16_t speedBlend, uint8_t reverseDir); void cruiseControl(uint8_t button); diff --git a/Src/comms.c b/Src/comms.c index 881b2cc..d1626e5 100644 --- a/Src/comms.c +++ b/Src/comms.c @@ -80,28 +80,3 @@ void consoleLog(char *message) #endif #endif } - -void print( const char * format, ... ) -{ - va_list args; - va_start (args, format); - - static volatile uint8_t buffer[100]; - int strLength; - strLength = vsprintf((char *)(uintptr_t)buffer,format, args); - - #ifdef DEBUG_SERIAL_USART2 - while(__HAL_DMA_GET_COUNTER(huart2.hdmatx) > 0) { - HAL_Delay(1); - } - HAL_UART_Transmit_DMA(&huart2, (uint8_t *)buffer, strLength); - #endif - #ifdef DEBUG_SERIAL_USART3 - while(__HAL_DMA_GET_COUNTER(huart3.hdmatx) > 0) { - HAL_Delay(1); - } - HAL_UART_Transmit_DMA(&huart3, (uint8_t *)buffer, strLength); - #endif - - va_end (args); -} \ No newline at end of file diff --git a/Src/main.c b/Src/main.c index 7f3384d..09e156f 100644 --- a/Src/main.c +++ b/Src/main.c @@ -409,10 +409,10 @@ int main(void) { // ####### DEBUG SERIAL OUT ####### #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms - setScopeChannel(0, (int16_t)input1); // 1: ADC1 - setScopeChannel(1, (int16_t)input2); // 2: ADC2 - setScopeChannel(2, (int16_t)cmd1); //speedR); // 3: output command: [-1000, 1000] - setScopeChannel(3, (int16_t)cmd2); //speedL); // 4: output command: [-1000, 1000] + setScopeChannel(0, (int16_t)input1); // 1: INPUT1 + setScopeChannel(1, (int16_t)input2); // 2: INPUT2 + setScopeChannel(2, (int16_t)speedR); // 3: output command: [-1000, 1000] + setScopeChannel(3, (int16_t)speedL); // 4: output command: [-1000, 1000] setScopeChannel(4, (int16_t)adc_buffer.batt1); // 5: for battery voltage calibration setScopeChannel(5, (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); // 6: for verifying battery voltage calibration setScopeChannel(6, (int16_t)board_temp_adcFilt); // 7: for board temperature calibration diff --git a/Src/util.c b/Src/util.c index 9630769..e7de42c 100644 --- a/Src/util.c +++ b/Src/util.c @@ -455,86 +455,87 @@ void adcCalibLim(void) { filtLowPass32(input1, FILTER, &input1_fixdt); filtLowPass32(input2, FILTER, &input2_fixdt); - INPUT1_MID_temp = (int16_t)CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer - INPUT2_MID_temp = (int16_t)CLAMP(input2_fixdt >> 16, INPUT2_MIN, INPUT2_MAX); + INPUT1_MID_temp = (int16_t)(input1_fixdt >> 16);// CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer + INPUT2_MID_temp = (int16_t)(input2_fixdt >> 16);// CLAMP(input2_fixdt >> 16, INPUT2_MIN, INPUT2_MAX); INPUT1_MIN_temp = MIN(INPUT1_MIN_temp, INPUT1_MID_temp); INPUT1_MAX_temp = MAX(INPUT1_MAX_temp, INPUT1_MID_temp); INPUT2_MIN_temp = MIN(INPUT2_MIN_temp, INPUT2_MID_temp); INPUT2_MAX_temp = MAX(INPUT2_MAX_temp, INPUT2_MID_temp); HAL_Delay(5); } - + uint16_t input_margin = 0; #ifdef CONTROL_ADC input_margin = 100; #endif - int16_t threshold = (INPUT1_MAX - INPUT1_MIN) / 10; INPUT1_MIN_CAL = INPUT1_MIN_temp + input_margin; INPUT1_MID_CAL = INPUT1_MID_temp; INPUT1_MAX_CAL = INPUT1_MAX_temp - input_margin; - if ( (INPUT1_MIN_temp/threshold) == (INPUT1_MAX_temp/threshold) ){ - // MIN MID and MAX are close, there is no input - INPUT1_TYPE_CAL = 0; - consoleLog("Input1 is ignored"); - } else { - if ( (INPUT1_MIN_temp/threshold) == (INPUT1_MID_temp/threshold) ){ - // MIN and MID are close, it's a normal pot - INPUT1_TYPE_CAL = 1; - consoleLog("Input1 is a normal pot"); - }else { - INPUT1_TYPE_CAL = 2; - consoleLog("Input1 is a mid-resting pot"); - } + INPUT1_TYPE_CAL = checkInputType(INPUT1_MIN_CAL,INPUT1_MID_CAL,INPUT1_MAX_CAL); - HAL_Delay(10); - #ifdef CONTROL_ADC - if ( ((int16_t)INPUT1_MIN_CAL - ADC_PROTECT_THRESH) > 0 && ((int16_t)INPUT1_MAX_CAL + ADC_PROTECT_THRESH) < 4095){ - consoleLog(" and protected"); - } - #endif - input_cal_valid = 1; + INPUT2_MIN_CAL = INPUT2_MIN_temp + input_margin; + INPUT2_MID_CAL = INPUT2_MID_temp; + INPUT2_MAX_CAL = INPUT2_MAX_temp - input_margin; + INPUT2_TYPE_CAL = checkInputType(INPUT2_MIN_CAL,INPUT2_MID_CAL,INPUT2_MAX_CAL); + + consoleLog("Saved limits\n"); + HAL_Delay(10); + setScopeChannel(0, (int16_t)INPUT1_MIN_CAL); + setScopeChannel(1, (uint16_t)INPUT1_MID_CAL); + setScopeChannel(2, (int16_t)INPUT1_MAX_CAL); + setScopeChannel(3, (int16_t)0); + setScopeChannel(4, (int16_t)INPUT2_MIN_CAL); + setScopeChannel(5, (uint16_t)INPUT2_MID_CAL); + setScopeChannel(6, (int16_t)INPUT2_MAX_CAL); + setScopeChannel(7, (int16_t)0); + consoleScope(); + HAL_Delay(20); + consoleLog("OK\n"); + + #endif +} + +int checkInputType(int16_t min, int16_t mid, int16_t max){ + + HAL_Delay(10); + + int type = 0; + + // Threshold to define if values are too close + int16_t threshold = 200; + #ifdef CONTROL_ADC + threshold = 400; + #endif + + if ( (min/threshold) == (max/threshold) ){ + // MIN MID and MAX are close, disable input + consoleLog("Input is ignored"); + type = 0; + } else { + if ( (min/threshold) == (mid/threshold) ){ + // MIN and MID are close, it's a normal pot + consoleLog("Input is a normal pot"); + type = 1; + }else { + // it's a mid resting pot + consoleLog("Input is a mid-resting pot"); + type = 2; + } + + HAL_Delay(10); + #ifdef CONTROL_ADC + if ( (min - ADC_PROTECT_THRESH) > 0 && (max + ADC_PROTECT_THRESH) < 4095){ + consoleLog(" and protected"); + } + #endif } HAL_Delay(10); consoleLog("\n"); HAL_Delay(10); - - threshold = (INPUT2_MAX - INPUT2_MIN) / 10; - INPUT2_MIN_CAL = INPUT2_MIN_temp + input_margin; - INPUT2_MID_CAL = INPUT2_MID_temp; - INPUT2_MAX_CAL = INPUT2_MAX_temp - input_margin; - if ( (INPUT2_MIN_temp/threshold) == (INPUT2_MAX_temp/threshold) ){ - // MIN MID and MAX are close, there is no input - INPUT2_TYPE_CAL = 0; - consoleLog("Input2 is ignored"); - } else { - if ( (INPUT2_MIN_temp/threshold) == (INPUT2_MID_temp/threshold) ){ - // MIN and MID are close, it's a normal pot - INPUT2_TYPE_CAL = 1; - consoleLog("Input2 is a normal pot"); - }else { - INPUT2_TYPE_CAL = 2; - consoleLog("Input2 is a mid-resting pot"); - } - - HAL_Delay(10); - - #ifdef CONTROL_ADC - if ( ((int16_t)INPUT2_MIN_CAL - ADC_PROTECT_THRESH) > 0 && ((int16_t)INPUT2_MAX_CAL + ADC_PROTECT_THRESH) < 4095 ){ - consoleLog(" and protected"); - } - #endif - input_cal_valid = 1; - } - - print("\n"); - print("Saved limits\n"); - print("INPUT1_MIN:%i INPUT1_MID:%i INPUT1_MAX:%i\n", (int16_t)INPUT1_MIN_CAL,(int16_t)INPUT1_MID_CAL,(int16_t)INPUT1_MAX_CAL); - print("INPUT2_MIN:%i INPUT2_MID:%i INPUT2_MAX:%i\n", (int16_t)INPUT2_MIN_CAL,(int16_t)INPUT2_MID_CAL,(int16_t)INPUT2_MAX_CAL); - print("OK\n"); - #endif + return type; } @@ -571,13 +572,13 @@ void updateCurSpdLim(void) { 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 spd_factor = CLAMP((input2_fixdt - ((int16_t)INPUT2_MIN_CAL << 16)) / ((int16_t)INPUT2_MAX_CAL - (int16_t)INPUT2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm - if (INPUT1_TYPE != 0){ + if (INPUT1_TYPE_CAL != 0){ // Update current limit rtP_Right.i_max = rtP_Left.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4) cur_spd_valid = 1; } - if (INPUT2_TYPE != 0){ + if (INPUT2_TYPE_CAL != 0){ // Update speed limit rtP_Right.n_max = rtP_Left.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4) cur_spd_valid = 1; @@ -635,13 +636,22 @@ void saveConfig() { * Add Dead-band to a signal * This function realizes a dead-band around 0 and scales the input between [out_min, out_max] */ -int addDeadBand(int16_t u, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max) { - if( u > in_mid - deadBand && u < in_mid + deadBand ) { - return 0; - } else if(u > in_mid) { - return CLAMP( MAP(u, in_mid + deadBand, in_max, 0, out_max), 0 , out_max); - } else { - return CLAMP( MAP(u, in_mid - deadBand, in_min, 0, out_min), out_min, 0); +int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max) { + switch (type){ + case 0: // Input is ignored + return 0; + case 1: // Input is a normal pot + return CLAMP(MAP( u , in_min, in_max, 0, out_max ), 0, out_max); + case 2: // Input is a mid resting pot + if( u > in_mid - deadBand && u < in_mid + deadBand ) { + return 0; + } else if(u > in_mid) { + return CLAMP( MAP(u, in_mid + deadBand, in_max, 0, out_max), 0 , out_max); + } else { + return CLAMP( MAP(u, in_mid - deadBand, in_min, 0, out_min), out_min, 0); + } + default: + return 0; } } @@ -896,38 +906,11 @@ void readCommand(void) { #endif #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - switch (INPUT1_TYPE_CAL){ - case 0: // Input1 is ignored - cmd1 = 0; - break; - case 1: // Input1 is a normal pot - cmd1 = CLAMP(MAP( input1 , INPUT1_MIN_CAL, INPUT1_MAX_CAL, 0, INPUT_MAX ), 0, INPUT_MAX); - break; - case 2: // Input1 is a mid resting pot - cmd1 = addDeadBand(input1, INPUT1_DEADBAND, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL, INPUT_MIN, INPUT_MAX); - break; - default: - cmd1 = 0; - break; - } - + cmd1 = addDeadBand(input1, INPUT1_TYPE_CAL, INPUT1_DEADBAND, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL, INPUT_MIN, INPUT_MAX); #if !defined(VARIANT_SKATEBOARD) - switch (INPUT2_TYPE_CAL){ - case 0: // Input2 is ignored - cmd2 = 0; - break; - case 1: // Input2 is a normal pot - cmd2 = CLAMP(MAP( input2 , INPUT2_MIN_CAL, INPUT2_MAX_CAL, 0, INPUT_MAX ), 0, INPUT_MAX); - break; - case 2: // Input2 is a mid resting pot - cmd2 = addDeadBand(input2, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT_MIN, INPUT_MAX); - break; - default: - cmd2 = 0; - break; - } + cmd2 = addDeadBand(input2, INPUT2_TYPE_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT_MIN, INPUT_MAX); #else - cmd2 = addDeadBand(input2, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT2_OUT_MIN, INPUT_MAX); + cmd2 = addDeadBand(input2, INPUT2_TYPE_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT2_OUT_MIN, INPUT_MAX); #endif #endif diff --git a/platformio.ini b/platformio.ini index 2a45d1b..9926883 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,7 +11,7 @@ src_dir = Src ; ;default_envs = VARIANT_ADC ; Variant for control via ADC input ;default_envs = VARIANT_USART ; Variant for Serial control via USART3 input -default_envs = VARIANT_NUNCHUK ; Variant for Nunchuk controlled vehicle build +;default_envs = VARIANT_NUNCHUK ; Variant for Nunchuk controlled vehicle build ;default_envs = VARIANT_PPM ; Variant for RC-Remotes with PPM-Sum signal ;default_envs = VARIANT_PWM ; Variant for RC-Remotes with PWM signal ;default_envs = VARIANT_IBUS ; Variant for RC-Remotes with FLYSKY IBUS