From ad0c4c526bad3c39ec5fc4e099d3ec3912fa648c Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 15 Jul 2024 13:19:34 +0200 Subject: [PATCH] feactoring and added ramping for current sense align --- README.md | 3 +- src/common/base_classes/CurrentSense.cpp | 121 ++++++++++++++--------- src/common/foc_utils.h | 2 +- 3 files changed, 76 insertions(+), 50 deletions(-) diff --git a/README.md b/README.md index 9fb24dd1..543e6533 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,8 @@ ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev) ![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev) -[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![arduino-library-badge](https://ardubadge.simplefoc.com?lib=Simple%20FOC)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![PlatformIO Registry](https://badges.registry.platformio.org/packages/askuric/library/Simple%20FOC.svg)](https://registry.platformio.org/libraries/askuric/Simple%20FOC) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 1b3ae498..5ef906e0 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -177,11 +177,17 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ bool phases_switched = 0; bool phases_inverted = 0; - + float center = driver->voltage_limit/2.0; + // set phase A active and phases B and C down - bldc_driver->setPwm(voltage, 0, 0); + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i) , 0, 0); + _delay(3); + } _delay(500); PhaseCurrent_s c_a = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); // check if currents are to low (lower than 100mA) // TODO calculate the 100mA threshold from the ADC resolution // if yes throw an error and return 0 @@ -191,6 +197,7 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); return 0; // measurement current too low } + // now we have to determine // 1) which pin correspond to which phase of the bldc driver @@ -284,7 +291,11 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // set phase B active and phases A and C down - bldc_driver->setPwm(0, voltage, 0); + // 300 ms of ramping + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(0, voltage/100.0*((float)i), 0); + _delay(3); + } _delay(500); PhaseCurrent_s c_b = readAverageCurrents(); bldc_driver->setPwm(0, 0, 0); @@ -384,53 +395,67 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive bool phases_switched = 0; bool phases_inverted = 0; - if(_isset(pinA)){ - // set phase A active to high and B to low - stepper_driver->setPwm(voltage, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - // disable the phases - stepper_driver->setPwm(0, 0); - if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ - SIMPLEFOC_DEBUG("CS: Err too low current!"); - return 0; // measurement current too low - } - // align phase A - // check if measured current a is positive and invert if not - // check if current b is around zero and if its not - // check if current a is near zero and if it is invert them - if (fabs(c.a) < fabs(c.b)){ - SIMPLEFOC_DEBUG("CS: Switch A-B"); - // switch phase A and B - _swap(pinA, pinB); - _swap(offset_ia, offset_ib); - _swap(gain_a, gain_b); - gain_a *= _sign(c.b); - phases_switched = true; // signal that pins have been switched - }else if (c.a < 0){ - SIMPLEFOC_DEBUG("CS: Inv A"); - gain_a *= -1; - phases_inverted = true; // signal that pins have been inverted - } + if(!_isset(pinA) || !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); + return 0; } - if(_isset(pinB)){ - // set phase B active and phases A and C down - stepper_driver->setPwm(voltage, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - stepper_driver->setPwm(0, 0); - if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ - SIMPLEFOC_DEBUG("CS: Err too low current!"); - return 0; // measurement current too low - } - // align phase A - // check if measured current a is positive and invert if not - if (c.b < 0){ - SIMPLEFOC_DEBUG("CS: Inv B"); - gain_b *= -1; - phases_inverted = true; // signal that pins have been inverted - } + // set phase A active and phases B down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + stepper_driver->setPwm(0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // align phase A + // 1) only one phase can be measured so we first measure which ADC pin corresponds + // to the phase A by comparing the magnitude + if (fabs(c.a) < fabs(c.b)){ + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + phases_switched = true; // signal that pins have been switched + } + // 2) check if measured current a is positive and invert if not + if (c.a < 0){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // at this point the driver's phase A is aligned with the ADC pinA + // and the pin B should be the phase B + + // set phase B active and phases A down + // ramp 300ms + for(int i=0; i < 100; i++){ + stepper_driver->setPwm(0, voltage/100.0*((float)i)); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + stepper_driver->setPwm(0, 0); + + // phase B should be aligned + // 1) we just need to verify that it has been measured + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // 2) check if measured current a is positive and invert if not + if (c.b < 0){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted } // construct the return flag diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 9a5b53ea..2094ab26 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -35,7 +35,7 @@ #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 -#define _NC (NOT_SET) +#define _NC ((int) NOT_SET) #define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f)