feedc0de/bobbycar-controller-firmware
1
0
Fork 0
Code Issues 8 Pull Requests 5 Actions Packages Projects Releases Wiki Activity

More cleanups

Browse Source
This commit is contained in:
0xFEEDC0DE64
2021-05-14 21:56:47 +02:00
parent 274466e017
commit a88af55928
1 changed files with 76 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

131
main.cpp
View File

@ -184,6 +184,7 @@ int main()
enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC }; enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC };
left.rtP = rtP_Left; left.rtP = rtP_Left;
left.rtP.b_angleMeasEna = 0;
#ifdef PETERS_PLATINE #ifdef PETERS_PLATINE
left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; // Left motor measured current phases = {iB, iC} -> do NOT change left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; // Left motor measured current phases = {iB, iC} -> do NOT change
#else #else
@ -198,12 +199,9 @@ int main()
left.rtP.a_phaAdvMax = left.state.phaseAdvMax << 4; // fixdt(1,16,4) left.rtP.a_phaAdvMax = left.state.phaseAdvMax << 4; // fixdt(1,16,4)
left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4)
left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4)
left.rtM.defaultParam = &left.rtP;
left.rtM.dwork = &left.rtDW;
left.rtM.inputs = &left.rtU;
left.rtM.outputs = &left.rtY;
right.rtP = rtP_Left; right.rtP = rtP_Left;
right.rtP.b_angleMeasEna = 0;
right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; // Right motor measured current phases = {iB, iC} -> do NOT change right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; // Right motor measured current phases = {iB, iC} -> do NOT change
right.rtP.z_ctrlTypSel = uint8_t(right.state.ctrlTyp); right.rtP.z_ctrlTypSel = uint8_t(right.state.ctrlTyp);
right.rtP.b_diagEna = DIAG_ENA; right.rtP.b_diagEna = DIAG_ENA;
@ -214,6 +212,12 @@ int main()
right.rtP.a_phaAdvMax = right.state.phaseAdvMax << 4; // fixdt(1,16,4) right.rtP.a_phaAdvMax = right.state.phaseAdvMax << 4; // fixdt(1,16,4)
right.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) right.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4)
right.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) right.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4)
left.rtM.defaultParam = &left.rtP;
left.rtM.dwork = &left.rtDW;
left.rtM.inputs = &left.rtU;
left.rtM.outputs = &left.rtY;
right.rtM.defaultParam = &right.rtP; right.rtM.defaultParam = &right.rtP;
right.rtM.dwork = &right.rtDW; right.rtM.dwork = &right.rtDW;
right.rtM.inputs = &right.rtU; right.rtM.inputs = &right.rtU;
@ -311,12 +315,62 @@ int main()
} }
#endif #endif
left.rtP.z_ctrlTypSel = uint8_t(left.state.ctrlTyp);
left.rtP.i_max = (left.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4)
left.rtP.n_max = left.state.nMotMax << 4; // fixdt(1,16,4)
left.rtP.id_fieldWeakMax = (left.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4)
left.rtP.a_phaAdvMax = left.state.phaseAdvMax << 4; // fixdt(1,16,4)
left.rtU.z_ctrlModReq = uint8_t(left.state.ctrlMod);
left.rtU.r_inpTgt = left.state.pwm;
right.rtP.z_ctrlTypSel = uint8_t(right.state.ctrlTyp);
right.rtP.i_max = (right.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4)
right.rtP.n_max = right.state.nMotMax << 4; // fixdt(1,16,4)
right.rtP.id_fieldWeakMax = (right.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4)
right.rtP.a_phaAdvMax = right.state.phaseAdvMax << 4; // fixdt(1,16,4)
right.rtU.z_ctrlModReq = uint8_t(right.state.ctrlMod);
right.rtU.r_inpTgt = right.state.pwm;
main_loop_counter++; main_loop_counter++;
timeout++; timeout++;
} }
} }
namespace { namespace {
void updateBatVoltage()
{
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
}
}
void updateBuzzer()
{
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
}
//create square wave for buzzer
buzzer.timer++;
if (buzzer.state.freq != 0 && (buzzer.timer / 1000) % (buzzer.state.pattern + 1) == 0)
{
if (buzzer.timer % buzzer.state.freq == 0)
{
HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN);
}
}
else
{
HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, GPIO_PIN_RESET);
}
}
void updateMotors() void updateMotors()
{ {
DMA1->IFCR = DMA_IFCR_CTCIF1; DMA1->IFCR = DMA_IFCR_CTCIF1;
@ -333,12 +387,6 @@ void updateMotors()
return; 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 // Get Left motor currents
#ifdef PETERS_PLATINE #ifdef PETERS_PLATINE
int16_t curL_phaB = (int16_t)(offsetrl1 - adc_buffer.rl1)*2; int16_t curL_phaB = (int16_t)(offsetrl1 - adc_buffer.rl1)*2;
@ -359,48 +407,27 @@ void updateMotors()
#endif #endif
int16_t curR_DC = (int16_t)(offsetdcr - adc_buffer.dcr); int16_t curR_DC = (int16_t)(offsetdcr - adc_buffer.dcr);
const int8_t chopL = std::abs(curL_DC) > (left.state.iDcMax * A2BIT_CONV); const bool chopL = std::abs(curL_DC) > (left.state.iDcMax * A2BIT_CONV);
if (chopL) if (chopL)
left.chops++; left.chops++;
const int8_t chopR = std::abs(curR_DC) > (right.state.iDcMax * A2BIT_CONV); const bool chopR = std::abs(curR_DC) > (right.state.iDcMax * A2BIT_CONV);
if (chopR) if (chopR)
right.chops++; right.chops++;
const auto timeoutVal = timeout.load();
// Disable PWM when current limit is reached (current chopping) // 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 // This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX
const auto timeoutVal = timeout.load(); if (chopL || timeoutVal > TIMEOUT || !left.state.enable)
if (chopL || timeoutVal > TIMEOUT || left.state.enable == 0)
{
LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
}
else else
{
LEFT_TIM->BDTR |= TIM_BDTR_MOE; LEFT_TIM->BDTR |= TIM_BDTR_MOE;
}
if (chopR || timeoutVal > TIMEOUT || right.state.enable == 0) if (chopR || timeoutVal > TIMEOUT || !right.state.enable)
{
RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
}
else else
{
RIGHT_TIM->BDTR |= TIM_BDTR_MOE; RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
}
//create square wave for buzzer
buzzer.timer++;
if (buzzer.state.freq != 0 && (buzzer.timer / 1000) % (buzzer.state.pattern + 1) == 0)
{
if (buzzer.timer % buzzer.state.freq == 0)
{
HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN);
}
}
else
{
HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, GPIO_PIN_RESET);
}
// ############################### MOTOR CONTROL ############################### // ############################### MOTOR CONTROL ###############################
@ -416,11 +443,13 @@ void updateMotors()
/* Make sure to stop BOTH motors in case of an error */ /* Make sure to stop BOTH motors in case of an error */
constexpr bool ignoreOtherMotor =
#ifdef FEATURE_IGNORE_OTHER_MOTOR #ifdef FEATURE_IGNORE_OTHER_MOTOR
constexpr bool ignoreOtherMotor = false; false
#else #else
constexpr bool ignoreOtherMotor = true; true
#endif #endif
;
const bool enableLFin = left.state.enable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor); const bool enableLFin = left.state.enable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor);
const bool enableRFin = right.state.enable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0; const bool enableRFin = right.state.enable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0;
@ -432,15 +461,7 @@ void updateMotors()
bool hall_wl = !(LEFT_HALL_W_PORT->IDR & LEFT_HALL_W_PIN); bool hall_wl = !(LEFT_HALL_W_PORT->IDR & LEFT_HALL_W_PIN);
/* Set motor inputs here */ /* Set motor inputs here */
left.rtP.z_ctrlTypSel = uint8_t(left.state.ctrlTyp);
left.rtP.i_max = (left.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4)
left.rtP.n_max = left.state.nMotMax << 4; // fixdt(1,16,4)
left.rtP.id_fieldWeakMax = (left.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4)
left.rtP.a_phaAdvMax = left.state.phaseAdvMax << 4; // fixdt(1,16,4)
left.rtU.b_motEna = enableLFin; left.rtU.b_motEna = enableLFin;
left.rtU.z_ctrlModReq = uint8_t(left.state.ctrlMod);
left.rtU.r_inpTgt = left.state.pwm;
left.rtU.b_hallA = hall_ul; left.rtU.b_hallA = hall_ul;
left.rtU.b_hallB = hall_vl; left.rtU.b_hallB = hall_vl;
left.rtU.b_hallC = hall_wl; left.rtU.b_hallC = hall_wl;
@ -470,15 +491,7 @@ void updateMotors()
bool hall_wr = !(RIGHT_HALL_W_PORT->IDR & RIGHT_HALL_W_PIN); bool hall_wr = !(RIGHT_HALL_W_PORT->IDR & RIGHT_HALL_W_PIN);
/* Set motor inputs here */ /* Set motor inputs here */
right.rtP.z_ctrlTypSel = uint8_t(right.state.ctrlTyp);
right.rtP.i_max = (right.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4)
right.rtP.n_max = right.state.nMotMax << 4; // fixdt(1,16,4)
right.rtP.id_fieldWeakMax = (right.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4)
right.rtP.a_phaAdvMax = right.state.phaseAdvMax << 4; // fixdt(1,16,4)
right.rtU.b_motEna = enableRFin; right.rtU.b_motEna = enableRFin;
right.rtU.z_ctrlModReq = uint8_t(right.state.ctrlMod);
right.rtU.r_inpTgt = right.state.pwm;
right.rtU.b_hallA = hall_ur; right.rtU.b_hallA = hall_ur;
right.rtU.b_hallB = hall_vr; right.rtU.b_hallB = hall_vr;
right.rtU.b_hallC = hall_wr; right.rtU.b_hallC = hall_wr;
@ -1329,7 +1342,15 @@ extern "C" void SysTick_Handler()
// ================================= // =================================
extern "C" void DMA1_Channel1_IRQHandler() extern "C" void DMA1_Channel1_IRQHandler()
{ {
/* USER CODE BEGIN DMA1_Channel1_IRQn 0 */
/* USER CODE END DMA1_Channel1_IRQn 0 */
updateMotors(); updateMotors();
updateBatVoltage();
updateBuzzer();
/* USER CODE BEGIN DMA1_Channel1_IRQn 1 */
/* USER CODE END DMA1_Channel1_IRQn 1 */
} }
#ifdef HUARN2 #ifdef HUARN2