Skip to content

add constructor taking mV per Amp to current sense #253

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 5 commits into from
Feb 20, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/workflows/ccpp.yml
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ jobs:

- arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example
platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json
sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, sdouble_full_control_example.ino, stm32_current_control_low_side.ino
sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino


# Do not cancel all jobs / architectures if one job fails
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
// Motor instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);


// encoder instance
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ void doA(){encoder.handleA();}
void doB(){encoder.handleB();}

// inline current sensor instance
InlineCurrentSense current_sense = InlineCurrentSense(0.001, 50.0, A0, A1);
InlineCurrentSense current_sense = InlineCurrentSense(0.001f, 50.0f, A0, A1);

// commander communication instance
Commander command = Commander(Serial);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,11 @@ void doB2(){encoder2.handleB();}

// inline current sensor instance
// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor)
InlineCurrentSense current_sense1 = InlineCurrentSense(0.01, 50.0, A0, A2);
InlineCurrentSense current_sense1 = InlineCurrentSense(0.01f, 50.0f, A0, A2);

// inline current sensor instance
// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor)
InlineCurrentSense current_sense2 = InlineCurrentSense(0.01, 50.0, A1, A3);
InlineCurrentSense current_sense2 = InlineCurrentSense(0.01f, 50.0f, A1, A3);

// commander communication instance
Commander command = Commander(Serial);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ void doB(){encoder.handleB();}

// inline current sensor instance
// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor)
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2);

// commander communication instance
Commander command = Commander(Serial);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ void doB(){encoder.handleB();}

// inline current sensor instance
// ACS712-05B has the resolution of 0.185mV per Amp
InlineCurrentSense current_sense = InlineCurrentSense(1, 0.185, A0, A2);
InlineCurrentSense current_sense = InlineCurrentSense(1.0f, 0.185f, A0, A2);

// commander communication instance
Commander command = Commander(Serial);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ void doA(){encoder.handleA();}
void doB(){encoder.handleB();}

// current sensor
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2);

// current set point variable
float target_current = 0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
// shunt resistor value
// gain value
// pins phase A,B, (C optional)
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2);


void setup() {
Expand Down
17 changes: 16 additions & 1 deletion src/current_sense/InlineCurrentSense.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,22 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _
gain_a = volts_to_amps_ratio;
gain_b = volts_to_amps_ratio;
gain_c = volts_to_amps_ratio;
}
};


InlineCurrentSense::InlineCurrentSense(float _mVpA, int _pinA, int _pinB, int _pinC){
pinA = _pinA;
pinB = _pinB;
pinC = _pinC;

volts_to_amps_ratio = _mVpA / 1000.0f; // mV to amps
// gains for each phase
gain_a = volts_to_amps_ratio;
gain_b = volts_to_amps_ratio;
gain_c = volts_to_amps_ratio;
};



// Inline sensor init function
int InlineCurrentSense::init(){
Expand Down
8 changes: 8 additions & 0 deletions src/current_sense/InlineCurrentSense.h
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,14 @@ class InlineCurrentSense: public CurrentSense{
@param phC C phase adc pin (optional)
*/
InlineCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
/**
InlineCurrentSense class constructor
@param mVpA mV per Amp ratio
@param phA A phase adc pin
@param phB B phase adc pin
@param phC C phase adc pin (optional)
*/
InlineCurrentSense(float mVpA, int pinA, int pinB, int pinC = NOT_SET);

// CurrentSense interface implementing functions
int init() override;
Expand Down
14 changes: 14 additions & 0 deletions src/current_sense/LowsideCurrentSense.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,20 @@ LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int
gain_c = volts_to_amps_ratio;
}


LowsideCurrentSense::LowsideCurrentSense(float _mVpA, int _pinA, int _pinB, int _pinC){
pinA = _pinA;
pinB = _pinB;
pinC = _pinC;

volts_to_amps_ratio = _mVpA / 1000.0f; // mV to amps
// gains for each phase
gain_a = volts_to_amps_ratio;
gain_b = volts_to_amps_ratio;
gain_c = volts_to_amps_ratio;
}


// Lowside sensor init function
int LowsideCurrentSense::init(){
// configure ADC variables
Expand Down
8 changes: 8 additions & 0 deletions src/current_sense/LowsideCurrentSense.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,14 @@ class LowsideCurrentSense: public CurrentSense{
@param phC C phase adc pin (optional)
*/
LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = _NC);
/**
LowsideCurrentSense class constructor
@param mVpA mV per Amp ratio
@param phA A phase adc pin
@param phB B phase adc pin
@param phC C phase adc pin (optional)
*/
LowsideCurrentSense(float mVpA, int pinA, int pinB, int pinC = _NC);

// CurrentSense interface implementing functions
int init() override;
Expand Down