diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 7577422b..98f1ce9d 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -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 diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino index b6d6b8eb..51ac990e 100644 --- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -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 diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index a508bb19..eb52f51b 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -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); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index 2d28790c..b40ab62a 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -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); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index 688c8b6a..76a7296a 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -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); diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino index afcfd01d..02593286 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v3/single_full_control_example/single_full_control_example.ino @@ -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); diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index 3f3ccb18..c9123401 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -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; diff --git a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino index 798273c1..c1a5139b 100644 --- a/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino +++ b/examples/utils/current_sense_test/inline_current_sense_test/inline_current_sense_test.ino @@ -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() { diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 571cd861..acbe4e45 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -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(){ diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index 7a0424d9..5fdca7d7 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -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; diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index b5b72052..a18145ca 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -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 diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 654e0d45..1652b330 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -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;