Merge pull request #121 from EmanuelFeru/dual-input

Dual input
This commit is contained in:
EmanuelFeru 2020-12-22 22:34:27 +01:00 committed by GitHub
commit 1e18802a4c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
19 changed files with 1097 additions and 848 deletions

View File

@ -120,7 +120,7 @@
* 3. If you re-calibrate the Field Weakening please take all the safety measures! The motors can spin very fast!
Inputs:
- cmd1 and cmd2: analog normalized input values. INPUT_MIN to INPUT_MAX
- input1[inIdx].cmd and input2[inIdx].cmd: normalized input values. INPUT_MIN to INPUT_MAX
- button1 and button2: digital input values. 0 or 1
- adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095
Outputs:
@ -169,7 +169,9 @@
// Default settings will be applied at the end of this config file if not set before
#define INACTIVITY_TIMEOUT 8 // Minutes of not driving until poweroff. it is not very precise.
#define BEEPS_BACKWARD 1 // 0 or 1
#define FLASH_WRITE_KEY 0x1233 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#define ADC_MARGIN 100 // ADC input margin applied on the raw ADC min and max to make sure the MIN and MAX values are reached even in the presence of noise
#define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken
#define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values
/* FILTER is in fixdt(0,16,16): VAL_fixedPoint = VAL_floatingPoint * 2^16. In this case 6553 = 0.1 * 2^16
* Value of COEFFICIENT is in fixdt(1,16,14)
@ -185,6 +187,24 @@
// ############################## INPUT FORMAT ############################
/* ***_INPUT: TYPE, MIN, MID, MAX, DEADBAND
* -----------------------------------------
* TYPE: 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
* MIN: min ADC1-value while poti at minimum-position (0 - 4095)
* MID: mid ADC1-value while poti at mid-position (INPUT_MIN - INPUT_MAX)
* MAX: max ADC2-value while poti at maximum-position (0 - 4095)
* DEADBAND: how much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
*
* Dual-inputs
* PRI_INPUT: Primary Input. These limits will be used for the input with priority 0
* AUX_INPUT: Auxiliary Input. These limits will be used for the input with priority 1
* -----------------------------------------
*/
// ############################## END OF INPUT FORMAT ############################
// ############################## CRUISE CONTROL SETTINGS ############################
/* Cruise Control info:
* enable CRUISE_CONTROL_SUPPORT and (SUPPORT_BUTTONS_LEFT or SUPPORT_BUTTONS_RIGHT depending on which cable is the button installed)
@ -210,10 +230,10 @@
* DEBUG_SERIAL_ASCII output is:
* // "in1:345 in2:1337 cmdL:0 cmdR:0 BatADC:0 BatV:0 TempADC:0 Temp:0\r\n"
*
* in1: (int16_t)input1); raw input1: ADC1, UART, PWM, PPM, iBUS
* in2: (int16_t)input2); raw input2: ADC2, UART, PWM, PPM, iBUS
* cmdL: (int16_t)speedL); output command: [-1000, 1000]
* cmdR: (int16_t)speedR); output command: [-1000, 1000]
* in1: (int16_t)input1[inIdx].raw); raw input1: ADC1, UART, PWM, PPM, iBUS
* in2: (int16_t)input2[inIdx].raw); raw input2: ADC2, UART, PWM, PPM, iBUS
* cmdL: (int16_t)cmdL); output command Left: [-1000, 1000]
* cmdR: (int16_t)cmdR); output command Right: [-1000, 1000]
* BatADC: (int16_t)adc_buffer.batt1); Battery adc-value measured by mainboard
* BatV: (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); Battery calibrated voltage multiplied by 100 for verifying battery voltage calibration
* TempADC: (int16_t)board_temp_adcFilt); for board temperature calibration
@ -250,26 +270,27 @@
* Procedure:
* - connect gnd, rx and tx of a usb-uart converter in 3.3V mode to the right sensor board cable (do NOT use the red 15V wire!)
* - readout values using a serial terminal in 115200 baud rate
* - turn the potis to minimum position, write value 1 to INPUT1_MIN and value 2 to INPUT2_MIN
* - turn the potis to maximum position, write value 1 to INPUT1_MAX and value 2 to INPUT2_MAX
* - for middle resting potis: Let the potis in the middle resting position, write value 1 to INPUT1_MID and value 2 to INPUT2_MID
* - turn the potis to minimum position, write value in1 to PRI_INPUT1 MIN and value in2 to PRI_INPUT2 MIN
* - turn the potis to maximum position, write value in1 to PRI_INPUT1 MAX and value in2 to PRI_INPUT2 MAX
* - for middle resting potis: Let the potis in the middle resting position, write value in1 to PRI_INPUT1 MID and value in2 to PRI_INPUT2 MID
*/
#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken
#define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values
#define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT1_MIN 0 // min ADC1-value while poti at min-position (0 - 4095)
#define INPUT1_MID 0 // mid ADC1-value while poti at mid-position (INPUT1_MIN - INPUT1_MAX)
#define INPUT1_MAX 4095 // max ADC1-value while poti at max-position (0 - 4095)
#define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT2_MIN 0 // min ADC2-value while poti at min-position (0 - 4095)
#define INPUT2_MID 0 // mid ADC2-value while poti at mid-position (INPUT2_MIN - INPUT2_MAX)
#define INPUT2_MAX 4095 // max ADC2-value while poti at max-position (0 - 4095)
#define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
// #define DUAL_INPUTS // ADC*(Primary) + UART(Auxiliary). Uncomment this to use Dual-inputs
#define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#ifdef DUAL_INPUTS
#define FLASH_WRITE_KEY 0x1101 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
// #define SIDEBOARD_SERIAL_USART3 1
#define CONTROL_SERIAL_USART3 1 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino
#define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
#define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#else
#define FLASH_WRITE_KEY 0x1001 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
#endif
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
// #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#endif
@ -279,25 +300,27 @@
// ############################ VARIANT_USART SETTINGS ############################
#ifdef VARIANT_USART
// #define SIDEBOARD_SERIAL_USART2
// #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 SIDEBOARD_SERIAL_USART2 0
#define CONTROL_SERIAL_USART2 0 // 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 SIDEBOARD_SERIAL_USART3
#define CONTROL_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino
#define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
// Min / Max values of each channel (use DEBUG to determine these values)
#define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT1_MIN -1000 // (-1000 - 0)
#define INPUT1_MID 0
#define INPUT1_MAX 1000 // (0 - 1000)
#define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
// #define SIDEBOARD_SERIAL_USART3 0
// #define CONTROL_SERIAL_USART3 0 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino
// #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
// #define DUAL_INPUTS // UART*(Primary) + SIDEBOARD(Auxiliary). Uncomment this to use Dual-inputs
#define PRI_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#ifdef DUAL_INPUTS
#define FLASH_WRITE_KEY 0x1102 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
// #define SIDEBOARD_SERIAL_USART2 1 // left sideboard
#define SIDEBOARD_SERIAL_USART3 1 // right sideboard
#define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#else
#define FLASH_WRITE_KEY 0x1002 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#endif
#define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT2_MIN -1000 // (-1000 - 0)
#define INPUT2_MID 0
#define INPUT2_MAX 1000 // (0 - 1000)
#define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
// #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#endif
@ -313,25 +336,28 @@
* use original nunchuk. most clones does not work very well.
* Recommendation: Nunchuk Breakout Board https://github.com/Jan--Henrik/hoverboard-breakout
*/
#define CONTROL_NUNCHUK // use nunchuk as input. disable FEEDBACK_SERIAL_USART3, DEBUG_SERIAL_USART3!
// Min / Max values of each channel (use DEBUG to determine these values)
#define INPUT1_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT1_MIN -1024 // (-1024 - 0)
#define INPUT1_MID 0
#define INPUT1_MAX 1024 // (0 - 1024)
#define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define CONTROL_NUNCHUK 0 // use nunchuk as input. Number indicates priority for dual-input. Disable FEEDBACK_SERIAL_USART3, DEBUG_SERIAL_USART3!
// #define DUAL_INPUTS // Nunchuk*(Primary) + UART(Auxiliary). Uncomment this to use Dual-inputs
#define PRI_INPUT1 2, -1024, 0, 1024, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 2, -1024, 0, 1024, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#ifdef DUAL_INPUTS
#define FLASH_WRITE_KEY 0x1103 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
// #define SIDEBOARD_SERIAL_USART2 1
#define CONTROL_SERIAL_USART2 1 // 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 AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#else
#define FLASH_WRITE_KEY 0x1003 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#define DEBUG_SERIAL_USART2 // left sensor cable debug
#endif
#define INPUT2_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT2_MIN -1024 // (-1024 - 0)
#define INPUT2_MID 0
#define INPUT2_MAX 1024 // (0 - 1024)
#define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
// # maybe good for ARMCHAIR #
#define FILTER 3276 // 0.05f
#define SPEED_COEFFICIENT 8192 // 0.5f
#define STEER_COEFFICIENT 62259 // -0.2f
#define DEBUG_SERIAL_USART2 // left sensor cable debug
// #define SUPPORT_BUTTONS // Define for Nunchuck buttons support
// #define SUPPORT_BUTTONS // Define for Nunchuk buttons support
#endif
// ############################# END OF VARIANT_NUNCHUK SETTINGS #########################
@ -343,29 +369,33 @@
* Right sensor board cable. Channel 1: steering, Channel 2: speed.
* https://gist.github.com/peterpoetzi/1b63a4a844162196613871767189bd05
*/
// #define CONTROL_PPM_LEFT // use PPM-Sum as input on the LEFT cable . disable CONTROL_SERIAL_USART2!
#define CONTROL_PPM_RIGHT // use PPM-Sum as input on the RIGHT cable. disable CONTROL_SERIAL_USART3!
#ifdef CONTROL_PPM_RIGHT
#define DEBUG_SERIAL_USART2 // left sensor cable debug
// #define DUAL_INPUTS // ADC*(Primary) + PPM(Auxiliary). Uncomment this to use Dual-inputs
#ifdef DUAL_INPUTS
#define FLASH_WRITE_KEY 0x1104 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define CONTROL_PPM_RIGHT 1 // use PPM-Sum as input on the RIGHT cable. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART3!
#define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#else
#define DEBUG_SERIAL_USART3 // right sensor cable debug
#define FLASH_WRITE_KEY 0x1004 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
// #define CONTROL_PPM_LEFT 0 // use PPM-Sum as input on the LEFT cable. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2!
#define CONTROL_PPM_RIGHT 0 // use PPM-Sum as input on the RIGHT cable. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART3!
#define PRI_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#endif
#define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used.
// Min / Max values of each channel (use DEBUG to determine these values)
#define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT1_MIN -1000 // (-1000 - 0)
#define INPUT1_MID 0
#define INPUT1_MAX 1000 // (0 - 1000)
#define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT2_MIN -1000 // (-1000 - 0)
#define INPUT2_MID 0
#define INPUT2_MAX 1000 // (0 - 1000)
#define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
// #define SUPPORT_BUTTONS // Define for PPM buttons support
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
// #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#if defined(CONTROL_PPM_RIGHT) && !defined(DUAL_INPUTS)
#define DEBUG_SERIAL_USART2 // left sensor cable debug
#elif defined(CONTROL_PPM_LEFT) && !defined(DUAL_INPUTS)
#define DEBUG_SERIAL_USART3 // right sensor cable debug
#endif
#endif
// ############################# END OF VARIANT_PPM SETTINGS ############################
@ -376,25 +406,22 @@
* Right sensor board cable. Connect PA2 to channel 1 and PA3 to channel 2 on receiver.
* Channel 1: steering, Channel 2: speed.
*/
// #define CONTROL_PWM_LEFT // use RC PWM as input on the LEFT cable. disable DEBUG_SERIAL_USART2!
#define CONTROL_PWM_RIGHT // use RC PWM as input on the RIGHT cable. disable DEBUG_SERIAL_USART3!
#ifdef CONTROL_PWM_RIGHT
#define DEBUG_SERIAL_USART2 // left sensor cable debug
// #define DUAL_INPUTS // ADC*(Primary) + PWM(Auxiliary). Uncomment this to use Dual-inputs
#ifdef DUAL_INPUTS
#define FLASH_WRITE_KEY 0x1105 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define CONTROL_PWM_RIGHT 1 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3!
#define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#else
#define DEBUG_SERIAL_USART3 // right sensor cable debug
#define FLASH_WRITE_KEY 0x1005 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
// #define CONTROL_PWM_LEFT 0 // use RC PWM as input on the LEFT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART2!
#define CONTROL_PWM_RIGHT 0 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3!
#define PRI_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#endif
// Min / Max values of each channel (use DEBUG to determine these values)
#define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT1_MIN -1000 // (-1000 - 0)
#define INPUT1_MID 0
#define INPUT1_MAX 1000 // (0 - 1000)
#define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT2_MIN -1000 // (-1000 - 0)
#define INPUT2_MID 0
#define INPUT2_MAX 1000 // (0 - 1000)
#define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0].
#define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14
@ -403,6 +430,12 @@
// #define INVERT_L_DIRECTION
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
// #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#if defined(CONTROL_PWM_RIGHT) && !defined(DUAL_INPUTS)
#define DEBUG_SERIAL_USART2 // left sensor cable debug
#elif defined(CONTROL_PWM_LEFT) && !defined(DUAL_INPUTS)
#define DEBUG_SERIAL_USART3 // right sensor cable debug
#endif
#endif
// ############################# END OF VARIANT_PWM SETTINGS ############################
@ -413,33 +446,35 @@
/* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL
* Connected to Right sensor board cable. Channel 1: steering, Channel 2: speed.
*/
#define CONTROL_IBUS // use IBUS as input
#define CONTROL_IBUS // use IBUS as input. Number indicates priority for dual-input.
#define IBUS_NUM_CHANNELS 14 // total number of IBUS channels to receive, even if they are not used.
#define IBUS_LENGTH 0x20
#define IBUS_COMMAND 0x40
#undef USART3_BAUD
#define USART3_BAUD 115200
#define CONTROL_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used!
// #define DUAL_INPUTS // ADC*(Primary) + iBUS(Auxiliary). Uncomment this to use Dual-inputs
#ifdef DUAL_INPUTS
#define FLASH_WRITE_KEY 0x1106 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define CONTROL_SERIAL_USART3 1 // use RC iBUS input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3!
#define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used!
#ifdef CONTROL_SERIAL_USART3
#define DEBUG_SERIAL_USART2 // left sensor cable debug
#define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#else
#define DEBUG_SERIAL_USART3 // right sensor cable debug
#define FLASH_WRITE_KEY 0x1006 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#define CONTROL_SERIAL_USART3 0 // use RC iBUS input on the RIGHT cable, disable if ADC or PPM is used!
#define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used!
#define PRI_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#endif
// Min / Max values of each channel (use DEBUG to determine these values)
#define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT1_MIN -1000 // (-1000 - 0)
#define INPUT1_MID 0
#define INPUT1_MAX 1000 // (0 - 1000)
#define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT2_MIN -1000 // (-1000 - 0)
#define INPUT2_MID 0
#define INPUT2_MAX 1000 // (0 - 1000)
#define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#if defined(CONTROL_SERIAL_USART3) && !defined(DUAL_INPUTS)
#define DEBUG_SERIAL_USART2 // left sensor cable debug
#elif defined(DEBUG_SERIAL_USART2) && !defined(DUAL_INPUTS)
#define DEBUG_SERIAL_USART3 // right sensor cable debug
#endif
#endif
// ############################# END OF VARIANT_IBUS SETTINGS ############################
@ -447,30 +482,23 @@
// ############################ VARIANT_HOVERCAR SETTINGS ############################
#ifdef VARIANT_HOVERCAR
#define FLASH_WRITE_KEY 0x1107 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#undef CTRL_MOD_REQ
#define CTRL_MOD_REQ TRQ_MODE // HOVERCAR works best in TORQUE Mode
#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken
#define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values
#define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define SIDEBOARD_SERIAL_USART3 1 // Rx from right sensor board: to use photosensors as buttons. Number indicates priority for dual-input. Comment-out if sideboard is not used!
#define FEEDBACK_SERIAL_USART3 // Tx to right sensor board: for LED battery indication. Comment-out if sideboard is not used!
#define INPUT1_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT1_MIN 1000 // min ADC1-value while poti at minimum-position (0 - 4095)
#define INPUT1_MID 0
#define INPUT1_MAX 2500 // max ADC1-value while poti at maximum-position (0 - 4095)
#define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define INPUT2_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT2_MIN 500 // min ADC2-value while poti at minimum-position (0 - 4095)
#define INPUT2_MID 0
#define INPUT2_MAX 2200 // max ADC2-value while poti at maximum-position (0 - 4095)
#define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define DUAL_INPUTS // ADC*(Primary) + Sideboard_R(Auxiliary). Uncomment this to use Dual-inputs
#define PRI_INPUT1 1, 1000, 0, 2500, 0 // Pedal Brake TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 1, 500, 0, 2200, 0 // Pedal Accel TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT1 2, -1000, 0, 1000, 0 // Sideboard Steer TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define AUX_INPUT2 2, -1000, 0, 1000, 0 // Sideboard Speed TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define SPEED_COEFFICIENT 16384 // 1.0f
#define STEER_COEFFICIENT 0 // 0.0f
#define STEER_COEFFICIENT 8192 // 0.5f Only active in Sideboard input
// #define INVERT_R_DIRECTION // Invert rotation of right motor
// #define INVERT_L_DIRECTION // Invert rotation of left motor
#define SIDEBOARD_SERIAL_USART3 // Tx -> Rx of right sensor board: for LED battery indication. Comment-out if sideboard is not used!
#define FEEDBACK_SERIAL_USART3 // Rx <- Tx of right sensor board: to use photosensors as buttons. Comment-out if sideboard is not used!
// #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
// Extra functionality
@ -494,10 +522,18 @@
// Communication: [DONE]
// Balancing controller: [TODO]
#ifdef VARIANT_HOVERBOARD
#define SIDEBOARD_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
#define FLASH_WRITE_KEY 0x1008 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#define SIDEBOARD_SERIAL_USART2 1 // left sensor board cable. Number indicates priority for dual-input. Disable if ADC or PPM is used!
#define FEEDBACK_SERIAL_USART2
#define SIDEBOARD_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
#define SIDEBOARD_SERIAL_USART3 0 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used!
#define FEEDBACK_SERIAL_USART3
// If an iBUS RC receiver is connected to either Left Sideboard (AUX_INPUT) or Right Sideboard (PRI_INPUT)
// PRIMARY INPUT: TYPE, MIN, MID, MAX, DEADBAND /* TYPE: 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect */
#define PRI_INPUT1 3, -1000, 0, 1000, 0 // Priority Sideboard can be used to send commands via an iBUS Receiver connected to the sideboard
#define PRI_INPUT2 3, -1000, 0, 1000, 0 // Priority Sideboard can be used to send commands via an iBUS Receiver connected to the sideboard
#define AUX_INPUT1 3, -1000, 0, 1000, 0 // not used
#define AUX_INPUT2 3, -1000, 0, 1000, 0 // not used
#endif
// ######################## END OF VARIANT_HOVERBOARD SETTINGS #########################
@ -506,6 +542,7 @@
// ################################# VARIANT_TRANSPOTTER SETTINGS ############################
//TODO ADD VALIDATION
#ifdef VARIANT_TRANSPOTTER
#define FLASH_WRITE_KEY 0x1009 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#define CONTROL_GAMETRAK
#define SUPPORT_LCD
// #define SUPPORT_NUNCHUK
@ -517,6 +554,8 @@
#define STEER_COEFFICIENT 8192 // 0.5f - higher value == stronger. if you do not want any steering, set it to 0.0; 0.0 to 1.0
#define INVERT_R_DIRECTION // Invert right motor
#define INVERT_L_DIRECTION // Invert left motor
#define PRI_INPUT1 2, -1000, 0, 1000, 0 // dummy input, TRANSPOTTER does not use input limitations
#define PRI_INPUT2 2, -1000, 0, 1000, 0 // dummy input, TRANSPOTTER does not use input limitations
#endif
// ############################# END OF VARIANT_TRANSPOTTER SETTINGS ########################
@ -527,28 +566,15 @@
* right sensor board cable. Connect PB10 to channel 1 and PB11 to channel 2 on receiver.
* Channel 1: steering, Channel 2: speed.
*/
#define FLASH_WRITE_KEY 0x1010 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#undef CTRL_MOD_REQ
#define CTRL_MOD_REQ TRQ_MODE // SKATEBOARD works best in TORQUE Mode
//#define CONTROL_PWM_LEFT // use RC PWM as input on the LEFT cable. disable DEBUG_SERIAL_USART2!
#define CONTROL_PWM_RIGHT // use RC PWM as input on the RIGHT cable. disable DEBUG_SERIAL_USART3!
#ifdef CONTROL_PWM_RIGHT
#define DEBUG_SERIAL_USART2 // left sensor cable debug
#else
#define DEBUG_SERIAL_USART3 // right sensor cable debug
#endif
// Min / Max values of each channel (use DEBUG to determine these values)
#define INPUT1_TYPE 0 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT1_MIN -1000 // (-1000 - 0)
#define INPUT1_MID 0
#define INPUT1_MAX 1000 // (0 - 1000)
#define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
// #define CONTROL_PWM_LEFT 0 // use RC PWM as input on the LEFT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART2!
#define CONTROL_PWM_RIGHT 0 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3!
#define INPUT2_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define INPUT2_MIN -800 // (-1000 - 0)
#define INPUT2_MID 0
#define INPUT2_MAX 700 // (0 - 1000)
#define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define INPUT2_BRAKE -400 // (-1000 - 0) Change this value to adjust the braking amount
#define PRI_INPUT1 0, -1000, 0, 1000, 0 // Disabled. TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 2, -800, 0, 700, 100 // Active. TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT_BRK -400 // (-1000 - 0) Change this value to adjust the braking amount
#define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0].
#define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14
@ -558,6 +584,12 @@
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
// #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
// #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode.
#ifdef CONTROL_PWM_RIGHT
#define DEBUG_SERIAL_USART2 // left sensor cable debug
#else
#define DEBUG_SERIAL_USART3 // right sensor cable debug
#endif
#endif
// ############################# END OF VARIANT_SKATEBOARD SETTINGS ############################
@ -599,10 +631,10 @@
#ifndef STEER_COEFFICIENT
#define STEER_COEFFICIENT DEFAULT_STEER_COEFFICIENT
#endif
#ifdef CONTROL_ADC
#define INPUT_MARGIN 100 // Input margin applied on the raw ADC min and max to make sure the motor MIN and MAX values are reached even in the presence of noise
#if defined(PRI_INPUT1) && defined(PRI_INPUT2) && defined(AUX_INPUT1) && defined(AUX_INPUT2)
#define INPUTS_NR 2
#else
#define INPUT_MARGIN 0
#define INPUTS_NR 1
#endif
// ########################### END OF APPLY DEFAULT SETTING ############################
@ -616,11 +648,6 @@
// General checks
#if (defined(CONTROL_ADC) || defined(CONTROL_SERIAL_USART2) || defined(CONTROL_PPM_LEFT) || defined(CONTROL_PWM_LEFT)) && \
(defined(CONTROL_NUNCHUK) || defined(CONTROL_SERIAL_USART3) || defined(CONTROL_PPM_RIGHT) || defined(CONTROL_PWM_RIGHT))
#warning !! Multiple control input sources defined !! If NOT handled correctly, it can lead to undesired behavior!
#endif
#if defined(CONTROL_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART2)
#error CONTROL_SERIAL_USART2 and SIDEBOARD_SERIAL_USART2 not allowed, choose one.
#endif

