interfisch
/
hoverboard-firmware-hack-foc-serial-esc
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #111 from Candas1/master 

Improvement of Auto-calibration
This commit is contained in:
EmanuelFeru 2020-11-19 21:56:00 +01:00 committed by GitHub
parent c2647ed686 925008467b
commit fe55de6766
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 507 additions and 376 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

208
Inc/config.h
View File

@ -168,7 +168,7 @@
// Default settings will be applied at the end of this config file if not set before // 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 INACTIVITY_TIMEOUT 8 // Minutes of not driving until poweroff. it is not very precise.
#define BEEPS_BACKWARD 1 // 0 or 1 #define BEEPS_BACKWARD 1 // 0 or 1
#define FLASH_WRITE_KEY 0x1234 // Flash writing key, used when writing data to flash memory #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
/* FILTER is in fixdt(0,16,16): VAL_fixedPoint = VAL_floatingPoint * 2^16. In this case 6553 = 0.1 * 2^16 /* 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) * Value of COEFFICIENT is in fixdt(1,16,14)
@ -201,7 +201,7 @@
// ############################### DEBUG SERIAL ############################### // ############################### DEBUG SERIAL ###############################
/* Connect GND and RX of a 3.3v uart-usb adapter to the left (USART2) or right sensor board cable (USART3) /* Connect GND and RX of a 3.3v uart-usb adapter to the left (USART2) or right sensor board cable (USART3)
* Be careful not to use the red wire of the cable. 15v will destroye evrything. * Be careful not to use the red wire of the cable. 15v will destroy everything.
* If you are using VARIANT_NUNCHUK, disable it temporarily. * If you are using VARIANT_NUNCHUK, disable it temporarily.
* enable DEBUG_SERIAL_USART3 or DEBUG_SERIAL_USART2 * enable DEBUG_SERIAL_USART3 or DEBUG_SERIAL_USART2
* and DEBUG_SERIAL_ASCII use asearial terminal. * and DEBUG_SERIAL_ASCII use asearial terminal.
@ -210,8 +210,8 @@
* DEBUG_SERIAL_ASCII output is: * DEBUG_SERIAL_ASCII output is:
* // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n" * // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n"
* *
* 1: (int16_t)adc_buffer.l_tx2); ADC1 * 1: (int16_t)input1); raw input1: ADC1, UART, PWM, PPM, iBUS
* 2: (int16_t)adc_buffer.l_rx2); ADC2 * 2: (int16_t)input2); raw input2: ADC2, UART, PWM, PPM, iBUS
* 3: (int16_t)speedR); output command: [-1000, 1000] * 3: (int16_t)speedR); output command: [-1000, 1000]
* 4: (int16_t)speedL); output command: [-1000, 1000] * 4: (int16_t)speedL); output command: [-1000, 1000]
* 5: (int16_t)adc_buffer.batt1); Battery adc-value measured by mainboard * 5: (int16_t)adc_buffer.batt1); Battery adc-value measured by mainboard
@ -251,17 +251,19 @@
* Make, flash and test it. * Make, flash and test it.
*/ */
#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! #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 100 // ADC Protection: number of wrong / missing input commands before safety state is taken #define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken
#define ADC_PROTECT_THRESH 300 // ADC Protection threshold below/above the MIN/MAX ADC values #define ADC_PROTECT_THRESH 200 // 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 INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define ADC1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095) #define INPUT1_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 INPUT1_MID 0 // 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 INPUT1_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 INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#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 INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define ADC2_MAX 4095 // max ADC2-value while poti at maximum-position (0 - 4095) #define INPUT2_MIN 0 // min ADC2-value while poti at minimum-position (0 - 4095)
#define INPUT2_MID 0 // mid ADC2-value while poti at minimum-position (ADC2_MIN - ADC2_MAX)
#define INPUT2_MAX 4095 // max ADC2-value while poti at maximum-position (0 - 4095)
#define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #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 SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#endif #endif
@ -278,6 +280,18 @@
// #define SIDEBOARD_SERIAL_USART3 // #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 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! #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 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_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 SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#endif #endif
@ -287,17 +301,30 @@
// ################################# VARIANT_NUNCHUK SETTINGS ############################ // ################################# VARIANT_NUNCHUK SETTINGS ############################
#ifdef VARIANT_NUNCHUK #ifdef VARIANT_NUNCHUK
/* left sensor board cable. USART3 /* on Right sensor cable
* keep cable short, use shielded cable, use ferrits, stabalize voltage in nunchuk, * keep cable short, use shielded cable, use ferrits, stabalize voltage in nunchuk,
* use the right one of the 2 types of nunchuks, add i2c pullups. * use the right one of the 2 types of nunchuks, add i2c pullups.
* use original nunchuk. most clones does not work very well. * use original nunchuk. most clones does not work very well.
* Recommendation: Nunchuk Breakout Board https://github.com/Jan--Henrik/hoverboard-breakout * 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! #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 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 # // # maybe good for ARMCHAIR #
#define FILTER 3276 // 0.05f #define FILTER 3276 // 0.05f
#define SPEED_COEFFICIENT 8192 // 0.5f #define SPEED_COEFFICIENT 8192 // 0.5f
#define STEER_COEFFICIENT 62259 // -0.2f #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 Nunchuck buttons support
#endif #endif
// ############################# END OF VARIANT_NUNCHUK SETTINGS ######################### // ############################# END OF VARIANT_NUNCHUK SETTINGS #########################
@ -307,23 +334,29 @@
// ################################# VARIANT_PPM SETTINGS ############################## // ################################# VARIANT_PPM SETTINGS ##############################
#ifdef VARIANT_PPM #ifdef VARIANT_PPM
/* ###### CONTROL VIA RC REMOTE ###### /* ###### CONTROL VIA RC REMOTE ######
* left sensor board cable. Channel 1: steering, Channel 2: speed. * Right sensor board cable. Channel 1: steering, Channel 2: speed.
* https://gist.github.com/peterpoetzi/1b63a4a844162196613871767189bd05 * 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_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! #define CONTROL_PPM_RIGHT // use PPM-Sum as input on the RIGHT cable. disable CONTROL_SERIAL_USART3!
#ifdef CONTROL_PPM_RIGHT #ifdef CONTROL_PPM_RIGHT
#define DEBUG_SERIAL_USART2 // left sensor cable debug #define DEBUG_SERIAL_USART2 // left sensor cable debug
#else #else
#define DEBUG_SERIAL_USART3 // right sensor cable debug #define DEBUG_SERIAL_USART3 // right sensor cable debug
#endif #endif
#define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used. #define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used.
#define PPM_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
// Min / Max values of each channel (use DEBUG to determine these values) // Min / Max values of each channel (use DEBUG to determine these values)
#define PPM_CH1_MAX 1000 // (0 - 1000) #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define PPM_CH1_MIN -1000 // (-1000 - 0) #define INPUT1_MIN -1000 // (-1000 - 0)
#define PPM_CH2_MAX 1000 // (0 - 1000) #define INPUT1_MID 0
#define PPM_CH2_MIN -1000 // (-1000 - 0) #define INPUT1_MAX 1000 // (0 - 1000)
#define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define INPUT2_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 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 // Define for PPM buttons support
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #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 SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
@ -334,17 +367,29 @@
// ################################# VARIANT_PWM SETTINGS ############################## // ################################# VARIANT_PWM SETTINGS ##############################
#ifdef VARIANT_PWM #ifdef VARIANT_PWM
/* ###### CONTROL VIA RC REMOTE ###### /* ###### CONTROL VIA RC REMOTE ######
* left sensor board cable. Connect PA2 to channel 1 and PA3 to channel 2 on receiver. * Right sensor board cable. Connect PA2 to channel 1 and PA3 to channel 2 on receiver.
* Channel 1: steering, Channel 2: speed. * 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_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! #define CONTROL_PWM_RIGHT // use RC PWM as input on the RIGHT cable. disable DEBUG_SERIAL_USART3!
#define PWM_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #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) // Min / Max values of each channel (use DEBUG to determine these values)
#define PWM_CH1_MAX 1000 // (0 - 1000) #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define PWM_CH1_MIN -1000 // (-1000 - 0) #define INPUT1_MIN -1000 // (-1000 - 0)
#define PWM_CH2_MAX 1000 // (0 - 1000) #define INPUT1_MID 0
#define PWM_CH2_MIN -1000 // (-1000 - 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 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 #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
#define STEER_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. If you do not want any steering, set it to 0. #define STEER_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. If you do not want any steering, set it to 0.
@ -352,11 +397,6 @@
// #define INVERT_L_DIRECTION // #define INVERT_L_DIRECTION
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #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 SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. 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
#endif #endif
// ############################# END OF VARIANT_PWM SETTINGS ############################ // ############################# END OF VARIANT_PWM SETTINGS ############################
@ -365,17 +405,35 @@
// ################################# VARIANT_IBUS SETTINGS ############################## // ################################# VARIANT_IBUS SETTINGS ##############################
#ifdef VARIANT_IBUS #ifdef VARIANT_IBUS
/* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL /* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL
* Connected to Left sensor board cable. Channel 1: steering, Channel 2: speed. * 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
#define IBUS_NUM_CHANNELS 14 // total number of IBUS channels to receive, even if they are not used. #define IBUS_NUM_CHANNELS 14 // total number of IBUS channels to receive, even if they are not used.
#define IBUS_LENGTH 0x20 #define IBUS_LENGTH 0x20
#define IBUS_COMMAND 0x40 #define IBUS_COMMAND 0x40
#undef USART2_BAUD #undef USART3_BAUD
#define USART2_BAUD 115200 #define USART3_BAUD 115200
#define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! #define CONTROL_SERIAL_USART3 // left sensor board cable, disable if ADC or PPM is used!
#define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! #define FEEDBACK_SERIAL_USART3 // left sensor board cable, disable if ADC or PPM is used!
#ifdef CONTROL_SERIAL_USART3
#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 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)
#endif #endif
// ############################# END OF VARIANT_IBUS SETTINGS ############################ // ############################# END OF VARIANT_IBUS SETTINGS ############################
@ -386,13 +444,21 @@
#undef CTRL_MOD_REQ #undef CTRL_MOD_REQ
#define CTRL_MOD_REQ TRQ_MODE // HOVERCAR works best in TORQUE Mode #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 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 100 // ADC Protection: number of wrong / missing input commands before safety state is taken #define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken
#define ADC_PROTECT_THRESH 300 // ADC Protection threshold below/above the MIN/MAX ADC values #define ADC_PROTECT_THRESH 200 // 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 INPUT1_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define ADC2_MIN 500 // min ADC2-value while poti at minimum-position (0 - 4095) #define INPUT1_MIN 1000 // min ADC1-value while poti at minimum-position (0 - 4095)
#define ADC2_MAX 2200 // max ADC2-value while poti at maximum-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 SPEED_COEFFICIENT 16384 // 1.0f #define SPEED_COEFFICIENT 16384 // 1.0f
#define STEER_COEFFICIENT 0 // 0.0f #define STEER_COEFFICIENT 0 // 0.0f
// #define INVERT_R_DIRECTION // Invert rotation of right motor // #define INVERT_R_DIRECTION // Invert rotation of right motor
@ -457,15 +523,27 @@
*/ */
#undef CTRL_MOD_REQ #undef CTRL_MOD_REQ
#define CTRL_MOD_REQ TRQ_MODE // SKATEBOARD works best in TORQUE Mode #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_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! #define CONTROL_PWM_RIGHT // use RC PWM as input on the RIGHT cable. disable DEBUG_SERIAL_USART3!
#define PWM_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #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) // Min / Max values of each channel (use DEBUG to determine these values)
#define PWM_CH1_MAX 1000 // (0 - 1000) #define INPUT1_TYPE 0 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect
#define PWM_CH1_MIN -1000 // (-1000 - 0) #define INPUT1_MIN -1000 // (-1000 - 0)
#define PWM_CH2_MAX 700 // (0 - 1000) #define INPUT1_MID 0
#define PWM_CH2_MIN -800 // (-1000 - 0) #define INPUT1_MAX 1000 // (0 - 1000)
#define PWM_CH2_OUT_MIN -400 // (-1000 - 0) Change this value to adjust the braking amount #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
#define INPUT2_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 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 FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.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 #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
#define STEER_COEFFICIENT 0 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0. #define STEER_COEFFICIENT 0 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0.
@ -474,11 +552,6 @@
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #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 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. // #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 #endif
// ############################# END OF VARIANT_SKATEBOARD SETTINGS ############################ // ############################# END OF VARIANT_SKATEBOARD SETTINGS ############################
@ -498,7 +571,9 @@
#define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B #define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
#endif #endif
#if defined(FEEDBACK_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) #if defined(FEEDBACK_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3)
#ifndef USART3_BAUD
#define USART3_BAUD 38400 // UART3 baud rate (short wired cable) #define USART3_BAUD 38400 // UART3 baud rate (short wired cable)
#endif
#define USART3_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B #define USART3_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
#endif #endif
// ########################### UART SETIINGS ############################ // ########################### UART SETIINGS ############################
@ -518,6 +593,11 @@
#ifndef STEER_COEFFICIENT #ifndef STEER_COEFFICIENT
#define STEER_COEFFICIENT DEFAULT_STEER_COEFFICIENT #define STEER_COEFFICIENT DEFAULT_STEER_COEFFICIENT
#endif #endif
#ifdef CONTROL_ADC
#define INPUT_MARGIN 100 // Input margin applied on the raw ADC min and max to make sure the motor MIN and MAX values are reached even in the presence of noise
#else
#define INPUT_MARGIN 0
#endif
// ########################### END OF APPLY DEFAULT SETTING ############################ // ########################### END OF APPLY DEFAULT SETTING ############################
@ -637,16 +717,6 @@
// Functional checks // Functional checks
#if defined(ADC_PROTECT_ENA) && ((ADC1_MIN - ADC_PROTECT_THRESH) <= 0 || (ADC1_MAX + ADC_PROTECT_THRESH) >= 4095)
#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) >= 4095)
#warning ADC2 Protection NOT possible! Adjust the ADC thresholds.
#undef ADC_PROTECT_ENA
#endif
#if (defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)) && !defined(PPM_NUM_CHANNELS) #if (defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)) && !defined(PPM_NUM_CHANNELS)
#error Total number of PPM channels needs to be set #error Total number of PPM channels needs to be set
#endif #endif

