diff --git a/.travis.yml b/.travis.yml index de604c4..87f8388 100644 --- a/.travis.yml +++ b/.travis.yml @@ -44,7 +44,7 @@ matrix: before_script: arm-none-eabi-gcc --version - name: platformio - script: platformio run -e VARIANT_ADC -e VARIANT_USART3 -e TRANSPOTTER + script: platformio run -e VARIANT_ADC -e VARIANT_USART3 -e HOVERCAR -e TRANSPOTTER language: python python: - "2.7" diff --git a/01_Matlab/01_Libraries/01_Controller/BLDC_controller_Lib.slx b/01_Matlab/01_Libraries/01_Controller/BLDC_controller_Lib.slx index 7d74270..73c4913 100644 Binary files a/01_Matlab/01_Libraries/01_Controller/BLDC_controller_Lib.slx and b/01_Matlab/01_Libraries/01_Controller/BLDC_controller_Lib.slx differ diff --git a/01_Matlab/02_Figures/motor_winding.png b/01_Matlab/02_Figures/motor_winding.png new file mode 100644 index 0000000..cd42738 Binary files /dev/null and b/01_Matlab/02_Figures/motor_winding.png differ diff --git a/01_Matlab/03_CreateParamTable/tableParamType.xlsx b/01_Matlab/03_CreateParamTable/tableParamType.xlsx index 20aa626..077ab20 100644 Binary files a/01_Matlab/03_CreateParamTable/tableParamType.xlsx and b/01_Matlab/03_CreateParamTable/tableParamType.xlsx differ diff --git a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx index a7b9dee..9e8e65b 100644 Binary files a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx and b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx differ diff --git a/01_Matlab/init_model.m b/01_Matlab/init_model.m index 18832d6..0e45046 100644 --- a/01_Matlab/init_model.m +++ b/01_Matlab/init_model.m @@ -89,9 +89,9 @@ cf_currFilt = 0.12; % [%] Current filter coefficient [0, 1]. Lower v %% F02_Diagnostics b_diagEna = 1; % [-] Diagnostics enable flag: 0 = Disabled, 1 = Enabled (default) -t_errQual = 0.6 * f_ctrl/3; % [s] Error qualification time -t_errDequal = 2.0 * f_ctrl/3; % [s] Error dequalification time -r_errInpTgtThres = 400; % [-] Error input target threshold (for "Blocked motor" detection) +t_errQual = 0.24 * f_ctrl/3; % [s] Error qualification time +t_errDequal = 1.80 * f_ctrl/3; % [s] Error dequalification time +r_errInpTgtThres = 600; % [-] Error input target threshold (for "Blocked motor" detection) %% F03_Control_Mode_Manager dV_openRate = 1000 / (f_ctrl/3);% [V/s] Rate for voltage cut-off in Open Mode (Sample Time included in the rate) diff --git a/Inc/BLDC_controller.h b/Inc/BLDC_controller.h index acfba7a..2bf7c56 100644 --- a/Inc/BLDC_controller.h +++ b/Inc/BLDC_controller.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'BLDC_controller'. * - * Model version : 1.1249 + * Model version : 1.1256 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Thu Dec 12 20:22:31 2019 + * C/C++ source code generated on : Mon Dec 30 21:36:12 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -43,7 +43,7 @@ typedef struct { /* Block signals and states (auto storage) for system '/Low_Pass_Filter' */ typedef struct { - int16_T UnitDelay3_DSTATE[2]; /* '/UnitDelay3' */ + int32_T UnitDelay1_DSTATE[2]; /* '/UnitDelay1' */ } DW_Low_Pass_Filter; /* Block signals and states (auto storage) for system '/I_backCalc_fixdt' */ @@ -85,7 +85,7 @@ typedef struct { int32_T Divide1; /* '/Divide1' */ int32_T UnitDelay_DSTATE; /* '/UnitDelay' */ int16_T Gain4[3]; /* '/Gain4' */ - int16_T Sum1[2]; /* '/Sum1' */ + int16_T DataTypeConversion[2]; /* '/Data Type Conversion' */ int16_T z_counterRawPrev; /* '/z_counterRawPrev' */ int16_T Merge1; /* '/Merge1' */ int16_T Divide3; /* '/Divide3' */ diff --git a/Inc/config.h b/Inc/config.h index 8dafd56..5ad463e 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -6,14 +6,14 @@ // For any particular needs, feel free to change this file according to your needs. // Select the VARIANT_ADC as default variant, in case NO variant is defined -#if !defined(VARIANT_ADC) && !defined(VARIANT_USART3) && !defined(TRANSPOTTER) +#if !defined(VARIANT_ADC) && !defined(VARIANT_USART3) && !defined(HOVERCAR) && !defined(TRANSPOTTER) #define VARIANT_ADC #endif // ############################### DO-NOT-TOUCH SETTINGS ############################### #define PWM_FREQ 16000 // PWM frequency in Hz -#define DEAD_TIME 32 // PWM deadtime +#define DEAD_TIME 48 // PWM deadtime #ifdef TRANSPOTTER #define DELAY_IN_MAIN_LOOP 2 #else @@ -55,8 +55,8 @@ * Then you can verify voltage on value 6 (to get calibrated voltage multiplied by 100). */ #define BAT_FILT_COEF 655 // battery voltage filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16 -#define BAT_CALIB_REAL_VOLTAGE 4300 // input voltage measured by multimeter (multiplied by 100). In this case 43.00 V * 100 = 4300 -#define BAT_CALIB_ADC 1704 // adc-value measured by mainboard (value nr 5 on UART debug output) +#define BAT_CALIB_REAL_VOLTAGE 3970 // input voltage measured by multimeter (multiplied by 100). In this case 43.00 V * 100 = 4300 +#define BAT_CALIB_ADC 1492 // adc-value measured by mainboard (value nr 5 on UART debug output) #define BAT_CELLS 10 // battery number of cells. Normal Hoverboard battery: 10s #define BAT_LOW_LVL1_ENABLE 0 // to beep or not to beep, 1 or 0 @@ -109,7 +109,7 @@ #define USART3_BAUD 38400 // UART3 baud rate (short wired cable) #define USART3_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B -#if defined(VARIANT_ADC) +#if defined(VARIANT_ADC) || defined(HOVERCAR) // #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino // #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! // #define DEBUG_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! @@ -149,15 +149,28 @@ * Make, flash and test it. */ #ifdef VARIANT_ADC - #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! - // #define ADC1_MID_POT // ADC1 middle resting poti: comment-out if NOT a middle resting poti - #define ADC1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095) - #define ADC1_MID 1963 // mid ADC1-value while poti at minimum-position (ADC1_MIN - ADC1_MAX) - #define ADC1_MAX 4095 // max ADC1-value while poti at maximum-position (0 - 4095) - // #define ADC2_MID_POT // ADC2 middle resting poti: comment-out if NOT a middle resting poti - #define ADC2_MIN 0 // min ADC2-value while poti at minimum-position (0 - 4095) - #define ADC2_MID 2006 // mid ADC2-value while poti at minimum-position (ADC2_MIN - ADC2_MAX) - #define ADC2_MAX 4095 // max ADC2-value while poti at maximum-position (0 - 4095) + #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + // #define ADC_PROTECT_ENA // ADC Protection Enable flag. Use this flag to make sure the ADC is protected when GND or Vcc wire is disconnected + #define ADC_PROTECT_TIMEOUT 30 // ADC Protection: number of wrong / missing input commands before safety state is taken + #define ADC_PROTECT_THRESH 400 // ADC Protection threshold below/above the MIN/MAX ADC values + // #define ADC1_MID_POT // ADC1 middle resting poti: comment-out if NOT a middle resting poti + #define ADC1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095) + #define ADC1_MID 2048 // mid ADC1-value while poti at minimum-position (ADC1_MIN - ADC1_MAX) + #define ADC1_MAX 4095 // max ADC1-value while poti at maximum-position (0 - 4095) + // #define ADC2_MID_POT // ADC2 middle resting poti: comment-out if NOT a middle resting poti + #define ADC2_MIN 0 // min ADC2-value while poti at minimum-position (0 - 4095) + #define ADC2_MID 2048 // mid ADC2-value while poti at minimum-position (ADC2_MIN - ADC2_MAX) + #define ADC2_MAX 4095 // max ADC2-value while poti at maximum-position (0 - 4095) +#endif +#ifdef HOVERCAR + #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + #define ADC_PROTECT_ENA // ADC Protection Enable flag. Use this flag to make sure the ADC is protected when GND or Vcc wire is disconnected + #define ADC_PROTECT_TIMEOUT 30 // ADC Protection: number of wrong / missing input commands before safety state is taken + #define ADC_PROTECT_THRESH 400 // ADC Protection threshold below/above the MIN/MAX ADC values + #define ADC1_MIN 1000 // min ADC1-value while poti at minimum-position (0 - 4095) + #define ADC1_MAX 2500 // max ADC1-value while poti at maximum-position (0 - 4095) + #define ADC2_MIN 500 // min ADC2-value while poti at minimum-position (0 - 4095) + #define ADC2_MAX 2200 // max ADC2-value while poti at maximum-position (0 - 4095) #endif // ###### CONTROL VIA NINTENDO NUNCHUCK ###### @@ -221,16 +234,22 @@ */ // Beep in Reverse -#define BEEPS_BACKWARD 0 // 0 or 1 +#define BEEPS_BACKWARD 1 // 0 or 1 + +// Multiple tap detection: default DOUBLE Tap (4 pulses) +#define MULTIPLE_TAP_NR 2 * 2 // [-] Define tap number: MULTIPLE_TAP_NR = number_of_taps * 2, number_of_taps = 1 (for single taping), 2 (for double tapping), 3 (for triple tapping), etc... +#define MULTIPLE_TAP_HI 600 // [-] Multiple tap detection High hysteresis threshold +#define MULTIPLE_TAP_LO 200 // [-] Multiple tap detection Low hysteresis threshold +#define MULTIPLE_TAP_TIMEOUT 2000 // [ms] Multiple tap detection Timeout period. The taps need to happen within this time window to be accepted. // Value of RATE is in fixdt(1,16,4): VAL_fixedPoint = VAL_floatingPoint * 2^4. In this case 480 = 30 * 2^4 -#define RATE 480 // 30.0f [-] lower value == slower rate [0, 32767] = [0.0, 2047.9375]. Do NOT make rate negative (>32767) +#define RATE 480 // 30.0f [-] lower value == slower rate [0, 32767] = [0.0, 2047.9375]. Do NOT make rate negative (>32767) // Value of FILTER is in fixdt(0,16,16): VAL_fixedPoint = VAL_floatingPoint * 2^16. In this case 6553 = 0.1 * 2^16 -#define FILTER 6553 // 0.1f [-] lower value == softer filter [0, 65535] = [0.0 - 1.0]. +#define FILTER 6553 // 0.1f [-] lower value == softer filter [0, 65535] = [0.0 - 1.0]. // ################################# DEFAULT SETTINGS ############################ -#ifndef TRANSPOTTER +#if !defined(HOVERCAR) && !defined(TRANSPOTTER) // Value of COEFFICIENT is in fixdt(1,16,14) // If VAL_floatingPoint >= 0, VAL_fixedPoint = VAL_floatingPoint * 2^14 // If VAL_floatingPoint < 0, VAL_fixedPoint = 2^16 + floor(VAL_floatingPoint * 2^14). @@ -241,6 +260,15 @@ #define INVERT_L_DIRECTION #endif +// ################################# HOVERCAR SETTINGS ############################ +#ifdef HOVERCAR + #define SPEED_COEFFICIENT 16384 // 1.0f + #define STEER_COEFFICIENT 0 // 0.0f + + // #define INVERT_R_DIRECTION + // #define INVERT_L_DIRECTION +#endif + // ################################# TRANSPOTTER SETTINGS ############################ #ifdef TRANSPOTTER #define CONTROL_GAMETRAK @@ -252,7 +280,7 @@ #define ROT_P 1.2 // P coefficient for the direction controller. Positive / Negative values to invert gametrak steering direction. - //#define INVERT_R_DIRECTION // Invert right motor + #define INVERT_R_DIRECTION // Invert right motor #define INVERT_L_DIRECTION // Invert left motor // during nunchuck control (only relevant when activated) @@ -312,3 +340,13 @@ #if defined(CONTROL_PPM) && defined(CONTROL_ADC) && defined(CONTROL_NUNCHUCK) || defined(CONTROL_PPM) && defined(CONTROL_ADC) || defined(CONTROL_ADC) && defined(CONTROL_NUNCHUCK) || defined(CONTROL_PPM) && defined(CONTROL_NUNCHUCK) #error only 1 input method allowed. use CONTROL_PPM or CONTROL_ADC or CONTROL_NUNCHUCK. #endif + +#if defined(ADC_PROTECT_ENA) && ((ADC1_MIN - ADC_PROTECT_THRESH) <= 0 || (ADC1_MAX + ADC_PROTECT_THRESH) >= 4096) + #warning ADC1 Protection NOT possible! Adjust the ADC thresholds. + #undef ADC_PROTECT_ENA +#endif + +#if defined(ADC_PROTECT_ENA) && ((ADC2_MIN - ADC_PROTECT_THRESH) <= 0 || (ADC2_MAX + ADC_PROTECT_THRESH) >= 4096) + #warning ADC2 Protection NOT possible! Adjust the ADC thresholds. + #undef ADC_PROTECT_ENA +#endif diff --git a/Inc/defines.h b/Inc/defines.h index 4b4e939..51462c6 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -136,7 +136,7 @@ #define STEP(from, to, step) (((from) < (to)) ? (MIN((from) + (step), (to))) : (MAX((from) - (step), (to)))) #define DEG(a) ((a)*M_PI / 180.0f) #define RAD(a) ((a)*180.0f / M_PI) -#define SIGN(a) (((a) < 0.0f) ? (-1.0f) : (((a) > 0.0f) ? (1.0f) : (0.0f))) +#define SIGN(a) (((a) < 0) ? (-1) : (((a) > 0) ? (1) : (0))) #define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x))) #define SCALE(value, high, max) MIN(MAX(((max) - (value)) / ((max) - (high)), 0.0f), 1.0f) #define MIN(a, b) (((a) < (b)) ? (a) : (b)) @@ -157,15 +157,22 @@ typedef struct { uint16_t l_rx2; } adc_buf_t; +typedef struct { + uint32_t t_timePrev; + uint8_t z_pulseCntPrev; + uint8_t b_hysteresis; + uint8_t b_multipleTap; +} MultipleTap; + // Define Beep functions void longBeep(uint8_t freq); void shortBeep(uint8_t freq); -// Define low-pass filter functions. Implementation is in main.c -void filtLowPass16(int16_t u, uint16_t coef, int16_t *y); -void filtLowPass32(int32_t u, uint16_t coef, int32_t *y); +// Define additional functions. Implementation is in main.c +void filtLowPass32(int16_t u, uint16_t coef, int32_t *y); void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t *rty_speedL); void rateLimiter16(int16_t u, int16_t rate, int16_t *y); +void multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x); // Define I2C and Nunchuck functions void I2C_Init(void); diff --git a/Inc/rtwtypes.h b/Inc/rtwtypes.h index 0731f04..6c93fd9 100644 --- a/Inc/rtwtypes.h +++ b/Inc/rtwtypes.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'BLDC_controller'. * - * Model version : 1.1249 + * Model version : 1.1256 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Thu Dec 12 20:22:31 2019 + * C/C++ source code generated on : Mon Dec 30 21:36:12 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex diff --git a/README.md b/README.md index 9b4c5b0..616b168 100644 --- a/README.md +++ b/README.md @@ -149,10 +149,13 @@ Most robust way for input is to use the ADC and potis. It works well even on 1m --- ## Examples -Have a look at the config.h in the Inc directory. That's where you configure to firmware to match your project. -Currently supported: Wii Nunchuck, analog potentiometer and PPM-Sum signal from a RC remote. -A good example of control via UART, eg. from an Arduino or raspberryPi, can be found here: -https://github.com/p-h-a-i-l/hoverboard-firmware-hack +This firmware offers currently 4 variants (selectable in platformio.ino): +- **VARIANT_ADC**: In this variant the motors are controlled by two potentiometers connected to the Left sensor cable (long wired) +- **VARIANT_USART3**: In this variant the motors are controlled via Serial protocol on USART3 Right sensor cable (short wired). The commands can be sent from an Arduino. Check out the [hoverserial.ino](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC/tree/master/02_Arduino/hoverserial) as an example sketch. +- **HOVERCAR**: In this variant the motors are controlled by two pedals Brake and Throttle. Reverse is engaged by double tapping on the Brake pedal at standstill. +- **TRANSPOTTER**: This build is for Transpotter which is a hoverboard based transportation system. For more details on how to build it check [here](https://github.com/NiklasFauth/hoverboard-firmware-hack/wiki/Build-Instruction:-TranspOtter) and [here](https://hackaday.io/project/161891-transpotter-ng). + +Of course the firmware can be further customized for other needs or projects. --- ## Acknowledgements diff --git a/Src/BLDC_controller.c b/Src/BLDC_controller.c index 54551bf..6bd2bd1 100644 --- a/Src/BLDC_controller.c +++ b/Src/BLDC_controller.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'BLDC_controller'. * - * Model version : 1.1249 + * Model version : 1.1256 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Thu Dec 12 20:22:31 2019 + * C/C++ source code generated on : Mon Dec 30 21:36:12 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -354,41 +354,66 @@ void PI_clamp_fixdt(int16_T rtu_err, uint16_T rtu_P, uint16_T rtu_I, int16_T /* System reset for atomic system: '/Low_Pass_Filter' */ void Low_Pass_Filter_Reset(DW_Low_Pass_Filter *localDW) { - /* InitializeConditions for UnitDelay: '/UnitDelay3' */ - localDW->UnitDelay3_DSTATE[0] = 0; - localDW->UnitDelay3_DSTATE[1] = 0; + /* InitializeConditions for UnitDelay: '/UnitDelay1' */ + localDW->UnitDelay1_DSTATE[0] = 0; + localDW->UnitDelay1_DSTATE[1] = 0; } /* Output and update for atomic system: '/Low_Pass_Filter' */ void Low_Pass_Filter(const int16_T rtu_u[2], uint16_T rtu_coef, int16_T rty_y[2], DW_Low_Pass_Filter *localDW) { - uint16_T rtb_Sum5; + int32_T rtb_Sum3_g; - /* Sum: '/Sum5' */ - rtb_Sum5 = (uint16_T)(65535U - rtu_coef); - - /* Sum: '/Sum1' incorporates: - * Product: '/Divide1' - * Product: '/Divide2' - * UnitDelay: '/UnitDelay3' + /* Sum: '/Sum2' incorporates: + * UnitDelay: '/UnitDelay1' */ - rty_y[0] = (int16_T)(((rtu_u[0] * rtu_coef) >> 16) + - ((localDW->UnitDelay3_DSTATE[0] * rtb_Sum5) >> 16)); + rtb_Sum3_g = rtu_u[0] - (localDW->UnitDelay1_DSTATE[0] >> 16); + if (rtb_Sum3_g > 32767) { + rtb_Sum3_g = 32767; + } else { + if (rtb_Sum3_g < -32768) { + rtb_Sum3_g = -32768; + } + } - /* Update for UnitDelay: '/UnitDelay3' */ - localDW->UnitDelay3_DSTATE[0] = rty_y[0]; - - /* Sum: '/Sum1' incorporates: - * Product: '/Divide1' - * Product: '/Divide2' - * UnitDelay: '/UnitDelay3' + /* Sum: '/Sum3' incorporates: + * Product: '/Divide3' + * Sum: '/Sum2' + * UnitDelay: '/UnitDelay1' */ - rty_y[1] = (int16_T)(((rtu_u[1] * rtu_coef) >> 16) + - ((localDW->UnitDelay3_DSTATE[1] * rtb_Sum5) >> 16)); + rtb_Sum3_g = rtu_coef * rtb_Sum3_g + localDW->UnitDelay1_DSTATE[0]; - /* Update for UnitDelay: '/UnitDelay3' */ - localDW->UnitDelay3_DSTATE[1] = rty_y[1]; + /* DataTypeConversion: '/Data Type Conversion' */ + rty_y[0] = (int16_T)(rtb_Sum3_g >> 16); + + /* Update for UnitDelay: '/UnitDelay1' */ + localDW->UnitDelay1_DSTATE[0] = rtb_Sum3_g; + + /* Sum: '/Sum2' incorporates: + * UnitDelay: '/UnitDelay1' + */ + rtb_Sum3_g = rtu_u[1] - (localDW->UnitDelay1_DSTATE[1] >> 16); + if (rtb_Sum3_g > 32767) { + rtb_Sum3_g = 32767; + } else { + if (rtb_Sum3_g < -32768) { + rtb_Sum3_g = -32768; + } + } + + /* Sum: '/Sum3' incorporates: + * Product: '/Divide3' + * Sum: '/Sum2' + * UnitDelay: '/UnitDelay1' + */ + rtb_Sum3_g = rtu_coef * rtb_Sum3_g + localDW->UnitDelay1_DSTATE[1]; + + /* DataTypeConversion: '/Data Type Conversion' */ + rty_y[1] = (int16_T)(rtb_Sum3_g >> 16); + + /* Update for UnitDelay: '/UnitDelay1' */ + localDW->UnitDelay1_DSTATE[1] = rtb_Sum3_g; } /* @@ -1502,10 +1527,10 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* Disable for If: '/If2' */ if (rtDW->If2_ActiveSubsystem_a == 0) { /* Disable for Outport: '/iq' */ - rtDW->Sum1[0] = 0; + rtDW->DataTypeConversion[0] = 0; /* Disable for Outport: '/id' */ - rtDW->Sum1[1] = 0; + rtDW->DataTypeConversion[1] = 0; } rtDW->If2_ActiveSubsystem_a = -1; @@ -1518,10 +1543,10 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->Gain4[2] = 0; /* Disable for Outport: '/r_devSignal1' */ - rtDW->Sum1[0] = 0; + rtDW->DataTypeConversion[0] = 0; /* Disable for Outport: '/r_devSignal2' */ - rtDW->Sum1[1] = 0; + rtDW->DataTypeConversion[1] = 0; } if (UnitDelay3 == 0) { @@ -1639,10 +1664,10 @@ void BLDC_controller_step(RT_MODEL *const rtM) rtDW->If2_ActiveSubsystem_a = UnitDelay3; if ((rtb_Sum2_h != UnitDelay3) && (rtb_Sum2_h == 0)) { /* Disable for Outport: '/iq' */ - rtDW->Sum1[0] = 0; + rtDW->DataTypeConversion[0] = 0; /* Disable for Outport: '/id' */ - rtDW->Sum1[1] = 0; + rtDW->DataTypeConversion[1] = 0; } if (UnitDelay3 == 0) { @@ -1715,7 +1740,7 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* Outputs for Atomic SubSystem: '/Low_Pass_Filter' */ Low_Pass_Filter(rtb_TmpSignalConversionAtLow_Pa, rtP->cf_currFilt, - rtDW->Sum1, &rtDW->Low_Pass_Filter_m); + rtDW->DataTypeConversion, &rtDW->Low_Pass_Filter_m); /* End of Outputs for SubSystem: '/Low_Pass_Filter' */ @@ -1805,10 +1830,10 @@ void BLDC_controller_step(RT_MODEL *const rtM) switch (rtDW->z_ctrlMod) { case 1: /* Abs: '/Abs5' */ - if (rtDW->Sum1[0] < 0) { - rtb_Merge_f_idx_1 = (int16_T)-rtDW->Sum1[0]; + if (rtDW->DataTypeConversion[0] < 0) { + rtb_Merge_f_idx_1 = (int16_T)-rtDW->DataTypeConversion[0]; } else { - rtb_Merge_f_idx_1 = rtDW->Sum1[0]; + rtb_Merge_f_idx_1 = rtDW->DataTypeConversion[0]; } /* End of Abs: '/Abs5' */ @@ -1843,13 +1868,13 @@ void BLDC_controller_step(RT_MODEL *const rtM) * RelationalOperator: '/UpperRelop' * Switch: '/Switch' */ - if (rtDW->Sum1[0] > rtDW->Divide1_a) { + if (rtDW->DataTypeConversion[0] > rtDW->Divide1_a) { rtb_Merge_f_idx_1 = rtDW->Divide1_a; - } else if (rtDW->Sum1[0] < rtDW->Gain1) { + } else if (rtDW->DataTypeConversion[0] < rtDW->Gain1) { /* Switch: '/Switch' */ rtb_Merge_f_idx_1 = rtDW->Gain1; } else { - rtb_Merge_f_idx_1 = rtDW->Sum1[0]; + rtb_Merge_f_idx_1 = rtDW->DataTypeConversion[0]; } /* End of Switch: '/Switch2' */ @@ -1858,8 +1883,8 @@ void BLDC_controller_step(RT_MODEL *const rtM) * Constant: '/cf_iqKiLimProt' * Sum: '/Sum3' */ - rtDW->Divide1 = (int16_T)(rtb_Merge_f_idx_1 - rtDW->Sum1[0]) * - rtP->cf_iqKiLimProt; + rtDW->Divide1 = (int16_T)(rtb_Merge_f_idx_1 - rtDW->DataTypeConversion[0]) + * rtP->cf_iqKiLimProt; /* End of Outputs for SubSystem: '/Speed_Mode_Protection' */ break; @@ -1924,7 +1949,7 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* End of Switch: '/Switch2' */ /* Sum: '/Sum3' */ - rtb_Gain3 = rtb_toNegative - rtDW->Sum1[1]; + rtb_Gain3 = rtb_toNegative - rtDW->DataTypeConversion[1]; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; } else { @@ -2055,7 +2080,7 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* End of Switch: '/Switch2' */ /* Sum: '/Sum2' */ - rtb_Gain3 = rtb_Merge_f_idx_1 - rtDW->Sum1[0]; + rtb_Gain3 = rtb_Merge_f_idx_1 - rtDW->DataTypeConversion[0]; if (rtb_Gain3 > 32767) { rtb_Gain3 = 32767; } else { @@ -2476,12 +2501,12 @@ void BLDC_controller_step(RT_MODEL *const rtM) /* Outport: '/r_devSignal1' incorporates: * DataTypeConversion: '/Data Type Conversion4' */ - rtY->r_devSignal1 = (int16_T)(rtDW->Sum1[0] >> 4); + rtY->r_devSignal1 = (int16_T)(rtDW->DataTypeConversion[0] >> 4); /* Outport: '/r_devSignal2' incorporates: * DataTypeConversion: '/Data Type Conversion5' */ - rtY->r_devSignal2 = (int16_T)(rtDW->Sum1[1] >> 4); + rtY->r_devSignal2 = (int16_T)(rtDW->DataTypeConversion[1] >> 4); /* End of Outputs for SubSystem: '/BLDC_controller' */ } diff --git a/Src/BLDC_controller_data.c b/Src/BLDC_controller_data.c index 5a11f15..9ce9b3a 100644 --- a/Src/BLDC_controller_data.c +++ b/Src/BLDC_controller_data.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'BLDC_controller'. * - * Model version : 1.1249 + * Model version : 1.1256 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Thu Dec 12 20:22:31 2019 + * C/C++ source code generated on : Mon Dec 30 21:36:12 2019 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -184,12 +184,12 @@ P rtP_Left = { /* Variable: t_errDequal * Referenced by: '/t_errDequal' */ - 10667U, + 9600U, /* Variable: t_errQual * Referenced by: '/t_errQual' */ - 3200U, + 1280U, /* Variable: Vd_max * Referenced by: @@ -266,7 +266,7 @@ P rtP_Left = { /* Variable: r_errInpTgtThres * Referenced by: '/r_errInpTgtThres' */ - 6400, + 9600, /* Variable: r_fieldWeakHi * Referenced by: '/r_fieldWeakHi' diff --git a/Src/bldc.c b/Src/bldc.c index a2ec2ce..29edeab 100644 --- a/Src/bldc.c +++ b/Src/bldc.c @@ -78,7 +78,7 @@ static int16_t offsetdcl = 2000; static int16_t offsetdcr = 2000; int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE; -static int16_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 4; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point +static int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 20; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point // ================================= // DMA interrupt frequency =~ 16 kHz @@ -101,8 +101,8 @@ void DMA1_Channel1_IRQHandler(void) { } if (buzzerTimer % 1000 == 0) { // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point - filtLowPass16(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt); - batVoltage = batVoltageFixdt >> 4; // convert fixed-point to integer + filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt); + batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer } // Get Left motor currents diff --git a/Src/main.c b/Src/main.c index de8782b..a3743fd 100644 --- a/Src/main.c +++ b/Src/main.c @@ -99,6 +99,11 @@ extern I2C_HandleTypeDef hi2c2; uint8_t nunchuck_connected = 1; #endif +#if defined(CONTROL_ADC) && defined(ADC_PROTECT_ENA) +static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection +#endif +static uint8_t timeoutFlagADC = 0; // Timeout Flag for for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data) + #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) typedef struct{ uint16_t start; @@ -107,9 +112,9 @@ typedef struct{ uint16_t checksum; } Serialcommand; static volatile Serialcommand command; -static int16_t timeoutCnt = 0; // Timeout counter for Rx Serial command +static int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command #endif -static uint8_t timeoutFlag = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) +static uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) #if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3) typedef struct{ @@ -126,7 +131,7 @@ typedef struct{ } SerialFeedback; static SerialFeedback Feedback; #endif -static uint8_t serialSendCounter; // serial send counter +static uint8_t serialSendCnt; // serial send counter #if defined(CONTROL_NUNCHUCK) || defined(SUPPORT_NUNCHUCK) || defined(CONTROL_PPM) || defined(CONTROL_ADC) static uint8_t button1, button2; @@ -139,11 +144,16 @@ static int cmd2; // normalized input value. -1000 to 1000 static int16_t speed; // local variable for speed. -1000 to 1000 #ifndef TRANSPOTTER static int16_t steer; // local variable for steering. -1000 to 1000 - static int16_t steerFixdt; // local fixed-point variable for steering low-pass filter - static int16_t speedFixdt; // local fixed-point variable for speed low-pass filter static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter + static int32_t steerFixdt; // local fixed-point variable for steering low-pass filter + static int32_t speedFixdt; // local fixed-point variable for speed low-pass filter #endif +#ifdef HOVERCAR + static MultipleTap MultipleTapBreak; // define multiple tap functionality for the Break pedal +#endif +static int16_t speedAvg; // average measured speed +static int16_t speedAvgAbs; // average measured speed in absolute extern volatile int pwml; // global variable for pwm left. -1000 to 1000 extern volatile int pwmr; // global variable for pwm right. -1000 to 1000 @@ -217,8 +227,6 @@ int main(void) { // ############################################################################### /* Set BLDC controller parameters */ - rtP_Right = rtP_Left; // Copy the Left motor parameters to the Right motor parameters - rtP_Left.b_selPhaABCurrMeas = 1; // Left motor measured current phases = {iA, iB} -> do NOT change rtP_Left.z_ctrlTypSel = CTRL_TYP_SEL; rtP_Left.b_diagEna = DIAG_ENA; @@ -230,16 +238,8 @@ int main(void) { rtP_Left.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) rtP_Left.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) + rtP_Right = rtP_Left; // Copy the Left motor parameters to the Right motor parameters rtP_Right.b_selPhaABCurrMeas = 0; // Left motor measured current phases = {iB, iC} -> do NOT change - rtP_Right.z_ctrlTypSel = CTRL_TYP_SEL; - rtP_Right.b_diagEna = DIAG_ENA; - rtP_Right.i_max = (I_MOT_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4) - rtP_Right.n_max = N_MOT_MAX << 4; // fixdt(1,16,4) - rtP_Right.b_fieldWeakEna = FIELD_WEAK_ENA; - rtP_Right.id_fieldWeakMax = (FIELD_WEAK_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4) - rtP_Right.a_phaAdvMax = PHASE_ADV_MAX << 4; // fixdt(1,16,4) - rtP_Right.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) - rtP_Right.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) /* Pack LEFT motor data into RTM */ rtM_Left->defaultParam = &rtP_Left; @@ -355,7 +355,7 @@ int main(void) { int16_t lastSpeedL = 0, lastSpeedR = 0; int16_t speedL = 0, speedR = 0; - int16_t board_temp_adcFixdt = adc_buffer.temp << 4; // Fixed-point filter output initialized with current ADC converted to fixed-point + int32_t board_temp_adcFixdt = adc_buffer.temp << 20; // Fixed-point filter output initialized with current ADC converted to fixed-point int16_t board_temp_adcFilt = adc_buffer.temp; int16_t board_temp_deg_c; @@ -409,12 +409,11 @@ int main(void) { } if (distance - (int)(setDistance * 1345) > -300) { #ifdef INVERT_R_DIRECTION - pwmr = -speedR; - #endif - #ifndef INVERT_R_DIRECTION pwmr = speedR; #endif - + #ifndef INVERT_R_DIRECTION + pwmr = -speedR; + #endif #ifdef INVERT_L_DIRECTION pwml = -speedL; #endif @@ -472,7 +471,32 @@ int main(void) { -CLAMP((ADC2_MID - adc_buffer.l_rx2) * INPUT_MAX / (ADC2_MID - ADC2_MIN), 0, INPUT_MAX); // ADC2 #else cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * INPUT_MAX / (ADC2_MAX - ADC2_MIN), 0, INPUT_MAX); // ADC2 - #endif + #endif + + #ifdef ADC_PROTECT_ENA + if (adc_buffer.l_tx2 >= (ADC1_MIN - ADC_PROTECT_THRESH) && adc_buffer.l_tx2 <= (ADC1_MAX + ADC_PROTECT_THRESH) && + adc_buffer.l_rx2 >= (ADC2_MIN - ADC_PROTECT_THRESH) && adc_buffer.l_rx2 <= (ADC2_MAX + ADC_PROTECT_THRESH)) { + if (timeoutFlagADC) { // Check for previous timeout flag + if (timeoutCntADC-- <= 0) // Timeout de-qualification + timeoutFlagADC = 0; // Timeout flag cleared + } else { + timeoutCntADC = 0; // Reset the timeout counter + } + } else { + if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification + timeoutFlagADC = 1; // Timeout detected + timeoutCntADC = ADC_PROTECT_TIMEOUT; // Limit timout counter value + } + } + + if (timeoutFlagADC) { // In case of timeout bring the system to a Safe State + ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way + cmd1 = 0; + cmd2 = 0; + } else { + ctrlModReq = ctrlModReqRaw; // Follow the Mode request + } + #endif // use ADCs as button inputs: button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1 @@ -485,19 +509,19 @@ int main(void) { // Handle received data validity, timeout and fix out-of-sync if necessary if (command.start == START_FRAME && command.checksum == (uint16_t)(command.start ^ command.steer ^ command.speed)) { - if (timeoutFlag) { // Check for previous timeout flag - if (timeoutCnt-- <= 0) // Timeout de-qualification - timeoutFlag = 0; // Timeout flag cleared + if (timeoutFlagSerial) { // Check for previous timeout flag + if (timeoutCntSerial-- <= 0) // Timeout de-qualification + timeoutFlagSerial = 0; // Timeout flag cleared } else { cmd1 = CLAMP((int16_t)command.steer, INPUT_MIN, INPUT_MAX); cmd2 = CLAMP((int16_t)command.speed, INPUT_MIN, INPUT_MAX); - command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle - timeoutCnt = 0; // Reset the timeout counter + command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle + timeoutCntSerial = 0; // Reset the timeout counter } } else { - if (timeoutCnt++ >= SERIAL_TIMEOUT) { // Timeout qualification - timeoutFlag = 1; // Timeout detected - timeoutCnt = SERIAL_TIMEOUT; // Limit timout counter value + if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification + timeoutFlagSerial = 1; // Timeout detected + timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value } // Check the received Start Frame. If it is NOT OK, most probably we are out-of-sync. // Try to re-sync by reseting the DMA @@ -507,7 +531,7 @@ int main(void) { } } - if (timeoutFlag) { // In case of timeout bring the system to a Safe State + if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way cmd1 = 0; cmd2 = 0; @@ -518,26 +542,76 @@ int main(void) { #endif + // Calculate measured average speed. The minus sign (-) is beacause motors spin in opposite directions + #if !defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION) + speedAvg = ( rtY_Left.n_mot - rtY_Right.n_mot) / 2; + #elif !defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION) + speedAvg = ( rtY_Left.n_mot + rtY_Right.n_mot) / 2; + #elif defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION) + speedAvg = (-rtY_Left.n_mot - rtY_Right.n_mot) / 2; + #elif defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION) + speedAvg = (-rtY_Left.n_mot + rtY_Right.n_mot) / 2; + #endif + + // Handle the case when SPEED_COEFFICIENT sign is negative (which is when most significant bit is 1) + if ((SPEED_COEFFICIENT & (1 << 16)) >> 16) { + speedAvg = -speedAvg; + } + speedAvgAbs = abs(speedAvg); + #ifndef TRANSPOTTER // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) ####### - if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ + if (enable == 0 && (!errCode_Left && !errCode_Right) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ shortBeep(6); // make 2 beeps indicating the motor enable shortBeep(4); HAL_Delay(100); enable = 1; // enable motors } + // ####### HOVERCAR ####### + #ifdef HOVERCAR + // Calculate speed Blend, a number between [0, 1] in fixdt(0,16,15) + uint16_t speedBlend; + speedBlend = (uint16_t)(((CLAMP(speedAvgAbs,30,90) - 30) << 15) / 60); // speedBlend [0,1] is within [30 rpm, 90rpm] + + // Check if Hovercar is physically close to standstill to enable Double tap detection on Brake pedal for Reverse functionality + if (speedAvgAbs < 20) { + multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBreak); // Break pedal in this case is "cmd1" variable + } + + // If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving + if (cmd1 > 20) { + cmd2 = (int16_t)((cmd2 * speedBlend) >> 15); + } + + // Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving by Brake pedal) + if (speedAvg > 0) { + cmd1 = (int16_t)((-cmd1 * speedBlend) >> 15); + } else { + cmd1 = (int16_t)(( cmd1 * speedBlend) >> 15); + } + #endif + // ####### LOW-PASS FILTER ####### rateLimiter16(cmd1, RATE, &steerRateFixdt); rateLimiter16(cmd2, RATE, &speedRateFixdt); - filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt); - filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt); - steer = steerFixdt >> 4; // convert fixed-point to integer - speed = speedFixdt >> 4; // convert fixed-point to integer + filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); + filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); + steer = (int16_t)(steerFixdt >> 20); // convert fixed-point to integer + speed = (int16_t)(speedFixdt >> 20); // convert fixed-point to integer + + // ####### HOVERCAR ####### + #ifdef HOVERCAR + if (!MultipleTapBreak.b_multipleTap) { // Check driving direction + speed = steer + speed; // Forward driving + } else { + speed = steer - speed; // Reverse driving + } + #endif // ####### MIXER ####### // speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000); // speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000); - mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above + mixerFcn(speed << 4, steer << 4, &speedR, &speedL); // This function implements the equations above #ifdef ADDITIONAL_CODE ADDITIONAL_CODE; @@ -637,13 +711,13 @@ int main(void) { // ####### CALC BOARD TEMPERATURE ####### - filtLowPass16(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt); - board_temp_adcFilt = board_temp_adcFixdt >> 4; // convert fixed-point to integer + filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt); + board_temp_adcFilt = (int16_t)(board_temp_adcFixdt >> 20); // convert fixed-point to integer board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C; - serialSendCounter++; // Increment the counter - if (serialSendCounter > 20) { // Send data every 100 ms = 20 * 5 ms, where 5 ms is approximately the main loop duration - serialSendCounter = 0; // Reset the counter + serialSendCnt++; // Increment the counter + if (serialSendCnt > 20) { // Send data every 100 ms = 20 * 5 ms, where 5 ms is approximately the main loop duration + serialSendCnt = 0; // Reset the counter // ####### DEBUG SERIAL OUT ####### #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) @@ -651,8 +725,8 @@ int main(void) { setScopeChannel(0, (int16_t)adc_buffer.l_tx2); // 1: ADC1 setScopeChannel(1, (int16_t)adc_buffer.l_rx2); // 2: ADC2 #endif - setScopeChannel(2, (int16_t)speedR); // 1: output command: [-1000, 1000] - setScopeChannel(3, (int16_t)speedL); // 2: output command: [-1000, 1000] + 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 @@ -696,25 +770,29 @@ int main(void) { // ####### BEEP AND EMERGENCY POWEROFF ####### - if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && abs(speed) < 20) || (batVoltage < BAT_LOW_DEAD && abs(speed) < 20)) { // poweroff before mainboard burns OR low bat 3 + if (errCode_Left || errCode_Right) { // disable motors and beep in case of Motor error - fast beep + enable = 0; + buzzerFreq = 8; + buzzerPattern = 1; + } else if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_LOW_DEAD && speedAvgAbs < 20)) { // poweroff before mainboard burns OR low bat 3 poweroff(); } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot - buzzerFreq = 4; + buzzerFreq = 4; buzzerPattern = 1; } else if (batVoltage < BAT_LOW_LVL1 && batVoltage >= BAT_LOW_LVL2 && BAT_LOW_LVL1_ENABLE) { // low bat 1: slow beep - buzzerFreq = 5; + buzzerFreq = 5; buzzerPattern = 42; } else if (batVoltage < BAT_LOW_LVL2 && batVoltage >= BAT_LOW_DEAD && BAT_LOW_LVL2_ENABLE) { // low bat 2: fast beep - buzzerFreq = 5; + buzzerFreq = 5; buzzerPattern = 6; - } else if (errCode_Left || errCode_Right || timeoutFlag) { // beep in case of Motor error or serial timeout - fast beep - buzzerFreq = 12; + } else if (timeoutFlagADC || timeoutFlagSerial) { // beep in case of ADC or Serial timeout - fast beep + buzzerFreq = 24; buzzerPattern = 1; - } else if (BEEPS_BACKWARD && speed < -50) { // backward beep - buzzerFreq = 5; + } else if (BEEPS_BACKWARD && speed < -50 && speedAvg < 0) { // backward beep + buzzerFreq = 5; buzzerPattern = 1; } else { // do not beep - buzzerFreq = 0; + buzzerFreq = 0; buzzerPattern = 0; } @@ -752,67 +830,27 @@ void shortBeep(uint8_t freq){ } // =========================================================== - /* Low pass filter fixed-point 16 bits: fixdt(1,16,4) + /* Low pass filter fixed-point 32 bits: fixdt(1,32,20) * Max: 2047.9375 * Min: -2048 * Res: 0.0625 * * Inputs: u = int16 - * Outputs: y = fixdt(1,16,4) + * Outputs: y = fixdt(1,32,20) * Parameters: coef = fixdt(0,16,16) = [0,65535U] * * Example: * If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point) * filtLowPass16(u, 52429, &y); - * yint = y >> 4; // the integer output is the fixed-point ouput shifted by 4 bits + * yint = (int16_t)(y >> 20); // the integer output is the fixed-point ouput shifted by 20 bits */ -void filtLowPass16(int16_t u, uint16_t coef, int16_t *y) +void filtLowPass32(int16_t u, uint16_t coef, int32_t *y) { int32_t tmp; - - tmp = (((int16_t)(u << 4) * coef) >> 16) + - (((int32_t)(65535U - coef) * (*y)) >> 16); - - // Overflow protection - tmp = CLAMP(tmp, -32768, 32767); - - *y = (int16_t)tmp; -} - -// =========================================================== - /* Low pass filter fixed-point 32 bits: fixdt(1,32,16) - * Max: 32767.99998474121 - * Min: -32768 - * Res: 1.52587890625e-5 - * - * Inputs: u = int32 - * Outputs: y = fixdt(1,32,16) - * Parameters: coef = fixdt(0,16,16) = [0,65535U] - * - * Example: - * If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point) - * filtLowPass16(u, 52429, &y); - * yint = y >> 16; // the integer output is the fixed-point ouput shifted by 16 bits - */ -void filtLowPass32(int32_t u, uint16_t coef, int32_t *y) -{ - int32_t q0; - int32_t q1; - int32_t tmp; - - q0 = (int32_t)(((int64_t)(u << 16) * coef) >> 16); - q1 = (int32_t)(((int64_t)(65535U - coef) * (*y)) >> 16); - - // Overflow protection - if ((q0 < 0) && (q1 < MIN_int32_T - q0)) { - tmp = MIN_int32_T; - } else if ((q0 > 0) && (q1 > MAX_int32_T - q0)) { - tmp = MAX_int32_T; - } else { - tmp = q0 + q1; - } - - *y = tmp; + + tmp = (int16_t)(u << 4) - (*y >> 16); + tmp = CLAMP(tmp, -32768, 32767); // Overflow protection + *y = coef * tmp + (*y); } // =========================================================== @@ -866,6 +904,67 @@ void rateLimiter16(int16_t u, int16_t rate, int16_t *y) *y = q0 + *y; } +// =========================================================== + /* multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x) + * This function detects multiple tap presses, such as double tapping, triple tapping, etc. + * Inputs: u = int16_t (input signal); timeNow = uint32_t (current time) + * Outputs: x->b_multipleTap (get the output here) + */ +void multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x) +{ + uint8_t b_timeout; + uint8_t b_hyst; + uint8_t b_pulse; + uint8_t z_pulseCnt; + uint8_t z_pulseCntRst; + uint32_t t_time; + + // Detect hysteresis + if (x->b_hysteresis) { + b_hyst = (u > MULTIPLE_TAP_LO); + } else { + b_hyst = (u > MULTIPLE_TAP_HI); + } + + // Detect pulse + b_pulse = (b_hyst != x->b_hysteresis); + + // Save time when first pulse is detected + if (b_hyst && b_pulse && (x->z_pulseCntPrev == 0)) { + t_time = timeNow; + } else { + t_time = x->t_timePrev; + } + + // Create timeout boolean + b_timeout = (timeNow - t_time > MULTIPLE_TAP_TIMEOUT); + + // Create pulse counter + if ((!b_hyst) && (x->z_pulseCntPrev == 0)) { + z_pulseCnt = 0U; + } else { + z_pulseCnt = b_pulse; + } + + // Reset counter if we detected complete tap presses OR there is a timeout + if ((x->z_pulseCntPrev >= MULTIPLE_TAP_NR) || b_timeout) { + z_pulseCntRst = 0U; + } else { + z_pulseCntRst = x->z_pulseCntPrev; + } + z_pulseCnt = z_pulseCnt + z_pulseCntRst; + + // Check if complete tap presses are detected AND no timeout + if ((z_pulseCnt >= MULTIPLE_TAP_NR) && (!b_timeout)) { + x->b_multipleTap = !x->b_multipleTap; // Toggle output + } + + // Update states + x->z_pulseCntPrev = z_pulseCnt; + x->b_hysteresis = b_hyst; + x->t_timePrev = t_time; +} + // =========================================================== /** System Clock Configuration diff --git a/docs/firmware_architecture.pptx b/docs/firmware_architecture.pptx index 9f5d105..725fff9 100644 Binary files a/docs/firmware_architecture.pptx and b/docs/firmware_architecture.pptx differ diff --git a/docs/motor_winding.pptx b/docs/motor_winding.pptx new file mode 100644 index 0000000..84abf15 Binary files /dev/null and b/docs/motor_winding.pptx differ diff --git a/docs/pictures/FW_architecture.png b/docs/pictures/FW_architecture.png index 34e3f7d..310120b 100644 Binary files a/docs/pictures/FW_architecture.png and b/docs/pictures/FW_architecture.png differ diff --git a/docs/pictures/motor_ferite1.JPG b/docs/pictures/motor_ferite1.JPG new file mode 100644 index 0000000..d71e50a Binary files /dev/null and b/docs/pictures/motor_ferite1.JPG differ diff --git a/docs/pictures/motor_ferite2.png b/docs/pictures/motor_ferite2.png new file mode 100644 index 0000000..71d879a Binary files /dev/null and b/docs/pictures/motor_ferite2.png differ diff --git a/docs/pictures/paramTable.png b/docs/pictures/paramTable.png index fffaa74..16550a6 100644 Binary files a/docs/pictures/paramTable.png and b/docs/pictures/paramTable.png differ diff --git a/platformio.ini b/platformio.ini index 0cefdec..847e647 100644 --- a/platformio.ini +++ b/platformio.ini @@ -6,9 +6,10 @@ include_dir = Inc src_dir = Src ;=================== VARIANT SELECTION ========================== -default_envs = VARIANT_ADC ; Variant for control via ADC input +default_envs = VARIANT_ADC ; Variant for control via ADC input ;default_envs = VARIANT_USART3 ; Variant for Serial control via USART3 input -;default_envs = TRANSPOTTER ; Variant for TRANSPOTTER build https://github.com/Jan--Henrik/transpOtterNG +;default_envs = HOVERCAR ; Variant for HOVERCAR build +;default_envs = TRANSPOTTER ; Variant for TRANSPOTTER build https://github.com/NiklasFauth/hoverboard-firmware-hack/wiki/Build-Instruction:-TranspOtter https://hackaday.io/project/161891-transpotter-ng ;================================================================ [env:VARIANT_ADC] @@ -31,6 +32,7 @@ build_flags = -Wl,-lm -g -ggdb ; to generate correctly the 'firmware.elf' for STM STUDIO vizualization # -Wl,-lnosys + -D VARIANT_ADC [env:VARIANT_USART3] platform = ststm32 @@ -54,6 +56,28 @@ build_flags = # -Wl,-lnosys -D VARIANT_USART3 +[env:HOVERCAR] +platform = ststm32 +framework = stm32cube +board = genericSTM32F103RC +debug_tool = stlink +upload_protocol = stlink + +; Serial Port settings (make sure the COM port is correct) +monitor_port = COM5 +monitor_speed = 38400 + +build_flags = + -I${PROJECT_DIR}/inc/ + -DUSE_HAL_DRIVER + -DSTM32F103xE + -Wl,-T./STM32F103RCTx_FLASH.ld + -Wl,-lc + -Wl,-lm + -g -ggdb ; to generate correctly the 'firmware.elf' for STM STUDIO vizualization +# -Wl,-lnosys + -D HOVERCAR + [env:TRANSPOTTER] platform = ststm32 framework = stm32cube