View File

@ -222,12 +222,10 @@ void PWM_ISR_CH2_Callback(void);
#define SENSOR2_SET (0x02)
#define SENSOR_MPU (0x04)
// RC iBUS switch definitions. Flysky FS-i6S has SW1, SW4 - 2 positions; SW2, SW3 - 3 positions
// RC iBUS switch definitions. Flysky FS-i6S has [SW1, SW2, SW3, SW4] = [2, 3, 3, 2] positions switch
#define SW1_SET (0x0100) // 0000 0001 0000 0000
#define SW2_SET1 (0x0200) // 0000 0010 0000 0000
#define SW2_SET2 (0x0400) // 0000 0100 0000 0000
#define SW3_SET1 (0x0800) // 0000 1000 0000 0000
#define SW3_SET2 (0x1000) // 0001 0000 0000 0000
#define SW2_SET (0x0600) // 0000 0110 0000 0000
#define SW3_SET (0x1800) // 0001 1000 0000 0000
#define SW4_SET (0x2000) // 0010 0000 0000 0000

View File

@ -209,7 +209,7 @@
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
#define NB_OF_VAR ((uint8_t)0x0B)
#define NB_OF_VAR ((uint8_t)0x13) /* 19 Variables */
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/

View File

@ -55,6 +55,18 @@
} SerialSideboard;
#endif
// Input Structure
typedef struct {
int16_t raw; // raw input
int16_t cmd; // command
uint8_t typ; // type
uint8_t typDef; // type Defined
int16_t min; // minimum
int16_t mid; // middle
int16_t max; // maximum
int16_t dband; // deadband
} InputStruct;
// Initialization Functions
void BLDC_Init(void);
void Input_Lim_Init(void);
@ -68,21 +80,17 @@ void beepLong(uint8_t freq);
void beepShort(uint8_t freq);
void beepShortMany(uint8_t cnt, int8_t dir);
void calcAvgSpeed(void);
int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max);
int checkInputType(int16_t min, int16_t mid, int16_t max);
void adcCalibLim(void);
void updateCurSpdLim(void);
void saveConfig(void);
void standstillHold(void);
void electricBrake(uint16_t speedBlend, uint8_t reverseDir);
void cruiseControl(uint8_t button);
int checkInputType(int16_t min, int16_t mid, int16_t max);
// Poweroff Functions
void poweroff(void);
void poweroffPressCheck(void);
// Read Functions
void readInput(void);
// Input Functions
void calcInputCmd(InputStruct *in, int16_t out_min, int16_t out_max);
void readInputRaw(void);
void handleTimeout(void);
void readCommand(void);
void usart2_rx_check(void);
void usart3_rx_check(void);
@ -100,6 +108,11 @@ void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sid
void sideboardLeds(uint8_t *leds);
void sideboardSensors(uint8_t sensors);
// Poweroff Functions
void saveConfig(void);
void poweroff(void);
void poweroffPressCheck(void);
// Filtering Functions
void filtLowPass32(int32_t u, uint16_t coef, int32_t *y);
void rateLimiter16(int16_t u, int16_t rate, int16_t *y);

