Skip to content

Commit

Permalink
feactoring and added ramping for current sense align
Browse files Browse the repository at this point in the history
  • Loading branch information
@askuric
askuric committed Jul 15, 2024
1 parent 780bda9 commit ad0c4c5
Show file tree
Hide file tree
Showing 3 changed files with 76 additions and 50 deletions.
3 changes: 2 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
121 changes: 73 additions & 48 deletions src/common/base_classes/CurrentSense.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion src/common/foc_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down

0 comments on commit ad0c4c5

Please sign in to comment.