Update util.c

This commit is contained in:
Candas1 2020-10-28 19:02:21 +01:00 committed by GitHub
parent 1b32555fb0
commit 78246f4e53
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 39 additions and 34 deletions

View File

@ -740,36 +740,33 @@ void readCommand(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 cmd1_in = (nunchuk_data[0] - 127) * 8; // X axis 0-255
cmd2 = CLAMP((nunchuk_data[1] - 128) * 8, INPUT_MIN, INPUT_MAX); // y - axis cmd2_in = (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;
#endif #endif
} }
#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, 0, PPM_CH1_MAX, INPUT_MIN, INPUT_MAX); cmd1_in = (ppm_captured_value[0] - 500) * 2;
cmd2 = addDeadBand((ppm_captured_value[1] - 500) * 2, PPM_DEADBAND, PPM_CH2_MIN, 0, PPM_CH2_MAX, INPUT_MIN, INPUT_MAX); cmd2_in = (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) #elif defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN); button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_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 // 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, 0, PWM_CH1_MAX, INPUT_MIN, INPUT_MAX); cmd1_in = (pwm_captured_ch1_value - 500) * 2;
#if !defined(VARIANT_SKATEBOARD) cmd2_in = (pwm_captured_ch2_value - 500) * 2;
cmd2 = addDeadBand((pwm_captured_ch2_value - 500) * 2, PWM_DEADBAND, PWM_CH2_MIN, 0, PWM_CH2_MAX, INPUT_MIN, INPUT_MAX);
#else
cmd2 = addDeadBand((pwm_captured_ch2_value - 500) * 2, PWM_DEADBAND, PWM_CH2_MIN, 0, PWM_CH2_MAX, PWM_CH2_OUT_MIN, INPUT_MAX);
#endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT) #if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN); button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN); button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
@ -778,19 +775,8 @@ void readCommand(void) {
#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 cmd1_in = adc_buffer.l_tx2;
cmd1 = addDeadBand(adc_buffer.l_tx2, ADC_DEADBAND, ADC1_MIN_CAL, ADC1_MID_CAL, ADC1_MAX_CAL, INPUT_MIN, INPUT_MAX); cmd2_in = adc_buffer.l_rx2;
#else
// cmd1 = MAP( adc_buffer.l_tx2 , ADC1_MIN_CAL, ADC1_MAX_CAL, 0, INPUT_MAX ); // ADC1
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN_CAL) * INPUT_MAX / (ADC1_MAX_CAL - ADC1_MIN_CAL), 0, INPUT_MAX); // ADC1
#endif
#ifdef ADC2_MID_POT
cmd2 = addDeadBand(adc_buffer.l_rx2, ADC_DEADBAND, ADC2_MIN_CAL, ADC2_MID_CAL, ADC2_MAX_CAL, INPUT_MIN, INPUT_MAX);
#else
// cmd2 = MAP( adc_buffer.l_rx2 , ADC2_MIN_CAL, ADC2_MAX_CAL, 0, INPUT_MAX ); // ADC2
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN_CAL) * INPUT_MAX / (ADC2_MAX_CAL - ADC2_MIN_CAL), 0, INPUT_MAX); // ADC2
#endif
#ifdef ADC_PROTECT_ENA #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) && if (adc_buffer.l_tx2 >= (ADC1_MIN_CAL - ADC_PROTECT_THRESH) && adc_buffer.l_tx2 <= (ADC1_MAX_CAL + ADC_PROTECT_THRESH) &&
@ -822,8 +808,8 @@ void readCommand(void) {
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) { 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 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); cmd1_in = (ibus_captured_value[0] - 500) * 2;
cmd2 = CLAMP((ibus_captured_value[1] - 500) * 2, INPUT_MIN, INPUT_MAX); cmd2_in = (ibus_captured_value[1] - 500) * 2;
#else #else
if (IN_RANGE(command.steer, INPUT_MIN, INPUT_MAX) && IN_RANGE(command.speed, INPUT_MIN, INPUT_MAX)) { if (IN_RANGE(command.steer, INPUT_MIN, INPUT_MAX) && IN_RANGE(command.speed, INPUT_MIN, INPUT_MAX)) {
cmd1 = command.steer; cmd1 = command.steer;
@ -855,6 +841,25 @@ void readCommand(void) {
#if defined(SIDEBOARD_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART3) #if defined(SIDEBOARD_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART3)
timeoutFlagSerial = timeoutFlagSerial_L || timeoutFlagSerial_R; timeoutFlagSerial = timeoutFlagSerial_L || timeoutFlagSerial_R;
#endif #endif
cmd1 = addDeadBand(cmd1_in, INPUT1_DEADBAND, INPUT1_MIN, INPUT1_MID, INPUT1_MAX, INPUT_MIN, INPUT_MAX);
#if !defined(VARIANT_SKATEBOARD)
cmd2 = addDeadBand(cmd2_in, INPUT2_DEADBAND, INPUT2_MIN, INPUT2_MID, INPUT2_MAX, INPUT_MIN, INPUT_MAX);
#else
cmd2 = addDeadBand(cmd2_in, INPUT2_DEADBAND, INPUT2_MIN, INPUT2_MID, INPUT2_MAX, INPUT_OUT_MIN, INPUT_MAX);
#endif
#ifdef ADC1_MID_POT
#else
cmd1 = MAP( adc_buffer.l_tx2 , ADC1_MIN_CAL, ADC1_MAX_CAL, 0, INPUT_MAX ); // ADC1
#endif
#ifdef ADC2_MID_POT
#else
cmd2 = MAP( adc_buffer.l_rx2 , ADC2_MIN_CAL, ADC2_MAX_CAL, 0, INPUT_MAX ); // ADC2
#endif
#ifdef VARIANT_HOVERCAR #ifdef VARIANT_HOVERCAR
brakePressed = (uint8_t)(cmd1 > 50); brakePressed = (uint8_t)(cmd1 > 50);