View File

@ -15,6 +15,7 @@ Table of Contents
* [Hardware](#hardware)
* [FOC Firmware](#foc-firmware)
* [Example Variants](#example-variants)
* [Dual Inputs](#dual-inputs)
* [Flashing](#flashing)
* [Troubleshooting](#troubleshooting)
* [Diagnostics](#diagnostics)
@ -76,11 +77,9 @@ In this firmware 3 control types are available:
|FOC SPEED| +++ | +++ | + | ++ | n.a. | +++ |
|FOC TORQUE| +++ | +++ | +++ | ++ | +++<sup>(1)</sup> | n.a<sup>(2)</sup> |
<sup>(1)</sup> By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter.
<sup>(1)</sup> By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter.<br/>
<sup>(2)</sup> The standstill hold functionality can be forced by enabling `STANDSTILL_HOLD_ENABLE` in `config.h`.
In all FOC control modes, the controller features maximum motor speed and maximum motor current protection. This brings great advantages to fulfil the needs of many robotic applications while maintaining safe operation.
@ -119,6 +118,29 @@ This firmware offers currently these variants (selectable in [platformio.ini](/p
Of course the firmware can be further customized for other needs or projects.
---
## Dual Inputs
The firmware supports the input to be provided from two different sources connected to the Left and Right cable, respectively. To enable dual-inputs functionality uncomment `#define DUAL_INPUTS` in config.h for the respective variant. Various dual-inputs combinations can be realized as illustrated in the following table:
| Left | Right | Availability |
| --- | --- | --- |
| ADC<sup>(0)</sup> | UART<sup>(1)</sup> | VARIANT_ADC |
| ADC<sup>(0)</sup> | {PPM,PWM,iBUS}<sup>(1)</sup> | VARIANT_{PPM,PWM,IBUS} |
| ADC<sup>(0)</sup> | Sideboard<sup></sup><sup>(1*)</sup> | VARIANT_HOVERCAR |
| UART<sup>(0)</sup> | Sideboard<sup>(1*)</sup> | VARIANT_UART |
| UART<sup>(1)</sup> | Nunchuk<sup>(0)</sup> | VARIANT_NUNCHUK |
<sup>(0)</sup> Primary input: this input is used when the Auxilliary input is not available or not connected.<br/>
<sup>(1)</sup> Auxilliary input: this inputs is used when connected or enabled by a switch<sup>(*)</sup>. If the Auxilliary input is disconnected, the firmware will automatically switch to the Primary input. Timeout is reported **only** on the Primary input.
With slight modifications in config.h, other dual-inputs combinations can be realized as:
| Left | Right | Possibility |
| --- | --- | --- |
| Sideboard<sup>(1*)</sup> | UART<sup>(0)</sup> | VARIANT_UART |
| UART<sup>(0)</sup> | {PPM,PWM,iBUS}<sup>(1)</sup> | VARIANT_{PPM,PWM,IBUS} |
| {PPM,PWM,iBUS}<sup>(1)</sup> | Nunchuk<sup>(0)</sup> | VARIANT_{PPM,PWM,IBUS} |
---
## Flashing
@ -223,7 +245,7 @@ The errors reported by the board are in the form of audible beeps:
- **1 beep (low pitch)**: Motor error (see [possible causes](https://github.com/EmanuelFeru/bldc-motor-control-FOC#diagnostics))
- **2 beeps (low pitch)**: ADC timeout
- **3 beeps (low pitch)**: Serial communication timeout
- **4 beeps (low pitch)**: General timeout (PPM, PWM, Nunchuck)
- **4 beeps (low pitch)**: General timeout (PPM, PWM, Nunchuk)
- **5 beeps (low pitch)**: Mainboard temperature warning
- **1 beep slow (medium pitch)**: Low battery voltage < 36V
- **1 beep fast (medium pitch)**: Low battery voltage < 35V

View File

@ -10,7 +10,8 @@ TIM_HandleTypeDef TimHandle;
TIM_HandleTypeDef TimHandle2;
uint8_t ppm_count = 0;
uint8_t pwm_count = 0;
uint32_t timeoutCnt = 0;
uint32_t timeoutCntGen = TIMEOUT;
uint8_t timeoutFlgGen = 0;
uint8_t nunchuk_data[6] = {0};
uint8_t i2cBuffer[2];
@ -34,7 +35,8 @@ void PPM_ISR_Callback(void) {
if (rc_delay > 3000) {
if (ppm_valid && ppm_count == PPM_NUM_CHANNELS) {
ppm_timeout = 0;
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
memcpy(ppm_captured_value, ppm_captured_value_buffer, sizeof(ppm_captured_value));
}
ppm_valid = true;
@ -122,7 +124,8 @@ void PWM_ISR_CH1_Callback(void) {
} else { // Falling Edge interrupt -> measure pulse duration
uint16_t rc_signal = TIM2->CNT - pwm_CNT_prev_ch1;
if (IN_RANGE(rc_signal, 900, 2100)){
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
pwm_timeout_ch1 = 0;
pwm_captured_ch1_value = CLAMP(rc_signal, 1000, 2000) - 1000;
}
@ -141,7 +144,8 @@ void PWM_ISR_CH2_Callback(void) {
} else { // Falling Edge interrupt -> measure pulse duration
uint16_t rc_signal = TIM2->CNT - pwm_CNT_prev_ch2;
if (IN_RANGE(rc_signal, 900, 2100)){
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
pwm_timeout_ch2 = 0;
pwm_captured_ch2_value = CLAMP(rc_signal, 1000, 2000) - 1000;
}
@ -237,11 +241,12 @@ void Nunchuk_Read(void) {
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 10);
HAL_Delay(3);
if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuk_data, 6, 10) == HAL_OK) {
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
}
#ifndef TRANSPOTTER
if (timeoutCnt > 3) {
if (timeoutCntGen > 3) {
HAL_Delay(50);
Nunchuk_Init();
}

View File

@ -62,16 +62,16 @@ extern ExtY rtY_Left; /* External outputs */
extern ExtY rtY_Right; /* External outputs */
//---------------
extern int16_t cmd1; // normalized input value. -1000 to 1000
extern int16_t cmd2; // normalized input value. -1000 to 1000
extern int16_t input1; // Non normalized input value
extern int16_t input2; // Non normalized input value
extern uint8_t inIdx; // input index used for dual-inputs
extern InputStruct input1[]; // input structure
extern InputStruct input2[]; // input structure
extern int16_t speedAvg; // Average measured speed
extern int16_t speedAvgAbs; // Average measured speed in absolute
extern volatile uint32_t timeoutCnt; // Timeout counter for the General timeout (PPM, PWM, Nunchuck)
extern uint8_t timeoutFlagADC; // Timeout Flag for for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data)
extern uint8_t timeoutFlagSerial; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
extern volatile uint32_t timeoutCntGen; // Timeout counter for the General timeout (PPM, PWM, Nunchuk)
extern volatile uint8_t timeoutFlgGen; // Timeout Flag for the General timeout (PPM, PWM, Nunchuk)
extern uint8_t timeoutFlgADC; // Timeout Flag for for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data)
extern uint8_t timeoutFlgSerial; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
@ -191,7 +191,7 @@ int main(void) {
HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET);
int16_t cmdL = 0, cmdR = 0;
int16_t lastCmdL = 0, lastCmdR = 0;
int16_t cmdL_prev = 0, cmdR_prev = 0;
int32_t board_temp_adcFixdt = adc_buffer.temp << 16; // Fixed-point filter output initialized with current ADC converted to fixed-point
int16_t board_temp_adcFilt = adc_buffer.temp;
@ -201,12 +201,12 @@ int main(void) {
while(1) {
HAL_Delay(DELAY_IN_MAIN_LOOP); // delay in ms
readCommand(); // Read Command: cmd1, cmd2
readCommand(); // Read Command: input1[inIdx].cmd, input2[inIdx].cmd
calcAvgSpeed(); // Calculate average measured speed: speedAvg, speedAvgAbs
#ifndef VARIANT_TRANSPOTTER
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (input1[inIdx].cmd > -50 && input1[inIdx].cmd < 50) && (input2[inIdx].cmd > -50 && input2[inIdx].cmd < 50)){
beepShort(6); // make 2 beeps indicating the motor enable
beepShort(4); HAL_Delay(100);
steerFixdt = speedFixdt = 0; // reset filters
@ -227,14 +227,16 @@ int main(void) {
#endif
#ifdef VARIANT_HOVERCAR
if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input)
if (speedAvgAbs < 60) { // Check if Hovercar is physically close to standstill to enable Double tap detection on Brake pedal for Reverse functionality
multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "cmd1" variable
multipleTapDet(input1[inIdx].cmd, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "input1" variable
}
if (cmd1 > 30) { // If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving
cmd2 = (int16_t)((cmd2 * speedBlend) >> 15);
if (input1[inIdx].cmd > 30) { // If Brake pedal (input1) is pressed, bring to 0 also the Throttle pedal (input2) to avoid "Double pedal" driving
input2[inIdx].cmd = (int16_t)((input2[inIdx].cmd * speedBlend) >> 15);
cruiseControl((uint8_t)rtP_Left.b_cruiseCtrlEna); // Cruise control deactivated by Brake pedal if it was active
}
}
#endif
#ifdef ELECTRIC_BRAKE_ENABLE
@ -242,26 +244,28 @@ int main(void) {
#endif
#ifdef VARIANT_HOVERCAR
if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input)
if (speedAvg > 0) { // 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)
cmd1 = (int16_t)((-cmd1 * speedBlend) >> 15);
input1[inIdx].cmd = (int16_t)((-input1[inIdx].cmd * speedBlend) >> 15);
} else {
cmd1 = (int16_t)(( cmd1 * speedBlend) >> 15);
input1[inIdx].cmd = (int16_t)(( input1[inIdx].cmd * speedBlend) >> 15);
}
}
#endif
#ifdef VARIANT_SKATEBOARD
if (cmd2 < 0) { // When Throttle is negative, it acts as brake. This condition is to make sure it goes to 0 as we reach standstill (to avoid Reverse driving)
if (input2[inIdx].cmd < 0) { // When Throttle is negative, it acts as brake. This condition is to make sure it goes to 0 as we reach standstill (to avoid Reverse driving)
if (speedAvg > 0) { // Make sure the braking is opposite to the direction of motion
cmd2 = (int16_t)(( cmd2 * speedBlend) >> 15);
input2[inIdx].cmd = (int16_t)(( input2[inIdx].cmd * speedBlend) >> 15);
} else {
cmd2 = (int16_t)((-cmd2 * speedBlend) >> 15);
input2[inIdx].cmd = (int16_t)((-input2[inIdx].cmd * speedBlend) >> 15);
}
}
#endif
// ####### LOW-PASS FILTER #######
rateLimiter16(cmd1, RATE, &steerRateFixdt);
rateLimiter16(cmd2, RATE, &speedRateFixdt);
rateLimiter16(input1[inIdx].cmd , RATE, &steerRateFixdt);
rateLimiter16(input2[inIdx].cmd , RATE, &speedRateFixdt);
filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt);
filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt);
steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer
@ -269,20 +273,23 @@ int main(void) {
// ####### VARIANT_HOVERCAR #######
#ifdef VARIANT_HOVERCAR
if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input)
if (!MultipleTapBrake.b_multipleTap) { // Check driving direction
speed = steer + speed; // Forward driving: in this case steer = Brake, speed = Throttle
} else {
speed = steer - speed; // Reverse driving: in this case steer = Brake, speed = Throttle
}
steer = 0; // Do not apply steering to avoid side effects if STEER_COEFFICIENT is NOT 0
}
#endif
// ####### MIXER #######
// cmdR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA);
// cmdL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA);
// cmdR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MAX);
// cmdL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MAX);
mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL); // This function implements the equations above
// ####### SET OUTPUTS (if the target change is less than +/- 100) #######
if ((cmdL > lastCmdL-100 && cmdL < lastCmdL+100) && (cmdR > lastCmdR-100 && cmdR < lastCmdR+100)) {
if ((cmdL > cmdL_prev-100 && cmdL < cmdL_prev+100) && (cmdR > cmdR_prev-100 && cmdR < cmdR_prev+100)) {
#ifdef INVERT_R_DIRECTION
pwmr = cmdR;
#else
@ -297,14 +304,14 @@ int main(void) {
#endif
#ifdef VARIANT_TRANSPOTTER
distance = CLAMP(cmd1 - 180, 0, 4095);
steering = (cmd2 - 2048) / 2048.0;
distance = CLAMP(input1[inIdx].cmd - 180, 0, 4095);
steering = (input2[inIdx].cmd - 2048) / 2048.0;
distanceErr = distance - (int)(setDistance * 1345);
if (nunchuk_connected == 0) {
cmdL = cmdL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
cmdR = cmdR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
if ((cmdL < lastCmdL + 50 && cmdL > lastCmdL - 50) && (cmdR < lastCmdR + 50 && cmdR > lastCmdR - 50)) {
if ((cmdL < cmdL_prev + 50 && cmdL > cmdL_prev - 50) && (cmdR < cmdR_prev + 50 && cmdR > cmdR_prev - 50)) {
if (distanceErr > 0) {
enable = 1;
}
@ -331,10 +338,11 @@ int main(void) {
enable = 0;
}
}
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
}
if (timeoutCnt > TIMEOUT) {
if (timeoutFlgGen) {
pwml = 0;
pwmr = 0;
enable = 0;
@ -368,7 +376,8 @@ int main(void) {
#ifdef SUPPORT_LCD
LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control");
#endif
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
HAL_Delay(1000);
nunchuk_connected = 1;
}
@ -394,11 +403,11 @@ int main(void) {
#endif
// ####### SIDEBOARDS HANDLING #######
#if defined(SIDEBOARD_SERIAL_USART2)
#if defined(SIDEBOARD_SERIAL_USART2) && defined(FEEDBACK_SERIAL_USART2)
sideboardLeds(&sideboard_leds_L);
sideboardSensors((uint8_t)Sideboard_L.sensors);
#endif
#if defined(SIDEBOARD_SERIAL_USART3)
#if defined(SIDEBOARD_SERIAL_USART3) && defined(FEEDBACK_SERIAL_USART3)
sideboardLeds(&sideboard_leds_R);
sideboardSensors((uint8_t)Sideboard_R.sensors);
#endif
@ -412,8 +421,8 @@ int main(void) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms
printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i\r\n",
input1, // 1: INPUT1
input2, // 2: INPUT2
input1[inIdx].raw, // 1: INPUT1
input2[inIdx].raw, // 2: INPUT2
cmdL, // 3: output command: [-1000, 1000]
cmdR, // 4: output command: [-1000, 1000]
adc_buffer.batt1, // 5: for battery voltage calibration
@ -427,8 +436,8 @@ int main(void) {
#if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3)
if (main_loop_counter % 2 == 0) { // Send data periodically every 10 ms
Feedback.start = (uint16_t)SERIAL_START_FRAME;
Feedback.cmd1 = (int16_t)cmd1;
Feedback.cmd2 = (int16_t)cmd2;
Feedback.cmd1 = (int16_t)input1[inIdx].cmd;
Feedback.cmd2 = (int16_t)input2[inIdx].cmd;
Feedback.speedR_meas = (int16_t)rtY_Right.n_mot;
Feedback.speedL_meas = (int16_t)rtY_Left.n_mot;
Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
@ -464,11 +473,11 @@ int main(void) {
} else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors
enable = 0;
beepCount(1, 24, 1);
} else if (timeoutFlagADC) { // 2 beeps (low pitch): ADC timeout
} else if (timeoutFlgADC) { // 2 beeps (low pitch): ADC timeout
beepCount(2, 24, 1);
} else if (timeoutFlagSerial) { // 3 beeps (low pitch): Serial timeout
} else if (timeoutFlgSerial) { // 3 beeps (low pitch): Serial timeout
beepCount(3, 24, 1);
} else if (timeoutCnt > TIMEOUT) { // 4 beeps (low pitch): General timeout (PPM, PWM, Nunchuck)
} else if (timeoutFlgGen) { // 4 beeps (low pitch): General timeout (PPM, PWM, Nunchuk)
beepCount(4, 24, 1);
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // 5 beeps (low pitch): Mainboard temperature warning
beepCount(5, 24, 1);
@ -497,10 +506,9 @@ int main(void) {
// HAL_GPIO_TogglePin(LED_PORT, LED_PIN); // This is to measure the main() loop duration with an oscilloscope connected to LED_PIN
// Update main loop states
lastCmdL = cmdL;
lastCmdR = cmdR;
cmdL_prev = cmdL;
cmdR_prev = cmdR;
main_loop_counter++;
timeoutCnt++;
}
}

1227
Src/util.c

File diff suppressed because it is too large Load Diff

Binary file not shown.

Binary file not shown.

View File

@ -0,0 +1,5 @@
https://s.click.aliexpress.com/e/_ATlz43
https://www.banggood.com/custlink/vvDdP7tGZ2
https://www.banggood.com/custlink/3KvdPnfDPc
https://www.banggood.com/custlink/KD3RFswKcT
https://www.banggood.com/custlink/K33dH7HDjt

Binary file not shown.

After

Width:  |  Height:  |  Size: 233 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 566 KiB

View File

Before

Width:  |  Height:  |  Size: 42 KiB

After

Width:  |  Height:  |  Size: 42 KiB

View File

Before

Width:  |  Height:  |  Size: 44 KiB

After

Width:  |  Height:  |  Size: 44 KiB

View File

Before

Width:  |  Height:  |  Size: 21 KiB

After

Width:  |  Height:  |  Size: 21 KiB

View File

Before

Width:  |  Height:  |  Size: 49 KiB

After

Width:  |  Height:  |  Size: 49 KiB