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/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 433198e1..f3ddbe52 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); @@ -46,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. */ @@ -55,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); @@ -78,6 +98,7 @@ static void after_moving(void) repeat_blink(10, 100); } reset_motion(); + check_battery_voltage(); } /** @@ -116,6 +137,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. */ @@ -124,16 +159,9 @@ int main(void) setup(); systick_interrupt_enable(); competition(); - add_goal(3, 5); - set_target_goal(); - initialize_solver_direction(); - set_speed_mode(0, false); while (1) { - if (button_left_read_consecutive(500)) { - before_moving(); - explore(); - after_moving(); - } + if (button_left_read_consecutive(500)) + training(); execute_commands(); } diff --git a/src/setup.c b/src/setup.c index 094d578d..8d446b59 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); } @@ -218,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); @@ -252,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. * @@ -321,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. * @@ -332,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. * @@ -345,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}; @@ -359,45 +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 TIM1 setup. + * @brief Setup for ADC 2: configured for regular conversion. * - * 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. + * - 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 The TIM1 is conected to the APB2 prescaler. + * @note This ADC reads the battery status. * - * @see Reference manual (RM0008) "Advanced-control timers" + * @see Reference manual (RM0008) "Analog-to-digital converter". */ -static void setup_timer1(void) +static void setup_adc2(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); + 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); } /** @@ -408,11 +443,12 @@ void setup(void) setup_clock(); setup_exceptions(); setup_gpio(); + setup_adc1(); + setup_adc2(); setup_usart(); setup_encoders(); - setup_pwm(); + setup_motor_driver(); + setup_speaker(); setup_mpu(); setup_systick(); - setup_timer1(); - setup_adc1(); } 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 */ diff --git a/src/target/detection.c b/src/target/detection.c index 4820452d..9150e4a5 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,86 +189,52 @@ 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 TIM1 interruption routine. - * - * - Manage the update event interruption flag. - * - Trigger state machine to manage sensors. + * @brief Start and wait for complete injection of sensor readings. */ -void tim1_up_isr(void) +static void inject_readings(void) { - if (timer_get_flag(TIM1, TIM_SR_UIF)) { - timer_clear_flag(TIM1, TIM_SR_UIF); - sm_emitter_adc(); - } + adc_start_conversion_injected(ADC1); + while (!(adc_eoc_injected(ADC1))) + ; + // Clear injected end of conversion + ADC_SR(ADC1) &= ~ADC_SR_JEOC; } /** - * @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 update_raw_readings(void) { uint8_t i = 0; for (i = 0; i < NUM_SENSOR; i++) { - off[i] = sensors_off[i]; - on[i] = sensors_on[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); + sleep_us(10); + inject_readings(); + sensors_on[i] = adc_read_injected(ADC1, (i + 1)); + set_emitter_off(i); } } @@ -305,6 +270,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]) - @@ -447,7 +413,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;