feedc0de/bobbycar-foc-model
1
0
Fork 0
forked from EFeru/hoverboard-firmware-hack-FOC
Code Pull Requests Activity

Merge branch 'master' into pr/9

Browse Source
This commit is contained in:
EmanuelFeru
2019-12-14 13:51:37 +01:00
parent 1f1e1c7dd7 ed31ac07a5
commit 0344a9c225
271 changed files with 162679 additions and 98729 deletions
Download Patch File Download Diff File Expand all files Collapse all files

305
Src/main.c
View File

@@ -4,6 +4,7 @@
* Copyright (C) 2017-2018 Rene Hopf <renehopf@mac.com>
* Copyright (C) 2017-2018 Nico Stute <crinq@crinq.de>
* Copyright (C) 2017-2018 Niklas Fauth <niklas.fauth@kit.fail>
* Copyright (C) 2019-2020 Emanuel FERU <aerdronix@gmail.com>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
@@ -41,18 +42,18 @@
RT_MODEL rtM_Left_; /* Real-time model */
RT_MODEL rtM_Right_; /* Real-time model */
RT_MODEL *const rtM_Left = &rtM_Left_;
RT_MODEL *const rtM_Right = &rtM_Right_;
RT_MODEL *const rtM_Left = &rtM_Left_;
RT_MODEL *const rtM_Right = &rtM_Right_;
P rtP_Left; /* Block parameters (auto storage) */
DW rtDW_Left; /* Observable states */
ExtU rtU_Left; /* External inputs */
ExtY rtY_Left; /* External outputs */
P rtP_Left; /* Block parameters (auto storage) */
DW rtDW_Left; /* Observable states */
ExtU rtU_Left; /* External inputs */
ExtY rtY_Left; /* External outputs */
P rtP_Right; /* Block parameters (auto storage) */
DW rtDW_Right; /* Observable states */
ExtU rtU_Right; /* External inputs */
ExtY rtY_Right; /* External outputs */
P rtP_Right; /* Block parameters (auto storage) */
DW rtDW_Right; /* Observable states */
ExtU rtU_Right; /* External inputs */
ExtY rtY_Right; /* External outputs */
extern uint8_t errCode_Left; /* Global variable to handle Motor error codes */
extern uint8_t errCode_Right; /* Global variable to handle Motor error codes */
@@ -70,7 +71,11 @@ extern volatile adc_buf_t adc_buffer;
LCD_PCF8574_HandleTypeDef lcd;
#endif
extern I2C_HandleTypeDef hi2c2;
extern UART_HandleTypeDef huart2;
#ifndef TRANSPOTTER
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;
static UART_HandleTypeDef huart;
#endif
#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
extern uint8_t LCDerrorFlag;
@@ -92,37 +97,66 @@ extern UART_HandleTypeDef huart2;
uint16_t saveValue = 0;
uint16_t counter = 0;
#else
uint8_t nunchuck_connected = 1;
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
typedef struct{
int16_t steer;
int16_t speed;
//uint32_t crc;
uint16_t start;
int16_t steer;
int16_t speed;
uint16_t checksum;
} Serialcommand;
static volatile Serialcommand command;
static int16_t timeoutCnt = 0; // Timeout counter for Rx Serial command
#endif
static uint8_t timeoutFlag = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
#if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3)
typedef struct{
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedR;
int16_t speedL;
int16_t speedR_meas;
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
uint16_t checksum;
} SerialFeedback;
static SerialFeedback Feedback;
#endif
static uint8_t serialSendCounter; // serial send counter
#if defined(CONTROL_NUNCHUCK) || defined(SUPPORT_NUNCHUCK) || defined(CONTROL_PPM) || defined(CONTROL_ADC)
static uint8_t button1, button2;
#endif
static int cmd1; // normalized input value. -1000 to 1000
static int cmd2; // normalized input value. -1000 to 1000
static int16_t steer; // local variable for steering. -1000 to 1000
static int16_t speed; // local variable for speed. -1000 to 1000
static int16_t steerFixdt; // local fixed-point variable for steering low-pass filter
static int16_t speedFixdt; // local fixed-point variable for speed low-pass filter
static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter
static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter
uint8_t ctrlModReqRaw = CTRL_MOD_REQ;
uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request
static int cmd1; // normalized input value. -1000 to 1000
static int cmd2; // normalized input value. -1000 to 1000
static int16_t speed; // local variable for speed. -1000 to 1000
#ifndef TRANSPOTTER
static int16_t steer; // local variable for steering. -1000 to 1000
static int16_t steerFixdt; // local fixed-point variable for steering low-pass filter
static int16_t speedFixdt; // local fixed-point variable for speed low-pass filter
static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter
static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter
#endif
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t enable; // global variable for motor enable
extern uint8_t enable; // global variable for motor enable
extern volatile uint32_t timeout; // global variable for timeout
extern int16_t batVoltage; // global variable for battery voltage
extern volatile uint32_t timeout; // global variable for timeout
extern int16_t batVoltage; // global variable for battery voltage
static uint32_t inactivity_timeout_counter;
@@ -136,6 +170,7 @@ void poweroff(void) {
// if (abs(speed) < 20) { // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons
buzzerPattern = 0;
enable = 0;
consoleLog("-- Motors disabled --\r\n");
for (int i = 0; i < 8; i++) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
@@ -175,10 +210,6 @@ int main(void) {
MX_ADC1_Init();
MX_ADC2_Init();
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
UART_Init();
#endif
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 1);
HAL_ADC_Start(&hadc1);
@@ -193,16 +224,24 @@ int main(void) {
rtP_Left.b_selPhaABCurrMeas = 1; // Left motor measured current phases = {iA, iB} -> do NOT change
rtP_Left.z_ctrlTypSel = CTRL_TYP_SEL;
rtP_Left.b_diagEna = DIAG_ENA;
rtP_Left.i_max = (I_MOT_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4)
rtP_Left.n_max = N_MOT_MAX << 4; // fixdt(1,16,4)
rtP_Left.b_fieldWeakEna = FIELD_WEAK_ENA;
rtP_Left.i_max = I_MOT_MAX;
rtP_Left.n_max = N_MOT_MAX;
rtP_Left.id_fieldWeakMax = (FIELD_WEAK_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4)
rtP_Left.a_phaAdvMax = PHASE_ADV_MAX << 4; // fixdt(1,16,4)
rtP_Left.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4)
rtP_Left.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4)
rtP_Right.b_selPhaABCurrMeas = 0; // Left motor measured current phases = {iB, iC} -> do NOT change
rtP_Right.z_ctrlTypSel = CTRL_TYP_SEL;
rtP_Right.b_diagEna = DIAG_ENA;
rtP_Right.i_max = (I_MOT_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4)
rtP_Right.n_max = N_MOT_MAX << 4; // fixdt(1,16,4)
rtP_Right.b_fieldWeakEna = FIELD_WEAK_ENA;
rtP_Right.i_max = I_MOT_MAX;
rtP_Right.n_max = N_MOT_MAX;
rtP_Right.id_fieldWeakMax = (FIELD_WEAK_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4)
rtP_Right.a_phaAdvMax = PHASE_ADV_MAX << 4; // fixdt(1,16,4)
rtP_Right.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4)
rtP_Right.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4)
/* Pack LEFT motor data into RTM */
rtM_Left->defaultParam = &rtP_Left;
@@ -230,24 +269,24 @@ int main(void) {
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
#ifdef TRANSPOTTER
int lastDistance = 0;
enable = 1;
uint8_t checkRemote = 0;
#ifdef TRANSPOTTER
int lastDistance = 0;
enable = 1;
uint8_t checkRemote = 0;
HAL_FLASH_Unlock();
HAL_FLASH_Unlock();
/* EEPROM Init */
EE_Init();
/* EEPROM Init */
EE_Init();
EE_ReadVariable(VirtAddVarTab[0], &saveValue);
EE_ReadVariable(VirtAddVarTab[0], &saveValue);
HAL_FLASH_Lock();
float setDistance = saveValue / 1000.0;
if (setDistance < 0.2) {
setDistance = 1.0;
}
#endif
HAL_FLASH_Lock();
float setDistance = saveValue / 1000.0;
if (setDistance < 0.2) {
setDistance = 1.0;
}
#endif
#ifdef CONTROL_PPM
PPM_Init();
@@ -258,9 +297,16 @@ int main(void) {
Nunchuck_Init();
#endif
#ifdef CONTROL_SERIAL_USART2
UART_Control_Init();
HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, 4);
#if defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)
UART2_Init();
huart = huart2;
#endif
#if defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3)
UART3_Init();
huart = huart3;
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command));
#endif
#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
@@ -308,7 +354,6 @@ int main(void) {
LCD_WriteString(&lcd, "m)");
#endif
int16_t lastSpeedL = 0, lastSpeedR = 0;
int16_t speedL = 0, speedR = 0;
@@ -397,41 +442,37 @@ int main(void) {
#endif
#if defined(CONTROL_NUNCHUCK) || defined(SUPPORT_NUNCHUCK)
#ifdef TRANSPOTTER
if (nunchuck_connected != 0) {
#endif
Nunchuck_Read();
cmd1 = CLAMP((nunchuck_data[0] - 127) * 8, -1000, 1000); // x - axis. Nunchuck joystick readings range 30 - 230
cmd2 = CLAMP((nunchuck_data[1] - 128) * 8, -1000, 1000); // y - axis
Nunchuck_Read();
cmd1 = CLAMP((nunchuck_data[0] - 127) * 8, INPUT_MIN, INPUT_MAX); // x - axis. Nunchuck joystick readings range 30 - 230
cmd2 = CLAMP((nunchuck_data[1] - 128) * 8, INPUT_MIN, INPUT_MAX); // y - axis
button1 = (uint8_t)nunchuck_data[5] & 1;
button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
#ifdef TRANSPOTTER
button1 = (uint8_t)nunchuck_data[5] & 1;
button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
}
#endif
#endif
#ifdef CONTROL_PPM
cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, -1000, 1000);
cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, -1000, 1000);
button1 = ppm_captured_value[5] > 500;
cmd1 = CLAMP((ppm_captured_value[0] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX);
cmd2 = CLAMP((ppm_captured_value[1] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX);
button1 = ppm_captured_value[5] > INPUT_MID;
float scale = ppm_captured_value[2] / 1000.0f;
#endif
#ifdef CONTROL_ADC
// ADC values range: 0-4095, see ADC-calibration in config.h
#ifdef ADC1_MID_POT
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MID) * 1000 / (ADC1_MAX - ADC1_MID), 0, 1000)
-CLAMP((ADC1_MID - adc_buffer.l_tx2) * 1000 / (ADC1_MID - ADC1_MIN), 0, 1000); // ADC1
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MID) * INPUT_MAX / (ADC1_MAX - ADC1_MID), 0, INPUT_MAX)
-CLAMP((ADC1_MID - adc_buffer.l_tx2) * INPUT_MAX / (ADC1_MID - ADC1_MIN), 0, INPUT_MAX); // ADC1
#else
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN) * 1000 / (ADC1_MAX - ADC1_MIN), 0, 1000); // ADC1
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN) * INPUT_MAX / (ADC1_MAX - ADC1_MIN), 0, INPUT_MAX); // ADC1
#endif
#ifdef ADC2_MID_POT
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID) * 1000 / (ADC2_MAX - ADC2_MID), 0, 1000)
-CLAMP((ADC2_MID - adc_buffer.l_rx2) * 1000 / (ADC2_MID - ADC2_MIN), 0, 1000); // ADC2
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID) * INPUT_MAX / (ADC2_MAX - ADC2_MID), 0, INPUT_MAX)
-CLAMP((ADC2_MID - adc_buffer.l_rx2) * INPUT_MAX / (ADC2_MID - ADC2_MIN), 0, INPUT_MAX); // ADC2
#else
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * 1000 / (ADC2_MAX - ADC2_MIN), 0, 1000); // ADC2
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * INPUT_MAX / (ADC2_MAX - ADC2_MIN), 0, INPUT_MAX); // ADC2
#endif
// use ADCs as button inputs:
@@ -441,11 +482,41 @@ int main(void) {
timeout = 0;
#endif
#ifdef CONTROL_SERIAL_USART2
cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
#if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3
// Handle received data validity, timeout and fix out-of-sync if necessary
if (command.start == START_FRAME && command.checksum == (uint16_t)(command.start ^ command.steer ^ command.speed)) {
if (timeoutFlag) { // Check for previous timeout flag
if (timeoutCnt-- <= 0) // Timeout de-qualification
timeoutFlag = 0; // Timeout flag cleared
} else {
cmd1 = CLAMP((int16_t)command.steer, INPUT_MIN, INPUT_MAX);
cmd2 = CLAMP((int16_t)command.speed, INPUT_MIN, INPUT_MAX);
command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
timeoutCnt = 0; // Reset the timeout counter
}
} else {
if (timeoutCnt++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlag = 1; // Timeout detected
timeoutCnt = SERIAL_TIMEOUT; // Limit timout counter value
}
// Check the received Start Frame. If it is NOT OK, most probably we are out-of-sync.
// Try to re-sync by reseting the DMA
if (command.start != START_FRAME && command.start != 0xFFFF) {
HAL_UART_DMAStop(&huart);
HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command));
}
}
if (timeoutFlag) { // In case of timeout bring the system to a Safe State
ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way
cmd1 = 0;
cmd2 = 0;
} else {
ctrlModReq = ctrlModReqRaw; // Follow the Mode request
}
timeout = 0;
#endif
#ifndef TRANSPOTTER
@@ -491,6 +562,9 @@ int main(void) {
}
#endif
lastSpeedL = speedL;
lastSpeedR = speedR;
#ifdef TRANSPOTTER
if (timeout > TIMEOUT) {
pwml = 0;
@@ -509,12 +583,7 @@ int main(void) {
nunchuck_connected = 0;
}
#endif
lastSpeedL = speedL;
lastSpeedR = speedR;
#ifdef TRANSPOTTER
if ((distance / 1345.0) - setDistance > 0.5 && (lastDistance / 1345.0) - setDistance > 0.5) { // Error, robot too far away!
enable = 0;
longBeep();
@@ -544,7 +613,7 @@ int main(void) {
nunchuck_connected = 1;
}
}
}
}
#endif
#ifdef SUPPORT_LCD
@@ -570,25 +639,51 @@ int main(void) {
#endif
if (inactivity_timeout_counter % 25 == 0) {
// ####### CALC BOARD TEMPERATURE #######
filtLowPass16(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
board_temp_adcFilt = board_temp_adcFixdt >> 4; // convert fixed-point to integer
board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
// ####### CALC BOARD TEMPERATURE #######
filtLowPass16(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
board_temp_adcFilt = board_temp_adcFixdt >> 4; // convert fixed-point to integer
board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
serialSendCounter++; // Increment the counter
if (serialSendCounter > 20) { // Send data every 100 ms = 20 * 5 ms, where 5 ms is approximately the main loop duration
serialSendCounter = 0; // Reset the counter
// ####### DEBUG SERIAL OUT #######
#ifdef CONTROL_ADC
setScopeChannel(0, (int)adc_buffer.l_tx2); // 1: ADC1
setScopeChannel(1, (int)adc_buffer.l_rx2); // 2: ADC2
#endif
setScopeChannel(2, (int16_t)speedR); // 1: output command: [-1000, 1000]
setScopeChannel(3, (int16_t)speedL); // 2: output command: [-1000, 1000]
setScopeChannel(4, (int16_t)adc_buffer.batt1); // 5: for battery voltage calibration
setScopeChannel(5, (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); // 6: for verifying battery voltage calibration
setScopeChannel(6, (int16_t)board_temp_adcFilt); // 7: for board temperature calibration
setScopeChannel(7, (int16_t)board_temp_deg_c); // 8: for verifying board temperature calibration
consoleScope();
}
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
#ifdef CONTROL_ADC
setScopeChannel(0, (int16_t)adc_buffer.l_tx2); // 1: ADC1
setScopeChannel(1, (int16_t)adc_buffer.l_rx2); // 2: ADC2
#endif
setScopeChannel(2, (int16_t)speedR); // 1: output command: [-1000, 1000]
setScopeChannel(3, (int16_t)speedL); // 2: output command: [-1000, 1000]
setScopeChannel(4, (int16_t)adc_buffer.batt1); // 5: for battery voltage calibration
setScopeChannel(5, (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); // 6: for verifying battery voltage calibration
setScopeChannel(6, (int16_t)board_temp_adcFilt); // 7: for board temperature calibration
setScopeChannel(7, (int16_t)board_temp_deg_c); // 8: for verifying board temperature calibration
consoleScope();
// ####### FEEDBACK SERIAL OUT #######
#elif defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3)
if(UART_DMA_CHANNEL->CNDTR == 0) {
Feedback.start = (uint16_t)START_FRAME;
Feedback.cmd1 = (int16_t)cmd1;
Feedback.cmd2 = (int16_t)cmd2;
Feedback.speedR = (int16_t)speedR;
Feedback.speedL = (int16_t)speedL;
Feedback.speedR_meas = (int16_t)rtY_Left.n_mot;
Feedback.speedL_meas = (int16_t)rtY_Right.n_mot;
Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
Feedback.boardTemp = (int16_t)board_temp_deg_c;
Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR ^ Feedback.speedL
^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp);
UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN;
UART_DMA_CHANNEL->CNDTR = sizeof(Feedback);
UART_DMA_CHANNEL->CMAR = (uint32_t)&Feedback;
UART_DMA_CHANNEL->CCR |= DMA_CCR_EN;
}
#endif
}
HAL_GPIO_TogglePin(LED_PORT, LED_PIN);
// ####### POWEROFF BY POWER-BUTTON #######
@@ -615,9 +710,9 @@ int main(void) {
} else if (batVoltage < BAT_LOW_LVL2 && batVoltage >= BAT_LOW_DEAD && BAT_LOW_LVL2_ENABLE) { // low bat 2: fast beep
buzzerFreq = 5;
buzzerPattern = 6;
} else if (errCode_Left || errCode_Right) { // beep in case of Motor error - fast beep
buzzerFreq = 6;
buzzerPattern = 2;
} else if (errCode_Left || errCode_Right || timeoutFlag) { // beep in case of Motor error or serial timeout - fast beep
buzzerFreq = 12;
buzzerPattern = 1;
} else if (BEEPS_BACKWARD && speed < -50) { // backward beep
buzzerFreq = 5;
buzzerPattern = 1;
@@ -786,12 +881,12 @@ void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t
tmp = prodSpeed - prodSteer;
tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
*rty_speedR = (int16_t)(tmp >> 4); // Convert from fixed-point to int
*rty_speedR = CLAMP(*rty_speedR, -1000, 1000);
*rty_speedR = CLAMP(*rty_speedR, INPUT_MIN, INPUT_MAX);
tmp = prodSpeed + prodSteer;
tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
*rty_speedL = (int16_t)(tmp >> 4); // Convert from fixed-point to int
*rty_speedL = CLAMP(*rty_speedL, -1000, 1000);
*rty_speedL = CLAMP(*rty_speedL, INPUT_MIN, INPUT_MAX);
}
// ===========================================================