Implement angleDelta accumulation
This commit is contained in:
Submodule bobbycar-protocol updated: 39f76cb62b...b1f810e314
5
config.h
5
config.h
@ -2,11 +2,6 @@
|
||||
|
||||
#define PWM_FREQ 16000 // PWM frequency in Hz
|
||||
#define DEAD_TIME 48 // PWM deadtime
|
||||
#ifdef VARIANT_TRANSPOTTER
|
||||
#define DELAY_IN_MAIN_LOOP 2
|
||||
#else
|
||||
#define DELAY_IN_MAIN_LOOP 5 // in ms. default 5. it is independent of all the timing critical stuff. do not touch if you do not know what you are doing.
|
||||
#endif
|
||||
#define TIMEOUT 5 // number of wrong / missing input commands before emergency off
|
||||
#define A2BIT_CONV 50 // A to bit for current conversion on ADC. Example: 1 A = 50, 2 A = 100, etc
|
||||
|
||||
|
||||
151
main.cpp
151
main.cpp
@ -22,6 +22,7 @@
|
||||
*/
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
#include "stm32f1xx_hal.h"
|
||||
|
||||
@ -76,9 +77,6 @@ int16_t offsetrr2 = 2000;
|
||||
int16_t offsetdcl = 2000;
|
||||
int16_t offsetdcr = 2000;
|
||||
|
||||
int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE;
|
||||
int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 20; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point
|
||||
|
||||
int32_t board_temp_adcFixdt = adc_buffer.temp << 20; // Fixed-point filter output initialized with current ADC converted to fixed-point
|
||||
int16_t board_temp_adcFilt = adc_buffer.temp;
|
||||
int16_t board_temp_deg_c;
|
||||
@ -93,6 +91,8 @@ struct {
|
||||
MotorState state;
|
||||
|
||||
uint32_t chops = 0;
|
||||
int16_T lastAngle = 0;
|
||||
int32_t angleDelta = 0;
|
||||
} left, right;
|
||||
|
||||
struct {
|
||||
@ -215,7 +215,7 @@ int main()
|
||||
HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command));
|
||||
|
||||
for (;;) {
|
||||
HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms
|
||||
HAL_Delay(1); //delay in ms
|
||||
|
||||
parseCommand();
|
||||
|
||||
@ -262,11 +262,6 @@ void updateMotors()
|
||||
return;
|
||||
}
|
||||
|
||||
if (buzzer.timer % 1000 == 0) { // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point
|
||||
filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt);
|
||||
batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer
|
||||
}
|
||||
|
||||
// Get Left motor currents
|
||||
int16_t curL_phaA = (int16_t)(offsetrl1 - adc_buffer.rl1);
|
||||
int16_t curL_phaB = (int16_t)(offsetrl2 - adc_buffer.rl2);
|
||||
@ -285,6 +280,36 @@ void updateMotors()
|
||||
if (chopR)
|
||||
right.chops++;
|
||||
|
||||
const auto doTheAngleThing = [](auto &motor){
|
||||
if (motor.rtY.a_elecAngle < motor.lastAngle)
|
||||
{
|
||||
if (motor.lastAngle - motor.rtY.a_elecAngle < 180)
|
||||
{
|
||||
motor.angleDelta -= motor.lastAngle - motor.rtY.a_elecAngle;
|
||||
}
|
||||
else
|
||||
{
|
||||
// wrap
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (motor.rtY.a_elecAngle - motor.lastAngle < 180)
|
||||
{
|
||||
motor.angleDelta += motor.rtY.a_elecAngle - motor.lastAngle;
|
||||
}
|
||||
else
|
||||
{
|
||||
// wrap
|
||||
}
|
||||
}
|
||||
|
||||
motor.lastAngle = motor.rtY.a_elecAngle;
|
||||
};
|
||||
|
||||
doTheAngleThing(left);
|
||||
doTheAngleThing(right);
|
||||
|
||||
// Disable PWM when current limit is reached (current chopping)
|
||||
// This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX
|
||||
if(chopL || timeout > TIMEOUT || left.state.enable == 0) {
|
||||
@ -967,17 +992,15 @@ void MX_ADC2_Init() {
|
||||
}
|
||||
|
||||
void poweroff() {
|
||||
// if (abs(speed) < 20) { // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons
|
||||
buzzer.state.pattern = 0;
|
||||
left.state.enable = right.state.enable = 0;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
buzzer.state.freq = (uint8_t)i;
|
||||
HAL_Delay(50);
|
||||
}
|
||||
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
|
||||
for (int i = 0; i < 5; i++)
|
||||
HAL_Delay(1000);
|
||||
// }
|
||||
buzzer.state.pattern = 0;
|
||||
left.state.enable = right.state.enable = 0;
|
||||
for (uint8_t i = 0; i < 8; i++) {
|
||||
buzzer.state.freq = i;
|
||||
HAL_Delay(50);
|
||||
}
|
||||
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
|
||||
for (int i = 0; i < 5; i++)
|
||||
HAL_Delay(1000);
|
||||
}
|
||||
|
||||
void parseCommand()
|
||||
@ -1012,9 +1035,9 @@ void parseCommand()
|
||||
|
||||
if (!any_parsed)
|
||||
{
|
||||
if (timeoutCntSerial++ >= 100) // Timeout qualification
|
||||
if (timeoutCntSerial++ >= 500) // Timeout qualification
|
||||
{
|
||||
timeoutCntSerial = 100; // Limit timout counter value
|
||||
timeoutCntSerial = 500; // Limit timout counter value
|
||||
|
||||
left.state = right.state = {.enable=true};
|
||||
|
||||
@ -1034,45 +1057,49 @@ void parseCommand()
|
||||
|
||||
void sendFeedback()
|
||||
{
|
||||
if (main_loop_counter % 50 == 0) { // Send data periodically
|
||||
if(UART_DMA_CHANNEL->CNDTR == 0) {
|
||||
feedback.start = Feedback::VALID_HEADER;
|
||||
if(UART_DMA_CHANNEL->CNDTR == 0)
|
||||
{
|
||||
feedback.start = Feedback::VALID_HEADER;
|
||||
|
||||
feedback.left.angle = left.rtY.a_elecAngle;
|
||||
feedback.right.angle = right.rtY.a_elecAngle;
|
||||
feedback.left.angle = left.rtY.a_elecAngle;
|
||||
feedback.right.angle = right.rtY.a_elecAngle;
|
||||
|
||||
feedback.left.speed = left.rtY.n_mot;
|
||||
feedback.right.speed = right.rtY.n_mot;
|
||||
feedback.left.speed = left.rtY.n_mot;
|
||||
feedback.right.speed = right.rtY.n_mot;
|
||||
|
||||
feedback.left.error = left.rtY.z_errCode;
|
||||
feedback.right.error = right.rtY.z_errCode;
|
||||
feedback.left.error = left.rtY.z_errCode;
|
||||
feedback.right.error = right.rtY.z_errCode;
|
||||
|
||||
feedback.left.current = left.rtU.i_DCLink;
|
||||
feedback.right.current = right.rtU.i_DCLink;
|
||||
feedback.left.current = left.rtU.i_DCLink;
|
||||
feedback.right.current = right.rtU.i_DCLink;
|
||||
|
||||
feedback.left.chops = left.chops;
|
||||
feedback.right.chops = right.chops;
|
||||
left.chops = 0;
|
||||
right.chops = 0;
|
||||
feedback.left.chops = left.chops;
|
||||
feedback.right.chops = right.chops;
|
||||
left.chops = 0;
|
||||
right.chops = 0;
|
||||
|
||||
feedback.left.hallA = left.rtU.b_hallA;
|
||||
feedback.left.hallB = left.rtU.b_hallB;
|
||||
feedback.left.hallC = left.rtU.b_hallC;
|
||||
feedback.right.hallA = right.rtU.b_hallA;
|
||||
feedback.right.hallB = right.rtU.b_hallB;
|
||||
feedback.right.hallC = right.rtU.b_hallC;
|
||||
feedback.left.angleDelta = left.angleDelta;
|
||||
feedback.right.angleDelta = right.angleDelta;
|
||||
left.angleDelta = 0;
|
||||
right.angleDelta = 0;
|
||||
|
||||
feedback.batVoltage = batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;
|
||||
feedback.boardTemp = board_temp_deg_c;
|
||||
feedback.timeoutCntSerial = timeoutCntSerial;
|
||||
feedback.left.hallA = left.rtU.b_hallA;
|
||||
feedback.left.hallB = left.rtU.b_hallB;
|
||||
feedback.left.hallC = left.rtU.b_hallC;
|
||||
feedback.right.hallA = right.rtU.b_hallA;
|
||||
feedback.right.hallB = right.rtU.b_hallB;
|
||||
feedback.right.hallC = right.rtU.b_hallC;
|
||||
|
||||
feedback.checksum = calculateChecksum(feedback);
|
||||
feedback.batVoltage = adc_buffer.batt1 * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;
|
||||
feedback.boardTemp = board_temp_deg_c;
|
||||
feedback.timeoutCntSerial = timeoutCntSerial;
|
||||
|
||||
UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN;
|
||||
UART_DMA_CHANNEL->CNDTR = sizeof(feedback);
|
||||
UART_DMA_CHANNEL->CMAR = uint64_t(&feedback);
|
||||
UART_DMA_CHANNEL->CCR |= DMA_CCR_EN;
|
||||
}
|
||||
feedback.checksum = calculateChecksum(feedback);
|
||||
|
||||
UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN;
|
||||
UART_DMA_CHANNEL->CNDTR = sizeof(feedback);
|
||||
UART_DMA_CHANNEL->CMAR = uint64_t(&feedback);
|
||||
UART_DMA_CHANNEL->CCR |= DMA_CCR_EN;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1101,8 +1128,7 @@ extern "C" void HardFault_Handler() {
|
||||
/* USER CODE BEGIN HardFault_IRQn 0 */
|
||||
|
||||
/* USER CODE END HardFault_IRQn 0 */
|
||||
while(1) {
|
||||
}
|
||||
for (;;);
|
||||
/* USER CODE BEGIN HardFault_IRQn 1 */
|
||||
|
||||
/* USER CODE END HardFault_IRQn 1 */
|
||||
@ -1115,8 +1141,7 @@ extern "C" void MemManage_Handler() {
|
||||
/* USER CODE BEGIN MemoryManagement_IRQn 0 */
|
||||
|
||||
/* USER CODE END MemoryManagement_IRQn 0 */
|
||||
while(1) {
|
||||
}
|
||||
for (;;);
|
||||
/* USER CODE BEGIN MemoryManagement_IRQn 1 */
|
||||
|
||||
/* USER CODE END MemoryManagement_IRQn 1 */
|
||||
@ -1129,8 +1154,7 @@ extern "C" void BusFault_Handler() {
|
||||
/* USER CODE BEGIN BusFault_IRQn 0 */
|
||||
|
||||
/* USER CODE END BusFault_IRQn 0 */
|
||||
while(1) {
|
||||
}
|
||||
for (;;);
|
||||
/* USER CODE BEGIN BusFault_IRQn 1 */
|
||||
|
||||
/* USER CODE END BusFault_IRQn 1 */
|
||||
@ -1143,8 +1167,7 @@ extern "C" void UsageFault_Handler() {
|
||||
/* USER CODE BEGIN UsageFault_IRQn 0 */
|
||||
|
||||
/* USER CODE END UsageFault_IRQn 0 */
|
||||
while(1) {
|
||||
}
|
||||
for (;;);
|
||||
/* USER CODE BEGIN UsageFault_IRQn 1 */
|
||||
|
||||
/* USER CODE END UsageFault_IRQn 1 */
|
||||
@ -1189,10 +1212,6 @@ extern "C" void PendSV_Handler() {
|
||||
/**
|
||||
* @brief This function handles System tick timer.
|
||||
*/
|
||||
#ifdef CONTROL_PPM
|
||||
extern "C" void PPM_SysTick_Callback();
|
||||
#endif
|
||||
|
||||
extern "C" void SysTick_Handler() {
|
||||
/* USER CODE BEGIN SysTick_IRQn 0 */
|
||||
|
||||
@ -1200,9 +1219,7 @@ extern "C" void SysTick_Handler() {
|
||||
HAL_IncTick();
|
||||
HAL_SYSTICK_IRQHandler();
|
||||
/* USER CODE BEGIN SysTick_IRQn 1 */
|
||||
#ifdef CONTROL_PPM
|
||||
PPM_SysTick_Callback();
|
||||
#endif
|
||||
|
||||
/* USER CODE END SysTick_IRQn 1 */
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user