change back to HAL_Delay
This commit is contained in:
parent
2e28a807ea
commit
f302ff8041
|
|
@ -89,7 +89,7 @@
|
||||||
#define START_FRAME 0xAAAA // [-] Start frame definition for serial commands
|
#define START_FRAME 0xAAAA // [-] Start frame definition for serial commands
|
||||||
#define SERIAL_TIMEOUT 160 // [-] Serial timeout duration for the received data. 160 ~= 0.8 sec. Calculation: 0.8 sec / 0.005 sec
|
#define SERIAL_TIMEOUT 160 // [-] Serial timeout duration for the received data. 160 ~= 0.8 sec. Calculation: 0.8 sec / 0.005 sec
|
||||||
|
|
||||||
#define USART2_BAUD 38400 // UART2 baud rate (long wired cable, soldered next to buzzer)
|
#define USART2_BAUD 38400 // UART2 baud rate (long wired cable, soldered next to buzzer)
|
||||||
#define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
|
#define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
|
||||||
#define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino
|
#define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino
|
||||||
#define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
|
#define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
|
||||||
|
|
|
||||||
423
Src/main.c
423
Src/main.c
|
|
@ -284,239 +284,234 @@ int main(void) {
|
||||||
int16_t board_temp_adcFilt = adc_buffer.temp;
|
int16_t board_temp_adcFilt = adc_buffer.temp;
|
||||||
int16_t board_temp_deg_c;
|
int16_t board_temp_deg_c;
|
||||||
|
|
||||||
uint32_t timePrev=0; //for loop timing
|
|
||||||
|
|
||||||
while(1) {
|
while(1) {
|
||||||
//HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms
|
HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms
|
||||||
if ((HAL_GetTick()-timePrev) > DELAY_IN_MAIN_LOOP) { // try with DELAY_IN_MAIN_LOOP = 5 ms and then with DELAY_IN_MAIN_LOOP = 10 ms
|
|
||||||
|
|
||||||
#ifdef CONTROL_NUNCHUCK
|
#ifdef CONTROL_NUNCHUCK
|
||||||
Nunchuck_Read();
|
Nunchuck_Read();
|
||||||
cmd1 = CLAMP((nunchuck_data[0] - 127) * 8, -1000, 1000); // x - axis. Nunchuck joystick readings range 30 - 230
|
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
|
cmd2 = CLAMP((nunchuck_data[1] - 128) * 8, -1000, 1000); // y - axis
|
||||||
|
|
||||||
button1 = (uint8_t)nunchuck_data[5] & 1;
|
button1 = (uint8_t)nunchuck_data[5] & 1;
|
||||||
button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
|
button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
|
||||||
|
#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;
|
||||||
|
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
|
||||||
|
#else
|
||||||
|
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN) * 1000 / (ADC1_MAX - ADC1_MIN), 0, 1000); // ADC1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef CONTROL_PPM
|
#ifdef ADC2_MID_POT
|
||||||
cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, -1000, 1000);
|
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID) * 1000 / (ADC2_MAX - ADC2_MID), 0, 1000)
|
||||||
cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, -1000, 1000);
|
-CLAMP((ADC2_MID - adc_buffer.l_rx2) * 1000 / (ADC2_MID - ADC2_MIN), 0, 1000); // ADC2
|
||||||
button1 = ppm_captured_value[5] > 500;
|
#else
|
||||||
float scale = ppm_captured_value[2] / 1000.0f;
|
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * 1000 / (ADC2_MAX - ADC2_MIN), 0, 1000); // ADC2
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef CONTROL_ADC
|
// use ADCs as button inputs:
|
||||||
// ADC values range: 0-4095, see ADC-calibration in config.h
|
button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1
|
||||||
#ifdef ADC1_MID_POT
|
button2 = (uint8_t)(adc_buffer.l_rx2 > 2000); // ADC2
|
||||||
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
|
|
||||||
#else
|
|
||||||
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN) * 1000 / (ADC1_MAX - ADC1_MIN), 0, 1000); // ADC1
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef ADC2_MID_POT
|
timeout = 0;
|
||||||
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID) * 1000 / (ADC2_MAX - ADC2_MID), 0, 1000)
|
#endif
|
||||||
-CLAMP((ADC2_MID - adc_buffer.l_rx2) * 1000 / (ADC2_MID - ADC2_MIN), 0, 1000); // ADC2
|
|
||||||
#else
|
|
||||||
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * 1000 / (ADC2_MAX - ADC2_MIN), 0, 1000); // ADC2
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// use ADCs as button inputs:
|
#if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3
|
||||||
button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1
|
|
||||||
button2 = (uint8_t)(adc_buffer.l_rx2 > 2000); // ADC2
|
|
||||||
|
|
||||||
timeout = 0;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#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.speedLeft ^ command.speedRight)) {
|
|
||||||
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, -1000, 1000);
|
|
||||||
cmd1 = CLAMP((int16_t)command.speedLeft, -1000, 1000);
|
|
||||||
//cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
|
|
||||||
cmd2 = CLAMP((int16_t)command.speedRight, -1000, 1000);
|
|
||||||
command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
|
|
||||||
timeoutCnt = 0; // Reset the timeout counter
|
|
||||||
}
|
|
||||||
|
|
||||||
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
|
|
||||||
if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
|
|
||||||
buzzerPattern = 0;
|
|
||||||
buzzerFreq = 6; HAL_Delay(100); // make 2 beeps indicating the motor enable
|
|
||||||
buzzerFreq = 4; HAL_Delay(200);
|
|
||||||
buzzerFreq = 0;
|
|
||||||
enable = 1; // enable motors
|
|
||||||
consoleLog("-- Motors enabled --\r\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
|
// Handle received data validity, timeout and fix out-of-sync if necessary
|
||||||
|
if (command.start == START_FRAME && command.checksum == (uint16_t)(command.start ^ command.speedLeft ^ command.speedRight)) {
|
||||||
|
if (timeoutFlag) { // Check for previous timeout flag
|
||||||
|
if (timeoutCnt-- <= 0) // Timeout de-qualification
|
||||||
|
timeoutFlag = 0; // Timeout flag cleared
|
||||||
} else {
|
} else {
|
||||||
if (timeoutCnt++ >= SERIAL_TIMEOUT) { // Timeout qualification
|
//cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
|
||||||
timeoutFlag = 1; // Timeout detected
|
cmd1 = CLAMP((int16_t)command.speedLeft, -1000, 1000);
|
||||||
timeoutCnt = SERIAL_TIMEOUT; // Limit timout counter value
|
//cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
|
||||||
}
|
cmd2 = CLAMP((int16_t)command.speedRight, -1000, 1000);
|
||||||
// Check the received Start Frame. If it is NOT OK, most probably we are out-of-sync.
|
command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
|
||||||
// Try to re-sync by reseting the DMA
|
timeoutCnt = 0; // Reset the timeout counter
|
||||||
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
|
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
|
||||||
ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way
|
if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
|
||||||
cmd1 = 0;
|
buzzerPattern = 0;
|
||||||
cmd2 = 0;
|
buzzerFreq = 6; HAL_Delay(100); // make 2 beeps indicating the motor enable
|
||||||
} else {
|
buzzerFreq = 4; HAL_Delay(200);
|
||||||
ctrlModReq = ctrlModReqRaw; // Follow the Mode request
|
buzzerFreq = 0;
|
||||||
|
enable = 1; // enable motors
|
||||||
|
consoleLog("-- Motors enabled --\r\n");
|
||||||
}
|
}
|
||||||
timeout = 0;
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// ####### LOW-PASS FILTER #######
|
|
||||||
/*
|
|
||||||
rateLimiter16(cmd1, RATE, &steerRateFixdt);
|
|
||||||
rateLimiter16(cmd2, RATE, &speedRateFixdt);
|
|
||||||
filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt);
|
|
||||||
filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt);
|
|
||||||
steer = steerFixdt >> 4; // convert fixed-point to integer
|
|
||||||
speed = speedFixdt >> 4; // convert fixed-point to integer
|
|
||||||
*/
|
|
||||||
rateLimiter16(cmd1, RATE, &speedLeftRateFixdt);
|
|
||||||
rateLimiter16(cmd2, RATE, &speedRightRateFixdt);
|
|
||||||
filtLowPass16(speedLeftRateFixdt >> 4, FILTER, &speedLeftFixdt);
|
|
||||||
filtLowPass16(speedRightRateFixdt >> 4, FILTER, &speedRightFixdt);
|
|
||||||
speedL = speedLeftFixdt >> 4; // convert fixed-point to integer
|
|
||||||
speedR = speedRightFixdt >> 4; // convert fixed-point to integer
|
|
||||||
|
|
||||||
speed = (speedL+speedR)/2;
|
|
||||||
|
|
||||||
// ####### MIXER #######
|
|
||||||
// speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000);
|
|
||||||
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000);
|
|
||||||
//mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above
|
|
||||||
|
|
||||||
// ####### SET OUTPUTS (if the target change is less than +/- 50) #######
|
|
||||||
if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) {
|
|
||||||
#ifdef INVERT_R_DIRECTION
|
|
||||||
pwmr = speedR;
|
|
||||||
#else
|
|
||||||
pwmr = -speedR;
|
|
||||||
#endif
|
|
||||||
#ifdef INVERT_L_DIRECTION
|
|
||||||
pwml = -speedL;
|
|
||||||
#else
|
|
||||||
pwml = speedL;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
lastSpeedL = speedL;
|
|
||||||
lastSpeedR = speedR;
|
|
||||||
|
|
||||||
|
|
||||||
// ####### 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 #######
|
|
||||||
#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 #######
|
|
||||||
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
|
|
||||||
enable = 0; // disable motors
|
|
||||||
while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released
|
|
||||||
if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger)
|
|
||||||
__HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags
|
|
||||||
} else {
|
|
||||||
poweroff(); // release power-latch
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// ####### BEEP AND EMERGENCY POWEROFF #######
|
|
||||||
if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && abs(speed) < 20) || (batVoltage < BAT_LOW_DEAD && abs(speed) < 20)) { // poweroff before mainboard burns OR low bat 3
|
|
||||||
poweroff();
|
|
||||||
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot
|
|
||||||
buzzerFreq = 4;
|
|
||||||
buzzerPattern = 1;
|
|
||||||
} else if (batVoltage < BAT_LOW_LVL1 && batVoltage >= BAT_LOW_LVL2 && BAT_LOW_LVL1_ENABLE) { // low bat 1: slow beep
|
|
||||||
buzzerFreq = 5;
|
|
||||||
buzzerPattern = 42;
|
|
||||||
} 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 || 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;
|
|
||||||
} else { // do not beep
|
|
||||||
buzzerFreq = 0;
|
|
||||||
buzzerPattern = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// ####### INACTIVITY TIMEOUT #######
|
|
||||||
if (abs(speedL) > 50 || abs(speedR) > 50) {
|
|
||||||
inactivity_timeout_counter = 0;
|
|
||||||
} else {
|
} else {
|
||||||
inactivity_timeout_counter ++;
|
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 (inactivity_timeout_counter > (INACTIVITY_TIMEOUT * 60 * 1000) / (DELAY_IN_MAIN_LOOP + 1)) { // rest of main loop needs maybe 1ms
|
|
||||||
poweroff();
|
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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// ####### LOW-PASS FILTER #######
|
||||||
|
/*
|
||||||
|
rateLimiter16(cmd1, RATE, &steerRateFixdt);
|
||||||
|
rateLimiter16(cmd2, RATE, &speedRateFixdt);
|
||||||
|
filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt);
|
||||||
|
filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt);
|
||||||
|
steer = steerFixdt >> 4; // convert fixed-point to integer
|
||||||
|
speed = speedFixdt >> 4; // convert fixed-point to integer
|
||||||
|
*/
|
||||||
|
rateLimiter16(cmd1, RATE, &speedLeftRateFixdt);
|
||||||
|
rateLimiter16(cmd2, RATE, &speedRightRateFixdt);
|
||||||
|
filtLowPass16(speedLeftRateFixdt >> 4, FILTER, &speedLeftFixdt);
|
||||||
|
filtLowPass16(speedRightRateFixdt >> 4, FILTER, &speedRightFixdt);
|
||||||
|
speedL = speedLeftFixdt >> 4; // convert fixed-point to integer
|
||||||
|
speedR = speedRightFixdt >> 4; // convert fixed-point to integer
|
||||||
|
|
||||||
|
speed = (speedL+speedR)/2;
|
||||||
|
|
||||||
|
// ####### MIXER #######
|
||||||
|
// speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000);
|
||||||
|
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000);
|
||||||
|
//mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above
|
||||||
|
|
||||||
|
// ####### SET OUTPUTS (if the target change is less than +/- 50) #######
|
||||||
|
if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) {
|
||||||
|
#ifdef INVERT_R_DIRECTION
|
||||||
|
pwmr = speedR;
|
||||||
|
#else
|
||||||
|
pwmr = -speedR;
|
||||||
|
#endif
|
||||||
|
#ifdef INVERT_L_DIRECTION
|
||||||
|
pwml = -speedL;
|
||||||
|
#else
|
||||||
|
pwml = speedL;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
lastSpeedL = speedL;
|
||||||
|
lastSpeedR = speedR;
|
||||||
|
|
||||||
|
|
||||||
|
// ####### 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 #######
|
||||||
|
#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 #######
|
||||||
|
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
|
||||||
|
enable = 0; // disable motors
|
||||||
|
while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released
|
||||||
|
if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger)
|
||||||
|
__HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags
|
||||||
|
} else {
|
||||||
|
poweroff(); // release power-latch
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
timePrev = HAL_GetTick();
|
|
||||||
|
|
||||||
|
// ####### BEEP AND EMERGENCY POWEROFF #######
|
||||||
|
if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && abs(speed) < 20) || (batVoltage < BAT_LOW_DEAD && abs(speed) < 20)) { // poweroff before mainboard burns OR low bat 3
|
||||||
|
poweroff();
|
||||||
|
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot
|
||||||
|
buzzerFreq = 4;
|
||||||
|
buzzerPattern = 1;
|
||||||
|
} else if (batVoltage < BAT_LOW_LVL1 && batVoltage >= BAT_LOW_LVL2 && BAT_LOW_LVL1_ENABLE) { // low bat 1: slow beep
|
||||||
|
buzzerFreq = 5;
|
||||||
|
buzzerPattern = 42;
|
||||||
|
} 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 || 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;
|
||||||
|
} else { // do not beep
|
||||||
|
buzzerFreq = 0;
|
||||||
|
buzzerPattern = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// ####### INACTIVITY TIMEOUT #######
|
||||||
|
if (abs(speedL) > 50 || abs(speedR) > 50) {
|
||||||
|
inactivity_timeout_counter = 0;
|
||||||
|
} else {
|
||||||
|
inactivity_timeout_counter ++;
|
||||||
|
}
|
||||||
|
if (inactivity_timeout_counter > (INACTIVITY_TIMEOUT * 60 * 1000) / (DELAY_IN_MAIN_LOOP + 1)) { // rest of main loop needs maybe 1ms
|
||||||
|
poweroff();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue