diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 2377cd61..6171ab77 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -4,11 +4,62 @@ jobs: build: name: Test compiling runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:avr:uno # arudino uno + - arduino:sam:arduino_due_x # arduino due + - arduino:samd:nano_33_iot # samd21 + - adafruit:samd:adafruit_metro_m4 # samd51 + - esp32:esp32:esp32doit-devkit-v1 # esm32 + - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - arduino:mbed_rp2040:pico # rpi pico + + include: + - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples + sketch-names: '**.ino' + required-libraries: PciManager + sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1_position_control + + - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example + sketch-names: single_full_control_example.ino + + - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 + sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino + + - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example + sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 # samd51 - one full example + platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json + sketch-names: single_full_control_example.ino + + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 + platform-url: https://dl.espressif.com/dl/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino + + - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino + + - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # one full example + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino + + + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false steps: - name: Checkout uses: actions/checkout@master - name: Compile all examples - uses: ArminJo/arduino-test-compile@v1.0.0 + uses: ArminJo/arduino-test-compile@master with: - libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} diff --git a/README.md b/README.md index b9042658..c0e019d6 100644 --- a/README.md +++ b/README.md @@ -16,24 +16,32 @@ Therefore this is an attempt to: - ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -##### Release notes be: SimpleFOClibrary v2.1.1 -> - bugfixes commander -> - bugfix `voltage_limit` when provided `phase_resistance` -> - bugfix `hall_sensor` examples -> - added examples fot the PowerShield -> - added initial support for `MagneticSensorPWM` -> - SAMD51 support -> - **initial support for Raspberry pi Pico** -> - added examples to find the raw max and min of the analog and pwm sensor -> - extension of the `Commander` interface -> - added pwm centering option `WC` -> - added pwm modulation cmd `WT` -> - improved esp32 implementation to avoid the need for mcpwm.h changes by @tschundler -> - issue #62: motor.shaft_angle setting in the motor.initFOC() -> - issue #76: esp32 division by zero -> - issue #46: commander end of line character - by @maxlem -> - [community fix](https://community.simplefoc.com/t/as5600-dead-spot-around-0/208) AS5600 register value -> - renamed `Pullup::EXTERN` and `Pullup::INTERN` to `Pullup::USE_EXTERN` and `Pullup::USE_INTERN` + +
+

NEW RELEASE 📢: SimpleFOClibrary v2.2 - see release

