From d5033c3b00022a6aaf88553fb50d3382363bf909 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20S=C3=A1nchez=20de=20Le=C3=B3n=20Peque?= Date: Tue, 17 Apr 2018 20:10:29 +0200 Subject: [PATCH 1/9] Remove TIM1 interruptions and configuration setup --- src/setup.c | 39 --------------------------------------- src/target/detection.c | 14 -------------- 2 files changed, 53 deletions(-) diff --git a/src/setup.c b/src/setup.c index 094d578d..b49db2c6 100644 --- a/src/setup.c +++ b/src/setup.c @@ -63,11 +63,9 @@ static void setup_clock(void) * * - Systick priority to 1 with SCB. * - USART3 with priority 2 with NVIC. - * - TIM1_UP with priority 0. * * Interruptions enabled: * - * - TIM1 Update interrupt. * - USART3 interrupt. * * @note The priority levels are assigned on steps of 16 because the processor @@ -77,11 +75,9 @@ static void setup_clock(void) */ static void setup_exceptions(void) { - nvic_set_priority(NVIC_TIM1_UP_IRQ, 0); nvic_set_priority(NVIC_SYSTICK_IRQ, PRIORITY_FACTOR * 1); nvic_set_priority(NVIC_USART3_IRQ, PRIORITY_FACTOR * 2); - nvic_enable_irq(NVIC_TIM1_UP_IRQ); nvic_enable_irq(NVIC_USART3_IRQ); } @@ -366,40 +362,6 @@ static void setup_adc1(void) adc_calibrate(ADC1); } -/** - * @brief TIM1 setup. - * - * The TIM1 generates an update event interruption that invokes the - * function tim1_up_isr. - * - * - Set TIM1 default values. - * - Configure the base time (no clock division ratio, no aligned mode, - * direction up). - * - Set clock division, prescaler and period parameters to get an update - * event with a frequency of 16 KHz. 16 interruptions by ms, 4 sensors with - * 4 states. - * - * \f$frequency = \frac{timerclock}{(preescaler + 1)(period + 1)}\f$ - * - * - Enable the TIM1. - * - Enable the interruption of type update event on the TIM1. - * - * @note The TIM1 is conected to the APB2 prescaler. - * - * @see Reference manual (RM0008) "Advanced-control timers" - */ -static void setup_timer1(void) -{ - rcc_periph_reset_pulse(RST_TIM1); - timer_set_mode(TIM1, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, - TIM_CR1_DIR_UP); - timer_set_clock_division(TIM1, 0x00); - timer_set_prescaler(TIM1, (rcc_apb2_frequency / 160000 - 1)); - timer_set_period(TIM1, 10 - 1); - timer_enable_counter(TIM1); - timer_enable_irq(TIM1, TIM_DIER_UIE); -} - /** * @brief Execute all setup functions. */ @@ -413,6 +375,5 @@ void setup(void) setup_pwm(); setup_mpu(); setup_systick(); - setup_timer1(); setup_adc1(); } diff --git a/src/target/detection.c b/src/target/detection.c index 4820452d..2d09b658 100644 --- a/src/target/detection.c +++ b/src/target/detection.c @@ -246,20 +246,6 @@ static void sm_emitter_adc(void) } } -/** - * @brief TIM1 interruption routine. - * - * - Manage the update event interruption flag. - * - Trigger state machine to manage sensors. - */ -void tim1_up_isr(void) -{ - if (timer_get_flag(TIM1, TIM_SR_UIF)) { - timer_clear_flag(TIM1, TIM_SR_UIF); - sm_emitter_adc(); - } -} - /** * @brief Get sensors values with emitter on and off. */ From 78072219b7f6c46c52d4d78c22ff7875b50b698c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20S=C3=A1nchez=20de=20Le=C3=B3n=20Peque?= Date: Fri, 20 Apr 2018 00:45:08 +0200 Subject: [PATCH 2/9] Read sensors with systick and just before control --- src/setup.c | 2 +- src/target/detection.c | 87 +++++++++++++++--------------------------- 2 files changed, 32 insertions(+), 57 deletions(-) diff --git a/src/setup.c b/src/setup.c index b49db2c6..98a3f889 100644 --- a/src/setup.c +++ b/src/setup.c @@ -370,10 +370,10 @@ void setup(void) setup_clock(); setup_exceptions(); setup_gpio(); + setup_adc1(); setup_usart(); setup_encoders(); setup_pwm(); setup_mpu(); setup_systick(); - setup_adc1(); } diff --git a/src/target/detection.c b/src/target/detection.c index 2d09b658..d194b874 100644 --- a/src/target/detection.c +++ b/src/target/detection.c @@ -3,7 +3,6 @@ #define SIDE_WALL_DETECTION (CELL_DIMENSION * 0.90) #define FRONT_WALL_DETECTION (CELL_DIMENSION * 1.5) #define SIDE_CALIBRATION_READINGS 20 -#define SENSORS_SM_TICKS 4 #define LOG_CONVERSION_TABLE_STEP 4 #define LOG_CONVERSION_TABLE_SIZE (ADC_RESOLUTION / LOG_CONVERSION_TABLE_STEP) @@ -190,72 +189,47 @@ static void set_emitter_off(uint8_t emitter) } /** - * @brief State machine to manage the sensors activation and deactivation - * states and readings. - * - * In order to get accurate distance values, the phototransistor's output - * will be read with the infrared emitter sensors powered on and powered - * off. Besides, to avoid undesired interactions between different emitters and - * phototranistors, the reads will be done one by one. - * - * The battery voltage is also read on the state 1. - * - * - State 1 (first because the emitter is OFF on start): - * -# Save phototranistors sensors (ADC1) from emitter OFF and - * power ON the emitter. - * - State 2: - * -# Start the phototranistors sensors (ADC1) read. - * - State 3: - * -# Save phototranistors sensors (ADC1) from emitter ON and - * power OFF the emitter. - * - State 4: - * -# Start the phototranistors sensors (ADC1) read. + * @brief Get sensors values with emitter on and off. */ -static void sm_emitter_adc(void) +void get_sensors_raw(uint16_t *off, uint16_t *on) { - static uint8_t emitter_status = 1; - static uint8_t sensor_index = SENSOR_SIDE_LEFT_ID; + uint8_t i = 0; - switch (emitter_status) { - case 1: - sensors_off[sensor_index] = - adc_read_injected(ADC1, (sensor_index + 1)); - set_emitter_on(sensor_index); - emitter_status = 2; - break; - case 2: - adc_start_conversion_injected(ADC1); - emitter_status = 3; - break; - case 3: - sensors_on[sensor_index] = - adc_read_injected(ADC1, (sensor_index + 1)); - set_emitter_off(sensor_index); - emitter_status = 4; - break; - case 4: - adc_start_conversion_injected(ADC1); - emitter_status = 1; - if (sensor_index == (NUM_SENSOR - 1)) - sensor_index = 0; - else - sensor_index++; - break; - default: - break; + for (i = 0; i < NUM_SENSOR; i++) { + off[i] = sensors_off[i]; + on[i] = sensors_on[i]; } } /** - * @brief Get sensors values with emitter on and off. + * @brief Update the sensors raw readings. */ -void get_sensors_raw(uint16_t *off, uint16_t *on) +static void inject_readings(void) +{ + adc_start_conversion_injected(ADC1); + while (!(adc_eoc_injected(ADC1))) + ; + // Clear injected end of conversion + ADC_SR(ADC1) &= ~ADC_SR_JEOC; +} + +/** + * @brief Update the sensors raw readings. + */ +static void update_raw_readings(void) { uint8_t i = 0; + inject_readings(); + for (i = 0; i < NUM_SENSOR; i++) + sensors_off[i] = adc_read_injected(ADC1, (i + 1)); + for (i = 0; i < NUM_SENSOR; i++) { - off[i] = sensors_off[i]; - on[i] = sensors_on[i]; + set_emitter_on(i); + sleep_us(10); + inject_readings(); + sensors_on[i] = adc_read_injected(ADC1, (i + 1)); + set_emitter_off(i); } } @@ -291,6 +265,7 @@ void update_distance_readings(void) { uint8_t i = 0; + update_raw_readings(); for (i = 0; i < NUM_SENSOR; i++) { distance[i] = (sensors_calibration_a[i] / raw_log(sensors_on[i], sensors_off[i]) - @@ -433,7 +408,7 @@ void side_sensors_calibration(void) for (i = 0; i < SIDE_CALIBRATION_READINGS; i++) { left_temp += distance[SENSOR_SIDE_LEFT_ID]; right_temp += distance[SENSOR_SIDE_RIGHT_ID]; - sleep_ticks(SENSORS_SM_TICKS); + sleep_ticks(2); } calibration_factor[SENSOR_SIDE_LEFT_ID] += (left_temp / SIDE_CALIBRATION_READINGS) - MIDDLE_MAZE_DISTANCE; From fc7001f84c19be30f00f05e5aa267ac1736ef578 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20S=C3=A1nchez=20de=20Le=C3=B3n=20Peque?= Date: Thu, 24 May 2018 01:54:40 +0200 Subject: [PATCH 3/9] More stable sensors readings --- src/target/detection.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/target/detection.c b/src/target/detection.c index d194b874..38011169 100644 --- a/src/target/detection.c +++ b/src/target/detection.c @@ -220,9 +220,14 @@ static void update_raw_readings(void) { uint8_t i = 0; - inject_readings(); - for (i = 0; i < NUM_SENSOR; i++) + for (i = 0; i < NUM_SENSOR; i++) { + inject_readings(); sensors_off[i] = adc_read_injected(ADC1, (i + 1)); + } + for (i = 0; i < NUM_SENSOR; i++) { + inject_readings(); + sensors_off[i] = adc_read_injected(ADC1, (i + 1)); + } for (i = 0; i < NUM_SENSOR; i++) { set_emitter_on(i); From 0860dedbda56666a9bbe9c24b1b10f620215c771 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20S=C3=A1nchez=20de=20Le=C3=B3n=20Peque?= Date: Mon, 8 Oct 2018 20:20:54 +0200 Subject: [PATCH 4/9] Simplify main function --- src/main.c | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/main.c b/src/main.c index 433198e1..01edc7cc 100644 --- a/src/main.c +++ b/src/main.c @@ -116,6 +116,20 @@ static void competition(void) running_phase(); } +/** + * @brief Training routine. + */ +static void training(void) +{ + initialize_solver_direction(); + add_goal(3, 5); + set_target_goal(); + exploration_phase(); + set_run_sequence(); + while (1) + running_phase(); +} + /** * @brief Initial setup and infinite wait. */ @@ -123,16 +137,10 @@ int main(void) { setup(); systick_interrupt_enable(); - competition(); - add_goal(3, 5); - set_target_goal(); - initialize_solver_direction(); - set_speed_mode(0, false); + // competition(); while (1) { if (button_left_read_consecutive(500)) { - before_moving(); - explore(); - after_moving(); + training(); } execute_commands(); } From 6faa22adf5dc3d9b981773b38e1074669aaa13e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20S=C3=A1nchez=20de=20Le=C3=B3n=20Peque?= Date: Mon, 8 Oct 2018 20:42:26 +0200 Subject: [PATCH 5/9] fixup! Read sensors with systick and just before control --- src/target/detection.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/target/detection.c b/src/target/detection.c index 38011169..9150e4a5 100644 --- a/src/target/detection.c +++ b/src/target/detection.c @@ -202,7 +202,7 @@ void get_sensors_raw(uint16_t *off, uint16_t *on) } /** - * @brief Update the sensors raw readings. + * @brief Start and wait for complete injection of sensor readings. */ static void inject_readings(void) { From 51e31c5b1f9ca0ae860317f5d325abf29909f966 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20S=C3=A1nchez=20de=20Le=C3=B3n=20Peque?= Date: Mon, 8 Oct 2018 21:47:03 +0200 Subject: [PATCH 6/9] fixup! Simplify main function --- src/main.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main.c b/src/main.c index 01edc7cc..9e33b2ad 100644 --- a/src/main.c +++ b/src/main.c @@ -137,11 +137,10 @@ int main(void) { setup(); systick_interrupt_enable(); - // competition(); + competition(); while (1) { - if (button_left_read_consecutive(500)) { + if (button_left_read_consecutive(500)) training(); - } execute_commands(); } From 4d45d2f7127365acf83b464c36b1118f7b3f188a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20S=C3=A1nchez=20de=20Le=C3=B3n=20Peque?= Date: Wed, 10 Oct 2018 01:58:12 +0200 Subject: [PATCH 7/9] Enable speaker sounds --- src/main.c | 1 + src/setup.c | 43 +++++++++++++++++++++++++++++++-- src/setup.h | 1 + src/speaker.c | 66 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/speaker.h | 12 ++++++++++ 5 files changed, 121 insertions(+), 2 deletions(-) create mode 100644 src/speaker.c create mode 100644 src/speaker.h diff --git a/src/main.c b/src/main.c index 9e33b2ad..d07d0c0d 100644 --- a/src/main.c +++ b/src/main.c @@ -12,6 +12,7 @@ #include "serial.h" #include "setup.h" #include "solve.h" +#include "speaker.h" #include "speed.h" static void competition(void); diff --git a/src/setup.c b/src/setup.c index 98a3f889..36b73261 100644 --- a/src/setup.c +++ b/src/setup.c @@ -214,7 +214,7 @@ void setup_spi_low_speed(void) * @see Reference manual (RM0008) "TIMx functional description" and in * particular "PWM mode" section. */ -static void setup_pwm(void) +static void setup_motor_driver(void) { timer_set_mode(TIM3, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); @@ -248,6 +248,44 @@ static void setup_pwm(void) timer_enable_counter(TIM3); } +/** + * @brief Setup PWM for the speaker. + * + * TIM1 is used to generate the PWM signals for the speaker: + * + * - Configure channel 3 as output GPIO. + * - Edge-aligned, up-counting timer. + * - Prescale to increment timer counter at TIM1CLK_FREQUENCY_HZ. + * - Set output compare mode to PWM1 (output is active when the counter is + * less than the compare register contents and inactive otherwise. + * - Disable output compare output (speaker is off by default). + * - Enable outputs in the break subsystem. + * + * @see Reference manual (RM0008) "TIMx functional description" and in + * particular "PWM mode" section. + */ +static void setup_speaker(void) +{ + gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, + GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_TIM1_CH3); + + rcc_periph_reset_pulse(RST_TIM1); + + timer_set_mode(TIM1, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, + TIM_CR1_DIR_UP); + + timer_set_prescaler(TIM1, + (rcc_apb2_frequency / TIM1CLK_FREQUENCY_HZ - 1)); + timer_set_repetition_counter(TIM1, 0); + timer_enable_preload(TIM1); + timer_continuous_mode(TIM1); + + timer_disable_oc_output(TIM1, TIM_OC3); + timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_PWM1); + + timer_enable_break_main_output(TIM1); +} + /** * @brief Configure timer to read a quadrature encoder. * @@ -373,7 +411,8 @@ void setup(void) setup_adc1(); setup_usart(); setup_encoders(); - setup_pwm(); + setup_motor_driver(); + setup_speaker(); setup_mpu(); setup_systick(); } diff --git a/src/setup.h b/src/setup.h index 8da9b7ea..403247fe 100644 --- a/src/setup.h +++ b/src/setup.h @@ -26,6 +26,7 @@ /** System clock frequency is set in `setup_clock` */ #define SYSCLK_FREQUENCY_HZ 72000000 +#define TIM1CLK_FREQUENCY_HZ 1000000 #define SYSTICK_FREQUENCY_HZ 1000 #define DRIVER_PWM_PERIOD 1024 diff --git a/src/speaker.c b/src/speaker.c new file mode 100644 index 00000000..c38018e2 --- /dev/null +++ b/src/speaker.c @@ -0,0 +1,66 @@ +#include "speaker.h" + +uint8_t BASE_NOTE = 9; +uint8_t BASE_OCTAVE = 4; +float BASE_FREQUENCY = 440.; +uint8_t POSITIONS[8] = {9, 11, 0, 2, 4, 5, 7}; + +/** + * @brief Set the frequency for the speaker. + * + * Frequency is set modulating the PWM signal sent to the speaker. + * + * @param[in] hz Frequency, in Hertz. + */ +static void speaker_set_frequency(float hz) +{ + uint16_t period; + + period = (uint16_t)(TIM1CLK_FREQUENCY_HZ / hz); + timer_set_period(TIM1, period); + timer_set_oc_value(TIM1, TIM_OC3, period / 2); +} + +/** + * @brief Turn on the speaker to play the set frequency. + */ +static void speaker_on(void) +{ + timer_enable_counter(TIM1); + timer_enable_oc_output(TIM1, TIM_OC3); +} + +/** + * @brief Turn off the speaker. + */ +static void speaker_off(void) +{ + timer_disable_counter(TIM1); + timer_disable_oc_output(TIM1, TIM_OC3); +} + +/** + * @brief Play a note through the speaker. + * + * Even if this function uses the scientific notation, notes are played with + * the concert pitch (standard pitch). That means A above middle C is the + * reference note, played at 440 Hz. + * + * @param[in] note Which note to play, in scientific notation. + * @param[in] octave Which octave to play, in scientific notation. + * @param[in] accidental Number of semitones to sum to the note. + * @param[in] duration Duration of the note, in seconds. + */ +void speaker_play(char note, uint8_t octave, int8_t accidental, float duration) +{ + int16_t sound; + float frequency; + + sound = POSITIONS[note - 'A'] - BASE_NOTE + (octave - BASE_OCTAVE) * 12; + sound += accidental; + frequency = pow(2, sound / 12.) * BASE_FREQUENCY; + speaker_set_frequency(frequency); + speaker_on(); + sleep_ticks((uint32_t)(duration * 1000)); + speaker_off(); +} diff --git a/src/speaker.h b/src/speaker.h new file mode 100644 index 00000000..39b90066 --- /dev/null +++ b/src/speaker.h @@ -0,0 +1,12 @@ +#ifndef __SPEAKER_H +#define __SPEAKER_H + +#include + +#include + +#include "setup.h" + +void speaker_play(char note, uint8_t octave, int8_t accidental, float duration); + +#endif /* __SPEAKER_H */ From b5b56eb1c548109fde0a369b97475e947e4ec382 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20S=C3=A1nchez=20de=20Le=C3=B3n=20Peque?= Date: Fri, 12 Oct 2018 21:19:12 +0200 Subject: [PATCH 8/9] Enable battery voltage readings again --- src/battery.c | 26 +++++++------------------ src/setup.c | 54 ++++++++++++++++++++++++++++++++++++++++++--------- 2 files changed, 52 insertions(+), 28 deletions(-) diff --git a/src/battery.c b/src/battery.c index 8b833650..0693aa4a 100644 --- a/src/battery.c +++ b/src/battery.c @@ -1,23 +1,5 @@ #include "battery.h" -/** - * @brief ADC1 and ADC2 interruption routine. - * - * - Manage the ADC2 analog watchdog interruption flag. - * - Send a message. - * - Toggle a LED. - * - Disable analog watchdog interruptions on injected channels. - */ -void adc1_2_isr(void) -{ - if (adc_get_flag(ADC2, ADC_SR_AWD)) { - adc_clear_flag(ADC2, ADC_SR_AWD); - LOG_WARNING("Battery low!"); - led_bluepill_toggle(); - adc_disable_analog_watchdog_injected(ADC2); - } -} - /** * @brief Function to get battery voltage. * @@ -33,7 +15,13 @@ void adc1_2_isr(void) float get_battery_voltage(void) { uint16_t battery_bits; + uint8_t channels[16]; - battery_bits = adc_read_injected(ADC2, 3); + channels[0] = ADC_CHANNEL0; + adc_set_regular_sequence(ADC2, 1, channels); + adc_start_conversion_direct(ADC2); + while (!adc_eoc(ADC2)) + ; + battery_bits = adc_read_regular(ADC2); return battery_bits * ADC_LSB * VOLT_DIV_FACTOR; } diff --git a/src/setup.c b/src/setup.c index 36b73261..8d446b59 100644 --- a/src/setup.c +++ b/src/setup.c @@ -355,6 +355,23 @@ void disable_systick_interruption(void) systick_interrupt_disable(); } +/** + * @brief Start the given ADC register base address. + * + * - Power on the ADC and wait for ADC starting up (at least 3 us). + * - Calibrate the ADC. + * + * @see Reference manual (RM0008) "Analog-to-digital converter". + */ +static void start_adc(uint32_t adc) +{ + adc_power_on(adc); + for (int i = 0; i < 800000; i++) + __asm__("nop"); + adc_reset_calibration(adc); + adc_calibrate(adc); +} + /** * @brief Setup for ADC 1: Four injected channels on scan mode. * @@ -366,8 +383,7 @@ void disable_systick_interruption(void) * - Configure the alignment (right) and the sample time (13.5 cycles of ADC * clock). * - Set injected sequence with channel_sequence structure. - * - Power on the ADC and wait for ADC starting up (at least 3 us). - * - Calibrate the ADC. + * - Start the ADC. * * @note This ADC reads phototransistor sensors measurements. * @@ -379,8 +395,6 @@ void disable_systick_interruption(void) */ static void setup_adc1(void) { - int i; - uint8_t channel_sequence[4] = {ADC_CHANNEL4, ADC_CHANNEL3, ADC_CHANNEL5, ADC_CHANNEL2}; @@ -393,11 +407,32 @@ static void setup_adc1(void) adc_set_injected_sequence( ADC1, sizeof(channel_sequence) / sizeof(channel_sequence[0]), channel_sequence); - adc_power_on(ADC1); - for (i = 0; i < 800000; i++) - __asm__("nop"); - adc_reset_calibration(ADC1); - adc_calibrate(ADC1); + start_adc(ADC1); +} + +/** + * @brief Setup for ADC 2: configured for regular conversion. + * + * - Power off the ADC to be sure that does not run during configuration. + * - Disable scan mode. + * - Set single conversion mode triggered by software. + * - Configure the alignment (right) and the sample time (13.5 cycles of ADC + * clock). + * - Start the ADC. + * + * @note This ADC reads the battery status. + * + * @see Reference manual (RM0008) "Analog-to-digital converter". + */ +static void setup_adc2(void) +{ + adc_power_off(ADC2); + adc_disable_scan_mode(ADC2); + adc_set_single_conversion_mode(ADC2); + adc_disable_external_trigger_regular(ADC2); + adc_set_right_aligned(ADC2); + adc_set_sample_time_on_all_channels(ADC2, ADC_SMPR_SMP_13DOT5CYC); + start_adc(ADC2); } /** @@ -409,6 +444,7 @@ void setup(void) setup_exceptions(); setup_gpio(); setup_adc1(); + setup_adc2(); setup_usart(); setup_encoders(); setup_motor_driver(); From 6aaaa734b33b2911deaa92c16c5bf4f79c410b59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20S=C3=A1nchez=20de=20Le=C3=B3n=20Peque?= Date: Fri, 12 Oct 2018 21:44:36 +0200 Subject: [PATCH 9/9] Warn about low battery before and after moving --- src/hmi.c | 11 +++++++++++ src/hmi.h | 2 ++ src/main.c | 20 ++++++++++++++++++++ 3 files changed, 33 insertions(+) diff --git a/src/hmi.c b/src/hmi.c index 895d382a..028e1e4e 100644 --- a/src/hmi.c +++ b/src/hmi.c @@ -110,6 +110,17 @@ void blink_collision(void) led_right_off(); } +/** + * @brief Warn low battery using speaker sounds. + */ +void warn_low_battery(void) +{ + speaker_play('C', 4, 0, 0.05); + sleep_ticks(50); + speaker_play('C', 3, 0, 0.05); + sleep_ticks(50); +} + /** * @brief Function to read button left. */ diff --git a/src/hmi.h b/src/hmi.h index 052a6558..bbb58416 100644 --- a/src/hmi.h +++ b/src/hmi.h @@ -5,6 +5,7 @@ #include "clock.h" #include "detection.h" +#include "speaker.h" #include "speed.h" void led_left_toggle(void); @@ -18,6 +19,7 @@ void led_right_off(void); void led_bluepill_off(void); void repeat_blink(uint8_t count, uint16_t time); void blink_collision(void); +void warn_low_battery(void); bool button_left_read(void); bool button_right_read(void); bool button_left_read_consecutive(uint32_t count); diff --git a/src/main.c b/src/main.c index d07d0c0d..f3ddbe52 100644 --- a/src/main.c +++ b/src/main.c @@ -47,6 +47,24 @@ static void user_configuration(bool run) set_speed_mode(mode, run); } +/** + * @brief Check battery voltage and warn if the voltage is getting too low. + */ +static void check_battery_voltage(void) +{ + float voltage; + + voltage = get_battery_voltage(); + if (voltage < 3.6) + warn_low_battery(); + if (voltage < 3.5) + warn_low_battery(); + if (voltage < 3.4) + warn_low_battery(); + if (voltage < 3.3) + speaker_play('C', 3, 0, 2.); +} + /** * @brief Includes the functions to be executed before robot starts to move. */ @@ -56,6 +74,7 @@ static void before_moving(void) disable_walls_control(); repeat_blink(10, 100); sleep_us(5000000); + check_battery_voltage(); led_left_on(); led_right_on(); wait_front_sensor_close_signal(0.12); @@ -79,6 +98,7 @@ static void after_moving(void) repeat_blink(10, 100); } reset_motion(); + check_battery_voltage(); } /**