2
Inc/defines.h
View File

@ -181,6 +181,8 @@
#define MIN3(a, b, c) MIN(a, MIN(b, c)) #define MIN3(a, b, c) MIN(a, MIN(b, c))
#define MAX3(a, b, c) MAX(a, MAX(b, c)) #define MAX3(a, b, c) MAX(a, MAX(b, c))
#define ARRAY_LEN(x) (uint32_t)(sizeof(x) / sizeof(*(x))) #define ARRAY_LEN(x) (uint32_t)(sizeof(x) / sizeof(*(x)))
#define MAP(x, in_min, in_max, out_min, out_max) (((((x) - (in_min)) * ((out_max) - (out_min))) / ((in_max) - (in_min))) + (out_min))
typedef struct { typedef struct {
uint16_t dcr; uint16_t dcr;

2
Inc/eeprom.h
View File

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

6
Inc/util.h
View File

@ -66,10 +66,11 @@ void shortBeep(uint8_t freq);
void shortBeepMany(uint8_t cnt, int8_t dir); void shortBeepMany(uint8_t cnt, int8_t dir);
void longBeep(uint8_t freq); void longBeep(uint8_t freq);
void calcAvgSpeed(void); 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 adcCalibLim(void);
void updateCurSpdLim(void); void updateCurSpdLim(void);
void saveConfig(void); void saveConfig(void);
int addDeadBand(int16_t u, int16_t deadBand, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max);
void standstillHold(void); void standstillHold(void);
void electricBrake(uint16_t speedBlend, uint8_t reverseDir); void electricBrake(uint16_t speedBlend, uint8_t reverseDir);
void cruiseControl(uint8_t button); void cruiseControl(uint8_t button);
@ -78,7 +79,8 @@ void cruiseControl(uint8_t button);
void poweroff(void); void poweroff(void);
void poweroffPressCheck(void); void poweroffPressCheck(void);
// Read Command Function // Read Functions
void readInput(void);
void readCommand(void); void readCommand(void);
void usart2_rx_check(void); void usart2_rx_check(void);
void usart3_rx_check(void); void usart3_rx_check(void);

11
Src/control.c
View File

@ -34,13 +34,13 @@ void PPM_ISR_Callback(void) {
if (rc_delay > 3000) { if (rc_delay > 3000) {
if (ppm_valid && ppm_count == PPM_NUM_CHANNELS) { if (ppm_valid && ppm_count == PPM_NUM_CHANNELS) {
ppm_timeout = 0; ppm_timeout = 0;
timeoutCnt = 0;
memcpy(ppm_captured_value, ppm_captured_value_buffer, sizeof(ppm_captured_value)); memcpy(ppm_captured_value, ppm_captured_value_buffer, sizeof(ppm_captured_value));
} }
ppm_valid = true; ppm_valid = true;
ppm_count = 0; ppm_count = 0;
} }
else if (ppm_count < PPM_NUM_CHANNELS && IN_RANGE(rc_delay, 900, 2100)){ else if (ppm_count < PPM_NUM_CHANNELS && IN_RANGE(rc_delay, 900, 2100)){
timeoutCnt = 0;
ppm_captured_value_buffer[ppm_count++] = CLAMP(rc_delay, 1000, 2000) - 1000; ppm_captured_value_buffer[ppm_count++] = CLAMP(rc_delay, 1000, 2000) - 1000;
} else { } else {
ppm_valid = false; ppm_valid = false;
@ -77,9 +77,18 @@ void PPM_Init(void) {
TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&TimHandle); HAL_TIM_Base_Init(&TimHandle);
#if defined(CONTROL_PPM_LEFT)
/* EXTI interrupt init*/ /* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0); HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI3_IRQn); HAL_NVIC_EnableIRQ(EXTI3_IRQn);
#endif
#if defined(CONTROL_PPM_RIGHT)
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
#endif
HAL_TIM_Base_Start(&TimHandle); HAL_TIM_Base_Start(&TimHandle);
} }
#endif #endif

17
Src/main.c
View File

@ -62,6 +62,8 @@ extern ExtY rtY_Right; /* External outputs */
extern int16_t cmd1; // normalized input value. -1000 to 1000 extern int16_t cmd1; // normalized input value. -1000 to 1000
extern int16_t cmd2; // 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 int16_t speedAvg; // Average measured speed extern int16_t speedAvg; // Average measured speed
extern int16_t speedAvgAbs; // Average measured speed in absolute extern int16_t speedAvgAbs; // Average measured speed in absolute
@ -208,6 +210,7 @@ int main(void) {
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) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
shortBeep(6); // make 2 beeps indicating the motor enable shortBeep(6); // make 2 beeps indicating the motor enable
shortBeep(4); HAL_Delay(100); shortBeep(4); HAL_Delay(100);
steerFixdt = speedFixdt = 0; // reset filters
enable = 1; // enable motors enable = 1; // enable motors
consoleLog("-- Motors enabled --\r\n"); consoleLog("-- Motors enabled --\r\n");
} }
@ -407,18 +410,8 @@ int main(void) {
// ####### DEBUG SERIAL OUT ####### // ####### DEBUG SERIAL OUT #######
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms
#ifdef CONTROL_ADC setScopeChannel(0, (int16_t)input1); // 1: INPUT1
setScopeChannel(0, (int16_t)adc_buffer.l_tx2); // 1: ADC1 setScopeChannel(1, (int16_t)input2); // 2: INPUT2
setScopeChannel(1, (int16_t)adc_buffer.l_rx2); // 2: ADC2
#endif
#if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)
setScopeChannel(0, ppm_captured_value[0]); // 1: CH1
setScopeChannel(1, ppm_captured_value[1]); // 2: CH2
#endif
#if defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT)
setScopeChannel(0, (pwm_captured_ch1_value - 500) * 2); // 1: CH1
setScopeChannel(1, (pwm_captured_ch2_value - 500) * 2); // 2: CH2
#endif
setScopeChannel(2, (int16_t)speedR); // 3: 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(3, (int16_t)speedL); // 4: output command: [-1000, 1000]
setScopeChannel(4, (int16_t)adc_buffer.batt1); // 5: for battery voltage calibration setScopeChannel(4, (int16_t)adc_buffer.batt1); // 5: for battery voltage calibration

449
Src/util.c
View File

@ -88,6 +88,8 @@ ExtY rtY_Right; /* External outputs */
int16_t cmd1; // normalized input value. -1000 to 1000 int16_t cmd1; // normalized input value. -1000 to 1000
int16_t cmd2; // normalized input value. -1000 to 1000 int16_t cmd2; // normalized input value. -1000 to 1000
int16_t input1; // Non normalized input value
int16_t input2; // Non normalized input value
int16_t speedAvg; // average measured speed int16_t speedAvg; // average measured speed
int16_t speedAvgAbs; // average measured speed in absolute int16_t speedAvgAbs; // average measured speed in absolute
@ -112,8 +114,8 @@ float setDistance;
uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1337}; // Virtual address defined by the user: 0xFFFF value is prohibited uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1337}; // Virtual address defined by the user: 0xFFFF value is prohibited
static uint16_t saveValue = 0; static uint16_t saveValue = 0;
static uint8_t saveValue_valid = 0; static uint8_t saveValue_valid = 0;
#elif defined(CONTROL_ADC) #elif !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300, 1301, 1302, 1303, 1304, 1305, 1306, 1307, 1308}; uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300, 1301, 1302, 1303, 1304, 1305, 1306, 1307, 1308, 1309, 1310};
#else #else
uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300}; // Dummy virtual address to avoid warnings uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300}; // Dummy virtual address to avoid warnings
#endif #endif
@ -125,26 +127,21 @@ uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300}; // Dummy virtual address to av
static int16_t INPUT_MAX; // [-] Input target maximum limitation static int16_t INPUT_MAX; // [-] Input target maximum limitation
static int16_t INPUT_MIN; // [-] Input target minimum limitation static int16_t INPUT_MIN; // [-] Input target minimum limitation
#ifdef CONTROL_ADC
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
static uint8_t cur_spd_valid = 0; static uint8_t cur_spd_valid = 0;
static uint8_t adc_cal_valid = 0; static uint8_t inp_cal_valid = 0;
static uint16_t ADC1_MIN_CAL = ADC1_MIN; static uint16_t INPUT1_TYP_CAL = INPUT1_TYPE;
static uint16_t ADC1_MAX_CAL = ADC1_MAX; static uint16_t INPUT1_MIN_CAL = INPUT1_MIN;
static uint16_t ADC2_MIN_CAL = ADC2_MIN; static uint16_t INPUT1_MID_CAL = INPUT1_MID;
static uint16_t ADC2_MAX_CAL = ADC2_MAX; static uint16_t INPUT1_MAX_CAL = INPUT1_MAX;
#ifdef ADC1_MID_POT static uint16_t INPUT2_TYP_CAL = INPUT2_TYPE;
static uint16_t ADC1_MID_CAL = ADC1_MID; static uint16_t INPUT2_MIN_CAL = INPUT2_MIN;
#else static uint16_t INPUT2_MID_CAL = INPUT2_MID;
static uint16_t ADC1_MID_CAL = 0; static uint16_t INPUT2_MAX_CAL = INPUT2_MAX;
#endif
#ifdef ADC1_MID_POT
static uint16_t ADC2_MID_CAL = ADC2_MID;
#else
static uint16_t ADC2_MID_CAL = 0;
#endif
#endif #endif
#if defined(CONTROL_ADC) && defined(ADC_PROTECT_ENA) #if defined(CONTROL_ADC)
static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection
#endif #endif
@ -275,28 +272,31 @@ void Input_Init(void) {
UART_DisableRxErrors(&huart3); UART_DisableRxErrors(&huart3);
#endif #endif
#ifdef CONTROL_ADC #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
uint16_t writeCheck, i_max, n_max; uint16_t writeCheck, i_max, n_max;
HAL_FLASH_Unlock(); HAL_FLASH_Unlock();
EE_Init(); /* EEPROM Init */ EE_Init(); /* EEPROM Init */
EE_ReadVariable(VirtAddVarTab[0], &writeCheck); EE_ReadVariable(VirtAddVarTab[0], &writeCheck);
if (writeCheck == FLASH_WRITE_KEY) { if (writeCheck == FLASH_WRITE_KEY) {
EE_ReadVariable(VirtAddVarTab[1], &ADC1_MIN_CAL); EE_ReadVariable(VirtAddVarTab[1] , &INPUT1_TYP_CAL);
EE_ReadVariable(VirtAddVarTab[2], &ADC1_MAX_CAL); EE_ReadVariable(VirtAddVarTab[2] , &INPUT1_MIN_CAL);
EE_ReadVariable(VirtAddVarTab[3], &ADC1_MID_CAL); EE_ReadVariable(VirtAddVarTab[3] , &INPUT1_MID_CAL);
EE_ReadVariable(VirtAddVarTab[4], &ADC2_MIN_CAL); EE_ReadVariable(VirtAddVarTab[4] , &INPUT1_MAX_CAL);
EE_ReadVariable(VirtAddVarTab[5], &ADC2_MAX_CAL); EE_ReadVariable(VirtAddVarTab[5] , &INPUT2_TYP_CAL);
EE_ReadVariable(VirtAddVarTab[6], &ADC2_MID_CAL); EE_ReadVariable(VirtAddVarTab[6] , &INPUT2_MIN_CAL);
EE_ReadVariable(VirtAddVarTab[7], &i_max); EE_ReadVariable(VirtAddVarTab[7] , &INPUT2_MID_CAL);
EE_ReadVariable(VirtAddVarTab[8], &n_max); EE_ReadVariable(VirtAddVarTab[8] , &INPUT2_MAX_CAL);
EE_ReadVariable(VirtAddVarTab[9] , &i_max);
EE_ReadVariable(VirtAddVarTab[10], &n_max);
rtP_Left.i_max = i_max; rtP_Left.i_max = i_max;
rtP_Left.n_max = n_max; rtP_Left.n_max = n_max;
rtP_Right.i_max = i_max; rtP_Right.i_max = i_max;
rtP_Right.n_max = n_max; rtP_Right.n_max = n_max;
} else { // Else If Input type is 3 (auto), identify the input type based on the values from config.h
if (INPUT1_TYPE == 3) { INPUT1_TYP_CAL = checkInputType(INPUT1_MIN, INPUT1_MID, INPUT1_MAX); }
if (INPUT2_TYPE == 3) { INPUT2_TYP_CAL = checkInputType(INPUT2_MIN, INPUT2_MID, INPUT2_MAX); }
} }
HAL_FLASH_Lock(); HAL_FLASH_Lock();
#endif #endif
#ifdef VARIANT_TRANSPOTTER #ifdef VARIANT_TRANSPOTTER
@ -360,11 +360,8 @@ void Input_Init(void) {
defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3)
void UART_DisableRxErrors(UART_HandleTypeDef *huart) void UART_DisableRxErrors(UART_HandleTypeDef *huart)
{ {
/* Disable PE (Parity Error) interrupts */ CLEAR_BIT(huart->Instance->CR1, USART_CR1_PEIE); /* Disable PE (Parity Error) interrupts */
CLEAR_BIT(huart->Instance->CR1, USART_CR1_PEIE); CLEAR_BIT(huart->Instance->CR3, USART_CR3_EIE); /* Disable EIE (Frame error, noise error, overrun error) interrupts */
/* Disable EIE (Frame error, noise error, overrun error) interrupts */
CLEAR_BIT(huart->Instance->CR3, USART_CR3_EIE);
} }
#endif #endif
@ -422,6 +419,70 @@ void calcAvgSpeed(void) {
speedAvgAbs = abs(speedAvg); speedAvgAbs = abs(speedAvg);
} }
/*
* Add Dead-band to a signal
* This function realizes a dead-band around 0 and scales the input between [out_min, out_max]
*/
int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max) {
switch (type){
case 0: // Input is ignored
return 0;
case 1: // Input is a normal pot
return CLAMP(MAP(u, in_min, in_max, 0, out_max), 0, out_max);
case 2: // Input is a mid resting pot
if( u > in_mid - deadBand && u < in_mid + deadBand ) {
return 0;
} else if(u > in_mid) {
return CLAMP(MAP(u, in_mid + deadBand, in_max, 0, out_max), 0, out_max);
} else {
return CLAMP(MAP(u, in_mid - deadBand, in_min, 0, out_min), out_min, 0);
}
default:
return 0;
}
}
/*
* Check Input Type
* This function identifies the input type: 0: Disabled, 1: Normal Pot, 2: Middle Resting Pot
*/
int checkInputType(int16_t min, int16_t mid, int16_t max){
int type = 0;
#ifdef CONTROL_ADC
int16_t threshold = 400; // Threshold to define if values are too close
#else
int16_t threshold = 200;
#endif
HAL_Delay(10);
if ((min / threshold) == (max / threshold) || (mid / threshold) == (max / threshold) || min > max || mid > max) {
type = 0;
consoleLog("Input is ignored"); // (MIN and MAX) OR (MID and MAX) are close, disable input
} else {
if ((min / threshold) == (mid / threshold)){
type = 1;
consoleLog("Input is a normal pot"); // MIN and MID are close, it's a normal pot
} else {
type = 2;
consoleLog("Input is a mid-resting pot"); // it's a mid resting pot
}
HAL_Delay(10);
#ifdef CONTROL_ADC
if ((min + INPUT_MARGIN - ADC_PROTECT_THRESH) > 0 && (max - INPUT_MARGIN + ADC_PROTECT_THRESH) < 4095) {
consoleLog(" and protected");
longBeep(2); // Indicate protection by a beep
}
#endif
}
HAL_Delay(10);
consoleLog("\n");
HAL_Delay(10);
return type;
}
/* /*
* Auto-calibration of the ADC Limits * Auto-calibration of the ADC Limits
* This function finds the Minimum, Maximum, and Middle for the ADC input * This function finds the Minimum, Maximum, and Middle for the ADC input
@ -435,64 +496,72 @@ void adcCalibLim(void) {
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
return; return;
} }
#ifdef CONTROL_ADC
consoleLog("ADC calibration started... ");
// Inititalization: MIN = a high values, MAX = a low value, #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
int32_t adc1_fixdt = adc_buffer.l_tx2 << 16; consoleLog("Input calibration started...\n");
int32_t adc2_fixdt = adc_buffer.l_rx2 << 16;
uint16_t adc_cal_timeout = 0;
uint16_t ADC1_MIN_temp = 4095;
uint16_t ADC1_MID_temp = 0;
uint16_t ADC1_MAX_temp = 0;
uint16_t ADC2_MIN_temp = 4095;
uint16_t ADC2_MID_temp = 0;
uint16_t ADC2_MAX_temp = 0;
adc_cal_valid = 1; readInput();
// Inititalization: MIN = a high value, MAX = a low value
int32_t input1_fixdt = input1 << 16;
int32_t input2_fixdt = input2 << 16;
int16_t INPUT1_MIN_temp = MAX_int16_T;
int16_t INPUT1_MID_temp = 0;
int16_t INPUT1_MAX_temp = MIN_int16_T;
int16_t INPUT2_MIN_temp = MAX_int16_T;
int16_t INPUT2_MID_temp = 0;
int16_t INPUT2_MAX_temp = MIN_int16_T;
uint16_t input_cal_timeout = 0;
// Extract MIN, MAX and MID from ADC while the power button is not pressed // Extract MIN, MAX and MID from ADC while the power button is not pressed
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && adc_cal_timeout++ < 4000) { // 20 sec timeout while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout
filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt); readInput();
filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt); filtLowPass32(input1, FILTER, &input1_fixdt);
ADC1_MID_temp = (uint16_t)CLAMP(adc1_fixdt >> 16, 0, 4095); // convert fixed-point to integer filtLowPass32(input2, FILTER, &input2_fixdt);
ADC2_MID_temp = (uint16_t)CLAMP(adc2_fixdt >> 16, 0, 4095);
ADC1_MIN_temp = MIN(ADC1_MIN_temp, ADC1_MID_temp); INPUT1_MID_temp = (int16_t)(input1_fixdt >> 16);// CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer
ADC1_MAX_temp = MAX(ADC1_MAX_temp, ADC1_MID_temp); INPUT2_MID_temp = (int16_t)(input2_fixdt >> 16);// CLAMP(input2_fixdt >> 16, INPUT2_MIN, INPUT2_MAX);
ADC2_MIN_temp = MIN(ADC2_MIN_temp, ADC2_MID_temp); INPUT1_MIN_temp = MIN(INPUT1_MIN_temp, INPUT1_MID_temp);
ADC2_MAX_temp = MAX(ADC2_MAX_temp, ADC2_MID_temp); INPUT1_MAX_temp = MAX(INPUT1_MAX_temp, INPUT1_MID_temp);
INPUT2_MIN_temp = MIN(INPUT2_MIN_temp, INPUT2_MID_temp);
INPUT2_MAX_temp = MAX(INPUT2_MAX_temp, INPUT2_MID_temp);
HAL_Delay(5); HAL_Delay(5);
} }
// ADC calibration checks INPUT1_TYP_CAL = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp);
#ifdef ADC_PROTECT_ENA if (INPUT1_TYP_CAL == INPUT1_TYPE || INPUT1_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
if ((ADC1_MIN_temp + 100 - ADC_PROTECT_THRESH) > 0 && (ADC1_MAX_temp - 100 + ADC_PROTECT_THRESH) < 4095 && INPUT1_MIN_CAL = INPUT1_MIN_temp + INPUT_MARGIN;
(ADC2_MIN_temp + 100 - ADC_PROTECT_THRESH) > 0 && (ADC2_MAX_temp - 100 + ADC_PROTECT_THRESH) < 4095) { INPUT1_MID_CAL = INPUT1_MID_temp;
adc_cal_valid = 1; INPUT1_MAX_CAL = INPUT1_MAX_temp - INPUT_MARGIN;
consoleLog("Input1 OK\n"); HAL_Delay(10);
} else { } else {
adc_cal_valid = 0; INPUT1_TYP_CAL = 0; // Disable input
consoleLog("FAIL (ADC out-of-range protection not possible)\n"); consoleLog("Input1 Fail\n"); HAL_Delay(10);
}
#endif
// Add final ADC margin to have exact 0 and MAX at the minimum and maximum ADC value
if (adc_cal_valid && (ADC1_MAX_temp - ADC1_MIN_temp) > 500 && (ADC2_MAX_temp - ADC2_MIN_temp) > 500) {
ADC1_MIN_CAL = ADC1_MIN_temp + 100;
ADC1_MID_CAL = ADC1_MID_temp;
ADC1_MAX_CAL = ADC1_MAX_temp - 100;
ADC2_MIN_CAL = ADC2_MIN_temp + 100;
ADC2_MID_CAL = ADC2_MID_temp;
ADC2_MAX_CAL = ADC2_MAX_temp - 100;
consoleLog("OK\n");
} else {
adc_cal_valid = 0;
consoleLog("FAIL (Pots travel too short)\n");
} }
INPUT2_TYP_CAL = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp);
if (INPUT2_TYP_CAL == INPUT2_TYPE || INPUT2_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
INPUT2_MIN_CAL = INPUT2_MIN_temp + INPUT_MARGIN;
INPUT2_MID_CAL = INPUT2_MID_temp;
INPUT2_MAX_CAL = INPUT2_MAX_temp - INPUT_MARGIN;
consoleLog("Input2 OK\n"); HAL_Delay(10);
} else {
INPUT2_TYP_CAL = 0; // Disable input
consoleLog("Input2 Fail\n"); HAL_Delay(10);
}
inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown
consoleLog("Limits: "); HAL_Delay(10);
setScopeChannel(0, (int16_t)INPUT1_TYP_CAL);
setScopeChannel(1, (int16_t)INPUT1_MIN_CAL);
setScopeChannel(2, (int16_t)INPUT1_MID_CAL);
setScopeChannel(3, (int16_t)INPUT1_MAX_CAL);
setScopeChannel(4, (int16_t)INPUT2_TYP_CAL);
setScopeChannel(5, (int16_t)INPUT2_MIN_CAL);
setScopeChannel(6, (int16_t)INPUT2_MID_CAL);
setScopeChannel(7, (int16_t)INPUT2_MAX_CAL);
consoleScope();
#endif #endif
} }
/* /*
* Update Maximum Motor Current Limit (via ADC1) and Maximum Speed Limit (via ADC2) * Update Maximum Motor Current Limit (via ADC1) and Maximum Speed Limit (via ADC2)
* Procedure: * Procedure:
@ -504,34 +573,50 @@ void updateCurSpdLim(void) {
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
return; return;
} }
#ifdef CONTROL_ADC
consoleLog("Torque and Speed limits update started... ");
int32_t adc1_fixdt = adc_buffer.l_tx2 << 16; #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
int32_t adc2_fixdt = adc_buffer.l_rx2 << 16; consoleLog("Torque and Speed limits update started...\n");
uint16_t cur_spd_timeout = 0;
int32_t input1_fixdt = input1 << 16;
int32_t input2_fixdt = input2 << 16;
uint16_t cur_factor; // fixdt(0,16,16) uint16_t cur_factor; // fixdt(0,16,16)
uint16_t spd_factor; // fixdt(0,16,16) uint16_t spd_factor; // fixdt(0,16,16)
uint16_t cur_spd_timeout = 0;
cur_spd_valid = 0;
// Wait for the power button press // Wait for the power button press
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout
filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt); readInput();
filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt); filtLowPass32(input1, FILTER, &input1_fixdt);
filtLowPass32(input2, FILTER, &input2_fixdt);
HAL_Delay(5); HAL_Delay(5);
} }
// Calculate scaling factors // Calculate scaling factors
cur_factor = CLAMP((adc1_fixdt - (ADC1_MIN_CAL << 16)) / (ADC1_MAX_CAL - ADC1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A cur_factor = CLAMP((input1_fixdt - ((int16_t)INPUT1_MIN_CAL << 16)) / ((int16_t)INPUT1_MAX_CAL - (int16_t)INPUT1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A
spd_factor = CLAMP((adc2_fixdt - (ADC2_MIN_CAL << 16)) / (ADC2_MAX_CAL - ADC2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm spd_factor = CLAMP((input2_fixdt - ((int16_t)INPUT2_MIN_CAL << 16)) / ((int16_t)INPUT2_MAX_CAL - (int16_t)INPUT2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm
// Update maximum limits if (INPUT1_TYP_CAL != 0){
rtP_Left.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4) // Update current limit
rtP_Left.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4) rtP_Left.i_max = rtP_Right.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
rtP_Right.i_max = rtP_Left.i_max; cur_spd_valid = 1; // Mark update to be saved in Flash at shutdown
rtP_Right.n_max = rtP_Left.n_max; }
cur_spd_valid = 1; if (INPUT2_TYP_CAL != 0){
consoleLog("OK\n"); // Update speed limit
rtP_Left.n_max = rtP_Right.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
cur_spd_valid += 2; // Mark update to be saved in Flash at shutdown
}
consoleLog("Limits: "); HAL_Delay(10);
setScopeChannel(0, (int16_t)cur_spd_valid); // 0 = No limit changed, 1 = Current limit changed, 2 = Speed limit changed, 3 = Both limits changed
setScopeChannel(1, (int16_t)input1_fixdt);
setScopeChannel(2, (int16_t)cur_factor);
setScopeChannel(3, (int16_t)rtP_Left.i_max);
setScopeChannel(4, (int16_t)0);
setScopeChannel(5, (int16_t)input2_fixdt);
setScopeChannel(6, (int16_t)spd_factor);
setScopeChannel(7, (int16_t)rtP_Left.n_max);
consoleScope();
#endif #endif
} }
@ -548,43 +633,25 @@ void saveConfig() {
HAL_FLASH_Lock(); HAL_FLASH_Lock();
} }
#endif #endif
#ifdef CONTROL_ADC #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
if (adc_cal_valid || cur_spd_valid) { if (inp_cal_valid || cur_spd_valid) {
HAL_FLASH_Unlock(); HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0], FLASH_WRITE_KEY); EE_WriteVariable(VirtAddVarTab[0] , FLASH_WRITE_KEY);
EE_WriteVariable(VirtAddVarTab[1], ADC1_MIN_CAL); EE_WriteVariable(VirtAddVarTab[1] , INPUT1_TYP_CAL);
EE_WriteVariable(VirtAddVarTab[2], ADC1_MAX_CAL); EE_WriteVariable(VirtAddVarTab[2] , INPUT1_MIN_CAL);
EE_WriteVariable(VirtAddVarTab[3], ADC1_MID_CAL); EE_WriteVariable(VirtAddVarTab[3] , INPUT1_MID_CAL);
EE_WriteVariable(VirtAddVarTab[4], ADC2_MIN_CAL); EE_WriteVariable(VirtAddVarTab[4] , INPUT1_MAX_CAL);
EE_WriteVariable(VirtAddVarTab[5], ADC2_MAX_CAL); EE_WriteVariable(VirtAddVarTab[5] , INPUT2_TYP_CAL);
EE_WriteVariable(VirtAddVarTab[6], ADC2_MID_CAL); EE_WriteVariable(VirtAddVarTab[6] , INPUT2_MIN_CAL);
EE_WriteVariable(VirtAddVarTab[7], rtP_Left.i_max); EE_WriteVariable(VirtAddVarTab[7] , INPUT2_MID_CAL);
EE_WriteVariable(VirtAddVarTab[8], rtP_Left.n_max); EE_WriteVariable(VirtAddVarTab[8] , INPUT2_MAX_CAL);
EE_WriteVariable(VirtAddVarTab[9] , rtP_Left.i_max);
EE_WriteVariable(VirtAddVarTab[10], rtP_Left.n_max);
HAL_FLASH_Lock(); HAL_FLASH_Lock();
} }
#endif #endif
} }
/*
* Add Dead-band to a signal
* This function realizes a dead-band around 0 and scales the input between [out_min, out_max]
*/
int addDeadBand(int16_t u, int16_t deadBand, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max) {
#if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT) || defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT)
int outVal = 0;
if(u > -deadBand && u < deadBand) {
outVal = 0;
} else if(u > 0) {
outVal = (out_max * CLAMP(u - deadBand, 0, in_max - deadBand)) / (in_max - deadBand);
} else {
outVal = (out_min * CLAMP(u + deadBand, in_min + deadBand, 0)) / (in_min + deadBand);
}
return outVal;
#else
return 0;
#endif
}
/* /*
* Standstill Hold Function * Standstill Hold Function
* This function uses Cruise Control to provide an anti-roll functionality at standstill. * This function uses Cruise Control to provide an anti-roll functionality at standstill.
@ -696,7 +763,7 @@ void poweroff(void) {
void poweroffPressCheck(void) { void poweroffPressCheck(void) {
#if defined(CONTROL_ADC) #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0; enable = 0;
uint16_t cnt_press = 0; uint16_t cnt_press = 0;
@ -705,7 +772,7 @@ void poweroffPressCheck(void) {
if (cnt_press++ == 5 * 100) { shortBeep(5); } if (cnt_press++ == 5 * 100) { shortBeep(5); }
} }
if (cnt_press >= 5 * 100) { // Check if press is more than 5 sec if (cnt_press >= 5 * 100) { // Check if press is more than 5 sec
HAL_Delay(300); HAL_Delay(1000);
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { // Double press: Adjust Max Current, Max Speed if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { // Double press: Adjust Max Current, Max Speed
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
longBeep(8); longBeep(8);
@ -751,17 +818,17 @@ void poweroffPressCheck(void) {
} }
/* =========================== Read Functions =========================== */
/* =========================== Read Command Function =========================== */ /*
* Function to read the raw Input values from various input devices
void readCommand(void) { */
void readInput(void) {
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) #if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK)
if (nunchuk_connected != 0) { if (nunchuk_connected != 0) {
Nunchuk_Read(); Nunchuk_Read();
cmd1 = CLAMP((nunchuk_data[0] - 127) * 8, INPUT_MIN, INPUT_MAX); // x - axis. Nunchuk joystick readings range 30 - 230 input1 = (nunchuk_data[0] - 127) * 8; // X axis 0-255
cmd2 = CLAMP((nunchuk_data[1] - 128) * 8, INPUT_MIN, INPUT_MAX); // y - axis input2 = (nunchuk_data[1] - 128) * 8; // Y axis 0-255
#ifdef SUPPORT_BUTTONS #ifdef SUPPORT_BUTTONS
button1 = (uint8_t)nunchuk_data[5] & 1; button1 = (uint8_t)nunchuk_data[5] & 1;
button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1; button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1;
@ -770,50 +837,53 @@ void readCommand(void) {
#endif #endif
#if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT) #if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)
cmd1 = addDeadBand((ppm_captured_value[0] - 500) * 2, PPM_DEADBAND, PPM_CH1_MIN, PPM_CH1_MAX, INPUT_MIN, INPUT_MAX); input1 = (ppm_captured_value[0] - 500) * 2;
cmd2 = addDeadBand((ppm_captured_value[1] - 500) * 2, PPM_DEADBAND, PPM_CH2_MIN, PPM_CH2_MAX, INPUT_MIN, INPUT_MAX); input2 = (ppm_captured_value[1] - 500) * 2;
#ifdef SUPPORT_BUTTONS #ifdef SUPPORT_BUTTONS
button1 = ppm_captured_value[5] > 500; button1 = ppm_captured_value[5] > 500;
button2 = 0; button2 = 0;
#elif defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif #endif
// float scale = ppm_captured_value[2] / 1000.0f; // not used for now, uncomment if needed
#endif #endif
#if defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT) #if defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT)
cmd1 = addDeadBand((pwm_captured_ch1_value - 500) * 2, PWM_DEADBAND, PWM_CH1_MIN, PWM_CH1_MAX, INPUT_MIN, INPUT_MAX); input1 = (pwm_captured_ch1_value - 500) * 2;
#if !defined(VARIANT_SKATEBOARD) input2 = (pwm_captured_ch2_value - 500) * 2;
cmd2 = addDeadBand((pwm_captured_ch2_value - 500) * 2, PWM_DEADBAND, PWM_CH2_MIN, PWM_CH2_MAX, INPUT_MIN, INPUT_MAX);
#else
cmd2 = addDeadBand((pwm_captured_ch2_value - 500) * 2, PWM_DEADBAND, PWM_CH2_MIN, PWM_CH2_MAX, PWM_CH2_OUT_MIN, INPUT_MAX);
#endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif
#endif #endif
#ifdef CONTROL_ADC #ifdef CONTROL_ADC
// ADC values range: 0-4095, see ADC-calibration in config.h // ADC values range: 0-4095, see ADC-calibration in config.h
#ifdef ADC1_MID_POT input1 = adc_buffer.l_tx2;
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MID_CAL) * INPUT_MAX / (ADC1_MAX_CAL - ADC1_MID_CAL), 0, INPUT_MAX) input2 = adc_buffer.l_rx2;
+CLAMP((ADC1_MID_CAL - adc_buffer.l_tx2) * INPUT_MIN / (ADC1_MID_CAL - ADC1_MIN_CAL), INPUT_MIN, 0); // ADC1 timeoutCnt = 0;
#else
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN_CAL) * INPUT_MAX / (ADC1_MAX_CAL - ADC1_MIN_CAL), 0, INPUT_MAX); // ADC1
#endif #endif
#ifdef ADC2_MID_POT #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID_CAL) * INPUT_MAX / (ADC2_MAX_CAL - ADC2_MID_CAL), 0, INPUT_MAX) // Handle received data validity, timeout and fix out-of-sync if necessary
+CLAMP((ADC2_MID_CAL - adc_buffer.l_rx2) * INPUT_MIN / (ADC2_MID_CAL - ADC2_MIN_CAL), INPUT_MIN, 0); // ADC2 #ifdef CONTROL_IBUS
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) {
ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000
}
input1 = (ibus_captured_value[0] - 500) * 2;
input2 = (ibus_captured_value[1] - 500) * 2;
#else #else
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN_CAL) * INPUT_MAX / (ADC2_MAX_CAL - ADC2_MIN_CAL), 0, INPUT_MAX); // ADC2 input1 = command.steer;
input2 = command.speed;
#endif #endif
timeoutCnt = 0;
#endif
}
#ifdef ADC_PROTECT_ENA /*
if (adc_buffer.l_tx2 >= (ADC1_MIN_CAL - ADC_PROTECT_THRESH) && adc_buffer.l_tx2 <= (ADC1_MAX_CAL + ADC_PROTECT_THRESH) && * Function to calculate the command to the motors. This function also manages:
adc_buffer.l_rx2 >= (ADC2_MIN_CAL - ADC_PROTECT_THRESH) && adc_buffer.l_rx2 <= (ADC2_MAX_CAL + ADC_PROTECT_THRESH)) { * - timeout detection
* - MIN/MAX limitations and deadband
*/
void readCommand(void) {
readInput();
#ifdef CONTROL_ADC
// If input1 or Input2 is either below MIN - Threshold or above MAX + Threshold, ADC protection timeout
if (IN_RANGE(input1, (int16_t)INPUT1_MIN_CAL - ADC_PROTECT_THRESH, (int16_t)INPUT1_MAX_CAL + ADC_PROTECT_THRESH) &&
IN_RANGE(input2, (int16_t)INPUT2_MIN_CAL - ADC_PROTECT_THRESH, (int16_t)INPUT2_MAX_CAL + ADC_PROTECT_THRESH)){
if (timeoutFlagADC) { // Check for previous timeout flag if (timeoutFlagADC) { // Check for previous timeout flag
if (timeoutCntADC-- <= 0) // Timeout de-qualification if (timeoutCntADC-- <= 0) // Timeout de-qualification
timeoutFlagADC = 0; // Timeout flag cleared timeoutFlagADC = 0; // Timeout flag cleared
@ -828,35 +898,6 @@ void readCommand(void) {
} }
#endif #endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif
timeoutCnt = 0;
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
// Handle received data validity, timeout and fix out-of-sync if necessary
#ifdef CONTROL_IBUS
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) {
ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000
}
cmd1 = CLAMP((ibus_captured_value[0] - 500) * 2, INPUT_MIN, INPUT_MAX);
cmd2 = CLAMP((ibus_captured_value[1] - 500) * 2, INPUT_MIN, INPUT_MAX);
#else
if (IN_RANGE(command.steer, INPUT_MIN, INPUT_MAX) && IN_RANGE(command.speed, INPUT_MIN, INPUT_MAX)) {
cmd1 = command.steer;
cmd2 = command.speed;
}
#endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif
timeoutCnt = 0;
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) #if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2)
if (timeoutCntSerial_L++ >= SERIAL_TIMEOUT) { // Timeout qualification if (timeoutCntSerial_L++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial_L = 1; // Timeout detected timeoutFlagSerial_L = 1; // Timeout detected
@ -875,8 +916,13 @@ void readCommand(void) {
timeoutFlagSerial = timeoutFlagSerial_L || timeoutFlagSerial_R; timeoutFlagSerial = timeoutFlagSerial_L || timeoutFlagSerial_R;
#endif #endif
#ifdef VARIANT_HOVERCAR #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
brakePressed = (uint8_t)(cmd1 > 50); cmd1 = addDeadBand(input1, INPUT1_TYP_CAL, INPUT1_DEADBAND, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL, INPUT_MIN, INPUT_MAX);
#if !defined(VARIANT_SKATEBOARD)
cmd2 = addDeadBand(input2, INPUT2_TYP_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT_MIN, INPUT_MAX);
#else
cmd2 = addDeadBand(input2, INPUT2_TYP_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT2_BRAKE, INPUT_MAX);
#endif
#endif #endif
#ifdef VARIANT_TRANSPOTTER #ifdef VARIANT_TRANSPOTTER
@ -890,6 +936,10 @@ void readCommand(void) {
#endif #endif
#endif #endif
#ifdef VARIANT_HOVERCAR
brakePressed = (uint8_t)(cmd1 > 50);
#endif
if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // In case of timeout bring the system to a Safe State if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // In case of timeout bring the system to a Safe State
ctrlModReq = OPEN_MODE; // Request OPEN_MODE. This will bring the motor power to 0 in a controlled way ctrlModReq = OPEN_MODE; // Request OPEN_MODE. This will bring the motor power to 0 in a controlled way
cmd1 = 0; cmd1 = 0;
@ -898,6 +948,11 @@ void readCommand(void) {
ctrlModReq = ctrlModReqRaw; // Follow the Mode request ctrlModReq = ctrlModReqRaw; // Follow the Mode request
} }
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif
#if defined(CRUISE_CONTROL_SUPPORT) && (defined(SUPPORT_BUTTONS) || defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)) #if defined(CRUISE_CONTROL_SUPPORT) && (defined(SUPPORT_BUTTONS) || defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT))
cruiseControl(button1); // Cruise control activation/deactivation cruiseControl(button1); // Cruise control activation/deactivation
#endif #endif