+ +
## Arduino *SimpleFOClibrary* v2.1 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 new file mode 100644 index 00000000..83451aa7 --- /dev/null +++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino @@ -0,0 +1,112 @@ +/** + * B-G431B-ESC1 position motion control example with encoder + * + */ +#include + +// Motor instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver6PWM driver = BLDCDriver6PWM(PHASE_UH, PHASE_UL, PHASE_VH, PHASE_VL, PHASE_WH, PHASE_WL); +LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, OP1_OUT, OP2_OUT, OP3_OUT); + + +// encoder instance +Encoder encoder = Encoder(HALL2, HALL3, 2048, HALL1); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} +void doIndex(){encoder.handleIndex();} + +// angle set point variable +float target_angle = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // current sensing + currentSense.init(); + // no need for aligning + currentSense.skip_align = true; + motor.linkCurrentSense(¤tSense); + + // aligning voltage [V] + motor.voltage_sensor_align = 3; + // index search velocity [rad/s] + motor.velocity_index_search = 3; + + // set motion control loop to be used + motor.controller = MotionControlType::velocity; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + // default voltage_power_supply + motor.voltage_limit = 6; + // jerk control using voltage voltage ramp + // default value is 300 volts per sec ~ 0.3V per millisecond + motor.PID_velocity.output_ramp = 1000; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle P controller + motor.P_angle.P = 20; + // maximal velocity of the position control + motor.velocity_limit = 4; + + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target angle"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target angle using serial terminal:")); + _delay(1000); +} + +// angle set point variable +float target_angle = 0; + +void loop() { + // main FOC algorithm function + + // Motion control function + motor.move(target_angle); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/build_opt.h b/examples/hardware_specific_examples/B_G431B_ESC1/build_opt.h new file mode 100644 index 00000000..6f547ecd --- /dev/null +++ b/examples/hardware_specific_examples/B_G431B_ESC1/build_opt.h @@ -0,0 +1 @@ +-DHAL_OPAMP_MODULE_ENABLED \ No newline at end of file diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index 267c00fb..be853f0e 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -1,9 +1,9 @@ /** - * + * * STM32 Bluepill position motion control example with encoder - * + * * The same example can be ran with any STM32 board - just make sure that put right pin numbers. - * + * */ #include @@ -32,10 +32,10 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB, doI); + encoder.enableInterrupts(doA, doB, doI); // link the motor to the sensor motor.linkSensor(&encoder); @@ -54,20 +54,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -75,11 +75,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -89,7 +89,7 @@ void setup() { command.add('T', doTarget, "target angle"); Serial.println(F("Motor ready.")); - Serial.println(F(("Set the target angle using serial terminal:")); + Serial.println(F("Set the target angle using serial terminal:")); _delay(1000); } @@ -97,7 +97,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -109,7 +109,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index 0adf4c17..e27f1e7a 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino @@ -1,9 +1,9 @@ /** - * + * * STM32 Bluepill position motion control example with magnetic sensor - * + * * The same example can be ran with any STM32 board - just make sure that put right pin numbers. - * + * */ #include @@ -47,37 +47,37 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -91,15 +91,12 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -112,7 +109,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 746dde3d..842f383a 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -1,15 +1,15 @@ /** * Comprehensive BLDC motor control example using encoder and the DRV8302 board - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * check the https://docs.simplefoc.com for full list of motor commands - * + * */ #include @@ -20,7 +20,7 @@ #define INH_C 11 #define EN_GATE 7 -#define M_PWM A1 +#define M_PWM A1 #define M_OC A2 #define OC_ADJ A3 @@ -45,7 +45,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -76,14 +76,14 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -111,7 +111,7 @@ void setup() { Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); Serial.println(F("Initial motion control loop is voltage loop.")); Serial.println(F("Initial target voltage 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 3e936460..6c75855a 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -1,15 +1,15 @@ /** * Comprehensive BLDC motor control example using encoder and the DRV8302 board - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * check the https://docs.simplefoc.com for full list of motor commands - * + * */ #include @@ -23,7 +23,7 @@ #define INL_C 10 #define EN_GATE 7 -#define M_PWM A1 +#define M_PWM A1 #define M_OC A2 #define OC_ADJ A3 @@ -48,7 +48,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -77,14 +77,14 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -112,7 +112,7 @@ void setup() { Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); Serial.println(F("Initial motion control loop is voltage loop.")); Serial.println(F("Initial target voltage 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino new file mode 100644 index 00000000..202aa337 --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/esp32_current_control_low_side/esp32_current_control_low_side.ino @@ -0,0 +1,164 @@ +/** + * Comprehensive BLDC motor control example using encoder and the DRV8302 board + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * check the https://docs.simplefoc.com for full list of motor commands + * + */ +#include + +// DRV8302 pins connections +// don't forget to connect the common ground pin +#define INH_A 21 +#define INH_B 19 +#define INH_C 18 + +#define EN_GATE 5 +#define M_PWM 25 +#define M_OC 26 +#define OC_ADJ 12 +#define OC_GAIN 14 + +#define IOUTA 34 +#define IOUTB 35 +#define IOUTC 32 + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE); + +// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V +LowsideCurrentSense cs = LowsideCurrentSense(0.005f, 12.22f, IOUTA, IOUTB, IOUTC); + +// encoder instance +Encoder encoder = Encoder(22, 23, 500); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + + +// commander interface +Commander command = Commander(Serial); +void onMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // DRV8302 specific code + // M_OC - enable overcurrent protection + pinMode(M_OC,OUTPUT); + digitalWrite(M_OC,LOW); + // M_PWM - enable 3pwm mode + pinMode(M_PWM,OUTPUT); + digitalWrite(M_PWM,HIGH); + // OD_ADJ - set the maximum overcurrent limit possible + // Better option would be to use voltage divisor to set exact value + pinMode(OC_ADJ,OUTPUT); + digitalWrite(OC_ADJ,HIGH); + pinMode(OC_GAIN,OUTPUT); + digitalWrite(OC_GAIN,LOW); + + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.pwm_frequency = 15000; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + motor.voltage_sensor_align = 0.5; + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + motor.motion_downsample = 0.0; + + // velocity loop PID + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 5.0; + // Low pass filtering time constant + motor.LPF_velocity.Tf = 0.02; + // angle loop PID + motor.P_angle.P = 20.0; + // Low pass filtering time constant + motor.LPF_angle.Tf = 0.0; + // current q loop PID + motor.PID_current_q.P = 3.0; + motor.PID_current_q.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_q.Tf = 0.02; + // current d loop PID + motor.PID_current_d.P = 3.0; + motor.PID_current_d.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_d.Tf = 0.02; + + // Limits + motor.velocity_limit = 100.0; // 100 rad/s velocity limit + motor.voltage_limit = 12.0; // 12 Volt limit + motor.current_limit = 2.0; // 2 Amp current limit + + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q + motor.monitor_downsample = 1000; + + // initialise motor + motor.init(); + + cs.init(); + cs.driverSync(&driver); + // driver 8302 has inverted gains on all channels + cs.gain_a *=-1; + cs.gain_b *=-1; + cs.gain_c *=-1; + motor.linkCurrentSense(&cs); + + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 0; + + // define the motor id + command.add('M', onMotor, "motor"); + + Serial.println(F("Full control example: ")); + Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); + Serial.println(F("Initial motion control loop is voltage loop.")); + Serial.println(F("Initial target voltage 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // monitoring the state variables + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 9864ca0b..f6dac940 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -1,4 +1,4 @@ -/** +/** * ESP32 position motion control example with encoder * */ @@ -23,14 +23,14 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -46,20 +46,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -67,11 +67,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -85,14 +85,11 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -104,7 +101,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 4bd53b55..f025895b 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -8,7 +8,7 @@ // MOSI 9 // SCK 14 // magnetic sensor instance - SPI -MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10); +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 15); // I2C Magnetic sensor instance (AS5600 example) // make sure to use the pull-ups!! @@ -44,37 +44,37 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -88,15 +88,12 @@ void setup() { _delay(1000); } -// angle set point variable -float target_angle = 0; - void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -109,7 +106,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index e9355f65..5b5c8e40 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -1,15 +1,15 @@ /** - * + * * HMBGC position motion control example with encoder - * + * * - Motor is connected the MOT1 connector (MOT1 9,10,11; MOT2 3,5,6) * - Encoder is connected to A0 and A1 - * - * This board doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library + * + * This board doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library * - For this example we use: PciManager library : https://github.com/prampec/arduino-pcimanager - * + * * See docs.simplefoc.com for more info. - * + * */ #include // software interrupt library @@ -49,7 +49,7 @@ void setup() { PciManager.registerListener(&listenerB); // link the motor to the sensor motor.linkSensor(&encoder); - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -65,20 +65,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -86,11 +86,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -109,7 +109,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -121,7 +121,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index fc46a338..70ec28b0 100644 --- a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -6,7 +6,6 @@ #include "Arduino.h" #include #include -#include // this is for an AS5048B absolute magnetic encoder on I2C address 0x41 MagneticSensorI2C sensor = MagneticSensorI2C(0x41, 14, 0xFE, 8); @@ -17,7 +16,7 @@ BLDCMotor motor = BLDCMotor(7); BLDCDriver3PWM driver = BLDCDriver3PWM(6,5,8); // velocity set point variable -float target_velocity = 2.0; +float target_velocity = 2.0f; // instantiate the commander Commander command = Commander(SerialUSB); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } @@ -35,11 +34,11 @@ void setup() { driver.init(); motor.linkDriver(&driver); motor.controller = MotionControlType::velocity; - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; - motor.PID_velocity.D = 0.001; + motor.PID_velocity.D = 0.001f; motor.PID_velocity.output_ramp = 1000; - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; motor.voltage_limit = 9; //motor.P_angle.P = 20; motor.init(); 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 16c7755b..dfad6e3a 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 @@ -16,13 +16,14 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.001, 50.0, A0, A1); // commander communication instance Commander command = Commander(Serial); -void doMotor(char* cmd){ command.motor(&motor, cmd); } +// void doMotor(char* cmd){ command.motor(&motor, cmd); } +void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -38,15 +39,15 @@ void setup() { motor.torque_controller = TorqueControlType::foc_current; motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -60,7 +61,7 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); @@ -68,17 +69,18 @@ void setup() { // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 0; // subscribe motor to the commander - command.add('M', doMotor, "motor"); + // command.add('M', doMotor, "motor"); + command.add('T', doTarget, "target"); + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println("Motor ready."); - // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println(F("Motor commands sketch | Initial motion control > torque/current : target 0Amps.")); - _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino index e9f4e840..c048956b 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino @@ -4,7 +4,7 @@ // BLDC motor & driver instance BLDCMotor motor1 = BLDCMotor(11); BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 5, 6, 8); - + // BLDC motor & driver instance BLDCMotor motor2 = BLDCMotor(11); BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 10, 11, 7); @@ -30,14 +30,14 @@ void setup() { // initialize encoder sensor hardware encoder1.init(); - encoder1.enableInterrupts(doA1, doB1); + encoder1.enableInterrupts(doA1, doB1); // initialize encoder sensor hardware encoder2.init(); - encoder2.enableInterrupts(doA2, doB2); + encoder2.enableInterrupts(doA2, doB2); // link the motor to the sensor motor1.linkSensor(&encoder1); motor2.linkSensor(&encoder2); - + // driver config // power supply voltage [V] @@ -55,14 +55,14 @@ void setup() { motor1.controller = MotionControlType::torque; motor2.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor1.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05f; motor1.PID_velocity.I = 1; motor1.PID_velocity.D = 0; // default voltage_power_supply motor1.voltage_limit = 12; - // contoller configuration based on the controll type - motor2.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05f; motor2.PID_velocity.I = 1; motor2.PID_velocity.D = 0; // default voltage_power_supply @@ -78,16 +78,16 @@ void setup() { // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); - + // initialise motor motor1.init(); // align encoder and start FOC - motor1.initFOC(); - + motor1.initFOC(); + // initialise motor motor2.init(); // align encoder and start FOC - motor2.initFOC(); + motor2.initFOC(); // set the inital target value motor1.target = 2; @@ -99,7 +99,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Double motor sketch ready.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino index 5d02ecf2..10e26c9f 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino @@ -19,7 +19,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -33,15 +33,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -55,11 +55,11 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 2; @@ -69,7 +69,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } 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 757ae0f1..259b23ee 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 @@ -4,7 +4,7 @@ // BLDC motor & driver instance BLDCMotor motor1 = BLDCMotor(11); BLDCDriver3PWM driver1 = BLDCDriver3PWM(5, 10, 6, 8); - + // BLDC motor & driver instance BLDCMotor motor2 = BLDCMotor(11); BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 9, 11, 7); @@ -32,21 +32,23 @@ InlineCurrentSense current_sense2 = InlineCurrentSense(0.01, 50.0, A1, A3); // commander communication instance Commander command = Commander(Serial); -void doMotor1(char* cmd){ command.motor(&motor1, cmd); } -void doMotor2(char* cmd){ command.motor(&motor2, cmd); } +// void doMotor1(char* cmd){ command.motor(&motor1, cmd); } +// void doMotor2(char* cmd){ command.motor(&motor2, cmd); } +void doTarget1(char* cmd){ command.scalar(&motor1.target, cmd); } +void doTarget2(char* cmd){ command.scalar(&motor2.target, cmd); } void setup() { // initialize encoder sensor hardware encoder1.init(); - encoder1.enableInterrupts(doA1, doB1); + encoder1.enableInterrupts(doA1, doB1); // initialize encoder sensor hardware encoder2.init(); - encoder2.enableInterrupts(doA2, doB2); + encoder2.enableInterrupts(doA2, doB2); // link the motor to the sensor motor1.linkSensor(&encoder1); motor2.linkSensor(&encoder2); - + // driver config // power supply voltage [V] @@ -64,14 +66,14 @@ void setup() { motor1.controller = MotionControlType::torque; motor2.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor1.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05f; motor1.PID_velocity.I = 1; motor1.PID_velocity.D = 0; // default voltage_power_supply motor1.voltage_limit = 12; - // contoller configuration based on the controll type - motor2.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05f; motor2.PID_velocity.I = 1; motor2.PID_velocity.D = 0; // default voltage_power_supply @@ -87,8 +89,8 @@ void setup() { // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); - - + + // current sense init and linking current_sense1.init(); motor1.linkCurrentSense(¤t_sense1); @@ -99,24 +101,26 @@ void setup() { // initialise motor motor1.init(); // align encoder and start FOC - motor1.initFOC(); - + motor1.initFOC(); + // initialise motor motor2.init(); // align encoder and start FOC - motor2.initFOC(); + motor2.initFOC(); // set the inital target value motor1.target = 2; motor2.target = 2; // subscribe motor to the commander - command.add('A', doMotor1, "motor 1"); - command.add('B', doMotor2, "motor 2"); + // command.add('A', doMotor1, "motor 1"); + // command.add('B', doMotor2, "motor 2"); + command.add('A', doTarget1, "target 1"); + command.add('B', doTarget2, "target 2"); // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println(F("Double motor sketch ready.")); - + Serial.println("Motors ready."); + _delay(1000); } @@ -132,4 +136,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} 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 c0f6a10b..2d6c3ca6 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 @@ -17,13 +17,14 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2); // commander communication instance Commander command = Commander(Serial); -void doMotor(char* cmd){ command.motor(&motor, cmd); } +void doTarget(char* cmd){ command.scalar(&motor.target, cmd); } +// void doMotor(char* cmd){ command.motor(&motor, cmd); } void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -37,15 +38,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -59,7 +60,7 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); @@ -67,17 +68,18 @@ void setup() { // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 2; // subscribe motor to the commander - command.add('M', doMotor, "motor"); - - // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) - Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); + command.add('T', doTarget, "target"); + // command.add('M', doMotor, "motor"); + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println("Motor ready."); + _delay(1000); } diff --git a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino new file mode 100644 index 00000000..80f2fe64 --- /dev/null +++ b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino @@ -0,0 +1,61 @@ +/** + * Smart Stepper support with SimpleFOClibrary + */ +#include + +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, A2); + +// Stepper motor & driver instance +StepperMotor motor = StepperMotor(50); +int in1[2] = {5, 6}; +int in2[2] = {A4, 7}; +StepperDriver2PWM driver = StepperDriver2PWM(4, in1, 9, in2); + +// instantiate the commander +Commander command = Commander(SerialUSB); +void doMotor(char* cmd) { command.motor(&motor, cmd); } + +void setup() { + + // initialise magnetic sensor hardware + sensor.init(); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // power supply voltage + driver.voltage_power_supply = 12; + driver.init(); + motor.linkDriver(&driver); + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with SerialUSB + SerialUSB.begin(115200); + // comment out if not needed + motor.useMonitoring(SerialUSB); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add target command M + command.add('M', doMotor, "my motor"); + + SerialUSB.println(F("Motor ready.")); + SerialUSB.println(F("Set the target voltage using Serial terminal:")); + _delay(1000); +} + +void loop() { + // main FOC algorithm function + motor.loopFOC(); + + // Motion control function + motor.move(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index a8e8b997..942f73dc 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -1,27 +1,27 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and encoder + * 1) Configure the motor and encoder * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * - * + * + * * NOTE : * > Arduino UNO example code for running velocity motion control using an encoder with index significantly * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger - * + * * > If you don't want to use index pin initialize the encoder class without index pin number: * > For example: * > - Encoder encoder = Encoder(2, 3, 8192); * > and initialize interrupts like this: * > - encoder.enableInterrupts(doA,doB) - * + * * Check the docs.simplefoc.com for more info about the possible encoder configuration. - * + * */ #include // software interrupt library @@ -53,10 +53,10 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -68,7 +68,7 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] @@ -77,11 +77,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -89,9 +89,9 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -99,11 +99,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -121,7 +121,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -133,7 +133,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index 90219b5b..4bd7163f 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -1,20 +1,20 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and hall sensor + * 1) Configure the motor and hall sensor * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * - * + * + * * NOTE : * > This is for Arduino UNO example code for running angle motion control specifically * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the sensr.enableInterrupts(doA,doB,doC) and avoid using PciManger - * - * + * + * */ #include // software interrupt library @@ -46,10 +46,10 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenC); // link the motor to the sensor @@ -71,11 +71,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -83,9 +83,9 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -93,11 +93,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -115,7 +115,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -127,7 +127,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index d22289bd..752030c9 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -1,11 +1,11 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and magnetic sensor + * 1) Configure the motor and magnetic sensor * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * + * */ #include @@ -42,38 +42,38 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 20; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -93,7 +93,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -106,7 +106,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file 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 ad75b573..8b1ec96b 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 @@ -1,7 +1,7 @@ /** - * + * * Torque control example using current control loop. - * + * */ #include @@ -25,11 +25,11 @@ float target_current = 0; Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_current, cmd); } -void setup() { - +void setup() { + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -46,10 +46,10 @@ void setup() { motor.linkCurrentSense(¤t_sense); // set torque mode: - // TorqueControlType::dc_current + // TorqueControlType::dc_current // TorqueControlType::voltage // TorqueControlType::foc_current - motor.torque_controller = TorqueControlType::foc_current; + motor.torque_controller = TorqueControlType::foc_current; // set motion control loop to be used motor.controller = MotionControlType::torque; @@ -58,17 +58,17 @@ void setup() { motor.PID_current_q.I= 300; motor.PID_current_d.P= 5; motor.PID_current_d.I = 300; - motor.LPF_current_q.Tf = 0.01; - motor.LPF_current_d.Tf = 0.01; + motor.LPF_current_q.Tf = 0.01f; + motor.LPF_current_d.Tf = 0.01f; // foc currnet control parameters (stm/esp/due/teensy) // motor.PID_current_q.P = 5; // motor.PID_current_q.I= 1000; // motor.PID_current_d.P= 5; // motor.PID_current_d.I = 1000; - // motor.LPF_current_q.Tf = 0.002; // 1ms default - // motor.LPF_current_d.Tf = 0.002; // 1ms default + // motor.LPF_current_q.Tf = 0.002f; // 1ms default + // motor.LPF_current_d.Tf = 0.002f; // 1ms default - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -91,7 +91,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index a3974b38..f0f1e3e8 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -1,28 +1,28 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and encoder + * 1) Configure the motor and encoder * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * - * + * + * + * * NOTE : * > Arduino UNO example code for running velocity motion control using an encoder with index significantly * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger - * + * * > If you don't want to use index pin initialize the encoder class without index pin number: * > For example: * > - Encoder encoder = Encoder(2, 3, 8192); * > and initialize interrupts like this: * > - encoder.enableInterrupts(doA,doB) - * + * * Check the docs.simplefoc.com for more info about the possible encoder configuration. - * + * */ #include // software interrupt library @@ -59,7 +59,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -80,11 +80,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -92,11 +92,11 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -119,7 +119,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -131,7 +131,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index 07b70dcd..1d39173a 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -1,20 +1,20 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and sensor + * 1) Configure the motor and sensor * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * - * + * + * + * * NOTE : - * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor + * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doC directly to the sensor.enableInterrupts(doA,doB,doC) and avoid using PciManger - * + * */ #include // software interrupt library @@ -49,7 +49,7 @@ void setup() { // initialize sensor sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -64,15 +64,15 @@ void setup() { // aligning voltage [V] motor.voltage_sensor_align = 3; - + // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -80,11 +80,11 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -107,7 +107,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -119,7 +119,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index a16385df..a57fe634 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -1,12 +1,12 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and magnetic sensor + * 1) Configure the motor and magnetic sensor * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * + * + * * By using the serial terminal set the velocity value you want to motor to obtain * */ @@ -32,7 +32,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { - + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -48,11 +48,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -60,13 +60,13 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering - // default 5ms - try different values to see what is the best. + // default 5ms - try different values to see what is the best. // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -88,7 +88,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -100,7 +100,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 30d18297..186d257e 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using encoder - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -36,7 +36,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -53,15 +53,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -87,7 +87,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index 7a202f94..df7b76ac 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using Hall sensor - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -43,7 +43,7 @@ void setup() { // initialize encoder sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenC); // link the motor to the sensor @@ -61,15 +61,15 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -95,7 +95,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } @@ -112,5 +112,3 @@ void loop() { // user communication command.run(); } - - diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 97216a9e..098f6888 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using magnetic sensor - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -51,15 +51,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the control type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the control type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -85,7 +85,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } @@ -98,8 +98,7 @@ void loop() { // velocity, position or voltage // if tatget not set in parameter uses motor.target variable motor.move(); - + // user communication command.run(); } - diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index b0a24d25..afb95294 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -93,11 +93,11 @@ void setup() { driver.init(); motor.linkDriver(&driver); motor.controller = MotionControlType::velocity; - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; - motor.PID_velocity.D = 0.001; + motor.PID_velocity.D = 0.001f; motor.PID_velocity.output_ramp = 1000; - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; motor.voltage_limit = 8; //motor.P_angle.P = 20; motor.init(); @@ -107,9 +107,9 @@ void setup() { } // velocity set point variable -float target_velocity = 2.0; +float target_velocity = 2.0f; // angle set point variable -float target_angle = 1.0; +float target_angle = 1.0f; void motorControl(OSCMessage &msg){ diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino index 917f569c..7c6aa4f0 100644 --- a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino @@ -30,7 +30,7 @@ WiFiUDP Udp; IPAddress outIp(192,168,1,13); // remote IP (not needed for receive) const unsigned int outPort = 8000; // remote port (not needed for receive) const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) -#define POWER_SUPPLY 7.4 +#define POWER_SUPPLY 7.4f @@ -46,8 +46,8 @@ String m2Prefix("/M2"); // we store seperate set-points for each motor, of course -float set_point1 = 0.0; -float set_point2 = 0.0; +float set_point1 = 0.0f; +float set_point2 = 0.0f; OSCErrorCode error; @@ -135,11 +135,11 @@ void setup() { motor1.linkDriver(&driver1); motor1.foc_modulation = FOCModulationType::SpaceVectorPWM; motor1.controller = MotionControlType::velocity; - motor1.PID_velocity.P = 0.2; + motor1.PID_velocity.P = 0.2f; motor1.PID_velocity.I = 20; - motor1.PID_velocity.D = 0.001; + motor1.PID_velocity.D = 0.001f; motor1.PID_velocity.output_ramp = 1000; - motor1.LPF_velocity.Tf = 0.01; + motor1.LPF_velocity.Tf = 0.01f; motor1.voltage_limit = POWER_SUPPLY; motor1.P_angle.P = 20; motor1.init(); @@ -152,11 +152,11 @@ void setup() { motor2.linkDriver(&driver2); motor2.foc_modulation = FOCModulationType::SpaceVectorPWM; motor2.controller = MotionControlType::velocity; - motor2.PID_velocity.P = 0.2; + motor2.PID_velocity.P = 0.2f; motor2.PID_velocity.I = 20; - motor2.PID_velocity.D = 0.001; + motor2.PID_velocity.D = 0.001f; motor2.PID_velocity.output_ramp = 1000; - motor2.LPF_velocity.Tf = 0.01; + motor2.LPF_velocity.Tf = 0.01f; motor2.voltage_limit = POWER_SUPPLY; motor2.P_angle.P = 20; motor2.init(); diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino index 708e2d9e..3fcd7b30 100644 --- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -9,7 +9,7 @@ MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); /** - * This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'. + * This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'. * It can be used to investigate how much non linearity there is between what we set (electrical angle) and what we read (sensor angle) * This non linearity could be down to magnet placement, coil winding differences or simply that the magnetic field when travelling through a pole pair is not linear * An alignment error of ~10 degrees and cogging of ~4 degrees is normal for small gimbal. @@ -21,6 +21,7 @@ void testAlignmentAndCogging(int direction) { motor.move(0); _delay(200); + sensor.update(); float initialAngle = sensor.getAngle(); const int shaft_rotation = 720; // 720 deg test - useful to see repeating cog pattern @@ -28,9 +29,9 @@ void testAlignmentAndCogging(int direction) { float stDevSum = 0; - float mean = 0.0; - float prev_mean = 0.0; - + float mean = 0.0f; + float prev_mean = 0.0f; + for (int i = 0; i < sample_count; i++) { @@ -39,7 +40,8 @@ void testAlignmentAndCogging(int direction) { motor.move(electricAngle * PI / 180); _delay(5); - // measure + // measure + sensor.update(); float sensorAngle = (sensor.getAngle() - initialAngle) * 180 / PI; float sensorElectricAngle = sensorAngle * motor.pole_pairs; float electricAngleError = electricAngle - sensorElectricAngle; @@ -87,7 +89,7 @@ void setup() { motor.voltage_sensor_align = 3; motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - + motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=motor.voltage_sensor_align; diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index e6a1d20b..c5dfbf43 100644 --- a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -1,17 +1,17 @@ /** * Utility arduino sketch which finds pole pair number of the motor - * + * * To run it just set the correct pin numbers for the BLDC driver and encoder A and B channel as well as the encoder PPR value. - * + * * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. - * The pole pair number will be outputted to the serial terminal. - * - * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. - * + * The pole pair number will be outputted to the serial terminal. + * + * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. + * * If the code calculates negative pole pair number please invert your encoder A and B channel pins or motor connector. - * - * Try running this code several times to avoid statistical errors. - * > But in general if your motor spins, you have a good pole pairs number. + * + * Try running this code several times to avoid statistical errors. + * > But in general if your motor spins, you have a good pole pairs number. */ #include @@ -31,7 +31,7 @@ void doA(){encoder.handleA();} void doB(){encoder.handleB();} void setup() { - + // initialise encoder hardware encoder.init(); // hardware interrupt enable @@ -57,24 +57,26 @@ void setup() { float pp_search_voltage = 4; // maximum power_supply_voltage/2 float pp_search_angle = 6*M_PI; // search electrical angle to turn - + // move motor to the electrical angle 0 motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=pp_search_voltage; motor.move(0); _delay(1000); - // read the encoder angle + // read the encoder angle + encoder.update(); float angle_begin = encoder.getAngle(); _delay(50); - + // move the motor slowly to the electrical angle pp_search_angle float motor_angle = 0; while(motor_angle <= pp_search_angle){ - motor_angle += 0.01; + motor_angle += 0.01f; motor.move(motor_angle); } _delay(1000); // read the encoder value for 180 + encoder.update(); float angle_end = encoder.getAngle(); _delay(50); // turn off the motor @@ -93,7 +95,7 @@ void setup() { Serial.print(" = "); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); - + // a bit of monitoring the result if(pp <= 0 ){ @@ -108,7 +110,7 @@ void setup() { Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); } - + // set FOC loop to be used motor.controller = MotionControlType::torque; // set the pole pair number to the motor @@ -129,7 +131,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -137,7 +139,7 @@ void loop() { // this function can be run at much lower frequency than loopFOC() function // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - + // communicate with the user serialReceiveUserCommand(); } @@ -146,10 +148,10 @@ void loop() { // utility function enabling serial communication with the user to set the target values // this function can be implemented in serialEvent function as well void serialReceiveUserCommand() { - + // a string to hold incoming data static String received_chars; - + while (Serial.available()) { // get the new byte: char inChar = (char)Serial.read(); @@ -157,13 +159,13 @@ void serialReceiveUserCommand() { received_chars += inChar; // end of user input if (inChar == '\n') { - + // change the motor target target_voltage = received_chars.toFloat(); Serial.print("Target voltage: "); Serial.println(target_voltage); - - // reset the command buffer + + // reset the command buffer received_chars = ""; } } diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index cc4bd4d0..0b07c772 100644 --- a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -1,17 +1,17 @@ /** * Utility arduino sketch which finds pole pair number of the motor - * + * * To run it just set the correct pin numbers for the BLDC driver and sensor CPR value and chip select pin. - * + * * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. - * The pole pair number will be outputted to the serial terminal. - * - * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. - * + * The pole pair number will be outputted to the serial terminal. + * + * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. + * * If the code calculates negative pole pair number please invert your motor connector. - * - * Try running this code several times to avoid statistical errors. - * > But in general if your motor spins, you have a good pole pairs number. + * + * Try running this code several times to avoid statistical errors. + * > But in general if your motor spins, you have a good pole pairs number. */ #include @@ -56,25 +56,27 @@ void setup() { float pp_search_voltage = 4; // maximum power_supply_voltage/2 float pp_search_angle = 6*M_PI; // search electrical angle to turn - + // move motor to the electrical angle 0 motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=pp_search_voltage; motor.move(0); _delay(1000); - // read the sensor angle + // read the sensor angle + sensor.update(); float angle_begin = sensor.getAngle(); _delay(50); - + // move the motor slowly to the electrical angle pp_search_angle float motor_angle = 0; while(motor_angle <= pp_search_angle){ - motor_angle += 0.01; - sensor.getAngle(); // keep track of the overflow + motor_angle += 0.01f; + sensor.update(); // keep track of the overflow motor.move(motor_angle); } _delay(1000); // read the sensor value for 180 + sensor.update(); float angle_end = sensor.getAngle(); _delay(50); // turn off the motor @@ -93,7 +95,7 @@ void setup() { Serial.print(F(" = ")); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); - + // a bit of monitoring the result if(pp <= 0 ){ @@ -108,7 +110,7 @@ void setup() { Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); } - + // set motion control loop to be used motor.controller = MotionControlType::torque; // set the pole pair number to the motor @@ -129,7 +131,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -137,7 +139,7 @@ void loop() { // this function can be run at much lower frequency than loopFOC() function // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - + // communicate with the user serialReceiveUserCommand(); } @@ -146,10 +148,10 @@ void loop() { // utility function enabling serial communication with the user to set the target values // this function can be implemented in serialEvent function as well void serialReceiveUserCommand() { - + // a string to hold incoming data static String received_chars; - + while (Serial.available()) { // get the new byte: char inChar = (char)Serial.read(); @@ -157,13 +159,13 @@ void serialReceiveUserCommand() { received_chars += inChar; // end of user input if (inChar == '\n') { - + // change the motor target target_voltage = received_chars.toFloat(); Serial.print("Target voltage: "); Serial.println(target_voltage); - - // reset the command buffer + + // reset the command buffer received_chars = ""; } } diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino index d723313c..b6f0ea5f 100644 --- a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino +++ b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino @@ -1,6 +1,6 @@ /** - * A simple example of reading step/dir communication - * - this example uses software interrupts - this code is intended primarily + * A simple example of reading step/dir communication + * - this example uses software interrupts - this code is intended primarily * for Arduino UNO/Mega and similar boards with very limited number of interrupt pins */ @@ -10,11 +10,11 @@ #include -// angle +// angle float received_angle = 0; // StepDirListener( step_pin, dir_pin, counter_to_value) -StepDirListener step_dir = StepDirListener(4, 5, 2.0*_PI/200.0); // receive the angle in radians +StepDirListener step_dir = StepDirListener(4, 5, 2.0f*_PI/200.0); // receive the angle in radians void onStep() { step_dir.handle(); } // If no available hadware interrupt pins use the software interrupt @@ -23,21 +23,21 @@ PciListenerImp listenStep(step_dir.pin_step, onStep); void setup() { Serial.begin(115200); - + // init step and dir pins step_dir.init(); // enable software interrupts PciManager.registerListener(&listenStep); - // attach the variable to be updated on each step (optional) + // attach the variable to be updated on each step (optional) // the same can be done asynchronously by caling step_dir.getValue(); step_dir.attach(&received_angle); - + Serial.println(F("Step/Dir listenning.")); _delay(1000); } void loop() { - Serial.print(received_angle); + Serial.print(received_angle); Serial.print("\t"); Serial.println(step_dir.getValue()); _delay(500); diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino index 932edc81..f8978577 100644 --- a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -19,7 +19,7 @@ void doA() { encoder.handleA(); } void doB() { encoder.handleB(); } // StepDirListener( step_pin, dir_pin, counter_to_value) -StepDirListener step_dir = StepDirListener(A4, A5, 2.0*_PI/200.0); +StepDirListener step_dir = StepDirListener(A4, A5, 2.0f*_PI/200.0); void onStep() { step_dir.handle(); } void setup() { @@ -48,7 +48,7 @@ void setup() { // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -58,7 +58,7 @@ void setup() { motor.PID_velocity.output_ramp = 1000; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 10; @@ -77,12 +77,12 @@ void setup() { // init step and dir pins step_dir.init(); - // enable interrupts + // enable interrupts step_dir.enableInterrupt(onStep); - // attach the variable to be updated on each step (optional) + // attach the variable to be updated on each step (optional) // the same can be done asynchronously by caling motor.move(step_dir.getValue()); step_dir.attach(&motor.target); - + Serial.println(F("Motor ready.")); Serial.println(F("Listening to step/dir commands!")); _delay(1000); diff --git a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 972a74db..478d3974 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -5,7 +5,7 @@ BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); void setup() { - + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -14,8 +14,8 @@ void setup() { driver.voltage_power_supply = 12; // Max DC voltage allowed - default voltage_power_supply driver.voltage_limit = 12; - // daad_zone [0,1] - default 0.02 - 2% - driver.dead_zone = 0.05; + // daad_zone [0,1] - default 0.02f - 2% + driver.dead_zone = 0.05f; // driver init driver.init(); diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 7289ac79..4627efa9 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -3,8 +3,10 @@ // Stepper driver instance -// StepperDriver2PWM(pwm1, in1a, in1b, pwm2, in2a, in2b, (en1, en2 optional)) -StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 10 , 9 , 8 , 11, 12); +// StepperDriver2PWM(pwm1, in1, pwm2, in2, (en1, en2 optional)) +int in1[] = {4,5}; +int in2[] = {9,8}; +StepperDriver2PWM driver = StepperDriver2PWM(3, in1, 10 , in2, 11, 12); // StepperDriver2PWM(pwm1, dir1, pwm2, dir2,(en1, en2 optional)) // StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 6, 11, 12); diff --git a/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino b/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino index 66aca6d8..f105f5b0 100644 --- a/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino +++ b/examples/utils/sensor_test/encoder/encoder_example/encoder_example.ino @@ -33,6 +33,10 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // not doing much for the encoder though + encoder.update(); // display the angle and the angular velocity to the terminal Serial.print(encoder.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino b/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino index a979d9a0..ebf2dd41 100644 --- a/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino +++ b/examples/utils/sensor_test/encoder/encoder_software_interrupts_example/encoder_software_interrupts_example.ino @@ -46,6 +46,10 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // not doing much for the encoder though + encoder.update(); // display the angle and the angular velocity to the terminal Serial.print(encoder.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino index 6c19a2e9..ef2e504c 100644 --- a/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_example/hall_sensor_example.ino @@ -37,6 +37,9 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino similarity index 92% rename from examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino rename to examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino index 523af03f..238eb2c4 100644 --- a/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupt_example.ino +++ b/examples/utils/sensor_test/hall_sensors/hall_sensor_software_interrupts_example/hall_sensors_software_interrupts_example.ino @@ -49,6 +49,9 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/find_raw_min_max/find_raw_min_max.ino similarity index 89% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/find_raw_min_max/find_raw_min_max.ino index e45b0278..34e68a1b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/find_raw_min_max/find_raw_min_max.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/find_raw_min_max/find_raw_min_max.ino @@ -33,6 +33,11 @@ int max_count = 0; int min_count = 100000; void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // keep track of min and max if(sensor.raw_count > max_count) max_count = sensor.raw_count; diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino similarity index 83% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino index 40d47ba8..2496151b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog_example/magnetic_sensor_analog/magnetic_sensor_analog.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_analog/magnetic_sensor_analog_example/magnetic_sensor_analog_example.ino @@ -25,6 +25,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino similarity index 77% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino index c781847f..c930437b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino @@ -25,6 +25,13 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor0.update(); + sensor1.update(); + _delay(200); Serial.print(sensor0.getAngle()); Serial.print(" - "); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino similarity index 74% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino index 019e088c..08fb145d 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/stm32_i2c_dual_bus_example/stm32_i2c_dual_bus_example.ino @@ -24,6 +24,13 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor0.update(); + sensor1.update(); + _delay(200); Serial.print(sensor0.getAngle()); Serial.print(" - "); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino similarity index 81% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino index 30afc90c..4e060c0a 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino @@ -30,6 +30,12 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); + // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/find_raw_min_max/find_raw_min_max.ino similarity index 85% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/find_raw_min_max/find_raw_min_max.ino index a88b7186..ac9bf044 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/find_raw_min_max/find_raw_min_max.ino @@ -27,6 +27,11 @@ int max_pulse= 0; int min_pulse = 10000; void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // keep track of min and max if(sensor.pulse_length_us > max_pulse) max_pulse = sensor.pulse_length_us; diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino similarity index 83% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino index 15a1f557..6ae0a3e9 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_example/magnetic_sensor_pwm_example.ino @@ -26,6 +26,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino similarity index 85% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino index 6b19a9fa..10dc8a6b 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino @@ -32,6 +32,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino new file mode 100644 index 00000000..cbb72af4 --- /dev/null +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/esp32_spi_alt_example/esp32_spi_alt_example.ino @@ -0,0 +1,41 @@ +#include + +// alternative pinout +#define HSPI_MISO 12 +#define HSPI_MOSI 13 +#define HSPI_SCLK 14 +#define HSPI_SS 15 + +// MagneticSensorSPI(int cs, float _cpr, int _angle_register) +// config - SPI config +// cs - SPI chip select pin +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, HSPI_SS); + +// for esp 32, it has 2 spi interfaces VSPI (default) and HPSI as the second one +// to enable it instatiate the object +SPIClass SPI_2(HSPI); + +void setup() { + // monitoring port + Serial.begin(115200); + + // start the newly defined spi communication + SPI_2.begin(HSPI_SCLK, HSPI_MISO, HSPI_MOSI, HSPI_SS); //SCLK, MISO, MOSI, SS + // initialise magnetic sensor hardware + sensor.init(&SPI_2); + + Serial.println("Sensor ready"); + _delay(1000); +} + +void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); + // display the angle and the angular velocity to the terminal + Serial.print(sensor.getAngle()); + Serial.print("\t"); + Serial.println(sensor.getVelocity()); +} diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/stm32_spi_alt_example/stm32_spi_alt_example.ino similarity index 75% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/stm32_spi_alt_example/stm32_spi_alt_example.ino index 249837e6..713a19b1 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_alt_example/magnetic_sensor_spi_alt_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_alternative_examples/stm32_spi_alt_example/stm32_spi_alt_example.ino @@ -20,6 +20,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino similarity index 76% rename from examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino rename to examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino index 319c3741..048620ac 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino @@ -20,6 +20,11 @@ void setup() { } void loop() { + // iterative function updating the sensor internal variables + // it is usually called in motor.loopFOC() + // this function reads the sensor hardware and + // has to be called before getAngle nad getVelocity + sensor.update(); // display the angle and the angular velocity to the terminal Serial.print(sensor.getAngle()); Serial.print("\t"); diff --git a/keywords.txt b/keywords.txt index 77152a36..b47c18fd 100644 --- a/keywords.txt +++ b/keywords.txt @@ -18,6 +18,7 @@ StepperDriver KEYWORD1 PIDController KEYWORD1 LowPassFilter KEYWORD1 InlineCurrentSense KEYWORD1 +LowsideCurrentSense KEYWORD1 CurrentSense KEYWORD1 Commander KEYWORD1 StepDirListener KEYWORD1 @@ -48,6 +49,9 @@ LPF_current_q KEYWORD2 LPF_current_d KEYWORD2 P_angle KEYWORD2 LPF_angle KEYWORD2 +lpf_a KEYWORD2 +lpf_b KEYWORD2 +lpf_c KEYWORD2 MotionControlType KEYWORD1 TorqueControlType KEYWORD1 @@ -73,6 +77,9 @@ ISR KEYWORD2 getVelocity KEYWORD2 setPhaseVoltage KEYWORD2 getAngle KEYWORD2 +getMechanicalAngle KEYWORD2 +getSensorAngle KEYWORD2 +update KEYWORD2 needsSearch KEYWORD2 useMonitoring KEYWORD2 angleOpenloop KEYWORD2 diff --git a/library.properties b/library.properties index af8559f1..89540636 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.1.1 +version=2.2 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9c2f89c0..ff451e07 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -101,7 +101,8 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update - shaft_angle = sensor->getAngle(); + sensor->update(); + shaft_angle = shaftAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); // aligning the current sensor - can be skipped @@ -159,18 +160,20 @@ int BLDCMotor::alignSensor() { // find natural direction // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; + float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } // take and angle in the middle + sensor->update(); float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; + float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } + sensor->update(); float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); @@ -188,7 +191,7 @@ int BLDCMotor::alignSensor() { // check pole pair number if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! + if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! if(monitor_port) monitor_port->print(F("fail - estimated pp:")); if(monitor_port) monitor_port->println(_2PI/moved,4); }else if(monitor_port) monitor_port->println(F("OK!")); @@ -201,7 +204,12 @@ int BLDCMotor::alignSensor() { // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); + //zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); @@ -217,7 +225,8 @@ int BLDCMotor::alignSensor() { // Encoder alignment the absolute zero angle // - to the index int BLDCMotor::absoluteZeroSearch() { - + // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision + // of float is sufficient. if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity float limit_vel = velocity_limit; @@ -226,10 +235,10 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = voltage_sensor_align; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5*_2PI); + angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search - sensor->getAngle(); + sensor->update(); } // disable motor setPhaseVoltage(0, 0, 0); @@ -247,17 +256,20 @@ int BLDCMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void BLDCMotor::loopFOC() { + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) sensor->update(); + // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle - shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated - + // if disabled do nothing if(!enabled) return; - // electrical angle - need shaftAngle to be called first + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI electrical_angle = electricalAngle(); - switch (torque_controller) { case TorqueControlType::voltage: // no need to do anything really @@ -300,31 +312,45 @@ void BLDCMotor::loopFOC() { // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { - // get angular velocity + // downsampling (optional) + if(motion_cnt++ < motion_downsample) return; + motion_cnt = 0; + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing if(!enabled) return; - // downsampling (optional) - if(motion_cnt++ < motion_downsample) return; - motion_cnt = 0; // set internal target variable if(_isset(new_target)) target = new_target; switch (controller) { case MotionControlType::torque: - if(torque_controller == TorqueControlType::voltage) // if voltage torque control + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control if(!_isset(phase_resistance)) voltage.q = target; else voltage.q = target*phase_resistance; - else + voltage.d = 0; + }else{ current_sp = target; // if current/foc_current torque control + } break; case MotionControlType::angle: + // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when + // the angles are large. This results in not being able to command small changes at high position values. + // to solve this, the delta-angle has to be calculated in a numerically precise way. // angle set point shaft_angle_sp = target; // calculate velocity set point shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); - // calculate the torque command + // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage if(torque_controller == TorqueControlType::voltage){ @@ -335,7 +361,7 @@ void BLDCMotor::move(float new_target) { } break; case MotionControlType::velocity: - // velocity set point + // velocity set point - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control @@ -348,13 +374,16 @@ void BLDCMotor::move(float new_target) { } break; case MotionControlType::velocity_openloop: - // velocity control in open loop + // velocity control in open loop - sensor precision: this calculation is numerically precise. shaft_velocity_sp = target; voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor voltage.d = 0; break; case MotionControlType::angle_openloop: - // angle control in open loop + // angle control in open loop - + // TODO sensor precision: this calculation NOT numerically precise, and subject + // to the same problems in small set-point changes at high angles + // as the closed loop version. shaft_angle_sp = target; voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor voltage.d = 0; @@ -456,8 +485,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { center = driver->voltage_limit/2; // Clarke transform Ua = Ualpha + center; - Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + center; - Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + center; + Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center; + Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center; if (!modulation_centered) { float Umin = min(Ua, min(Ub, Uc)); @@ -500,11 +529,11 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { sector = floor(angle_el / _PI_3) + 1; // calculate the duty cycles float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; - float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout; // two versions possible float T0 = 0; // pulled to 0 - better for low power supply voltage if (modulation_centered) { - T0 = 1 - T1 - T2; //modulation_centered around driver->voltage_limit/2 + T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2 } // calculate the duty cycles(times) @@ -568,9 +597,9 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); @@ -597,12 +626,15 @@ float BLDCMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) + // TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point + // where small position changes are no longer captured by the precision of floats + // when the total position is large. if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){ shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; shaft_velocity = velocity_limit; @@ -616,7 +648,8 @@ float BLDCMotor::angleOpenloop(float target_angle){ float Uq = voltage_limit; if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + // sensor precision: this calculation is OK due to the normalisation + setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index eee0297b..4f9f1108 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -109,6 +109,7 @@ void loop() { #include "drivers/StepperDriver4PWM.h" #include "drivers/StepperDriver2PWM.h" #include "current_sense/InlineCurrentSense.h" +#include "current_sense/LowsideCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 183c2c65..2acd6f11 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -11,7 +11,7 @@ StepperMotor::StepperMotor(int pp, float _R) // save phase resistance number phase_resistance = _R; - // torque control type is voltage by default + // torque control type is voltage by default // current and foc_current not supported yet torque_controller = TorqueControlType::voltage; } @@ -23,7 +23,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { driver = _driver; } -// init hardware pins +// init hardware pins void StepperMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Init")); @@ -37,7 +37,7 @@ void StepperMotor::init() { if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; - + // update the controller limits if(_isset(phase_resistance)){ // velocity control loop controls current @@ -52,7 +52,7 @@ void StepperMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); enable(); _delay(500); - + } @@ -99,16 +99,17 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update + sensor->update(); shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); - + if(exit_flag){ if(monitor_port) monitor_port->println(F("MOT: Ready.")); }else{ if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); disable(); } - + return exit_flag; } @@ -116,7 +117,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct int StepperMotor::alignSensor() { int exit_flag = 1; //success if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); - + // if unknown natural direction if(!_isset(sensor_direction)){ // check if sensor needs zero search @@ -127,22 +128,24 @@ int StepperMotor::alignSensor() { // find natural direction // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; + float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } // take and angle in the middle + sensor->update(); float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; + float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } + sensor->update(); float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); - // determine the direction the sensor moved + // determine the direction the sensor moved if (mid_angle == end_angle) { if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); return 0; // failed calibration @@ -153,10 +156,10 @@ int StepperMotor::alignSensor() { if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); sensor_direction = Direction::CW; } - // check pole pair number + // check pole pair number if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! + if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! if(monitor_port) monitor_port->print(F("fail - estimated pp:")); if(monitor_port) monitor_port->println(_2PI/moved,4); }else if(monitor_port) monitor_port->println(F("OK!")); @@ -166,10 +169,14 @@ int StepperMotor::alignSensor() { // zero electric angle not known if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees + // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); - zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); _delay(20); if(monitor_port){ monitor_port->print(F("MOT: Zero elec. angle: ")); @@ -185,7 +192,7 @@ int StepperMotor::alignSensor() { // Encoder alignment the absolute zero angle // - to the index int StepperMotor::absoluteZeroSearch() { - + if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity float limit_vel = velocity_limit; @@ -194,10 +201,10 @@ int StepperMotor::absoluteZeroSearch() { voltage_limit = voltage_sensor_align; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5*_2PI); + angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search - sensor->getAngle(); + sensor->update(); } // disable motor setPhaseVoltage(0, 0, 0); @@ -216,17 +223,25 @@ int StepperMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void StepperMotor::loopFOC() { + + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) sensor->update(); + // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle + // shaft angle shaft_angle = shaftAngle(); - + // if disabled do nothing - if(!enabled) return; + if(!enabled) return; + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI electrical_angle = electricalAngle(); - // set the phase voltage - FOC heart function :) + // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -236,20 +251,32 @@ void StepperMotor::loopFOC() { // - needs to be called iteratively it is asynchronous function // - if target is not set it uses motor.target value void StepperMotor::move(float new_target) { - // get angular velocity - shaft_velocity = shaftVelocity(); - // if disabled do nothing - if(!enabled) return; + // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity + shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated + + // if disabled do nothing + if(!enabled) return; + // set internal target variable if(_isset(new_target) ) target = new_target; // choose control loop switch (controller) { case MotionControlType::torque: if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control - else voltage.q = target*phase_resistance; + else voltage.q = target*phase_resistance; voltage.d = 0; break; case MotionControlType::angle: @@ -259,7 +286,7 @@ void StepperMotor::move(float new_target) { shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control - // if torque controlled through voltage + // if torque controlled through voltage // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*phase_resistance; @@ -270,7 +297,7 @@ void StepperMotor::move(float new_target) { shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control - // if torque controlled through voltage control + // if torque controlled through voltage control // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*phase_resistance; @@ -295,12 +322,12 @@ void StepperMotor::move(float new_target) { // Method using FOC to set Uq and Ud to the motor at the optimal angle // Function implementing Sine PWM algorithms // - space vector not implemented yet -// +// // Function using sine approximation // regular sin + cos ~300us (no memory usaage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { - // Sinusoidal PWM modulation + // Sinusoidal PWM modulation // Inverse Park transformation // angle normalization in between 0 and 2pi @@ -323,18 +350,18 @@ float StepperMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes shaft_velocity = target_velocity; - + // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -352,9 +379,9 @@ float StepperMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) @@ -368,12 +395,12 @@ float StepperMotor::angleOpenloop(float target_angle){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle - setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; - + return Uq; } \ No newline at end of file diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index 5764aa30..a657d17c 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -5,7 +5,7 @@ class BLDCDriver{ public: - + /** Initialise hardware */ virtual int init() = 0; /** Enable hardware */ @@ -14,26 +14,26 @@ class BLDCDriver{ virtual void disable() = 0; long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage + float voltage_power_supply; //!< power supply voltage float voltage_limit; //!< limiting voltage set to the motor - + float dc_a; //!< currently set duty cycle on phaseA float dc_b; //!< currently set duty cycle on phaseB float dc_c; //!< currently set duty cycle on phaseC - - /** - * Set phase voltages to the harware - * + + /** + * Set phase voltages to the harware + * * @param Ua - phase A voltage * @param Ub - phase B voltage * @param Uc - phase C voltage */ virtual void setPwm(float Ua, float Ub, float Uc) = 0; - /** - * Set phase state, enable/disable - * + /** + * Set phase state, enable/disable + * * @param sc - phase A state : active / disabled ( high impedance ) * @param sb - phase B state : active / disabled ( high impedance ) * @param sa - phase C state : active / disabled ( high impedance ) @@ -41,4 +41,4 @@ class BLDCDriver{ virtual void setPhaseState(int sa, int sb, int sc) = 0; }; -#endif \ No newline at end of file +#endif diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index eae18e67..922076a6 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -66,7 +66,9 @@ float FOCMotor::shaftVelocity() { } float FOCMotor::electricalAngle(){ - return _normalizeAngle((shaft_angle + sensor_offset) * pole_pairs - zero_electric_angle); + // if no sensor linked return previous value ( for open loop ) + if(!sensor) return electrical_angle; + return _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - zero_electric_angle ); } /** @@ -106,15 +108,19 @@ void FOCMotor::monitor() { DQCurrent_s c{0,0}; if(current_sense){ if(torque_controller == TorqueControlType::foc_current) c = current; - else c = current_sense->getFOCCurrents(electrical_angle); + else{ + c = current_sense->getFOCCurrents(electrical_angle); + c.q = LPF_current_q(c.q); + c.d = LPF_current_d(c.d); + } } if(monitor_variables & _MON_CURR_Q) { - monitor_port->print(c.q*1000,2); // mAmps + monitor_port->print(c.q*1000, 2); // mAmps monitor_port->print("\t"); printed= true; } if(monitor_variables & _MON_CURR_D) { - monitor_port->print(c.d*1000,2); // mAmps + monitor_port->print(c.d*1000, 2); // mAmps monitor_port->print("\t"); printed= true; } diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index fba66907..23a51d9b 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -2,31 +2,71 @@ #include "../foc_utils.h" #include "../time_utils.h" - /** - * returns 0 if it does need search for absolute zero - * 0 - magnetic sensor (& encoder with index which is found) - * 1 - ecoder with index (with index not found yet) - */ -int Sensor::needsSearch(){ - return 0; +// TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0 + +void Sensor::update() { + float val = getSensorAngle(); + angle_prev_ts = _micros(); + float d_angle = val - angle_prev; + // if overflow happened track it as full rotation + if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; + angle_prev = val; } - /** get current angular velocity (rad/s)*/ -float Sensor::getVelocity(){ + /** get current angular velocity (rad/s) */ +float Sensor::getVelocity() { // calculate sample time - unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - - // current angle - float angle_c = getAngle(); + if(Ts <= 0) Ts = 1e-3f; // velocity calculation - float vel = (angle_c - angle_prev)/Ts; - + float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; // save variables for future pass - angle_prev = angle_c; - velocity_calc_timestamp = now_us; + vel_angle_prev = angle_prev; + vel_full_rotations = full_rotations; + vel_angle_prev_ts = angle_prev_ts; return vel; -} \ No newline at end of file +} + +void Sensor::init() { + // initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero) + getSensorAngle(); // call once + delayMicroseconds(1); + vel_angle_prev = getSensorAngle(); // call again + vel_angle_prev_ts = _micros(); + delay(1); + getSensorAngle(); // call once + delayMicroseconds(1); + angle_prev = getSensorAngle(); // call again + angle_prev_ts = _micros(); +} + + +float Sensor::getMechanicalAngle() { + return angle_prev; +} + + + +float Sensor::getAngle(){ + return (float)full_rotations * _2PI + angle_prev; +} + + + +double Sensor::getPreciseAngle() { + return (double)full_rotations * (double)_2PI + (double)angle_prev; +} + + + +int32_t Sensor::getFullRotations() { + return full_rotations; +} + + + +int Sensor::needsSearch() { + return 0; // default false +} diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index 3b219401..efab1be1 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -1,6 +1,8 @@ #ifndef SENSOR_H #define SENSOR_H +#include + /** * Direction structure */ @@ -20,27 +22,110 @@ enum Pullup{ }; /** - * Sensor abstract class defintion - * Each sensor needs to have these functions implemented + * Sensor abstract class defintion + * + * This class is purposefully kept simple, as a base for all kinds of sensors. Currently we have + * Encoders, Magnetic Encoders and Hall Sensor implementations. This base class extracts the + * most basic common features so that a FOC driver can obtain the data it needs for operation. + * + * To implement your own sensors, create a sub-class of this class, and implement the getSensorAngle() + * method. getSensorAngle() returns a float value, in radians, representing the current shaft angle in the + * range 0 to 2*PI (one full turn). + * + * To function correctly, the sensor class update() method has to be called sufficiently quickly. Normally, + * the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly + * enough, both for correct motor and sensor operation. + * + * The Sensor base class provides an implementation of getVelocity(), and takes care of counting full + * revolutions in a precise way, but if you wish you can additionally override these methods to provide more + * optimal implementations for your hardware. + * */ class Sensor{ public: + /** + * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with + * the hardware. Base implementation uses the values returned by update() so that + * the same values are returned until update() is called again. + */ + virtual float getMechanicalAngle(); + + /** + * Get current position (in rad) including full rotations and shaft angle. + * Base implementation uses the values returned by update() so that the same + * values are returned until update() is called again. + * Note that this value has limited precision as the number of rotations increases, + * because the limited precision of float can't capture the large angle of the full + * rotations and the small angle of the shaft angle at the same time. + */ + virtual float getAngle(); + + /** + * On architectures supporting it, this will return a double precision position value, + * which should have improved precision for large position values. + * Base implementation uses the values returned by update() so that the same + * values are returned until update() is called again. + */ + virtual double getPreciseAngle(); - /** get current angle (rad) */ - virtual float getAngle()=0; - /** get current angular velocity (rad/s)*/ + /** + * Get current angular velocity (rad/s) + * Can be overridden in subclasses. Base implementation uses the values + * returned by update() so that it only makes sense to call this if update() + * has been called in the meantime. + */ virtual float getVelocity(); + /** + * Get the number of full rotations + * Base implementation uses the values returned by update() so that the same + * values are returned until update() is called again. + */ + virtual int32_t getFullRotations(); + + /** + * Updates the sensor values by reading the hardware sensor. + * Some implementations may work with interrupts, and not need this. + * The base implementation calls getSensorAngle(), and updates internal + * fields for angle, timestamp and full rotations. + * This method must be called frequently enough to guarantee that full + * rotations are not "missed" due to infrequent polling. + * Override in subclasses if alternative behaviours are required for your + * sensor hardware. + */ + virtual void update(); + /** * returns 0 if it does need search for absolute zero * 0 - magnetic sensor (& encoder with index which is found) * 1 - ecoder with index (with index not found yet) */ virtual int needsSearch(); - private: + protected: + /** + * Get current shaft angle from the sensor hardware, and + * return it as a float in radians, in the range 0 to 2PI. + * + * This method is pure virtual and must be implemented in subclasses. + * Calling this method directly does not update the base-class internal fields. + * Use update() when calling from outside code. + */ + virtual float getSensorAngle()=0; + /** + * Call Sensor::init() from your sensor subclass's init method if you want smoother startup + * The base class init() method calls getSensorAngle() several times to initialize the internal fields + * to current values, ensuring there is no discontinuity ("jump from zero") during the first calls + * to sensor.getAngle() and sensor.getVelocity() + */ + virtual void init(); + // velocity calculation variables - float angle_prev=0; //!< angle in previous velocity calculation step - long velocity_calc_timestamp=0; //!< last velocity calculation timestamp + float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity + long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity + float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity + long vel_angle_prev_ts=0; // last velocity calculation timestamp + int32_t full_rotations=0; // full rotation tracking + int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation }; #endif diff --git a/src/common/defaults.h b/src/common/defaults.h index 71f39098..df651e2a 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -1,12 +1,12 @@ // default configuration values // change this file to optimal values for your application -#define DEF_POWER_SUPPLY 12.0 //!< default power supply voltage +#define DEF_POWER_SUPPLY 12.0f //!< default power supply voltage // velocity PI controller params -#define DEF_PID_VEL_P 0.5 //!< default PID controller P value -#define DEF_PID_VEL_I 10.0 //!< default PID controller I value -#define DEF_PID_VEL_D 0.0 //!< default PID controller D value -#define DEF_PID_VEL_RAMP 1000.0 //!< default PID controller voltage ramp value +#define DEF_PID_VEL_P 0.5f //!< default PID controller P value +#define DEF_PID_VEL_I 10.0f //!< default PID controller I value +#define DEF_PID_VEL_D 0.0f //!< default PID controller D value +#define DEF_PID_VEL_RAMP 1000.0f //!< default PID controller voltage ramp value #define DEF_PID_VEL_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit // current sensing PID values @@ -14,33 +14,36 @@ // for 16Mhz controllers like Arduino uno and mega #define DEF_PID_CURR_P 2 //!< default PID controller P value #define DEF_PID_CURR_I 100 //!< default PID controller I value -#define DEF_PID_CURR_D 0.0 //!< default PID controller D value -#define DEF_PID_CURR_RAMP 1000.0 //!< default PID controller voltage ramp value +#define DEF_PID_CURR_D 0.0f //!< default PID controller D value +#define DEF_PID_CURR_RAMP 1000.0f //!< default PID controller voltage ramp value #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.01 //!< default velocity filter time constant -#else +#define DEF_CURR_FILTER_Tf 0.01f //!< default velocity filter time constant +#else // for stm32, due, teensy, esp32 and similar #define DEF_PID_CURR_P 3 //!< default PID controller P value -#define DEF_PID_CURR_I 300.0 //!< default PID controller I value -#define DEF_PID_CURR_D 0.0 //!< default PID controller D value +#define DEF_PID_CURR_I 300.0f //!< default PID controller I value +#define DEF_PID_CURR_D 0.0f //!< default PID controller D value #define DEF_PID_CURR_RAMP 0 //!< default PID controller voltage ramp value #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.005 //!< default currnet filter time constant +#define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant #endif // default current limit values -#define DEF_CURRENT_LIM 0.2 //!< 2Amps current limit by default +#define DEF_CURRENT_LIM 0.2f //!< 2Amps current limit by default // default monitor downsample #define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample -#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable +#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable // angle P params -#define DEF_P_ANGLE_P 20.0 //!< default P controller P value -#define DEF_VEL_LIM 20.0 //!< angle velocity limit default +#define DEF_P_ANGLE_P 20.0f //!< default P controller P value +#define DEF_VEL_LIM 20.0f //!< angle velocity limit default -// index search -#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0 //!< default index search velocity +// index search +#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0f //!< default index search velocity // align voltage -#define DEF_VOLTAGE_SENSOR_ALIGN 3.0 //!< default voltage for sensor and motor zero alignemt +#define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt // low pass filter velocity -#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant \ No newline at end of file +#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant + +// current sense default parameters +#define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f //!< default currnet sense per phase low pass filter time constant diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 7653f9be..11a1810f 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,6 +1,6 @@ #include "foc_utils.h" -// int array instead of float array +// int array instead of float array // 4x200 points per 360 deg // 2x storage save (int 2Byte float 4 Byte ) // sin*10000 @@ -13,21 +13,21 @@ const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,10 // it has to receive an angle in between 0 and 2PI float _sin(float a){ if(a < _PI_2){ - //return sine_array[(int)(199.0*( a / (_PI/2.0)))]; - //return sine_array[(int)(126.6873* a)]; // float array optimized - return 0.0001*sine_array[_round(126.6873* a)]; // int array optimized + //return sine_array[(int)(199.0f*( a / (_PI/2.0)))]; + //return sine_array[(int)(126.6873f* a)]; // float array optimized + return 0.0001f*sine_array[_round(126.6873f* a)]; // int array optimized }else if(a < _PI){ - // return sine_array[(int)(199.0*(1.0 - (a-_PI/2.0) / (_PI/2.0)))]; - //return sine_array[398 - (int)(126.6873*a)]; // float array optimized - return 0.0001*sine_array[398 - _round(126.6873*a)]; // int array optimized + // return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))]; + //return sine_array[398 - (int)(126.6873f*a)]; // float array optimized + return 0.0001f*sine_array[398 - _round(126.6873f*a)]; // int array optimized }else if(a < _3PI_2){ - // return -sine_array[(int)(199.0*((a - _PI) / (_PI/2.0)))]; - //return -sine_array[-398 + (int)(126.6873*a)]; // float array optimized - return -0.0001*sine_array[-398 + _round(126.6873*a)]; // int array optimized + // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))]; + //return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized + return -0.0001f*sine_array[-398 + _round(126.6873f*a)]; // int array optimized } else { - // return -sine_array[(int)(199.0*(1.0 - (a - 3*_PI/2) / (_PI/2.0)))]; - //return -sine_array[796 - (int)(126.6873*a)]; // float array optimized - return -0.0001*sine_array[796 - _round(126.6873*a)]; // int array optimized + // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))]; + //return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized + return -0.0001f*sine_array[796 - _round(126.6873f*a)]; // int array optimized } } @@ -54,7 +54,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { return (shaft_angle * pole_pairs); } -// square root approximation function using +// square root approximation function using // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root float _sqrtApprox(float number) {//low in fat @@ -67,7 +67,7 @@ float _sqrtApprox(float number) {//low in fat y = number; i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); - y = * ( float * ) &i; + y = * ( float * ) &i; // y = y * ( f - ( x * y * y ) ); // better precision return number * y; } diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 06978424..e0358750 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -5,37 +5,38 @@ // sign function #define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) -#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f)) #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) +#define _UNUSED(v) (void) (v) // utility defines -#define _2_SQRT3 1.15470053838 -#define _SQRT3 1.73205080757 -#define _1_SQRT3 0.57735026919 -#define _SQRT3_2 0.86602540378 -#define _SQRT2 1.41421356237 -#define _120_D2R 2.09439510239 -#define _PI 3.14159265359 -#define _PI_2 1.57079632679 -#define _PI_3 1.0471975512 -#define _2PI 6.28318530718 -#define _3PI_2 4.71238898038 -#define _PI_6 0.52359877559 +#define _2_SQRT3 1.15470053838f +#define _SQRT3 1.73205080757f +#define _1_SQRT3 0.57735026919f +#define _SQRT3_2 0.86602540378f +#define _SQRT2 1.41421356237f +#define _120_D2R 2.09439510239f +#define _PI 3.14159265359f +#define _PI_2 1.57079632679f +#define _PI_3 1.0471975512f +#define _2PI 6.28318530718f +#define _3PI_2 4.71238898038f +#define _PI_6 0.52359877559f #define NOT_SET -12345.0 #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 -// dq current structure +// dq current structure struct DQCurrent_s { float d; float q; }; -// phase current structure +// phase current structure struct PhaseCurrent_s { float a; @@ -53,27 +54,27 @@ struct DQVoltage_s /** * Function approximating the sine calculation by using fixed size array * - execution time ~40us (Arduino UNO) - * + * * @param a angle in between 0 and 2PI */ float _sin(float a); /** * Function approximating cosine calculation by using fixed size array * - execution time ~50us (Arduino UNO) - * + * * @param a angle in between 0 and 2PI */ float _cos(float a); -/** - * normalizing radian angle to [0,2PI] +/** + * normalizing radian angle to [0,2PI] * @param angle - angle to be normalized - */ + */ float _normalizeAngle(float angle); - -/** - * Electrical angle calculation + +/** + * Electrical angle calculation * * @param shaft_angle - shaft angle of the motor * @param pole_pairs - number of pole pairs @@ -83,7 +84,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs); /** * Function approximating square root function * - using fast inverse square root - * + * * @param value - number */ float _sqrtApprox(float value); diff --git a/src/common/lowpass_filter.cpp b/src/common/lowpass_filter.cpp index 4a643744..ffb15cfc 100644 --- a/src/common/lowpass_filter.cpp +++ b/src/common/lowpass_filter.cpp @@ -13,12 +13,15 @@ float LowPassFilter::operator() (float x) unsigned long timestamp = _micros(); float dt = (timestamp - timestamp_prev)*1e-6f; - if (dt < 0.0f || dt > 0.5f) - dt = 1e-3f; + if (dt < 0.0f ) dt = 1e-3f; + else if(dt > 0.3f) { + y_prev = x; + timestamp_prev = timestamp; + return x; + } float alpha = Tf/(Tf + dt); float y = alpha*y_prev + (1.0f - alpha)*x; - y_prev = y; timestamp_prev = timestamp; return y; diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 5290d814..aeeccf1a 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -6,9 +6,9 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) , D(D) , output_ramp(ramp) // output derivative limit [volts/second] , limit(limit) // output supply limit [volts] - , integral_prev(0.0) - , error_prev(0.0) - , output_prev(0.0) + , error_prev(0.0f) + , output_prev(0.0f) + , integral_prev(0.0f) { timestamp_prev = _micros(); } @@ -17,18 +17,18 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) float PIDController::operator() (float error){ // calculate the time from the last call unsigned long timestamp_now = _micros(); - float Ts = (timestamp_now - timestamp_prev) * 1e-6; + float Ts = (timestamp_now - timestamp_prev) * 1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // u(s) = (P + I/s + Ds)e(s) // Discrete implementations - // proportional part + // proportional part // u_p = P *e(k) float proportional = P * error; // Tustin transform of the integral part // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) - float integral = integral_prev + I*Ts*0.5*(error + error_prev); + float integral = integral_prev + I*Ts*0.5f*(error + error_prev); // antiwindup - limit the output integral = _constrain(integral, -limit, limit); // Discrete derivation @@ -56,4 +56,3 @@ float PIDController::operator() (float error){ timestamp_prev = timestamp_now; return output; } - diff --git a/src/common/pid.h b/src/common/pid.h index 39622ace..203d3c82 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -31,10 +31,10 @@ class PIDController float limit; //!< Maximum output value protected: - float integral_prev; //!< last integral component value float error_prev; //!< last tracking error value - unsigned long timestamp_prev; //!< Last execution timestamp float output_prev; //!< last pid output value + float integral_prev; //!< last integral component value + unsigned long timestamp_prev; //!< Last execution timestamp }; #endif // PID_H \ No newline at end of file diff --git a/src/common/time_utils.cpp b/src/common/time_utils.cpp index 65ebad22..2acb47a3 100644 --- a/src/common/time_utils.cpp +++ b/src/common/time_utils.cpp @@ -3,7 +3,7 @@ // function buffering delay() // arduino uno function doesn't work well with interrupts void _delay(unsigned long ms){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__) // if arduino uno and other atmega328p chips // use while instad of delay, // due to wrong measurement based on changed timer0 @@ -19,7 +19,7 @@ void _delay(unsigned long ms){ // function buffering _micros() // arduino function doesn't work well with interrupts unsigned long _micros(){ -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__) // if arduino uno and other atmega328p chips //return the value based on the prescaler if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); diff --git a/src/communication/Commander.h b/src/communication/Commander.h index aaa55b11..13777867 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -24,14 +24,14 @@ typedef void (* CommandCallback)(char*); //!< command callback function pointer /** * Commander class implementing string communication protocol based on IDvalue (ex AB5.321 - command id `A`, sub-command id `B`,value `5.321`) - * - * - This class can be used in combination with HardwareSerial instance which it would read and write + * + * - This class can be used in combination with HardwareSerial instance which it would read and write * or it can be used to parse strings that have been received from the user outside this library * - Commander class implements command protocol for few standard components of the SimpleFOC library * - FOCMotor * - PIDController * - LowPassFilter - * - Commander also provides a very simple command > callback interface that enables user to + * - Commander also provides a very simple command > callback interface that enables user to * attach a callback function to certain command id - see function add() */ class Commander @@ -40,7 +40,7 @@ class Commander /** * Default constructor receiving a serial interface that it uses to output the values to * Also if the function run() is used it uses this serial instance to read the serial for user commands - * + * * @param serial - Serial com port instance * @param eol - the end of line sentinel character * @param echo - echo last typed character (for command line feedback) @@ -49,45 +49,45 @@ class Commander Commander(char eol = '\n', bool echo = false); /** - * Function reading the serial port and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the serial port and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places - * '?' - Scan command - displays all the labels of attached nodes - */ + * '?' - Scan command - displays all the labels of attached nodes + */ void run(); /** - * Function reading the string of user input and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the string of user input and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes - * + * * @param reader - temporary stream to read user input * @param eol - temporary end of line sentinel - */ + */ void run(Stream &reader, char eol = '\n'); /** - * Function reading the string of user input and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the string of user input and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes - * + * * @param user_input - string of user inputs - */ + */ void run(char* user_input); /** * Function adding a callback to the coomander withe the command id * @param id - char command letter - * @param onCommand - function pointer void function(char*) + * @param onCommand - function pointer void function(char*) * @param label - string label to be displayed when scan command sent */ void add(char id , CommandCallback onCommand, char* label = nullptr); @@ -101,48 +101,48 @@ class Commander char eol = '\n'; //!< end of line sentinel character bool echo = false; //!< echo last typed character (for command line feedback) /** - * + * * FOC motor (StepperMotor and BLDCMotor) command interface * - It has several paramters (the letters can be changed in the commands.h file) * 'Q' - Q current PID controller & LPF (see function pid and lpf for commands) - * 'D' - D current PID controller & LPF (see function pid and lpf for commands) - * 'V' - Velocity PID controller & LPF (see function pid and lpf for commands) - * 'A' - Angle PID controller & LPF (see function pid and lpf for commands) - * 'L' - Limits + * 'D' - D current PID controller & LPF (see function pid and lpf for commands) + * 'V' - Velocity PID controller & LPF (see function pid and lpf for commands) + * 'A' - Angle PID controller & LPF (see function pid and lpf for commands) + * 'L' - Limits * sub-commands: - * 'C' - Current - * 'U' - Voltage - * 'V' - Velocity - * 'C' - Motion control type config + * 'C' - Current + * 'U' - Voltage + * 'V' - Velocity + * 'C' - Motion control type config * sub-commands: - * 'D' - downsample motiron loop - * '0' - torque - * '1' - velocity - * '2' - angle - * 'T' - Torque control type + * 'D' - downsample motiron loop + * '0' - torque + * '1' - velocity + * '2' - angle + * 'T' - Torque control type * sub-commands: - * '0' - voltage - * '1' - current - * '2' - foc_current - * 'E' - Motor status (enable/disable) + * '0' - voltage + * '1' - current + * '2' - foc_current + * 'E' - Motor status (enable/disable) * sub-commands: - * '0' - enable - * '1' - disable - * 'R' - Motor resistance - * 'S' - Sensor offsets + * '0' - enable + * '1' - disable + * 'R' - Motor resistance + * 'S' - Sensor offsets * sub-commands: - * 'M' - sensor offset - * 'E' - sensor electrical zero - * 'M' - Monitoring control + * 'M' - sensor offset + * 'E' - sensor electrical zero + * 'M' - Monitoring control * sub-commands: - * 'D' - downsample monitoring - * 'C' - clear monitor - * 'S' - set monitoring variables - * 'G' - get variable value - * '' - Target get/set - * + * 'D' - downsample monitoring + * 'C' - clear monitor + * 'S' - set monitoring variables + * 'G' - get variable value + * '' - Target get/set + * * - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance) - * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5) + * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f) * */ void motor(FOCMotor* motor, char* user_cmd); @@ -170,7 +170,7 @@ class Commander * Float variable scalar command interface * - It only has one property - one float value * - It can be get by sending an empty string '\n' - * - It can be set by sending 'value' - (ex. 0.01 for settin *value=0.01) + * - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01) */ void scalar(float* value, char* user_cmd); @@ -184,19 +184,19 @@ class Commander // helping variable for serial communication reading char received_chars[MAX_COMMAND_LENGTH] = {0}; //!< so far received user message - waiting for newline int rec_cnt = 0; //!< number of characters receives - + // serial printing functions /** * print the string message only if verbose mode on * @param message - message to be printed */ - void printVerbose(const char* message); + void printVerbose(const char* message); /** - * Print the string message only if verbose mode on + * Print the string message only if verbose mode on * - Function handling the case for strings defined by F macro * @param message - message to be printed */ - void printVerbose(const __FlashStringHelper *message); + void printVerbose(const __FlashStringHelper *message); /** * print the numbers to the serial with desired decimal point number * @param message - number to be printed diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 2e9c4421..bc3bc4c6 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -12,7 +12,7 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ shunt_resistor = _shunt_resistor; amp_gain = _gain; - volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps // gains for each phase gain_a = volts_to_amps_ratio; gain_b = volts_to_amps_ratio; @@ -22,7 +22,7 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ // Inline sensor init function void InlineCurrentSense::init(){ // configure ADC variables - _configureADC(pinA,pinB,pinC); + _configureADCInline(pinA,pinB,pinC); // calibrate zero offsets calibrateOffsets(); } @@ -36,9 +36,9 @@ void InlineCurrentSense::calibrateOffsets(){ offset_ic = 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < calibration_rounds; i++) { - offset_ia += _readADCVoltage(pinA); - offset_ib += _readADCVoltage(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltage(pinC); + offset_ia += _readADCVoltageInline(pinA); + offset_ib += _readADCVoltageInline(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC); _delay(1); } // calculate the mean offsets @@ -50,9 +50,9 @@ void InlineCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps + current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps + current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. @@ -72,36 +72,36 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver){ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; - + // set phase A active and phases B and C down driver->setPwm(voltage, 0, 0); - _delay(200); + _delay(200); PhaseCurrent_s c = getPhaseCurrents(); // read the current 100 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); // align phase A float ab_ratio = fabs(c.a / c.b); float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if( ab_ratio > 1.5 ){ // should be ~2 + if( ab_ratio > 1.5f ){ // should be ~2 gain_a *= _sign(c.a); - }else if( ab_ratio < 0.7 ){ // should be ~0.5 + }else if( ab_ratio < 0.7f ){ // should be ~0.5 // switch phase A and B int tmp_pinA = pinA; - pinA = pinB; + pinA = pinB; pinB = tmp_pinA; gain_a *= _sign(c.b); exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinA = pinA; - pinA = pinC; + pinA = pinC; pinC= tmp_pinA; gain_a *= _sign(c.c); exit_flag = 2;// signal that pins have been switched @@ -109,35 +109,35 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // error in current sense - phase either not measured or bad connection return 0; } - + // set phase B active and phases A and C down driver->setPwm(0, voltage, 0); - _delay(200); + _delay(200); c = getPhaseCurrents(); // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); float ba_ratio = fabs(c.b/c.a); float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if( ba_ratio > 1.5 ){ // should be ~2 + if( ba_ratio > 1.5f ){ // should be ~2 gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + }else if( ba_ratio < 0.7f ){ // it should be ~0.5 // switch phase A and B int tmp_pinB = pinB; - pinB = pinA; + pinB = pinA; pinA = tmp_pinB; gain_b *= _sign(c.a); exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinB = pinB; - pinB = pinC; + pinB = pinC; pinC = tmp_pinB; gain_b *= _sign(c.c); exit_flag = 2; // signal that pins have been switched @@ -150,12 +150,12 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ if(_isset(pinC)){ // set phase B active and phases A and C down driver->setPwm(0, 0, voltage); - _delay(200); + _delay(200); c = getPhaseCurrents(); // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 50; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.c = (c.c+c1.c)/50.0; + c.c = (c.c+c1.c)/50.0f; } driver->setPwm(0, 0, 0); gain_c *= _sign(c.c); @@ -170,4 +170,3 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // 4 - success but pins reconfigured and gains inverted return exit_flag; } - diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index da46d104..748d3f44 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -4,7 +4,9 @@ #include "Arduino.h" #include "../common/foc_utils.h" #include "../common/time_utils.h" +#include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" +#include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -33,25 +35,30 @@ class InlineCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain - private: + // // per phase low pass fileters + // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + private: + // hardware variables int pinA; //!< pin A analog pin for current measurement int pinB; //!< pin B analog pin for current measurement int pinC; //!< pin C analog pin for current measurement // gain variables - double shunt_resistor; //!< Shunt resistor value - double amp_gain; //!< amp gain value - double volts_to_amps_ratio; //!< Volts to amps ratio + float shunt_resistor; //!< Shunt resistor value + float amp_gain; //!< amp gain value + float volts_to_amps_ratio; //!< Volts to amps ratio /** * Function finding zero offsets of the ADC */ void calibrateOffsets(); - double offset_ia; //!< zero current A voltage value (center of the adc reading) - double offset_ib; //!< zero current B voltage value (center of the adc reading) - double offset_ic; //!< zero current C voltage value (center of the adc reading) + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) }; diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp new file mode 100644 index 00000000..ca4f51b4 --- /dev/null +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -0,0 +1,179 @@ +#include "LowsideCurrentSense.h" +// LowsideCurrentSensor constructor +// - shunt_resistor - shunt resistor value +// - gain - current-sense op-amp gain +// - phA - A phase adc pin +// - phB - B phase adc pin +// - phC - C phase adc pin (optional) +LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + shunt_resistor = _shunt_resistor; + amp_gain = _gain; + volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts 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 +void LowsideCurrentSense::init(){ + // configure ADC variables + _configureADCLowSide(pinA,pinB,pinC); + // sync the driver + _driverSyncLowSide(); + // calibrate zero offsets + calibrateOffsets(); +} +// Function finding zero offsets of the ADC +void LowsideCurrentSense::calibrateOffsets(){ + const int calibration_rounds = 1000; + + // find adc offset = zero current voltage + offset_ia = 0; + offset_ib = 0; + offset_ic = 0; + // read the adc voltage 1000 times ( arbitrary number ) + for (int i = 0; i < calibration_rounds; i++) { + _startADC3PinConversionLowSide(); + offset_ia += _readADCVoltageLowSide(pinA); + offset_ib += _readADCVoltageLowSide(pinB); + if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); + _delay(1); + } + // calculate the mean offsets + offset_ia = offset_ia / calibration_rounds; + offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current; + _startADC3PinConversionLowSide(); + current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps + current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps + return current; +} +// Function synchronizing current sense with motor driver. +// for in-line sensig no such thing is necessary +int LowsideCurrentSense::driverSync(BLDCDriver *driver){ + _driverSyncLowSide(); + return 1; +} + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ + + int exit_flag = 1; + if(skip_align) return exit_flag; + + // set phase A active and phases B and C down + driver->setPwm(voltage, 0, 0); + _delay(2000); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 100 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = fabs(c.a / c.b); + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if( ab_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if( ab_ratio < 0.7f ){ // should be ~0.5 + // switch phase A and B + int tmp_pinA = pinA; + pinA = pinB; + pinB = tmp_pinA; + gain_a *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 + // switch phase A and C + int tmp_pinA = pinA; + pinA = pinC; + pinC= tmp_pinA; + gain_a *= _sign(c.c); + exit_flag = 2;// signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + + // set phase B active and phases A and C down + driver->setPwm(0, voltage, 0); + _delay(200); + c = getPhaseCurrents(); + // read the current 50 times + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + float ba_ratio = fabs(c.b/c.a); + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if( ba_ratio > 1.5f ){ // should be ~2 + gain_b *= _sign(c.b); + }else if( ba_ratio < 0.7f ){ // it should be ~0.5 + // switch phase A and B + int tmp_pinB = pinB; + pinB = pinA; + pinA = tmp_pinB; + gain_b *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 + // switch phase A and C + int tmp_pinB = pinB; + pinB = pinC; + pinC = tmp_pinB; + gain_b *= _sign(c.c); + exit_flag = 2; // signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + + // if phase C measured + if(_isset(pinC)){ + // set phase B active and phases A and C down + driver->setPwm(0, 0, voltage); + _delay(200); + c = getPhaseCurrents(); + // read the adc voltage 500 times ( arbitrary number ) + for (int i = 0; i < 50; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.c = (c.c+c1.c)/50.0f; + } + driver->setPwm(0, 0, 0); + gain_c *= _sign(c.c); + } + + if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + // exit flag is either + // 0 - fail + // 1 - success and nothing changed + // 2 - success but pins reconfigured + // 3 - success but gains inverted + // 4 - success but pins reconfigured and gains inverted + + return exit_flag; +} diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h new file mode 100644 index 00000000..057aa623 --- /dev/null +++ b/src/current_sense/LowsideCurrentSense.h @@ -0,0 +1,66 @@ +#ifndef LOWSIDE_CS_LIB_H +#define LOWSIDE_CS_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "../common/base_classes/CurrentSense.h" +#include "../common/base_classes/FOCMotor.h" +#include "../common/lowpass_filter.h" +#include "hardware_api.h" + + +class LowsideCurrentSense: public CurrentSense{ + public: + /** + LowsideCurrentSense class constructor + @param shunt_resistor shunt resistor value + @param gain current-sense op-amp gain + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + + // CurrentSense interface implementing functions + void init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverSync(BLDCDriver *driver) override; + int driverAlign(BLDCDriver *driver, float voltage) override; + + // ADC measuremnet gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + // // per phase low pass fileters + // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + + private: + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + + // gain variables + float shunt_resistor; //!< Shunt resistor value + float amp_gain; //!< amp gain value + float volts_to_amps_ratio; //!< Volts to amps ratio + + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) + +}; + +#endif diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index ab376e8e..e1b50003 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -1,23 +1,46 @@ -#ifndef HARDWARE_UTILS_H -#define HARDWARE_UTILS_H +#ifndef HARDWARE_UTILS_CURRENT_H +#define HARDWARE_UTILS_CURRENT_H #include "../common/foc_utils.h" #include "../common/time_utils.h" /** * function reading an ADC value and returning the read voltage - * + * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ -float _readADCVoltage(const int pinA); +float _readADCVoltageInline(const int pinA); /** * function reading an ADC value and returning the read voltage - * + * * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C */ -void _configureADC(const int pinA,const int pinB,const int pinC = NOT_SET); +void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); -#endif \ No newline at end of file +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - adc pin A + * @param pinB - adc pin B + * @param pinC - adc pin C + */ +void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); + +void _startADC3PinConversionLowSide(); + +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ +float _readADCVoltageLowSide(const int pinA); + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide(); + +#endif diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp new file mode 100644 index 00000000..75265c03 --- /dev/null +++ b/src/current_sense/hardware_specific/atmega_mcu.cpp @@ -0,0 +1,55 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) + +#define _ADC_VOLTAGE 5.0f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +#ifndef cbi + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif +#ifndef sbi + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) + // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz + // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample + cbi(ADCSRA, ADPS2); + sbi(ADCSRA, ADPS1); + sbi(ADCSRA, ADPS0); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/due_mcu.cpp b/src/current_sense/hardware_specific/due_mcu.cpp new file mode 100644 index 00000000..ef450b35 --- /dev/null +++ b/src/current_sense/hardware_specific/due_mcu.cpp @@ -0,0 +1,38 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(__SAM3X8E__) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp new file mode 100644 index 00000000..76ee54e9 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_adc_driver.cpp @@ -0,0 +1,225 @@ +#include "esp32_adc_driver.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "rom/ets_sys.h" +#include "esp_attr.h" +#include "esp_intr.h" +#include "soc/rtc_io_reg.h" +#include "soc/rtc_cntl_reg.h" +#include "soc/sens_reg.h" + +static uint8_t __analogAttenuation = 3;//11db +static uint8_t __analogWidth = 3;//12 bits +static uint8_t __analogCycles = 8; +static uint8_t __analogSamples = 0;//1 sample +static uint8_t __analogClockDiv = 1; + +// Width of returned answer () +static uint8_t __analogReturnedWidth = 12; + +void __analogSetWidth(uint8_t bits){ + if(bits < 9){ + bits = 9; + } else if(bits > 12){ + bits = 12; + } + __analogReturnedWidth = bits; + __analogWidth = bits - 9; + SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); + + SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); +} + +void __analogSetCycles(uint8_t cycles){ + __analogCycles = cycles; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); +} + +void __analogSetSamples(uint8_t samples){ + if(!samples){ + return; + } + __analogSamples = samples - 1; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); +} + +void __analogSetClockDiv(uint8_t clockDiv){ + if(!clockDiv){ + return; + } + __analogClockDiv = clockDiv; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); +} + +void __analogSetAttenuation(uint8_t attenuation) +{ + __analogAttenuation = attenuation & 3; + uint32_t att_data = 0; + int i = 10; + while(i--){ + att_data |= __analogAttenuation << (i * 2); + } + WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels + WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); +} + +void IRAM_ATTR __analogInit(){ + static bool initialized = false; + if(initialized){ + return; + } + + __analogSetAttenuation(__analogAttenuation); + __analogSetCycles(__analogCycles); + __analogSetSamples(__analogSamples + 1);//in samples + __analogSetClockDiv(__analogClockDiv); + __analogSetWidth(__analogWidth + 9);//in bits + + SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV); + SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV); + + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_CTRL_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); + while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== + + initialized = true; +} + +void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) +{ + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0 || attenuation > 3){ + return ; + } + __analogInit(); + if(channel > 7){ + SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); + } else { + SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); + } +} + +bool IRAM_ATTR __adcAttachPin(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + int8_t pad = digitalPinToTouchChannel(pin); + if(pad >= 0){ + uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG); + if(touch & (1 << pad)){ + touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) + | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S)) + | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S))); + WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch); + } + } else if(pin == 25){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 + } else if(pin == 26){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 + } + + pinMode(pin, ANALOG); + + __analogInit(); + return true; +} + +bool IRAM_ATTR __adcStart(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 9){ + channel -= 10; + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + } else { + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + } + return true; +} + +bool IRAM_ATTR __adcBusy(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 7){ + return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); + } + return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); +} + +uint16_t IRAM_ATTR __adcEnd(uint8_t pin) +{ + + uint16_t value = 0; + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return 0;//not adc pin + } + if(channel > 7){ + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + } else { + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + } + + // Shift result if necessary + uint8_t from = __analogWidth + 9; + if (from == __analogReturnedWidth) { + return value; + } + if (from > __analogReturnedWidth) { + return value >> (from - __analogReturnedWidth); + } + return value << (__analogReturnedWidth - from); +} + +void __analogReadResolution(uint8_t bits) +{ + if(!bits || bits > 16){ + return; + } + __analogSetWidth(bits); // hadware from 9 to 12 + __analogReturnedWidth = bits; // software from 1 to 16 +} + +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ + if(!__adcAttachPin(pin) || !__adcStart(pin)){ + return 0; + } + return __adcEnd(pin); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32_adc_driver.h new file mode 100644 index 00000000..df8166f0 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_adc_driver.h @@ -0,0 +1,88 @@ + + +#ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ +#define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ + +#include "Arduino.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +/* + * Get ADC value for pin + * */ +uint16_t adcRead(uint8_t pin); + +/* + * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096). + * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted. + * Range is 1 - 16 + * + * Note: compatibility with Arduino SAM + */ +void __analogReadResolution(uint8_t bits); + +/* + * Sets the sample bits and read resolution + * Default is 12bit (0 - 4095) + * Range is 9 - 12 + * */ +void __analogSetWidth(uint8_t bits); + +/* + * Set number of cycles per sample + * Default is 8 and seems to do well + * Range is 1 - 255 + * */ +void __analogSetCycles(uint8_t cycles); + +/* + * Set number of samples in the range. + * Default is 1 + * Range is 1 - 255 + * This setting splits the range into + * "samples" pieces, which could look + * like the sensitivity has been multiplied + * that many times + * */ +void __analogSetSamples(uint8_t samples); + +/* + * Set the divider for the ADC clock. + * Default is 1 + * Range is 1 - 255 + * */ +void __analogSetClockDiv(uint8_t clockDiv); + +/* + * Set the attenuation for all channels + * Default is 11db + * */ +void __analogSetAttenuation(uint8_t attenuation); + +/* + * Set the attenuation for particular pin + * Default is 11db + * */ +void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation); + +/* + * Attach pin to ADC (will also clear any other analog mode that could be on) + * */ +bool __adcAttachPin(uint8_t pin); + +/* + * Start ADC conversion on attached pin's bus + * */ +bool __adcStart(uint8_t pin); + +/* + * Check if conversion on the pin's ADC bus is currently running + * */ +bool __adcBusy(uint8_t pin); + +/* + * Get the result of the conversion (will wait if it have not finished) + * */ +uint16_t __adcEnd(uint8_t pin); + +#endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ +#endif /* ESP32 */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp new file mode 100644 index 00000000..7b9ba448 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -0,0 +1,125 @@ +#include "../hardware_api.h" +#include "../../drivers/hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + +#include "driver/mcpwm.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +#include +#include + +#include "esp32_adc_driver.h" + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + + +/** + * Inline adc reading implementation +*/ +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = adcRead(pinA); + // uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} + +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB, const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +/** + * Low side adc reading implementation +*/ +static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; +int a1, a2, a3; // Current readings from internal current sensor amplifiers +int _pinA, _pinB, _pinC; +static void IRAM_ATTR isr_handler(void*); +byte currentState = 1; + + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin){ + uint32_t raw_adc; + + if (pin == _pinA) raw_adc = a1; + else if (pin == _pinB) raw_adc = a2; + else if (pin == _pinC) raw_adc = a3; + + return raw_adc * _ADC_CONV; +} + +// function reading an ADC value and returning the read voltage +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + _pinA = pinA; + _pinB = pinB; + _pinC = pinC; + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + +void _driverSyncLowSide(){ + // high side registers enable interrupt + // MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt + // MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tez_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt + // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tez_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt + + // low-side register enable interrupt + MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt + MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt + if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt + mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler +} + +// Read currents when interrupt is triggered +static void IRAM_ATTR isr_handler(void*){ + // // high side + // uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st; + // uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st; + // uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tez_int_st : 0; + + // low side + uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st; + uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st; + uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st : 0; + + switch (currentState) + { + case 1 : + if (mcpwm_intr_status_0 > 0) a1 = adcRead(_pinA); + currentState = 2; + break; + case 2 : + if (mcpwm_intr_status_1 > 0) a2 = adcRead(_pinB); + currentState = _isset(_pinC) ? 3 : 1; + break; + case 3 : + if (mcpwm_intr_status_2 > 0) a3 = adcRead(_pinC); + currentState = 1; + break; + } + + // high side + // MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; + // MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1; + // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tez_int_clr = mcpwm_intr_status_2; + // low side + MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0; + MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1; + if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2; +} + + +#endif diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index 423774c2..50f383ad 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,56 +1,31 @@ #include "../hardware_api.h" - - -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 - #ifndef cbi - #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) - #endif - #ifndef sbi - #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) - #endif -#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#elif defined(__arm__) && defined(__SAM3X8E__) // or due - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#elif defined(ESP_H) // or esp32 - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 4095.0 -#elif defined(_STM32_DEF_) // or stm32 - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#else - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 -#endif - -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - // function reading an ADC value and returning the read voltage -float _readADCVoltage(const int pinA){ +__attribute__((weak)) float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; + return raw_adc * 5.0f/1024.0f; } +// function reading an ADC value and returning the read voltage +__attribute__((weak)) void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} // function reading an ADC value and returning the read voltage -void _configureADC(const int pinA,const int pinB,const int pinC){ +__attribute__((weak)) float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} + +// Configure low side for generic mcu +// cannot do much but +__attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); +} - #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) - // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz - // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample - cbi(ADCSRA, ADPS2); - sbi(ADCSRA, ADPS1); - sbi(ADCSRA, ADPS0); - #endif -} \ No newline at end of file +// sync driver and the adc +__attribute__((weak)) void _driverSyncLowSide(){ } +__attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp new file mode 100644 index 00000000..8bb6d38c --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -0,0 +1,308 @@ +#ifdef _SAMD21_ + +#include "samd21_mcu.h" +#include "../hardware_api.h" + + +static bool freeRunning = false; +static int _pinA, _pinB, _pinC; +static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode +static SAMDCurrentSenseADCDMA instance; +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - adc pin A + * @param pinB - adc pin B + * @param pinC - adc pin C + */ +void _configureADCLowSide(const int pinA,const int pinB,const int pinC) +{ + _pinA = pinA; + _pinB = pinB; + _pinC = pinC; + freeRunning = true; + instance.init(pinA, pinB, pinC); + +} +void _startADC3PinConversionLowSide() +{ + instance.startADCScan(); +} +/** + * function reading an ADC value and returning the read voltage + * + * @param pinA - the arduino pin to be read (it has to be ADC pin) + */ +float _readADCVoltageLowSide(const int pinA) +{ + instance.readResults(a, b, c); + + if(pinA == _pinA) + return instance.toVolts(a); + if(pinA == _pinB) + return instance.toVolts(b); + if(pinA == _pinC) + return instance.toVolts(c); + + return NAN; +} + +/** + * function syncing the Driver with the ADC for the LowSide Sensing + */ +void _driverSyncLowSide() +{ + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); + instance.startADCScan(); + //TODO: hook with PWM interrupts +} + + + + + + + + + + + // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless + +static void adcStopWithDMA(void); +static void adcStartWithDMA(void); + +/** + * @brief ADC sync wait + * @retval void + */ +static __inline__ void ADCsync() __attribute__((always_inline, unused)); +static void ADCsync() { + while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free +} + +// ADC DMA sequential free running (6) with Interrupts ///////////////// + +SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() +{ + + return &instance; +} + +SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() +{ +} + +void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) +{ + this->pinA = pinA; + this->pinB = pinB; + this->pinC = pinC; + this->pinAREF = pinAREF; + this->channelDMA = channelDMA; + this->voltageAREF = voltageAREF; + this->maxCountsADC = 1 << adcBits; + countsToVolts = ( voltageAREF / maxCountsADC ); + + initPins(); + initADC(); + initDMA(); + startADCScan(); //so we have something to read next time we call readResults() +} + + +void SAMDCurrentSenseADCDMA::startADCScan(){ + adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); + adcStartWithDMA(); +} + +bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ + if(ADC->CTRLA.bit.ENABLE) + return false; + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + a = adcBuffer[ainA]; + b = adcBuffer[ainB]; + if(_isset(pinC)) + { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + c = adcBuffer[ainC]; + } + return true; +} + + +float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { + return counts * countsToVolts; +} + +void SAMDCurrentSenseADCDMA::initPins(){ + + pinMode(pinAREF, INPUT); + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + + uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; + uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + firstAIN = min(ainA, ainB); + lastAIN = max(ainA, ainB); + if( _isset(pinC) ) + { + uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + pinMode(pinC, INPUT); + firstAIN = min(firstAIN, ainC); + lastAIN = max(lastAIN, ainC); + } + + oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout + BufferSize = lastAIN - oneBeforeFirstAIN + 1; + +} + +void SAMDCurrentSenseADCDMA::initADC(){ + + analogRead(pinA); // do some pin init pinPeripheral() + analogRead(pinB); // do some pin init pinPeripheral() + analogRead(pinC); // do some pin init pinPeripheral() + + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + ADCsync(); + //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA + ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X + // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default + ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; + // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; + ADCsync(); // ref 31.6.16 + + /* + Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan + This register gives the number of input sources included in the pin scan. The number of input sources included is + INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS + + INPUTOFFSET + INPUTSCAN. + The range of the scan mode must not exceed the number of input channels available on the device. + Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection + These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If + the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit + group in the SamplingControl register must be written. + Table 32-14. Positive Mux Input Selection + MUXPOS[4:0] Group configuration Description + 0x00 PIN0 ADC AIN0 pin + 0x01 PIN1 ADC AIN1 pin + 0x02 PIN2 ADC AIN2 pin + 0x03 PIN3 ADC AIN3 pin + 0x04 PIN4 ADC AIN4 pin + 0x05 PIN5 ADC AIN5 pin + 0x06 PIN6 ADC AIN6 pin + 0x07 PIN7 ADC AIN7 pin + 0x08 PIN8 ADC AIN8 pin + 0x09 PIN9 ADC AIN9 pin + 0x0A PIN10 ADC AIN10 pin + 0x0B PIN11 ADC AIN11 pin + 0x0C PIN12 ADC AIN12 pin + 0x0D PIN13 ADC AIN13 pin + 0x0E PIN14 ADC AIN14 pin + 0x0F PIN15 ADC AIN15 pin + 0x10 PIN16 ADC AIN16 pin + 0x11 PIN17 ADC AIN17 pin + 0x12 PIN18 ADC AIN18 pin + 0x13 PIN19 ADC AIN19 pin + 0x14-0x17 Reserved + 0x18 TEMP Temperature reference + 0x19 BANDGAP Bandgap voltage + 0x1A SCALEDCOREVCC 1/4 scaled core supply + 0x1B SCALEDIOVCC 1/4 scaled I/O supply + 0x1C DAC DAC output + 0x1D-0x1F Reserved + */ + ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; + ADCsync(); + ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor + ADCsync(); + ADC->AVGCTRL.reg = 0x00 ; //no averaging + ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 + // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU + ADCsync(); + ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; + ADCsync(); +} + +volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); + +void SAMDCurrentSenseADCDMA::initDMA() { + // probably on by default + PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; + PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; + NVIC_EnableIRQ( DMAC_IRQn ) ; + DMAC->BASEADDR.reg = (uint32_t)descriptor_section; + DMAC->WRBADDR.reg = (uint32_t)wrb; + DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); +} + + +void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { + + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); + DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; + DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; + DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + + DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) + | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) + | DMAC_CHCTRLB_TRIGACT_BEAT; + DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts + descriptor.descaddr = 0; + descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; + descriptor.btcnt = hwords; + descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address + descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; + memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); + + // start channel + DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); + DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; +} + + +int iii = 0; + +void adcStopWithDMA(void){ + ADCsync(); + ADC->CTRLA.bit.ENABLE = 0x00; + // ADCsync(); + // if(iii++ % 1000 == 0) + // { + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c); + // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!"); + // } + + +} + +void adcStartWithDMA(void){ + ADCsync(); + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + ADCsync(); + ADC->SWTRIG.bit.FLUSH = 1; + ADCsync(); + ADC->CTRLA.bit.ENABLE = 0x01; + ADCsync(); +} + +void DMAC_Handler() { + uint8_t active_channel; + active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number + DMAC->CHID.reg = DMAC_CHID_ID(active_channel); + adcStopWithDMA(); + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; + DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; + +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h new file mode 100644 index 00000000..c0cec74a --- /dev/null +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -0,0 +1,67 @@ +#ifdef _SAMD21_ + +#ifndef CURRENT_SENSE_SAMD21_H +#define CURRENT_SENSE_SAMD21_H + +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif + +#include + typedef struct { + uint16_t btctrl; + uint16_t btcnt; + uint32_t srcaddr; + uint32_t dstaddr; + uint32_t descaddr; + } dmacdescriptor ; + + +class SAMDCurrentSenseADCDMA +{ + +public: + static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); + SAMDCurrentSenseADCDMA(); + void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); + void startADCScan(); + bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); + float toVolts(uint16_t counts); +private: + + void adcToDMATransfer(void *rxdata, uint32_t hwords); + + void initPins(); + void initADC(); + void initDMA(); + + uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout + uint32_t firstAIN; + uint32_t lastAIN; + uint32_t BufferSize = 0; + + uint16_t adcBuffer[20]; + + + uint32_t pinA; + uint32_t pinB; + uint32_t pinC; + uint32_t pinAREF; + uint32_t channelDMA; // DMA channel + bool freeRunning; + + float voltageAREF; + float maxCountsADC; + float countsToVolts; + + dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); + dmacdescriptor descriptor __attribute__ ((aligned (16))); + +}; + +#endif + + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp new file mode 100644 index 00000000..550d7bee --- /dev/null +++ b/src/current_sense/hardware_specific/samd_mcu.cpp @@ -0,0 +1,37 @@ +#include "../hardware_api.h" + +#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp new file mode 100644 index 00000000..797ac6a1 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -0,0 +1,39 @@ + +#include "../hardware_api.h" + +#if defined(_STM32_DEF_) and !defined(STM32G4xx) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32g4_hal.cpp new file mode 100644 index 00000000..f0204566 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32g4_hal.cpp @@ -0,0 +1,333 @@ +#include "../hardware_api.h" +#if defined(STM32G4xx) + +#include "stm32g4xx_hal.h" +#include "stm32g4xx_ll_pwr.h" +#include "stm32g4xx_hal_adc.h" +#include "stm32g4_hal.h" +// From STM32 cube IDE +/** + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2020 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + + +/** + * @brief GPIO Initialization Function + * @param None + * @retval None + */ +void MX_GPIO_Init(void) +{ + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOF_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + __HAL_RCC_ADC12_CLK_ENABLE(); +} + +/** + * Enable DMA controller clock + */ +void MX_DMA_Init(void) +{ + /* DMA controller clock enable */ + __HAL_RCC_DMAMUX1_CLK_ENABLE(); + __HAL_RCC_DMA1_CLK_ENABLE(); + __HAL_RCC_DMA2_CLK_ENABLE(); + + /* DMA interrupt init */ + /* DMA1_Channel1_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn); + /* DMA1_Channel2_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn); + + // Enable external clock for ADC + RCC_PeriphCLKInitTypeDef PeriphClkInit; + PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12; + PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_PLL; + HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit); +} + + +/** + * @brief ADC1 Initialization Function + * @param None + * @retval None + */ +void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) +{ + /* USER CODE BEGIN ADC1_Init 0 */ + + /* USER CODE END ADC1_Init 0 */ + + ADC_MultiModeTypeDef multimode = {0}; + ADC_ChannelConfTypeDef sConfig = {0}; + + /* USER CODE BEGIN ADC1_Init 1 */ + + /* USER CODE END ADC1_Init 1 */ + /** Common config + */ + hadc1->Instance = ADC1; + hadc1->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16; + hadc1->Init.Resolution = ADC_RESOLUTION_12B; + hadc1->Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc1->Init.GainCompensation = 0; + hadc1->Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc1->Init.LowPowerAutoWait = DISABLE; + hadc1->Init.ContinuousConvMode = DISABLE; + hadc1->Init.NbrOfConversion = 2; + hadc1->Init.DiscontinuousConvMode = DISABLE; + hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; + hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; + hadc1->Init.DMAContinuousRequests = ENABLE; + hadc1->Init.Overrun = ADC_OVR_DATA_PRESERVED; + + if (HAL_ADC_Init(hadc1) != HAL_OK) + { + Error_Handler(); + } + + /** Configure the ADC multi-mode + */ + multimode.Mode = ADC_MODE_INDEPENDENT; + if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK) + { + Error_Handler(); + } + /** Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_12; // ADC1_IN12 = PB1 = OP3_OUT + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + Error_Handler(); + } + /** Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_3; // ADC1_IN3 = PA2 = OP1_OUT + sConfig.Rank = ADC_REGULAR_RANK_2; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN ADC1_Init 2 */ + + /* USER CODE END ADC1_Init 2 */ + +} + +/** + * @brief ADC2 Initialization Function + * @param None + * @retval None + */ +void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) +{ + /* USER CODE BEGIN ADC2_Init 0 */ + + /* USER CODE END ADC2_Init 0 */ + + ADC_ChannelConfTypeDef sConfig = {0}; + + /* USER CODE BEGIN ADC2_Init 1 */ + + /* USER CODE END ADC2_Init 1 */ + /** Common config + */ + hadc2->Instance = ADC2; + hadc2->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16; + hadc2->Init.Resolution = ADC_RESOLUTION_12B; + hadc2->Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc2->Init.GainCompensation = 0; + hadc2->Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc2->Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc2->Init.LowPowerAutoWait = DISABLE; + hadc2->Init.ContinuousConvMode = DISABLE; + hadc2->Init.NbrOfConversion = 1; + hadc2->Init.DiscontinuousConvMode = DISABLE; + hadc2->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; + hadc2->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; + hadc2->Init.DMAContinuousRequests = ENABLE; + hadc2->Init.Overrun = ADC_OVR_DATA_PRESERVED; + + if (HAL_ADC_Init(hadc2) != HAL_OK) + { + Error_Handler(); + } + /** Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_3; // ADC2_IN3 = PA6 + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN ADC2_Init 2 */ + + /* USER CODE END ADC2_Init 2 */ + +} + +/** +* @brief OPAMP MSP Initialization +* This function configures the hardware resources used in this example +* @param hopamp-> OPAMP handle pointer +* @retval None +*/ +void HAL_OPAMP_MspInit(OPAMP_HandleTypeDef* hopamp) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(hopamp->Instance==OPAMP1) + { + /* USER CODE BEGIN OPAMP1_MspInit 0 */ + + /* USER CODE END OPAMP1_MspInit 0 */ + + __HAL_RCC_GPIOA_CLK_ENABLE(); + /**OPAMP1 GPIO Configuration + PA1 ------> OPAMP1_VINP + PA2 ------> OPAMP1_VOUT + PA3 ------> OPAMP1_VINM + */ + GPIO_InitStruct.Pin = GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USER CODE BEGIN OPAMP1_MspInit 1 */ + + /* USER CODE END OPAMP1_MspInit 1 */ + } + else if(hopamp->Instance==OPAMP2) + { + /* USER CODE BEGIN OPAMP2_MspInit 0 */ + + /* USER CODE END OPAMP2_MspInit 0 */ + + __HAL_RCC_GPIOA_CLK_ENABLE(); + /**OPAMP2 GPIO Configuration + PA5 ------> OPAMP2_VINM + PA6 ------> OPAMP2_VOUT + PA7 ------> OPAMP2_VINP + */ + GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USER CODE BEGIN OPAMP2_MspInit 1 */ + + /* USER CODE END OPAMP2_MspInit 1 */ + } + else if(hopamp->Instance==OPAMP3) + { + /* USER CODE BEGIN OPAMP3_MspInit 0 */ + + /* USER CODE END OPAMP3_MspInit 0 */ + + __HAL_RCC_GPIOB_CLK_ENABLE(); + /**OPAMP3 GPIO Configuration + PB0 ------> OPAMP3_VINP + PB1 ------> OPAMP3_VOUT + PB2 ------> OPAMP3_VINM + */ + GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* USER CODE BEGIN OPAMP3_MspInit 1 */ + + /* USER CODE END OPAMP3_MspInit 1 */ + } + +} + +/** +* @brief OPAMP MSP De-Initialization +* This function freeze the hardware resources used in this example +* @param hopamp-> OPAMP handle pointer +* @retval None +*/ +void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* hopamp) +{ + if(hopamp->Instance==OPAMP1) + { + /* USER CODE BEGIN OPAMP1_MspDeInit 0 */ + + /* USER CODE END OPAMP1_MspDeInit 0 */ + + /**OPAMP1 GPIO Configuration + PA1 ------> OPAMP1_VINP + PA2 ------> OPAMP1_VOUT + PA3 ------> OPAMP1_VINM + */ + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3); + + /* USER CODE BEGIN OPAMP1_MspDeInit 1 */ + + /* USER CODE END OPAMP1_MspDeInit 1 */ + } + else if(hopamp->Instance==OPAMP2) + { + /* USER CODE BEGIN OPAMP2_MspDeInit 0 */ + + /* USER CODE END OPAMP2_MspDeInit 0 */ + + /**OPAMP2 GPIO Configuration + PA5 ------> OPAMP2_VINM + PA6 ------> OPAMP2_VOUT + PA7 ------> OPAMP2_VINP + */ + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7); + + /* USER CODE BEGIN OPAMP2_MspDeInit 1 */ + + /* USER CODE END OPAMP2_MspDeInit 1 */ + } + else if(hopamp->Instance==OPAMP3) + { + /* USER CODE BEGIN OPAMP3_MspDeInit 0 */ + + /* USER CODE END OPAMP3_MspDeInit 0 */ + + /**OPAMP3 GPIO Configuration + PB0 ------> OPAMP3_VINP + PB1 ------> OPAMP3_VOUT + PB2 ------> OPAMP3_VINM + */ + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2); + + /* USER CODE BEGIN OPAMP3_MspDeInit 1 */ + + /* USER CODE END OPAMP3_MspDeInit 1 */ + } + +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32g4_hal.h new file mode 100644 index 00000000..13dd1de3 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32g4_hal.h @@ -0,0 +1,14 @@ +#ifndef stm32g4_hal +#define stm32g4_hal + +#if defined(STM32G4xx) +void MX_GPIO_Init(void); +void MX_DMA_Init(void); +void MX_ADC1_Init(ADC_HandleTypeDef* hadc1); +void MX_ADC2_Init(ADC_HandleTypeDef* hadc2); +void MX_OPAMP1_Init(OPAMP_HandleTypeDef* hopamp); +void MX_OPAMP2_Init(OPAMP_HandleTypeDef* hopamp); +void MX_OPAMP3_Init(OPAMP_HandleTypeDef* hopamp); +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32g4_mcu.cpp new file mode 100644 index 00000000..5b5d00ee --- /dev/null +++ b/src/current_sense/hardware_specific/stm32g4_mcu.cpp @@ -0,0 +1,130 @@ +#include "../hardware_api.h" +#include "stm32g4_hal.h" + +#if defined(STM32G4xx) +#define _ADC_VOLTAGE 3.3 +#define _ADC_RESOLUTION 4096.0 +#define ADC_BUF_LEN_1 2 +#define ADC_BUF_LEN_2 1 + +static ADC_HandleTypeDef hadc1; +static ADC_HandleTypeDef hadc2; +static OPAMP_HandleTypeDef hopamp1; +static OPAMP_HandleTypeDef hopamp2; +static OPAMP_HandleTypeDef hopamp3; + +static DMA_HandleTypeDef hdma_adc1; +static DMA_HandleTypeDef hdma_adc2; + +uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion +uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion + +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +// As DMA is being used just return the DMA result +float _readADCVoltageInline(const int pin){ + uint32_t raw_adc = 0; + if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1 + raw_adc = adcBuffer1[1]; + else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1 + raw_adc = adcBuffer2[0]; + else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1 + raw_adc = adcBuffer1[0]; + + return raw_adc * _ADC_CONV; +} +// do the same for low side sensing +float _readADCVoltageLowSide(const int pin){ + return _readADCVoltageInline(pin); +} + +void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ + // could this be replaced with LL_OPAMP calls?? + hopamp->Instance = OPAMPx_Def; + hopamp->Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED; + hopamp->Init.Mode = OPAMP_PGA_MODE; + hopamp->Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; + hopamp->Init.InternalOutput = DISABLE; + hopamp->Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; + hopamp->Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS; + hopamp->Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15; + hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY; + if (HAL_OPAMP_Init(hopamp) != HAL_OK) + { + Error_Handler(); + } +} +void _configureOPAMPs(OPAMP_HandleTypeDef *OPAMPA, OPAMP_HandleTypeDef *OPAMPB, OPAMP_HandleTypeDef *OPAMPC){ + // Configure the opamps + _configureOPAMP(OPAMPA, OPAMP1); + _configureOPAMP(OPAMPB, OPAMP2); + _configureOPAMP(OPAMPC, OPAMP3); +} + +void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Channel_TypeDef* channel, uint32_t request) { + hdma_adc->Instance = channel; + hdma_adc->Init.Request = request; + hdma_adc->Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_adc->Init.PeriphInc = DMA_PINC_DISABLE; + hdma_adc->Init.MemInc = DMA_MINC_ENABLE; + hdma_adc->Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + hdma_adc->Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + hdma_adc->Init.Mode = DMA_CIRCULAR; + hdma_adc->Init.Priority = DMA_PRIORITY_LOW; + HAL_DMA_DeInit(hdma_adc); + if (HAL_DMA_Init(hdma_adc) != HAL_OK) + { + Error_Handler(); + } + __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc); +} + +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + HAL_Init(); + MX_GPIO_Init(); + MX_DMA_Init(); + _configureOPAMPs(&hopamp1, &hopamp3, &hopamp2); + MX_ADC1_Init(&hadc1); + MX_ADC2_Init(&hadc2); + + MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1); + MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2); + + if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcBuffer1, ADC_BUF_LEN_1) != HAL_OK) + { + Error_Handler(); + } + if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK) + { + Error_Handler(); + } + + HAL_OPAMP_Start(&hopamp1); + HAL_OPAMP_Start(&hopamp2); + HAL_OPAMP_Start(&hopamp3); + + // Check if the ADC DMA is collecting any data. + // If this fails, it likely means timer1 has not started. Verify that your application starts + // the motor pwm (usually BLDCDriver6PWM::init()) before initializing the ADC engine. + _delay(5); + if (adcBuffer1[0] == 0 || adcBuffer1[1] == 0 || adcBuffer2[0] == 0) { + Error_Handler(); + } +} +// do the same for low side +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + _configureADCInline(pinA, pinB, pinC); +} + +extern "C" { +void DMA1_Channel1_IRQHandler(void) { + HAL_DMA_IRQHandler(&hdma_adc1); +} + +void DMA1_Channel2_IRQHandler(void) { + HAL_DMA_IRQHandler(&hdma_adc2); +} +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp new file mode 100644 index 00000000..606ce618 --- /dev/null +++ b/src/current_sense/hardware_specific/teensy_mcu.cpp @@ -0,0 +1,37 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA){ + return _readADCVoltageInline(pinA); +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + +#endif \ No newline at end of file diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 11215863..bc239875 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -14,15 +14,16 @@ BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } // enable motor driver void BLDCDriver3PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, HIGH); - if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, HIGH); - if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, HIGH); + if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high); + if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high); + if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, enable_active_high); // set zero to PWM setPwm(0,0,0); } @@ -33,13 +34,13 @@ void BLDCDriver3PWM::disable() // set zero to PWM setPwm(0, 0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, LOW); - if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, LOW); - if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, LOW); + if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, !enable_active_high); + if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, !enable_active_high); + if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, !enable_active_high); } -// init hardware pins +// init hardware pins int BLDCDriver3PWM::init() { // PWM pins pinMode(pwmA, OUTPUT); @@ -62,7 +63,7 @@ int BLDCDriver3PWM::init() { // Set voltage to the pwm pin -void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { +void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // disable if needed if( _isset(enableA_pin) && _isset(enableB_pin) && _isset(enableC_pin) ){ digitalWrite(enableA_pin, sa == _HIGH_IMPEDANCE ? LOW : HIGH); @@ -73,18 +74,18 @@ void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // Set voltage to the pwm pin void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { - + // limit the voltage in driver - Ua = _constrain(Ua, 0.0, voltage_limit); - Ub = _constrain(Ub, 0.0, voltage_limit); - Uc = _constrain(Uc, 0.0, voltage_limit); + Ua = _constrain(Ua, 0.0f, voltage_limit); + Ub = _constrain(Ub, 0.0f, voltage_limit); + Uc = _constrain(Uc, 0.0f, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); + dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); -} \ No newline at end of file +} diff --git a/src/drivers/BLDCDriver3PWM.h b/src/drivers/BLDCDriver3PWM.h index cea4463e..d6a83590 100644 --- a/src/drivers/BLDCDriver3PWM.h +++ b/src/drivers/BLDCDriver3PWM.h @@ -38,6 +38,7 @@ class BLDCDriver3PWM: public BLDCDriver int enableA_pin; //!< enable pin number int enableB_pin; //!< enable pin number int enableC_pin; //!< enable pin number + bool enable_active_high = true; /** * Set phase voltages to the harware diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index d51eec0a..eaeb2cfa 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -15,9 +15,10 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; - + pwm_frequency = NOT_SET; + // dead zone initial - 2% - dead_zone = 0.02; + dead_zone = 0.02f; } @@ -39,9 +40,9 @@ void BLDCDriver6PWM::disable() } -// init hardware pins +// init hardware pins int BLDCDriver6PWM::init() { - + // PWM pins pinMode(pwmA_h, OUTPUT); pinMode(pwmB_h, OUTPUT); @@ -55,22 +56,22 @@ int BLDCDriver6PWM::init() { // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; - // configure 6pwm + // configure 6pwm // hardware specific function - depending on driver and mcu return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); } // Set voltage to the pwm pin -void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { +void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver Ua = _constrain(Ua, 0, voltage_limit); Ub = _constrain(Ub, 0, voltage_limit); - Uc = _constrain(Uc, 0, voltage_limit); + Uc = _constrain(Uc, 0, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); + dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); @@ -78,6 +79,6 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // Set voltage to the pwm pin -void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { +void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { // TODO implement disabling } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 09dd3e16..a0e61ca0 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -1,13 +1,13 @@ #include "StepperDriver2PWM.h" -StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _in1a, int _in1b, int _pwm2, int _in2a, int _in2b, int en1, int en2){ +StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, int en1, int en2){ // Pin initialization pwm1 = _pwm1; // phase 1 pwm pin number - dir1a = _in1a; // phase 1 INA pin number - dir1b = _in1b; // phase 1 INB pin number + dir1a = _in1[0]; // phase 1 INA pin number + dir1b = _in1[1]; // phase 1 INB pin number pwm2 = _pwm2; // phase 2 pwm pin number - dir2a = _in2a; // phase 2 INA pin number - dir2b = _in2b; // phase 2 INB pin number + dir2a = _in2[0]; // phase 2 INA pin number + dir2b = _in2[1]; // phase 2 INB pin number // enable_pin pin enable_pin1 = en1; @@ -16,6 +16,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _in1a, int _in1b, int _pwm2, // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } @@ -27,7 +28,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, dir2a = _dir2; // phase 2 direction pin // these pins are not used dir1b = NOT_SET; - dir2b = NOT_SET; + dir2b = NOT_SET; // enable_pin pin enable_pin1 = en1; @@ -36,6 +37,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } @@ -59,7 +61,7 @@ void StepperDriver2PWM::disable() } -// init hardware pins +// init hardware pins int StepperDriver2PWM::init() { // PWM pins pinMode(pwm1, OUTPUT); @@ -83,22 +85,22 @@ int StepperDriver2PWM::init() { // Set voltage to the pwm pin -void StepperDriver2PWM::setPwm(float Ua, float Ub) { - float duty_cycle1(0.0),duty_cycle2(0.0); +void StepperDriver2PWM::setPwm(float Ua, float Ub) { + float duty_cycle1(0.0f),duty_cycle2(0.0f); // limit the voltage in driver Ua = _constrain(Ua, -voltage_limit, voltage_limit); Ub = _constrain(Ub, -voltage_limit, voltage_limit); // hardware specific writing - duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0,1.0); - duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0,1.0); - + duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0f,1.0f); + duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0f,1.0f); + // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); - if( _isset(dir1b) ) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH); + if( _isset(dir1b) ) digitalWrite(dir1b, Ua >= 0 ? HIGH : LOW ); // phase 2 direction digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH); - if( _isset(dir2b) ) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH); - + if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW ); + // write to hardware _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); } \ No newline at end of file diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h index cab796d3..b349af06 100644 --- a/src/drivers/StepperDriver2PWM.h +++ b/src/drivers/StepperDriver2PWM.h @@ -16,15 +16,13 @@ class StepperDriver2PWM: public StepperDriver /** StepperMotor class constructor @param pwm1 PWM1 phase pwm pin - @param in1a IN1A phase dir pin - @param in1b IN1B phase dir pin + @param in1 IN1A phase dir pin @param pwm2 PWM2 phase pwm pin - @param in2a IN2A phase dir pin - @param in2b IN2B phase dir pin + @param in2 IN2A phase dir @param en1 enable pin phase 1 (optional input) @param en2 enable pin phase 2 (optional input) */ - StepperDriver2PWM(int pwm1, int in1a, int in1b, int pwm2, int in2a, int in2b, int en1 = NOT_SET, int en2 = NOT_SET); + StepperDriver2PWM(int pwm1, int* in1, int pwm2, int* in2, int en1 = NOT_SET, int en2 = NOT_SET); /** StepperMotor class constructor diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index f752ea6e..6c905348 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -14,6 +14,7 @@ StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1 // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; } @@ -37,7 +38,7 @@ void StepperDriver4PWM::disable() } -// init hardware pins +// init hardware pins int StepperDriver4PWM::init() { // PWM pins @@ -59,21 +60,21 @@ int StepperDriver4PWM::init() { // Set voltage to the pwm pin -void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { - float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); +void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f); // limit the voltage in driver Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); // hardware specific writing if( Ualpha > 0 ) - duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - else - duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - + duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + else + duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + if( Ubeta > 0 ) - duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); else - duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); // write to hardware _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); } \ No newline at end of file diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 01c03867..0b4f7c65 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -1,5 +1,5 @@ -#ifndef HARDWARE_UTILS_H -#define HARDWARE_UTILS_H +#ifndef HARDWARE_UTILS_DRIVER_H +#define HARDWARE_UTILS_DRIVER_H #include "../common/foc_utils.h" #include "../common/time_utils.h" diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 5236fb03..2d3c0305 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -7,7 +7,7 @@ void _pinHighFrequency(const int pin){ // High PWM frequency // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 // https://forum.arduino.cc/index.php?topic=72092.0 - if (pin == 13 || pin == 4 ) { + if (pin == 13 || pin == 4 ) { TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 } @@ -15,13 +15,13 @@ void _pinHighFrequency(const int pin){ TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 10 || pin == 9 ) TCCR2B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 - else if (pin == 5 || pin == 3 || pin == 2) + else if (pin == 5 || pin == 3 || pin == 2) TCCR3B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 8 || pin == 7 || pin == 6) TCCR4B = ((TCCR4B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 44 || pin == 45 || pin == 46) TCCR5B = ((TCCR5B & 0b11111000) | 0x01); // set prescaler to 1 - + } @@ -46,23 +46,23 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int _pinHighFrequency(pinC); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins @@ -74,25 +74,25 @@ void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const i _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } // function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm // supports Arudino/ATmega2560 int _configureComplementaryPair(int pinH, int pinL) { - if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ + if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ // configure the pwm phase-corrected mode TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure complementary pwm on low side @@ -121,7 +121,7 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // High PWM frequency // - always max 32kHz @@ -132,12 +132,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return ret_flag; // returns -1 if not well configured } -// function setting the +// function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { int pwm_h = _constrain(val-dead_time/2,0,255); int pwm_l = _constrain(val+dead_time/2,0,255); - + analogWrite(pinH, pwm_h); if(pwm_l == 255 || pwm_l == 0) digitalWrite(pinL, pwm_l ? LOW : HIGH); @@ -148,7 +148,7 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index cf12c302..2c64ee64 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -1,28 +1,28 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // set pwm frequency to 32KHz void _pinHighFrequency(const int pin){ // High PWM frequency // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 - if (pin == 5 || pin == 6 ) { + if (pin == 5 || pin == 6 ) { TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 } if (pin == 9 || pin == 10 ) TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 - if (pin == 3 || pin == 11) + if (pin == 3 || pin == 11) TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 - -} +} // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic // supports Arudino/ATmega328 void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + _UNUSED(pwm_frequency); // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); @@ -34,6 +34,7 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // - hardware speciffic // supports Arudino/ATmega328 void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + _UNUSED(pwm_frequency); // High PWM frequency // - always max 32kHz _pinHighFrequency(pinA); @@ -41,47 +42,47 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int _pinHighFrequency(pinC); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } @@ -116,8 +117,10 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + _UNUSED(pwm_frequency); + _UNUSED(dead_zone); // High PWM frequency // - always max 32kHz int ret_flag = 0; @@ -127,12 +130,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return ret_flag; // returns -1 if not well configured } -// function setting the +// function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { int pwm_h = _constrain(val-dead_time/2,0,255); int pwm_l = _constrain(val+dead_time/2,0,255); - + analogWrite(pinH, pwm_h); if(pwm_l == 255 || pwm_l == 0) digitalWrite(pinL, pwm_l ? LOW : HIGH); @@ -143,7 +146,7 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp new file mode 100644 index 00000000..5a8abe2e --- /dev/null +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -0,0 +1,172 @@ + +#include "../hardware_api.h" + +#if defined(__AVR_ATmega32U4__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // reference： http://r6500.blogspot.com/2014/12/fast-pwm-on-arduino-leonardo.html + if (pin == 3 || pin == 11 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + else if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 5 ) + TCCR3B = ((TCCR3B & 0b11111000) | 0x01); // set prescaler to 1 + else if ( pin == 6 || pin == 13 ) { // a bit more complicated 10 bit timer + // PLL Configuration + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz + TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz + TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + + if (pin == 6) TCCR4A = 0x82; // activate channel A - pin 13 + else if (pin == 13) TCCR4C |= 0x09; // Activate channel D - pin 6 + } + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configureComplementaryPair(int pinH, int pinL) { + if( (pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 6 && pinL == 13 ) || (pinH == 13 && pinL == 6 ) ){ + // PLL Configuration + PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz + TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz + TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + + // configure complementary pwm on low side + if(pinH == 13 ){ + TCCR4A = 0x82; // activate channel A - pin 13 + TCCR4C |= 0x0d; // Activate complementary channel D - pin 6 + }else { + TCCR4C |= 0x09; // Activate channel D - pin 6 + TCCR4A = 0xc2; // activate complementary channel A - pin 13 + } + }else{ + return -1; + } + return 0; +} + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + _UNUSED(pwm_frequency); + _UNUSED(dead_zone); + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured + + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index 27097301..e0d39afd 100644 --- a/src/drivers/hardware_specific/due_mcu.cpp +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -1,6 +1,11 @@ #include "../hardware_api.h" -#if defined(__arm__) && defined(__SAM3X8E__) +#if defined(__arm__) && defined(__SAM3X8E__) + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + +#define _PWM_RES_MIN 255 // 50khz // pwm frequency and max duty cycle static unsigned long _pwm_frequency; @@ -101,7 +106,29 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ if (!PWMEnabled) { // PWM Startup code pmc_enable_periph_clk(PWM_INTERFACE_ID); - PWMC_ConfigureClocks(pwm_freq * _max_pwm_value, 0, VARIANT_MCK); + // this function does not work too well - I'll rewrite it + // PWMC_ConfigureClocks(PWM_FREQUENCY * _max_pwm_value, 0, VARIANT_MCK); + + // finding the divisors an prescalers form FindClockConfiguration function + uint32_t divisors[11] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024}; + uint8_t divisor = 0; + uint32_t prescaler; + + /* Find prescaler and divisor values */ + prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value); + while ((prescaler > 255) && (divisor < 11)) { + divisor++; + prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value); + } + // update the divisor*prescaler value + prescaler = prescaler | (divisor << 8); + + // now calculate the real resolution timer period necessary (pwm resolution) + // pwm_res = bus_freq / (pwm_freq * (prescaler)) + _max_pwm_value = (double)VARIANT_MCK / (double)pwm_freq / (double)(prescaler); + // set the prescaler value + PWM->PWM_CLK = prescaler; + PWMEnabled = 1; } @@ -112,6 +139,8 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ g_APinDescription[ulPin].ulPinType, g_APinDescription[ulPin].ulPin, g_APinDescription[ulPin].ulPinConfiguration); + // PWM_CMR_CALG - center align + // PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, PWM_CMR_CALG, 0); PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0); PWMC_SetPeriod(PWM_INTERFACE, chan, _max_pwm_value); PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0); @@ -123,7 +152,7 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin // We use MCLK/2 as clock. - const uint32_t TC = VARIANT_MCK / 2 / pwm_freq; + const uint32_t TC = VARIANT_MCK / 2 / pwm_freq ; // Setup Timer for this pin ETCChannel channel = g_APinDescription[ulPin].ulTCChannel; uint32_t chNo = channelToChNo[channel]; @@ -131,17 +160,18 @@ void initPWM(uint32_t ulPin, uint32_t pwm_freq){ Tc *chTC = channelToTC[channel]; uint32_t interfaceID = channelToId[channel]; - if (!TCChanEnabled[interfaceID]) { - pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID); - TC_Configure(chTC, chNo, - TC_CMR_TCCLKS_TIMER_CLOCK1 | - TC_CMR_WAVE | // Waveform mode - TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC - TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output) - TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR | - TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR); - TC_SetRC(chTC, chNo, TC); - } + if (!TCChanEnabled[interfaceID]) { + pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID); + TC_Configure(chTC, chNo, + TC_CMR_TCCLKS_TIMER_CLOCK1 | + TC_CMR_WAVE | // Waveform mode + TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC + TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output) + TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR | + TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR); + TC_SetRC(chTC, chNo, TC); + } + // disable the counter on start if (chA){ TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET); @@ -197,7 +227,7 @@ void setPwm(uint32_t ulPin, uint32_t ulValue) { const uint32_t TC = VARIANT_MCK / 2 / _pwm_frequency; // Map value to Timer ranges 0..max_duty_cycle => 0..TC // Setup Timer for this pin - ulValue = ulValue * TC; + ulValue = ulValue * TC ; pwm_counter_vals[channel] = ulValue / _max_pwm_value; // enable counter if (channelToAB[channel]) @@ -297,8 +327,8 @@ void TC8_Handler() // - BLDC motor - 3PWM setting // - hardware specific void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 35000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency _pwm_frequency = pwm_frequency; // cinfigure pwm pins @@ -314,8 +344,8 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int //- Stepper driver - 2PWM setting // - hardware specific void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency _pwm_frequency = pwm_frequency; // cinfigure pwm pins @@ -329,8 +359,8 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { // - Stepper motor - 4PWM setting // - hardware speciffic void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = 35000; // default frequency 50khz - else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // save the pwm frequency _pwm_frequency = pwm_frequency; // cinfigure pwm pins @@ -373,19 +403,4 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ } -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - return -1; -} - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - return; -} - - #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 0bc2d863..f6663d87 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -1,18 +1,18 @@ #include "../hardware_api.h" -#if defined(ESP_H) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" -// empty motor slot +// empty motor slot #define _EMPTY_SLOT -20 #define _TAKEN_SLOT -21 // ABI bus frequency - would be better to take it from somewhere // but I did nto find a good exposed variable -#define _MCPWM_FREQ 160e6 +#define _MCPWM_FREQ 160e6f // preferred pwm resolution default #define _PWM_RES_DEF 2048 @@ -70,7 +70,7 @@ typedef struct { } bldc_6pwm_motor_slots_t; // define bldc motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A @@ -78,19 +78,19 @@ bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { }; // define stepper motor slots array -bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { +bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - }; + }; // define 4pwm stepper motor slots array -stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { +stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 - }; + }; // define 2pwm stepper motor slots array -stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { +stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A @@ -109,12 +109,12 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings - + if (_isset(dead_zone)){ - // dead zone is configured - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + // dead zone is configured + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); } _delay(100); @@ -128,21 +128,21 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // calculate prescaler and period // step 1: calculate the prescaler using the default pwm resolution // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 - int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0 / (double)pwm_frequency) - 1; + int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1; // constrain prescaler - prescaler = _constrain(prescaler, 0, 128); + prescaler = _constrain(prescaler, 0, 128); // now calculate the real resolution timer period necessary (pwm resolution) // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) - int resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(prescaler + 1); + int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1); // if pwm resolution too low lower the prescaler - if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) - resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(--prescaler + 1); + if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) + resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1); resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); // set prescaler mcpwm_num->timer[0].period.prescale = prescaler; mcpwm_num->timer[1].period.prescale = prescaler; - mcpwm_num->timer[2].period.prescale = prescaler; + mcpwm_num->timer[2].period.prescale = prescaler; _delay(1); //set period mcpwm_num->timer[0].period.period = resolution_corrected; @@ -152,13 +152,13 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_num->timer[0].period.upmethod = 0; mcpwm_num->timer[1].period.upmethod = 0; mcpwm_num->timer[2].period.upmethod = 0; - _delay(1); - // _delay(1); + _delay(1); + // _delay(1); //restart the timers mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); - _delay(1); + _delay(1); // Cast here because MCPWM_SELECT_SYNC_INT0 (1) is not defined // in the default Espressif MCPWM headers. The correct const may be used // when https://github.com/espressif/esp-idf/issues/5429 is resolved. @@ -174,15 +174,15 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// supports Arudino/ATmega328, STM32 and ESP32 void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_2pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 4; slot_num++){ if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -191,8 +191,8 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { break; } } - - // disable all the slots with the same MCPWM + + // disable all the slots with the same MCPWM // disable 3pwm bldc motor which would go in the same slot esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; if( slot_num < 2 ){ @@ -219,15 +219,15 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// supports Arudino/ATmega328, STM32 and ESP32 void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max bldc_3pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 4; slot_num++){ if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -236,7 +236,7 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM // disable 2pwm steppr motor which would go in the same slot esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; if( slot_num < 2 ){ @@ -266,10 +266,10 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - hardware speciffic void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_4pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 2; slot_num++){ if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -278,7 +278,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM if( slot_num == 0 ){ // slots 0 and 1 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; @@ -297,7 +297,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } + } // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); @@ -314,7 +314,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int // - hardware speciffic // ESP32 uses MCPWM void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 4; i++){ if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found // se the PWM on the slot timers @@ -331,7 +331,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // - hardware speciffic // ESP32 uses MCPWM void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 4; i++){ if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found // se the PWM on the slot timers @@ -349,7 +349,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // - hardware speciffic // ESP32 uses MCPWM void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 2; i++){ if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found // se the PWM on the slot timers @@ -367,12 +367,12 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - BLDC driver - 6PWM setting // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency bldc_6pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 2; slot_num++){ if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -384,7 +384,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // if no slots available if(slot_num >= 2) return -1; - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM if( slot_num == 0 ){ // slots 0 and 1 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; @@ -397,7 +397,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - } + } // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); @@ -409,7 +409,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); - // return + // return return 0; } @@ -417,7 +417,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 2; i++){ if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found // se the PWM on the slot timers diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp new file mode 100644 index 00000000..ccc31aa2 --- /dev/null +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -0,0 +1,77 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266) + +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFreq(freq); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + _setHighFrequency(pwm_frequency, pinD); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); +} +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 4a2360d9..f89c0562 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,32 +1,18 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // if mcu is not atmega328 - -#elif defined(__AVR_ATmega2560__) // if mcu is not atmega2560 - -#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy - -#elif defined(__arm__) && defined(__SAM3X8E__) // or due - -#elif defined(ESP_H) // or esp32 - -#elif defined(_STM32_DEF_) // or stm32 - -#elif defined(_SAMD21_) // samd21 - -#elif defined(_SAMD51_) // samd51 - -#elif defined(__SAME51J19A__) || defined(__ATSAME51J19A__) // samd51 - -#elif defined(TARGET_RP2040) - -#else +// if the mcu doen't have defiend analogWrite +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; +#endif // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic // in generic case dont do anything -void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { +__attribute__((weak)) void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + _UNUSED(pwm_frequency); + _UNUSED(pinA); + _UNUSED(pinB); return; } @@ -34,7 +20,11 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // - BLDC motor - 3PWM setting // - hardware speciffic // in generic case dont do anything -void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { +__attribute__((weak)) void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + _UNUSED(pwm_frequency); + _UNUSED(pinA); + _UNUSED(pinB); + _UNUSED(pinC); return; } @@ -43,55 +33,75 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - Stepper motor - 4PWM setting // - hardware speciffic // in generic case dont do anything -void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { +__attribute__((weak)) void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + _UNUSED(pwm_frequency); + _UNUSED(pin1A); + _UNUSED(pin1B); + _UNUSED(pin2A); + _UNUSED(pin2B); return; } // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +__attribute__((weak)) int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + _UNUSED(pwm_frequency); + _UNUSED(dead_zone); + _UNUSED(pinA_h); + _UNUSED(pinB_h); + _UNUSED(pinC_h); + _UNUSED(pinA_l); + _UNUSED(pinB_l); + _UNUSED(pinC_l); return -1; } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ +__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _UNUSED(dc_a); + _UNUSED(dc_b); + _UNUSED(dc_c); + _UNUSED(dead_zone); + _UNUSED(pinA_h); + _UNUSED(pinB_h); + _UNUSED(pinC_h); + _UNUSED(pinA_l); + _UNUSED(pinB_l); + _UNUSED(pinC_l); return; } - - -#endif diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp new file mode 100644 index 00000000..b314ee40 --- /dev/null +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -0,0 +1,539 @@ + +#include "../hardware_api.h" + +#if defined(TARGET_PORTENTA_H7) + +#include "pwmout_api.h" +#include "pinDefinitions.h" +#include "platform/mbed_critical.h" +#include "platform/mbed_power_mgmt.h" +#include "platform/mbed_assert.h" +#include "PeripheralPins.h" +#include "pwmout_device.h" + +// default pwm parameters +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + + +// // 6pwm parameters +// #define _HARDWARE_6PWM 1 +// #define _SOFTWARE_6PWM 0 +// #define _ERROR_6PWM -1 + +typedef struct{ + int id ; + pwmout_t pins[6]; +} portenta_h7_mcu_enty_s; + +portenta_h7_mcu_enty_s motor_slot[10]; +int slot_index = 0; + + +/* Convert STM32 Cube HAL channel to LL channel */ +uint32_t _TIM_ChannelConvert_HAL2LL(uint32_t channel, pwmout_t *obj) +{ +#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED) + if (obj->inverted) { + switch (channel) { + case TIM_CHANNEL_1 : + return LL_TIM_CHANNEL_CH1N; + case TIM_CHANNEL_2 : + return LL_TIM_CHANNEL_CH2N; + case TIM_CHANNEL_3 : + return LL_TIM_CHANNEL_CH3N; +#if defined(LL_TIM_CHANNEL_CH4N) + case TIM_CHANNEL_4 : + return LL_TIM_CHANNEL_CH4N; +#endif + default : /* Optional */ + return 0; + } + } else +#endif + { + switch (channel) { + case TIM_CHANNEL_1 : + return LL_TIM_CHANNEL_CH1; + case TIM_CHANNEL_2 : + return LL_TIM_CHANNEL_CH2; + case TIM_CHANNEL_3 : + return LL_TIM_CHANNEL_CH3; + case TIM_CHANNEL_4 : + return LL_TIM_CHANNEL_CH4; + default : /* Optional */ + return 0; + } + } +} + + + +// int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){ +// return _pwm_init(obj, digitalPinToPinName(pin), frequency); +// } + +int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){ + int peripheral = (int)pinmap_peripheral(digitalPinToPinName(pin), PinMap_PWM); + int function = (int)pinmap_find_function(digitalPinToPinName(pin), PinMap_PWM); + + const PinMap static_pinmap = {digitalPinToPinName(pin), peripheral, function}; + + pwmout_init_direct(obj, &static_pinmap); + + TIM_HandleTypeDef TimHandle; + TimHandle.Instance = (TIM_TypeDef *)(obj->pwm); + RCC_ClkInitTypeDef RCC_ClkInitStruct; + uint32_t PclkFreq = 0; + uint32_t APBxCLKDivider = RCC_HCLK_DIV1; + uint8_t i = 0; + + + __HAL_TIM_DISABLE(&TimHandle); + + // Get clock configuration + // Note: PclkFreq contains here the Latency (not used after) + HAL_RCC_GetClockConfig(&RCC_ClkInitStruct, &PclkFreq); + + /* Parse the pwm / apb mapping table to find the right entry */ + while (pwm_apb_map_table[i].pwm != obj->pwm) i++; + // sanity check + if (pwm_apb_map_table[i].pwm == 0) return -1; + + + if (pwm_apb_map_table[i].pwmoutApb == PWMOUT_ON_APB1) { + PclkFreq = HAL_RCC_GetPCLK1Freq(); + APBxCLKDivider = RCC_ClkInitStruct.APB1CLKDivider; + } else { +#if !defined(PWMOUT_APB2_NOT_SUPPORTED) + PclkFreq = HAL_RCC_GetPCLK2Freq(); + APBxCLKDivider = RCC_ClkInitStruct.APB2CLKDivider; +#endif + } + + long period_us = 500000.0/((float)frequency); + /* By default use, 1us as SW pre-scaler */ + obj->prescaler = 1; + // TIMxCLK = PCLKx when the APB prescaler = 1 else TIMxCLK = 2 * PCLKx + if (APBxCLKDivider == RCC_HCLK_DIV1) { + TimHandle.Init.Prescaler = (((PclkFreq) / 1000000)) - 1; // 1 us tick + } else { + TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000)) - 1; // 1 us tick + } + TimHandle.Init.Period = (period_us - 1); + + /* In case period or pre-scalers are out of range, loop-in to get valid values */ + while ((TimHandle.Init.Period > 0xFFFF) || (TimHandle.Init.Prescaler > 0xFFFF)) { + obj->prescaler = obj->prescaler * 2; + if (APBxCLKDivider == RCC_HCLK_DIV1) { + TimHandle.Init.Prescaler = (((PclkFreq) / 1000000) * obj->prescaler) - 1; + } else { + TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000) * obj->prescaler) - 1; + } + TimHandle.Init.Period = (period_us - 1) / obj->prescaler; + /* Period decreases and prescaler increases over loops, so check for + * possible out of range cases */ + if ((TimHandle.Init.Period < 0xFFFF) && (TimHandle.Init.Prescaler > 0xFFFF)) { + break; + } + } + + TimHandle.Init.ClockDivision = 0; + TimHandle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; // center aligned + + if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) { + return -1; + } + + TIM_OC_InitTypeDef sConfig; + // Configure channels + sConfig.OCMode = TIM_OCMODE_PWM1; + sConfig.Pulse = obj->pulse / obj->prescaler; + sConfig.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfig.OCFastMode = TIM_OCFAST_DISABLE; +#if defined(TIM_OCIDLESTATE_RESET) + sConfig.OCIdleState = TIM_OCIDLESTATE_RESET; +#endif +#if defined(TIM_OCNIDLESTATE_RESET) + sConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#endif + + int channel = 0; + switch (obj->channel) { + case 1: + channel = TIM_CHANNEL_1; + break; + case 2: + channel = TIM_CHANNEL_2; + break; + case 3: + channel = TIM_CHANNEL_3; + break; + case 4: + channel = TIM_CHANNEL_4; + break; + default: + return -1; + } + + if (LL_TIM_CC_IsEnabledChannel(TimHandle.Instance, _TIM_ChannelConvert_HAL2LL(channel, obj)) == 0) { + // If channel is not enabled, proceed to channel configuration + if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, channel) != HAL_OK) { + return -1; + } + } + + // Save for future use + obj->period = period_us; +#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED) + if (obj->inverted) { + HAL_TIMEx_PWMN_Start(&TimHandle, channel); + } else +#endif + { + HAL_TIM_PWM_Start(&TimHandle, channel); + } + + return 0; +} + +// setting pwm to hardware pin - instead analogWrite() +void _pwm_write(pwmout_t *obj, float value){ + TIM_HandleTypeDef TimHandle; + int channel = 0; + + TimHandle.Instance = (TIM_TypeDef *)(obj->pwm); + + if (value < (float)0.0) { + value = 0.0; + } else if (value > (float)1.0) { + value = 1.0; + } + + obj->pulse = (uint32_t)((float)obj->period * value + 0.5); + + switch (obj->channel) { + case 1: + channel = TIM_CHANNEL_1; + break; + case 2: + channel = TIM_CHANNEL_2; + break; + case 3: + channel = TIM_CHANNEL_3; + break; + case 4: + channel = TIM_CHANNEL_4; + break; + default: + return; + } + + // If channel already enabled, only update compare value to avoid glitch + __HAL_TIM_SET_COMPARE(&TimHandle, channel, obj->pulse / obj->prescaler); +} + +// init low side pin +// HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +// { +// PinName pin = digitalPinToPinName(ulPin); +// TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); +// uint32_t index = get_timer_index(Instance); + +// if (HardwareTimer_Handle[index] == NULL) { +// HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); +// HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; +// HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); +// TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); +// sConfigOC.OCMode = TIM_OCMODE_PWM2; +// sConfigOC.Pulse = 100; +// sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; +// sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; +// sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; +// sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; +// sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +// uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); +// HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); +// } +// HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); +// uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); +// HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); +// HT->setOverflow(PWM_freq, HERTZ_FORMAT); +// HT->pause(); +// HT->refresh(); +// return HT; +// } + + +// align the timers to end the init +void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2){ + TIM_HandleTypeDef TimHandle1, TimHandle2; + TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm); + TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm); + __HAL_TIM_DISABLE(&TimHandle1); + __HAL_TIM_DISABLE(&TimHandle2); + __HAL_TIM_ENABLE(&TimHandle1); + __HAL_TIM_ENABLE(&TimHandle2); +} + +// align the timers to end the init +void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3){ + TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3; + TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm); + TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm); + TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm); + __HAL_TIM_DISABLE(&TimHandle1); + __HAL_TIM_DISABLE(&TimHandle2); + __HAL_TIM_DISABLE(&TimHandle3); + __HAL_TIM_ENABLE(&TimHandle1); + __HAL_TIM_ENABLE(&TimHandle2); + __HAL_TIM_ENABLE(&TimHandle3); +} + +// align the timers to end the init +void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3, pwmout_t *t4){ + TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3, TimHandle4; + TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm); + TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm); + TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm); + TimHandle4.Instance = (TIM_TypeDef *)(t4->pwm); + __HAL_TIM_DISABLE(&TimHandle1); + __HAL_TIM_DISABLE(&TimHandle2); + __HAL_TIM_DISABLE(&TimHandle3); + __HAL_TIM_DISABLE(&TimHandle4); + __HAL_TIM_ENABLE(&TimHandle1); + __HAL_TIM_ENABLE(&TimHandle2); + __HAL_TIM_ENABLE(&TimHandle3); + __HAL_TIM_ENABLE(&TimHandle4); +} + +// // configure hardware 6pwm interface only one timer with inverted channels +// HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +// { +// PinName uhPinName = digitalPinToPinName(pinA_h); +// PinName ulPinName = digitalPinToPinName(pinA_l); +// PinName vhPinName = digitalPinToPinName(pinB_h); +// PinName vlPinName = digitalPinToPinName(pinB_l); +// PinName whPinName = digitalPinToPinName(pinC_h); +// PinName wlPinName = digitalPinToPinName(pinC_l); + +// TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); + +// uint32_t index = get_timer_index(Instance); + +// if (HardwareTimer_Handle[index] == NULL) { +// HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); +// HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; +// HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); +// ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); +// } +// HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); +// HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); + +// // dead time is set in nanoseconds +// uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; +// uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); +// LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! +// LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + +// HT->pause(); +// HT->refresh(); +// HT->resume(); +// return HT; +// } + + +// // returns 0 if each pair of pwm channels has same channel +// // returns 1 all the channels belong to the same timer - hardware inverted channels +// // returns -1 if neither - avoid configuring - error!!! +// int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ +// PinName nameAH = digitalPinToPinName(pinA_h); +// PinName nameAL = digitalPinToPinName(pinA_l); +// PinName nameBH = digitalPinToPinName(pinB_h); +// PinName nameBL = digitalPinToPinName(pinB_l); +// PinName nameCH = digitalPinToPinName(pinC_h); +// PinName nameCL = digitalPinToPinName(pinC_l); +// int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); +// int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); +// int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); +// int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); +// int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); +// int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); +// if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) +// return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer +// else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) +// return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer +// else +// return _ERROR_6PWM; // might be error avoid configuration +// } + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + motor_slot[slot_index].id = pinA; + + core_util_critical_section_enter(); + _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); + // allign the timers + _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1])); + core_util_critical_section_exit(); + slot_index++; +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + motor_slot[slot_index].id = pinA; + core_util_critical_section_enter(); + _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); + // allign the timers + _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2])); + core_util_critical_section_exit(); + slot_index++; +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + motor_slot[slot_index].id = pinA; + core_util_critical_section_enter(); + _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency); + _pwm_init(&(motor_slot[slot_index].pins[3]), pinD, (long)pwm_frequency); + // allign the timers + _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2]), &(motor_slot[slot_index].pins[3])); + core_util_critical_section_exit(); + slot_index++; +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +//- hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ +for(int i=0; i1.0) ret = 1.0; + if (ret>1.0) ret = 1.0f; return ret; } diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index cf8db836..ab41b745 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -163,7 +163,7 @@ void configureSAMDClock() { while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured clock..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); #endif } } @@ -217,8 +217,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tc->COUNT8.CTRLA.bit.ENABLE = 1; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } else { @@ -255,13 +255,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print(" Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } } @@ -288,13 +288,13 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 69c44848..b59a43fc 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -31,8 +31,8 @@ struct wo_association WO_associations[] = { { PORTA, 9, TC0_CH1, 1, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTA, 10, TC1_CH0, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTA, 11, TC1_CH1, 1, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTB, 10, TC5_CH0, 0, TCC0_CH0, 4, TCC1_CH0, 0}, //? - { PORTB, 11, TC5_CH1, 1, TCC0_CH1, 5, TCC1_CH1, 1}, //? + { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4, TCC1_CH0, 0}, + { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5, TCC1_CH1, 1}, { PORTB, 12, TC4_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 0}, { PORTB, 13, TC4_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 1}, { PORTB, 14, TC5_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 2}, @@ -40,34 +40,34 @@ struct wo_association WO_associations[] = { { PORTD, 8, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, { PORTD, 9, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, { PORTD, 10, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, - { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH0, 4, NOT_ON_TIMER, 0}, //? - { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH1, 5, NOT_ON_TIMER, 0}, //? + { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, + { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, { PORTC, 10, NOT_ON_TIMER, 0, TCC0_CH0, 0, TCC1_CH0, 4}, { PORTC, 11, NOT_ON_TIMER, 0, TCC0_CH1, 1, TCC1_CH1, 5}, { PORTC, 12, NOT_ON_TIMER, 0, TCC0_CH2, 2, TCC1_CH2, 6}, { PORTC, 13, NOT_ON_TIMER, 0, TCC0_CH3, 3, TCC1_CH3, 7}, - { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH0, 4, TCC1_CH0, 0}, //? - { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH1, 5, TCC1_CH1, 1}, //? - { PORTA, 12, TC2_CH0, 0, TCC0_CH2, 6, TCC1_CH2, 2}, - { PORTA, 13, TC2_CH1, 1, TCC0_CH3, 7, TCC1_CH3, 3}, - { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, //? - { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, //? - { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH0, 4}, - { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH1, 5}, - { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH2, 6}, - { PORTA, 19, TC3_CH1, 1, TCC1_CH3, 3, TCC0_CH3, 7}, + { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH4, 4, TCC1_CH0, 0}, + { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH5, 5, TCC1_CH1, 1}, + { PORTA, 12, TC2_CH0, 0, TCC0_CH0, 6, TCC1_CH2, 2}, + { PORTA, 13, TC2_CH1, 1, TCC0_CH1, 7, TCC1_CH3, 3}, + { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, + { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, + { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH4, 4}, + { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH5, 5}, + { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH0, 6}, + { PORTA, 19, TC3_CH1, 1, TCC1_CH3, 3, TCC0_CH1, 7}, { PORTC, 16, NOT_ON_TIMER, 0, TCC0_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0 { PORTC, 17, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1 { PORTC, 18, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2 { PORTC, 19, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, - { PORTC, 20, NOT_ON_TIMER, 0, TCC0_CH0, 4, NOT_ON_TIMER, 0}, - { PORTC, 21, NOT_ON_TIMER, 0, TCC0_CH1, 5, NOT_ON_TIMER, 0}, - { PORTC, 22, NOT_ON_TIMER, 0, TCC0_CH2, 6, NOT_ON_TIMER, 0}, - { PORTC, 23, NOT_ON_TIMER, 0, TCC0_CH3, 7, NOT_ON_TIMER, 0}, + { PORTC, 20, NOT_ON_TIMER, 0, TCC0_CH4, 4, NOT_ON_TIMER, 0}, + { PORTC, 21, NOT_ON_TIMER, 0, TCC0_CH5, 5, NOT_ON_TIMER, 0}, + { PORTC, 22, NOT_ON_TIMER, 0, TCC0_CH0, 6, NOT_ON_TIMER, 0}, + { PORTC, 23, NOT_ON_TIMER, 0, TCC0_CH1, 7, NOT_ON_TIMER, 0}, { PORTD, 20, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0}, { PORTD, 21, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0}, - { PORTB, 16, TC6_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 4}, - { PORTB, 17, TC6_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 5}, + { PORTB, 16, TC6_CH0, 0, TCC3_CH0, 0, TCC0_CH4, 4}, + { PORTB, 17, TC6_CH1, 1, TCC3_CH1, 1, TCC0_CH5, 5}, { PORTB, 18, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0 { PORTB, 19, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1 { PORTB, 20, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2 @@ -89,8 +89,8 @@ struct wo_association WO_associations[] = { // PC24-PC28, PA27, RESET -> no TC/TCC peripherals { PORTA, 30, TC6_CH0, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0}, { PORTA, 31, TC6_CH1, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0}, - { PORTB, 30, TC0_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 6}, - { PORTB, 31, TC0_CH1, 1, TCC4_CH1, 1, TCC0_CH3, 7}, + { PORTB, 30, TC0_CH0, 0, TCC4_CH0, 0, TCC0_CH0, 6}, + { PORTB, 31, TC0_CH1, 1, TCC4_CH1, 1, TCC0_CH1, 7}, // PC30, PC31 -> no TC/TCC peripherals { PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, { PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, @@ -188,7 +188,7 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<SYNCBUSY.bit.ENABLE == 1 ); // wait for sync #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("(Re-)Initialized TCC "); - Serial.print(tccConfig.tcc.tccn); - Serial.print("-"); - Serial.print(tccConfig.tcc.chan); - Serial.print("["); - Serial.print(tccConfig.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { @@ -280,8 +280,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Initialized TC "); - Serial.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index fdbd7a7f..e1d38a80 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -287,7 +287,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -297,9 +297,9 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 2)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,7 +308,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... attachTCC(tccConfs[1]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -319,7 +319,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -377,7 +377,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -388,9 +388,9 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 3)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -401,7 +401,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -413,7 +413,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif } @@ -445,7 +445,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return; } @@ -457,9 +457,9 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 4)); - Serial.println(")"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -472,7 +472,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -485,7 +485,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return; // Someone with a stepper-setup who can test it? @@ -539,7 +539,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); #endif return -1; } @@ -553,7 +553,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Found configuration: "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -573,7 +573,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if (!allAttached) return -1; #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); #endif // set up clock - if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API @@ -590,7 +590,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) configureTCC(pinCl, pwm_frequency, true, -1.0); #ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif return 0; @@ -704,7 +704,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { // low-side on a different pin of same TCC - do dead-time in software... float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); } @@ -715,7 +715,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i tcc2 = getTccPinConfiguration(pinB_l); if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); } @@ -726,7 +726,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i tcc2 = getTccPinConfiguration(pinC_l); if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); } @@ -746,85 +746,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - Serial.println(); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - Serial.print(": TC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); else - Serial.print(": TCC Peripheral"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - Serial.print(" E "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; case PIO_TIMER_ALT: - Serial.print(" F "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - Serial.print(" G "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; #endif default: - Serial.print(" ? "); break; + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; } if (info.tcc.tccn>=0) { - Serial.print(info.tcc.tccn); - Serial.print("-"); - Serial.print(info.tcc.chan); - Serial.print("["); - Serial.print(info.wo); - Serial.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); } else - Serial.println(" None"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); } @@ -866,5 +866,3 @@ void printTCCConfiguration(tccConfiguration& info) { #endif #endif - - diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index f0fd5460..7faf7442 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -4,7 +4,10 @@ // uncomment to enable debug output to Serial port -#define SIMPLEFOC_SAMD_DEBUG +// #define SIMPLEFOC_SAMD_DEBUG +#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) +#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial +#endif #include "../hardware_api.h" diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 3f213711..540dd6c2 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -4,7 +4,7 @@ #if defined(_STM32_DEF_) // default pwm parameters #define _PWM_RESOLUTION 12 // 12bit -#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_RANGE 4095.0 // 2^12 -1 = 4095 #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz @@ -27,12 +27,12 @@ void _setPwm(int ulPin, uint32_t value, int resolution) } -// init pin pwm +// init pin pwm HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) { PinName pin = digitalPinToPinName(ulPin); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - + uint32_t index = get_timer_index(Instance); if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); @@ -61,7 +61,7 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) PinName pin = digitalPinToPinName(ulPin); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); uint32_t index = get_timer_index(Instance); - + if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; @@ -70,10 +70,14 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) sConfigOC.OCMode = TIM_OCMODE_PWM2; sConfigOC.Pulse = 100; sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; - sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; +#if defined(TIM_OCIDLESTATE_SET) + sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; +#endif +#if defined(TIM_OCNIDLESTATE_RESET) + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#endif uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); } @@ -121,6 +125,8 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,Ha // configure hardware 6pwm interface only one timer with inverted channels HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) { + +#if !defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface PinName uhPinName = digitalPinToPinName(pinA_h); PinName ulPinName = digitalPinToPinName(pinA_l); PinName vhPinName = digitalPinToPinName(pinB_h); @@ -129,17 +135,18 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in PinName wlPinName = digitalPinToPinName(pinC_l); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); - + uint32_t index = get_timer_index(Instance); - + if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); } HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - + HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); @@ -148,20 +155,31 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); // dead time is set in nanoseconds - uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone; + uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + // Set Trigger out for DMA transfer + LL_TIM_SetTriggerOutput(HT->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + HT->pause(); HT->refresh(); - HT->resume(); + + // adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs. + HT->getHandle()->Instance->CR1 |= TIM_CR1_DIR; + HT->getHandle()->Instance->CNT = TIM1->ARR; + + HT->resume(); return HT; +#else + return nullptr; // return nothing +#endif } // returns 0 if each pair of pwm channels has same channel -// returns 1 all the channels belong to the same timer - hardware inverted channels +// returns 1 all the channels belong to the same timer - hardware inverted channels // returns -1 if neither - avoid configuring - error!!! int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ PinName nameAH = digitalPinToPinName(pinA_h); @@ -176,12 +194,22 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); + +#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface + + if(tim1 == tim2 && tim3==tim4 && tim5==tim6) + return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer + else + return _ERROR_6PWM; // might be error avoid configuration +#else // the rest of stm32 boards + if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) - return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer + return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer else return _ERROR_6PWM; // might be error avoid configuration +#endif } @@ -230,7 +258,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); } @@ -315,11 +343,11 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); break; case _SOFTWARE_6PWM: - _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); break; } diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 8ec19643..67bb0ad1 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_api.h" +#include "../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) @@ -8,7 +8,7 @@ // configure High PWM frequency void _setHighFrequency(const long freq, const int pin){ analogWrite(pin, 0); - analogWriteFrequency(pin, freq); + analogWriteFrequency(pin, freq); } @@ -42,25 +42,25 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); - _setHighFrequency(pwm_frequency, pinD); + _setHighFrequency(pwm_frequency, pinD); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware @@ -68,24 +68,10 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - return -1; -} - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - return; -} #endif \ No newline at end of file diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 001330ba..7c164878 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -98,12 +98,34 @@ void Encoder::handleIndex() { } } + +void Encoder::update() { + // do nothing for Encoder +} + /* Shaft angle calculation */ +float Encoder::getSensorAngle(){ + return getAngle(); +} +// TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost +float Encoder::getMechanicalAngle(){ + return _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); +} + float Encoder::getAngle(){ return _2PI * (pulse_counter) / ((float)cpr); } +double Encoder::getPreciseAngle(){ + return _2PI * (pulse_counter) / ((double)cpr); +} +int32_t Encoder::getFullRotations(){ + return pulse_counter / (int)cpr; +} + + + /* Shaft velocity calculation function using mixed time and frequency measurement technique @@ -112,12 +134,12 @@ float Encoder::getVelocity(){ // timestamp long timestamp_us = _micros(); // sampling time calculation - float Ts = (timestamp_us - prev_timestamp_us) * 1e-6; + float Ts = (timestamp_us - prev_timestamp_us) * 1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // time from last impulse - float Th = (timestamp_us - pulse_timestamp) * 1e-6; + float Th = (timestamp_us - pulse_timestamp) * 1e-6f; long dN = pulse_counter - prev_pulse_counter; // Pulse per second calculation (Eq.3.) @@ -129,8 +151,8 @@ float Encoder::getVelocity(){ float dt = Ts + prev_Th - Th; pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; - // if more than 0.05 passed in between impulses - if ( Th > 0.1) pulse_per_second = 0; + // if more than 0.05f passed in between impulses + if ( Th > 0.1f) pulse_per_second = 0; // velocity calculation float velocity = pulse_per_second / ((float)cpr) * (_2PI); diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 7a0f7a40..3e775338 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -27,7 +27,7 @@ class Encoder: public Sensor{ Encoder(int encA, int encB , float ppr, int index = 0); /** encoder initialise pins */ - void init(); + void init() override; /** * function enabling hardware interrupts for the encoder channels with provided callback functions * if callback is not provided then the interrupt is not enabled @@ -60,9 +60,15 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ - float getAngle() override; + float getSensorAngle() override; + float getMechanicalAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; + float getAngle() override; + double getPreciseAngle() override; + int32_t getFullRotations() override; + virtual void update() override; + /** * returns 0 if it does need search for absolute zero * 0 - encoder without index diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 2ceb22cf..467ba9e7 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -7,14 +7,14 @@ - pp - pole pairs */ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ - + // hardware pins pinA = _hallA; pinB = _hallB; pinC = _hallC; // hall has 6 segments per electrical revolution - cpr = _pp * 6; + cpr = _pp * 6; // extern pullup as default pullup = Pullup::USE_EXTERN; @@ -40,12 +40,12 @@ void HallSensor::handleC() { /** * Updates the state and sector following an interrupt - */ + */ void HallSensor::updateState() { long new_pulse_timestamp = _micros(); int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2); - + // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed if (new_hall_state == hall_state) { return; @@ -74,7 +74,7 @@ void HallSensor::updateState() { } else { pulse_diff = 0; } - + pulse_timestamp = new_pulse_timestamp; total_interrupts++; old_direction = direction; @@ -87,16 +87,25 @@ void HallSensor::updateState() { * ... // for debug or call driver directly? * } * sensor.attachSectorCallback(onSectorChange); - */ + */ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { onSectorChange = _onSectorChange; } + + +float HallSensor::getSensorAngle() { + return getAngle(); +} + + + /* Shaft angle calculation + TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost */ -float HallSensor::getAngle() { - return ((electric_rotations * 6 + electric_sector) / cpr) * _2PI ; +float HallSensor::getMechanicalAngle() { + return ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; } /* @@ -104,14 +113,34 @@ float HallSensor::getAngle() { function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ - if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old + if (pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + return direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); } } + + +float HallSensor::getAngle() { + return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; +} + + +double HallSensor::getPreciseAngle() { + return ((double)(electric_rotations * 6 + electric_sector) / (double)cpr) * (double)_2PI ; +} + + +int32_t HallSensor::getFullRotations() { + return (int32_t)((electric_rotations * 6 + electric_sector) / cpr); +} + + + + + // HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ @@ -134,7 +163,7 @@ void HallSensor::init(){ B_active = digitalRead(pinB); C_active = digitalRead(pinC); updateState(); - + pulse_timestamp = _micros(); } @@ -149,4 +178,3 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); } - diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index c836cc5d..9e80ed2b 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -50,13 +50,17 @@ class HallSensor: public Sensor{ // HallSensor configuration Pullup pullup; //!< Configuration parameter internal or external pullups - float cpr;//!< HallSensor cpr number + int cpr;//!< HallSensor cpr number // Abstract functions of the Sensor class implementation /** get current angle (rad) */ + float getSensorAngle() override; + float getMechanicalAngle() override; float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; + double getPreciseAngle() override; + int32_t getFullRotations() override; // whether last step was CW (+1) or CCW (-1). Direction direction; @@ -77,7 +81,7 @@ class HallSensor: public Sensor{ Direction decodeDirection(int oldState, int newState); void updateState(); - volatile long pulse_timestamp;//!< last impulse timestamp in us + volatile unsigned long pulse_timestamp;//!< last impulse timestamp in us volatile int A_active; //!< current active states of A channel volatile int B_active; //!< current active states of B channel volatile int C_active; //!< current active states of C channel diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 35f6d73c..6d881657 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -6,7 +6,7 @@ * @param _max_raw_count the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note: For ESP32 (with 12bit ADC the value will be nearer 4096) */ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_count, int _max_raw_count){ - + pinAnalog = _pinAnalog; cpr = _max_raw_count - _min_raw_count; @@ -23,47 +23,15 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun void MagneticSensorAnalog::init(){ - - // velocity calculation init - angle_prev = 0; - velocity_calc_timestamp = _micros(); - - // full rotations tracking number - full_rotation_offset = 0; - raw_count_prev = getRawCount(); + raw_count = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorAnalog::getAngle(){ +float MagneticSensorAnalog::getSensorAngle(){ // raw data from the sensor - raw_count = getRawCount(); - - int delta = raw_count - raw_count_prev; - // if overflow happened track it as full rotation - if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - - float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; - - // calculate velocity here - long now = _micros(); - float Ts = ( now - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - velocity = (angle - angle_prev)/Ts; - - // save variables for future pass - raw_count_prev = raw_count; - angle_prev = angle; - velocity_calc_timestamp = now; - - return angle; -} - -// Shaft velocity calculation -float MagneticSensorAnalog::getVelocity(){ - // TODO: Refactor?: to avoid angle being called twice, velocity is pre-calculted during getAngle - return velocity; + raw_count = getRawCount(); + return ( (float) (raw_count) / (float)cpr) * _2PI; } // function reading the raw counter of the magnetic sensor diff --git a/src/sensors/MagneticSensorAnalog.h b/src/sensors/MagneticSensorAnalog.h index 12f78c76..6f787b95 100644 --- a/src/sensors/MagneticSensorAnalog.h +++ b/src/sensors/MagneticSensorAnalog.h @@ -29,10 +29,7 @@ class MagneticSensorAnalog: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getAngle() override; - /** get current angular velocity (rad/s) **/ - float getVelocity() override; - + float getSensorAngle() override; /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ int raw_count; @@ -48,16 +45,6 @@ class MagneticSensorAnalog: public Sensor{ */ int getRawCount(); - // total angle tracking variables - float full_rotation_offset; //!begin(); - - // velocity calculation init - angle_prev = 0; - velocity_calc_timestamp = _micros(); - - // full rotations tracking number - full_rotation_offset = 0; - angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorI2C::getAngle(){ - // raw data from the sensor - float angle_data = getRawCount(); - - // tracking the number of rotations - // in order to expand angle range form [0,2PI] - // to basically infinity - float d_angle = angle_data - angle_data_prev; - // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; - // save the current angle value for the next steps - // in order to know if overflow happened - angle_data_prev = angle_data; - // return the full angle +float MagneticSensorI2C::getSensorAngle(){ // (number of full rotations)*2PI + current sensor angle - return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; + return ( getRawCount() / (float)cpr) * _2PI ; } -// Shaft velocity calculation -float MagneticSensorI2C::getVelocity(){ - // calculate sample time - unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - - // current angle - float angle_c = getAngle(); - // velocity calculation - float vel = (angle_c - angle_prev)/Ts; - - // save variables for future pass - angle_prev = angle_c; - velocity_calc_timestamp = now_us; - return vel; -} // function reading the raw counter of the magnetic sensor @@ -119,7 +80,7 @@ int MagneticSensorI2C::getRawCount(){ return (int)MagneticSensorI2C::read(angle_register_msb); } -// I2C functions +// I2C functions /* * Read a register from the sensor * Takes the address of the register as a uint8_t @@ -133,14 +94,14 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { wire->beginTransmission(chip_address); wire->write(angle_reg_msb); wire->endTransmission(false); - + // read the data msb and lsb wire->requestFrom(chip_address, (uint8_t)2); for (byte i=0; i < 2; i++) { readArray[i] = wire->read(); } - // depending on the sensor architecture there are different combinations of + // depending on the sensor architecture there are different combinations of // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB // AS5048 uses 0..5 LSB and 6..13 MSB @@ -151,7 +112,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { /* * Checks whether other devices have locked the bus. Can clear SDA locks. -* This should be called before sensor.init() on devices that suffer i2c slaves locking sda +* This should be called before sensor.init() on devices that suffer i2c slaves locking sda * e.g some stm32 boards with AS5600 chips * Takes the sda_pin and scl_pin * Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW @@ -160,7 +121,7 @@ int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { pinMode(scl_pin, INPUT_PULLUP); pinMode(sda_pin, INPUT_PULLUP); - delay(250); + delay(250); if (digitalRead(scl_pin) == LOW) { // Someone else has claimed master!"); @@ -179,15 +140,15 @@ int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { } pinMode(sda_pin, INPUT); delayMicroseconds(20); - if (digitalRead(sda_pin) == LOW) { + if (digitalRead(sda_pin) == LOW) { // SDA still blocked - return 2; - } + return 2; + } _delay(1000); } // SDA is clear (HIGH) pinMode(sda_pin, INPUT); pinMode(scl_pin, INPUT); - - return 0; + + return 0; } diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index 9966d6eb..fa7ec96b 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -46,12 +46,10 @@ class MagneticSensorI2C: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getAngle() override; - /** get current angular velocity (rad/s) **/ - float getVelocity() override; + float getSensorAngle() override; /** experimental function to check and fix SDA locked LOW issues */ - int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); + int checkBus(byte sda_pin , byte scl_pin ); private: float cpr; //!< Maximum range of the magnetic sensor @@ -72,15 +70,7 @@ class MagneticSensorI2C: public Sensor{ * it uses angle_register variable */ int getRawCount(); - - // total angle tracking variables - float full_rotation_offset; //! (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - - float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; - - // calculate velocity here - long now = _micros(); - float Ts = (now - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - velocity = (angle - angle_prev)/Ts; - - // save variables for future pass - raw_count_prev = raw_count; - angle_prev = angle; - velocity_calc_timestamp = now; - - return angle; + return( (float) (raw_count) / (float)cpr) * _2PI; } -// get velocity (rad/s) -float MagneticSensorPWM::getVelocity(){ - return velocity; -} // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ @@ -80,7 +49,7 @@ int MagneticSensorPWM::getRawCount(){ void MagneticSensorPWM::handlePWM() { // unsigned long now_us = ticks(); unsigned long now_us = _micros(); - + // if falling edge, calculate the pulse length if (!digitalRead(pinPWM)) pulse_length_us = now_us - last_call_us; @@ -95,8 +64,6 @@ void MagneticSensorPWM::enableInterrupt(void (*doPWM)()){ // declare it's interrupt based is_interrupt_based = true; - #if !defined(__AVR_ATmega328P__) && !defined(__AVR_ATmega168__) && !defined(__AVR_ATmega328PB__) && !defined(__AVR_ATmega2560__) // if mcu is not atmega328 && if mcu is not atmega2560 - // enable interrupts on pwm input pin - attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE); - #endif + // enable interrupts on pwm input pin + attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE); } \ No newline at end of file diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 32d6daee..b42b23d6 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -23,9 +23,7 @@ class MagneticSensorPWM: public Sensor{ int pinPWM; // get current angle (rad) - float getAngle() override; - // get current angular velocity (rad/s) - float getVelocity() override; + float getSensorAngle() override; // pwm handler void handlePWM(); @@ -50,15 +48,6 @@ class MagneticSensorPWM: public Sensor{ */ int getRawCount(); - // total angle tracking variables - float full_rotation_offset; //!begin(); -#ifndef ESP_H // if not ESP32 board - spi->setBitOrder(MSBFIRST); // Set the SPI_1 bit order - spi->setDataMode(spi_mode) ; - spi->setClockDivider(SPI_CLOCK_DIV8); -#endif - + // do any architectures need to set the clock divider for SPI? Why was this in the code? + //spi->setClockDivider(SPI_CLOCK_DIV8); digitalWrite(chip_select_pin, HIGH); - // velocity calculation init - angle_prev = 0; - velocity_calc_timestamp = _micros(); - - // full rotations tracking number - full_rotation_offset = 0; - angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorSPI::getAngle(){ - // raw data from the sensor - float angle_data = getRawCount(); - - // tracking the number of rotations - // in order to expand angle range form [0,2PI] - // to basically infinity - float d_angle = angle_data - angle_data_prev; - // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; - // save the current angle value for the next steps - // in order to know if overflow happened - angle_data_prev = angle_data; - - // return the full angle - // (number of full rotations)*2PI + current sensor angle - return full_rotation_offset + ( angle_data / (float)cpr) * _2PI; +float MagneticSensorSPI::getSensorAngle(){ + return (getRawCount() / (float)cpr) * _2PI; } -// Shaft velocity calculation -float MagneticSensorSPI::getVelocity(){ - // calculate sample time - unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; - // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; - - // current angle - float angle_c = getAngle(); - // velocity calculation - float vel = (angle_c - angle_prev)/Ts; - - // save variables for future pass - angle_prev = angle_c; - velocity_calc_timestamp = now_us; - return vel; -} - - // function reading the raw counter of the magnetic sensor int MagneticSensorSPI::getRawCount(){ return (int)MagneticSensorSPI::read(angle_register); @@ -170,36 +120,28 @@ word MagneticSensorSPI::read(word angle_register){ command |= ((word)spiCalcEvenParity(command) << command_parity_bit); } -#if !defined(_STM32_DEF_) // if not stm chips //SPI - begin transaction spi->beginTransaction(settings); -#endif //Send the command digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); - digitalWrite(chip_select_pin,HIGH); -#if defined( ESP_H ) // if ESP32 board - delayMicroseconds(50); +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) // if ESP32 board + delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else - delayMicroseconds(10); + delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 #endif - + //Now read the response digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); word register_value = spi->transfer16(0x00); digitalWrite(chip_select_pin, HIGH); - digitalWrite(chip_select_pin,HIGH); -#if !defined(_STM32_DEF_) // if not stm chips //SPI - end transaction spi->endTransaction(); -#endif - + register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word const static word data_mask = 0xFFFF >> (16 - bit_resolution); diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index 77fcde4d..33aad3f3 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -44,9 +44,7 @@ class MagneticSensorSPI: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getAngle() override; - /** get current angular velocity (rad/s) **/ - float getVelocity() override; + float getSensorAngle() override; // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3 int spi_mode; @@ -74,14 +72,6 @@ class MagneticSensorSPI: public Sensor{ * it uses angle_register variable */ int getRawCount(); - - // total angle tracking variables - float full_rotation_offset; //!