Implemented HOVERCAR variant
Major: - created HOVERCAR variant (selectable via platformio.ini) for driving via 2 pedals: Brake (on cmd1) and Throttle (on cmd2) - implemented "Double tapping" on Brake pedal to engage Reverse driving - implemented that Brake pedal stops the vehicle but does not go to Reverse, to prevend unintended Reverse driving - implemented ADC Protection when GND and Vcc wire are disconnected. The functionality can be enabled/disabled via #define ADC_PROTECT_ENA - updated error handling: in case of major error the motors will be disabled for improved safety Minor: - fixed bug on low-pass filter for not reaching exact "0" value - calibrated the ADC Battery voltage reading - other minor visual updates
This commit is contained in:
EmanuelFeru
parent
183776ceb2
commit
b4b23bbe9b
|
|
@ -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"
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 419 KiB |
Binary file not shown.
Binary file not shown.
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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 '<S41>/Low_Pass_Filter' */
|
||||
typedef struct {
|
||||
int16_T UnitDelay3_DSTATE[2]; /* '<S50>/UnitDelay3' */
|
||||
int32_T UnitDelay1_DSTATE[2]; /* '<S50>/UnitDelay1' */
|
||||
} DW_Low_Pass_Filter;
|
||||
|
||||
/* Block signals and states (auto storage) for system '<S73>/I_backCalc_fixdt' */
|
||||
|
|
@ -85,7 +85,7 @@ typedef struct {
|
|||
int32_T Divide1; /* '<S71>/Divide1' */
|
||||
int32_T UnitDelay_DSTATE; /* '<S36>/UnitDelay' */
|
||||
int16_T Gain4[3]; /* '<S43>/Gain4' */
|
||||
int16_T Sum1[2]; /* '<S50>/Sum1' */
|
||||
int16_T DataTypeConversion[2]; /* '<S50>/Data Type Conversion' */
|
||||
int16_T z_counterRawPrev; /* '<S15>/z_counterRawPrev' */
|
||||
int16_T Merge1; /* '<S29>/Merge1' */
|
||||
int16_T Divide3; /* '<S5>/Divide3' */
|
||||
|
|
|
|||
76
Inc/config.h
76
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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
11
README.md
11
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
|
||||
|
|
|
|||
|
|
@ -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: '<S41>/Low_Pass_Filter' */
|
||||
void Low_Pass_Filter_Reset(DW_Low_Pass_Filter *localDW)
|
||||
{
|
||||
/* InitializeConditions for UnitDelay: '<S50>/UnitDelay3' */
|
||||
localDW->UnitDelay3_DSTATE[0] = 0;
|
||||
localDW->UnitDelay3_DSTATE[1] = 0;
|
||||
/* InitializeConditions for UnitDelay: '<S50>/UnitDelay1' */
|
||||
localDW->UnitDelay1_DSTATE[0] = 0;
|
||||
localDW->UnitDelay1_DSTATE[1] = 0;
|
||||
}
|
||||
|
||||
/* Output and update for atomic system: '<S41>/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: '<S50>/Sum5' */
|
||||
rtb_Sum5 = (uint16_T)(65535U - rtu_coef);
|
||||
|
||||
/* Sum: '<S50>/Sum1' incorporates:
|
||||
* Product: '<S50>/Divide1'
|
||||
* Product: '<S50>/Divide2'
|
||||
* UnitDelay: '<S50>/UnitDelay3'
|
||||
/* Sum: '<S50>/Sum2' incorporates:
|
||||
* UnitDelay: '<S50>/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: '<S50>/UnitDelay3' */
|
||||
localDW->UnitDelay3_DSTATE[0] = rty_y[0];
|
||||
|
||||
/* Sum: '<S50>/Sum1' incorporates:
|
||||
* Product: '<S50>/Divide1'
|
||||
* Product: '<S50>/Divide2'
|
||||
* UnitDelay: '<S50>/UnitDelay3'
|
||||
/* Sum: '<S50>/Sum3' incorporates:
|
||||
* Product: '<S50>/Divide3'
|
||||
* Sum: '<S50>/Sum2'
|
||||
* UnitDelay: '<S50>/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: '<S50>/UnitDelay3' */
|
||||
localDW->UnitDelay3_DSTATE[1] = rty_y[1];
|
||||
/* DataTypeConversion: '<S50>/Data Type Conversion' */
|
||||
rty_y[0] = (int16_T)(rtb_Sum3_g >> 16);
|
||||
|
||||
/* Update for UnitDelay: '<S50>/UnitDelay1' */
|
||||
localDW->UnitDelay1_DSTATE[0] = rtb_Sum3_g;
|
||||
|
||||
/* Sum: '<S50>/Sum2' incorporates:
|
||||
* UnitDelay: '<S50>/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: '<S50>/Sum3' incorporates:
|
||||
* Product: '<S50>/Divide3'
|
||||
* Sum: '<S50>/Sum2'
|
||||
* UnitDelay: '<S50>/UnitDelay1'
|
||||
*/
|
||||
rtb_Sum3_g = rtu_coef * rtb_Sum3_g + localDW->UnitDelay1_DSTATE[1];
|
||||
|
||||
/* DataTypeConversion: '<S50>/Data Type Conversion' */
|
||||
rty_y[1] = (int16_T)(rtb_Sum3_g >> 16);
|
||||
|
||||
/* Update for UnitDelay: '<S50>/UnitDelay1' */
|
||||
localDW->UnitDelay1_DSTATE[1] = rtb_Sum3_g;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -1502,10 +1527,10 @@ void BLDC_controller_step(RT_MODEL *const rtM)
|
|||
/* Disable for If: '<S6>/If2' */
|
||||
if (rtDW->If2_ActiveSubsystem_a == 0) {
|
||||
/* Disable for Outport: '<S41>/iq' */
|
||||
rtDW->Sum1[0] = 0;
|
||||
rtDW->DataTypeConversion[0] = 0;
|
||||
|
||||
/* Disable for Outport: '<S41>/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: '<S6>/r_devSignal1' */
|
||||
rtDW->Sum1[0] = 0;
|
||||
rtDW->DataTypeConversion[0] = 0;
|
||||
|
||||
/* Disable for Outport: '<S6>/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: '<S41>/iq' */
|
||||
rtDW->Sum1[0] = 0;
|
||||
rtDW->DataTypeConversion[0] = 0;
|
||||
|
||||
/* Disable for Outport: '<S41>/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: '<S41>/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: '<S41>/Low_Pass_Filter' */
|
||||
|
||||
|
|
@ -1805,10 +1830,10 @@ void BLDC_controller_step(RT_MODEL *const rtM)
|
|||
switch (rtDW->z_ctrlMod) {
|
||||
case 1:
|
||||
/* Abs: '<S6>/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: '<S6>/Abs5' */
|
||||
|
|
@ -1843,13 +1868,13 @@ void BLDC_controller_step(RT_MODEL *const rtM)
|
|||
* RelationalOperator: '<S74>/UpperRelop'
|
||||
* Switch: '<S74>/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: '<S74>/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: '<S74>/Switch2' */
|
||||
|
|
@ -1858,8 +1883,8 @@ void BLDC_controller_step(RT_MODEL *const rtM)
|
|||
* Constant: '<S71>/cf_iqKiLimProt'
|
||||
* Sum: '<S71>/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: '<S45>/Speed_Mode_Protection' */
|
||||
break;
|
||||
|
|
@ -1924,7 +1949,7 @@ void BLDC_controller_step(RT_MODEL *const rtM)
|
|||
/* End of Switch: '<S66>/Switch2' */
|
||||
|
||||
/* Sum: '<S54>/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: '<S61>/Switch2' */
|
||||
|
||||
/* Sum: '<S53>/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: '<Root>/r_devSignal1' incorporates:
|
||||
* DataTypeConversion: '<S1>/Data Type Conversion4'
|
||||
*/
|
||||
rtY->r_devSignal1 = (int16_T)(rtDW->Sum1[0] >> 4);
|
||||
rtY->r_devSignal1 = (int16_T)(rtDW->DataTypeConversion[0] >> 4);
|
||||
|
||||
/* Outport: '<Root>/r_devSignal2' incorporates:
|
||||
* DataTypeConversion: '<S1>/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: '<Root>/BLDC_controller' */
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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: '<S3>/t_errDequal'
|
||||
*/
|
||||
10667U,
|
||||
9600U,
|
||||
|
||||
/* Variable: t_errQual
|
||||
* Referenced by: '<S3>/t_errQual'
|
||||
*/
|
||||
3200U,
|
||||
1280U,
|
||||
|
||||
/* Variable: Vd_max
|
||||
* Referenced by:
|
||||
|
|
@ -266,7 +266,7 @@ P rtP_Left = {
|
|||
/* Variable: r_errInpTgtThres
|
||||
* Referenced by: '<S3>/r_errInpTgtThres'
|
||||
*/
|
||||
6400,
|
||||
9600,
|
||||
|
||||
/* Variable: r_fieldWeakHi
|
||||
* Referenced by: '<S5>/r_fieldWeakHi'
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
297
Src/main.c
297
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
|
||||
|
|
@ -474,6 +473,31 @@ int main(void) {
|
|||
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * INPUT_MAX / (ADC2_MAX - ADC2_MIN), 0, INPUT_MAX); // ADC2
|
||||
#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
|
||||
button2 = (uint8_t)(adc_buffer.l_rx2 > 2000); // ADC2
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 34 KiB After Width: | Height: | Size: 35 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 362 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 500 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 61 KiB After Width: | Height: | Size: 62 KiB |
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue