Skip to content

Commit

Permalink
Add a setting to disable the buzzer
Browse files Browse the repository at this point in the history
  • Loading branch information
jeanphilippehell committed Sep 2, 2023
1 parent efb438b commit eeb2f87
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 1 deletion.
5 changes: 5 additions & 0 deletions Inc/config.h
Original file line number Diff line number Diff line change
Expand Up @@ -261,6 +261,11 @@
// ########################### END OF DEBUG LCD ############################


// ############################### BUZZER ENABLE / DISABLE ###############################
#define BUZZER_ENABLED // If enabled the buzzer will buzz, otherwise not.
// ########################### END OF BUZZER ENABLE / DISABLE ############################



// ################################# VARIANT_ADC SETTINGS ############################
#ifdef VARIANT_ADC
Expand Down
2 changes: 2 additions & 0 deletions Src/comms.c
Original file line number Diff line number Diff line change
Expand Up @@ -208,7 +208,9 @@ int8_t setParamValInt(uint8_t index, int32_t newValue) {
}

// Beep if value was modified
#ifdef BUZZER_ENABLED
beepShort(5);
#endif // BUZZER_ENABLED
}

// Run callback function if assigned
Expand Down
14 changes: 13 additions & 1 deletion Src/main.c
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,9 @@ int main(void) {
HAL_ADC_Start(&hadc1);
HAL_ADC_Start(&hadc2);

#ifdef BUZZER_ENABLED
poweronMelody();
#endif // BUZZER_ENABLED
HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET);

int32_t board_temp_adcFixdt = adc_buffer.temp << 16; // Fixed-point filter output initialized with current ADC converted to fixed-point
Expand Down Expand Up @@ -258,8 +260,10 @@ int main(void) {
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
if (enable == 0 && !rtY_Left.z_errCode && !rtY_Right.z_errCode &&
ABS(input1[inIdx].cmd) < 50 && ABS(input2[inIdx].cmd) < 50){
#ifdef BUZZER_ENABLED
beepShort(6); // make 2 beeps indicating the motor enable
beepShort(4); HAL_Delay(100);
#endif // BUZZER_ENABLED
steerFixdt = speedFixdt = 0; // reset filters
enable = 1; // enable motors
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
Expand Down Expand Up @@ -416,7 +420,9 @@ int main(void) {

if ((distance / 1345.0) - setDistance > 0.5 && (lastDistance / 1345.0) - setDistance > 0.5) { // Error, robot too far away!
enable = 0;
#ifdef BUZZER_ENABLED
beepLong(5);
#endif // BUZZER_ENABLED
#ifdef SUPPORT_LCD
LCD_ClearDisplay(&lcd);
HAL_Delay(5);
Expand Down Expand Up @@ -457,7 +463,7 @@ int main(void) {
}
#endif
transpotter_counter++;
#endif
#endif // VARIANT_TRANSPOTTER

// ####### SIDEBOARDS HANDLING #######
#if defined(SIDEBOARD_SERIAL_USART2)
Expand Down Expand Up @@ -554,6 +560,7 @@ int main(void) {
poweroff();
} else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors
enable = 0;
#ifdef BUZZER_ENABLED
beepCount(1, 24, 1);
} else if (timeoutFlgADC) { // 2 beeps (low pitch): ADC timeout
beepCount(2, 24, 1);
Expand All @@ -567,11 +574,16 @@ int main(void) {
beepCount(0, 10, 6);
} else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // 1 beep slow (medium pitch): Low bat 2
beepCount(0, 10, 30);
#endif // BUZZER_ENABLED
} else if (BEEPS_BACKWARD && (((cmdR < -50 || cmdL < -50) && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // 1 beep fast (high pitch): Backward spinning motors
#ifdef BUZZER_ENABLED
beepCount(0, 5, 1);
#endif // BUZZER_ENABLED
backwardDrive = 1;
} else { // do not beep
#ifdef BUZZER_ENABLED
beepCount(0, 0, 0);
#endif // BUZZER_ENABLED
backwardDrive = 0;
}

Expand Down
10 changes: 10 additions & 0 deletions Src/util.c
Original file line number Diff line number Diff line change
Expand Up @@ -417,32 +417,40 @@ void UART_DisableRxErrors(UART_HandleTypeDef *huart)
/* =========================== General Functions =========================== */

void poweronMelody(void) {
#ifdef BUZZER_ENABLED
buzzerCount = 0; // prevent interraction with beep counter
for (int i = 8; i >= 0; i--) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
buzzerFreq = 0;
#endif
}

void beepCount(uint8_t cnt, uint8_t freq, uint8_t pattern) {
#ifdef BUZZER_ENABLED
buzzerCount = cnt;
buzzerFreq = freq;
buzzerPattern = pattern;
#endif
}

void beepLong(uint8_t freq) {
#ifdef BUZZER_ENABLED
buzzerCount = 0; // prevent interraction with beep counter
buzzerFreq = freq;
HAL_Delay(500);
buzzerFreq = 0;
#endif
}

void beepShort(uint8_t freq) {
#ifdef BUZZER_ENABLED
buzzerCount = 0; // prevent interraction with beep counter
buzzerFreq = freq;
HAL_Delay(100);
buzzerFreq = 0;
#endif
}

void beepShortMany(uint8_t cnt, int8_t dir) {
Expand Down Expand Up @@ -1543,12 +1551,14 @@ void poweroff(void) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("-- Motors disabled --\r\n");
#endif
#ifdef BUZZER_ENABLED
buzzerCount = 0; // prevent interraction with beep counter
buzzerPattern = 0;
for (int i = 0; i < 8; i++) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
#endif
saveConfig();
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
while(1) {}
Expand Down

0 comments on commit eeb2f87

Please sign in to comment.