From 241e8c04dbd4ee8ea3476b166d6e0684b1cd08e1 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 13 Jan 2024 20:25:13 -0600 Subject: [PATCH 1/6] LinearHall improvements and move to main repository Changes compared to the original pull request in the drivers repository https://github.com/simplefoc/Arduino-FOC-drivers/pull/12 1. Added a version of init which turns the motor one revolution to find the center values of the sensors. 2. Moved the calls to analogRead into a weakly bound function ReadLinearHalls so it can be overridden with custom ADC code on platforms with poor analogRead performance. 3. Commented out the pinMode calls in init, which makes it possible to pass in ADC channel numbers for custom ReadLinearHalls to use without having to remap them every update. 4. Changed to use the much faster _atan2 function that was added to foc_utils recently. 5. Added examples. --- .../angle_control/angle_control.ino | 117 ++++++++++++++++++ .../voltage_control/voltage_control.ino | 96 ++++++++++++++ .../velocity_control/velocity_control.ino | 109 ++++++++++++++++ src/SimpleFOC.h | 1 + src/sensors/LinearHall.cpp | 109 ++++++++++++++++ src/sensors/LinearHall.h | 45 +++++++ 6 files changed, 477 insertions(+) create mode 100644 examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino create mode 100644 examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino create mode 100644 examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino create mode 100644 src/sensors/LinearHall.cpp create mode 100644 src/sensors/LinearHall.h diff --git a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino new file mode 100644 index 00000000..8179008d --- /dev/null +++ b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino @@ -0,0 +1,117 @@ +/** + * + * Position/angle motion control example + * Steps: + * 1) Configure the motor and hall sensor + * 2) Run the code + * 3) Set the target angle (in radians) from serial terminal + */ +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +LinearHall sensor = LinearHall(A0, A1, 11); + +// 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() { + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + + // 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::angle; + + // contoller configuration + // default parameters in defaults.h + + // velocity PI controller parameters + motor.PID_velocity.P = 0.2f; + motor.PID_velocity.I = 2; + motor.PID_velocity.D = 0; + // 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.01f; + + // 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(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor 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); +} + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // 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_angle); + + // function intended to be used with serial plotter to monitor motor variables + // significantly slowing the execution down!!!! + // motor.monitor(); + + // user communication + command.run(); +} diff --git a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino new file mode 100644 index 00000000..e646849a --- /dev/null +++ b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino @@ -0,0 +1,96 @@ +/** + * + * Torque control example using voltage control loop. + * + * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers + * you a way to control motor torque by setting the voltage to the motor instead of the current. + * + * This makes the BLDC motor effectively a DC motor, and you can use it in a same way. + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +LinearHall sensor = LinearHall(A0, A1, 11); + + +// voltage set point variable +float target_voltage = 2; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } + +void setup() { + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // aligning voltage + motor.voltage_sensor_align = 3; + + // choose FOC modulation (optional) + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target voltage using serial terminal:")); + _delay(1000); +} + + +void loop() { + + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // 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); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino new file mode 100644 index 00000000..870a0899 --- /dev/null +++ b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino @@ -0,0 +1,109 @@ +/** + * + * Velocity motion control example + * Steps: + * 1) Configure the motor and sensor + * 2) Run the code + * 3) Set the target velocity (in radians per second) from serial terminal + */ +#include + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// hall sensor instance +LinearHall sensor = LinearHall(A0, A1, 11); + +// velocity set point variable +float target_velocity = 0; +// instantiate the commander +Commander command = Commander(Serial); +void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } + +void setup() { + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + + // aligning voltage [V] + motor.voltage_sensor_align = 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.2f; + motor.PID_velocity.I = 2; + motor.PID_velocity.D = 0; + // 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.01f; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + Serial.print("LinearHall centerA: "); + Serial.print(sensor.centerA); + Serial.print(", centerB: "); + Serial.println(sensor.centerB); + // link the motor to the sensor + motor.linkSensor(&sensor); + // align sensor and start FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); + _delay(1000); +} + + +void loop() { + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or voltage (defined in motor.controller) + // 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_velocity); + + // 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/src/SimpleFOC.h b/src/SimpleFOC.h index 4e6815e5..9e1c80c6 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -104,6 +104,7 @@ void loop() { #include "sensors/MagneticSensorAnalog.h" #include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" +#include "sensors/LinearHall.h" #include "sensors/GenericSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp new file mode 100644 index 00000000..36303aaa --- /dev/null +++ b/src/sensors/LinearHall.cpp @@ -0,0 +1,109 @@ +#include "LinearHall.h" + +// This function can be overridden with custom ADC code on platforms with poor analogRead performance. +__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) +{ + *a = analogRead(hallA); + *b = analogRead(hallB); +} + +LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ + centerA = 512; + centerB = 512; + pinA = _hallA; + pinB = _hallB; + pp = _pp; +} + +float LinearHall::getSensorAngle() { + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + //offset readings using center values, then compute angle + float reading = _atan2(lastA - centerA, lastB - centerB); + + //handle rollover logic between each electrical revolution of the motor + if (reading > prev_reading) { + if (reading - prev_reading >= PI) { + if (electrical_rev - 1 < 0) { + electrical_rev = pp - 1; + } else { + electrical_rev = electrical_rev - 1; + } + } + } else if (reading < prev_reading) { + if (prev_reading - reading >= PI) { + if (electrical_rev + 1 >= pp) { + electrical_rev = 0; + } else { + electrical_rev = electrical_rev + 1; + } + } + } + + //convert result from electrical angle and electrical revolution count to shaft angle in radians + float result = (reading + PI) / _2PI; + result = _2PI * (result + electrical_rev) / pp; + + //update previous reading for rollover handling + prev_reading = reading; + return result; +} + +void LinearHall::init(int _centerA, int _centerB) { + // Skip configuring the pins here because they normally default to input anyway, and + // this makes it possible to use ADC channel numbers instead of pin numbers when using + // custom implementation of ReadLinearHalls, to avoid having to remap them every update. + // If pins do need to be configured, it can be done by user code before calling init. + //pinMode(pinA, INPUT); + //pinMode(pinB, INPUT); + + centerA = _centerA; + centerB = _centerB; + + //establish initial reading for rollover handling + electrical_rev = 0; + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + prev_reading = _atan2(lastA - centerA, lastB - centerB); +} + +void LinearHall::init(FOCMotor *motor) { + if (!motor->enabled) { + SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC."); + return; + } + + //pinMode(pinA, INPUT); + //pinMode(pinB, INPUT); + + int minA, maxA, minB, maxB; + + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + minA = maxA = centerA = lastA; + minB = maxB = centerB = lastB; + + // move one mechanical revolution forward + for (int i = 0; i <= 2000; i++) + { + float angle = _3PI_2 + _2PI * i * pp / 2000.0f; + motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle); + + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + + if (lastA < minA) + minA = lastA; + if (lastA > maxA) + maxA = lastA; + centerA = (minA + maxA) / 2; + + if (lastB < minB) + minB = lastB; + if (lastB > maxB) + maxB = lastB; + centerB = (minB + maxB) / 2; + + _delay(2); + } + + //establish initial reading for rollover handling + electrical_rev = 0; + prev_reading = _atan2(lastA - centerA, lastB - centerB); +} diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h new file mode 100644 index 00000000..f05b7b2b --- /dev/null +++ b/src/sensors/LinearHall.h @@ -0,0 +1,45 @@ +#ifndef LINEAR_HALL_SENSOR_LIB_H +#define LINEAR_HALL_SENSOR_LIB_H + +#include + +class FOCMotor; +void ReadLinearHalls(int hallA, int hallB, int *a, int *b); + +/** + * This sensor class is for two linear hall effect sensors such as 49E, which are + * positioned 90 electrical degrees apart (if one is centered on a rotor magnet, + * the other is half way between rotor magnets). + * It can also be used for a single magnet mounted to the motor shaft (set pp to 1). + * + * For more information, see this forum thread and PDF + * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959 + * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 + */ +class LinearHall: public Sensor{ + public: + LinearHall(int hallA, int hallB, int pp); + + void init(int centerA, int centerB); // Initialize without moving motor + void init(FOCMotor *motor); // Move motor to find center values + + // 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. + float getSensorAngle() override; + + int centerA; + int centerB; + int lastA, lastB; + + private: + int pinA; + int pinB; + int pp; + int electrical_rev; + float prev_reading; +}; + +#endif From aeb923824dd2366ca341f2cabcc1cabf063d814c Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 13 Jan 2024 20:40:04 -0600 Subject: [PATCH 2/6] Minor formatting/comment edits --- src/sensors/LinearHall.cpp | 5 +++-- src/sensors/LinearHall.h | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp index 36303aaa..96cfae3e 100644 --- a/src/sensors/LinearHall.cpp +++ b/src/sensors/LinearHall.cpp @@ -3,8 +3,8 @@ // This function can be overridden with custom ADC code on platforms with poor analogRead performance. __attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) { - *a = analogRead(hallA); - *b = analogRead(hallB); + *a = analogRead(hallA); + *b = analogRead(hallB); } LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ @@ -71,6 +71,7 @@ void LinearHall::init(FOCMotor *motor) { return; } + // See comment in other version of init for why these are commented out //pinMode(pinA, INPUT); //pinMode(pinB, INPUT); diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h index f05b7b2b..d17035e4 100644 --- a/src/sensors/LinearHall.h +++ b/src/sensors/LinearHall.h @@ -3,7 +3,7 @@ #include -class FOCMotor; +// This function can be overridden with custom ADC code on platforms with poor analogRead performance. void ReadLinearHalls(int hallA, int hallB, int *a, int *b); /** @@ -21,7 +21,7 @@ class LinearHall: public Sensor{ LinearHall(int hallA, int hallB, int pp); void init(int centerA, int centerB); // Initialize without moving motor - void init(FOCMotor *motor); // Move motor to find center values + void init(class FOCMotor *motor); // Move motor to find center values // Get current shaft angle from the sensor hardware, and // return it as a float in radians, in the range 0 to 2PI. From 41de6b673ebb6528e90155f793019a72f47d8435 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:38:50 -0600 Subject: [PATCH 3/6] Moving LinearHall back to drivers repository As requested by runger --- src/sensors/LinearHall.cpp | 110 ------------------------------------- src/sensors/LinearHall.h | 45 --------------- 2 files changed, 155 deletions(-) delete mode 100644 src/sensors/LinearHall.cpp delete mode 100644 src/sensors/LinearHall.h diff --git a/src/sensors/LinearHall.cpp b/src/sensors/LinearHall.cpp deleted file mode 100644 index 96cfae3e..00000000 --- a/src/sensors/LinearHall.cpp +++ /dev/null @@ -1,110 +0,0 @@ -#include "LinearHall.h" - -// This function can be overridden with custom ADC code on platforms with poor analogRead performance. -__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) -{ - *a = analogRead(hallA); - *b = analogRead(hallB); -} - -LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ - centerA = 512; - centerB = 512; - pinA = _hallA; - pinB = _hallB; - pp = _pp; -} - -float LinearHall::getSensorAngle() { - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - //offset readings using center values, then compute angle - float reading = _atan2(lastA - centerA, lastB - centerB); - - //handle rollover logic between each electrical revolution of the motor - if (reading > prev_reading) { - if (reading - prev_reading >= PI) { - if (electrical_rev - 1 < 0) { - electrical_rev = pp - 1; - } else { - electrical_rev = electrical_rev - 1; - } - } - } else if (reading < prev_reading) { - if (prev_reading - reading >= PI) { - if (electrical_rev + 1 >= pp) { - electrical_rev = 0; - } else { - electrical_rev = electrical_rev + 1; - } - } - } - - //convert result from electrical angle and electrical revolution count to shaft angle in radians - float result = (reading + PI) / _2PI; - result = _2PI * (result + electrical_rev) / pp; - - //update previous reading for rollover handling - prev_reading = reading; - return result; -} - -void LinearHall::init(int _centerA, int _centerB) { - // Skip configuring the pins here because they normally default to input anyway, and - // this makes it possible to use ADC channel numbers instead of pin numbers when using - // custom implementation of ReadLinearHalls, to avoid having to remap them every update. - // If pins do need to be configured, it can be done by user code before calling init. - //pinMode(pinA, INPUT); - //pinMode(pinB, INPUT); - - centerA = _centerA; - centerB = _centerB; - - //establish initial reading for rollover handling - electrical_rev = 0; - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - prev_reading = _atan2(lastA - centerA, lastB - centerB); -} - -void LinearHall::init(FOCMotor *motor) { - if (!motor->enabled) { - SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC."); - return; - } - - // See comment in other version of init for why these are commented out - //pinMode(pinA, INPUT); - //pinMode(pinB, INPUT); - - int minA, maxA, minB, maxB; - - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - minA = maxA = centerA = lastA; - minB = maxB = centerB = lastB; - - // move one mechanical revolution forward - for (int i = 0; i <= 2000; i++) - { - float angle = _3PI_2 + _2PI * i * pp / 2000.0f; - motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle); - - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - - if (lastA < minA) - minA = lastA; - if (lastA > maxA) - maxA = lastA; - centerA = (minA + maxA) / 2; - - if (lastB < minB) - minB = lastB; - if (lastB > maxB) - maxB = lastB; - centerB = (minB + maxB) / 2; - - _delay(2); - } - - //establish initial reading for rollover handling - electrical_rev = 0; - prev_reading = _atan2(lastA - centerA, lastB - centerB); -} diff --git a/src/sensors/LinearHall.h b/src/sensors/LinearHall.h deleted file mode 100644 index d17035e4..00000000 --- a/src/sensors/LinearHall.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef LINEAR_HALL_SENSOR_LIB_H -#define LINEAR_HALL_SENSOR_LIB_H - -#include - -// This function can be overridden with custom ADC code on platforms with poor analogRead performance. -void ReadLinearHalls(int hallA, int hallB, int *a, int *b); - -/** - * This sensor class is for two linear hall effect sensors such as 49E, which are - * positioned 90 electrical degrees apart (if one is centered on a rotor magnet, - * the other is half way between rotor magnets). - * It can also be used for a single magnet mounted to the motor shaft (set pp to 1). - * - * For more information, see this forum thread and PDF - * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959 - * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 - */ -class LinearHall: public Sensor{ - public: - LinearHall(int hallA, int hallB, int pp); - - void init(int centerA, int centerB); // Initialize without moving motor - void init(class FOCMotor *motor); // Move motor to find center values - - // 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. - float getSensorAngle() override; - - int centerA; - int centerB; - int lastA, lastB; - - private: - int pinA; - int pinB; - int pp; - int electrical_rev; - float prev_reading; -}; - -#endif From 3a840213de36eddf5219e691b1f8482d5147c490 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:53:46 -0600 Subject: [PATCH 4/6] Moving LinearHall back to drivers repository --- .../angle_control/angle_control.ino | 117 ------------------ .../voltage_control/voltage_control.ino | 96 -------------- .../velocity_control/velocity_control.ino | 109 ---------------- 3 files changed, 322 deletions(-) delete mode 100644 examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino delete mode 100644 examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino delete mode 100644 examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino diff --git a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino deleted file mode 100644 index 8179008d..00000000 --- a/examples/motion_control/position_motion_control/linear_hall/angle_control/angle_control.ino +++ /dev/null @@ -1,117 +0,0 @@ -/** - * - * Position/angle motion control example - * Steps: - * 1) Configure the motor and hall sensor - * 2) Run the code - * 3) Set the target angle (in radians) from serial terminal - */ -#include - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor & driver instance -//StepperMotor motor = StepperMotor(50); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// hall sensor instance -LinearHall sensor = LinearHall(A0, A1, 11); - -// 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() { - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.init(); - // link the motor and the driver - motor.linkDriver(&driver); - - - // 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::angle; - - // contoller configuration - // default parameters in defaults.h - - // velocity PI controller parameters - motor.PID_velocity.P = 0.2f; - motor.PID_velocity.I = 2; - motor.PID_velocity.D = 0; - // 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.01f; - - // 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(); - // initialize sensor hardware. This moves the motor to find the min/max sensor readings and - // averages them to get the center values. The motor can't move until motor.init is called, and - // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. - // You can then take the values printed to the serial monitor and pass them to sensor.init to - // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init - // is called before or after motor.init. - sensor.init(&motor); - Serial.print("LinearHall centerA: "); - Serial.print(sensor.centerA); - Serial.print(", centerB: "); - Serial.println(sensor.centerB); - // link the motor to the sensor - motor.linkSensor(&sensor); - // align sensor 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); -} - -void loop() { - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // 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_angle); - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - // motor.monitor(); - - // user communication - command.run(); -} diff --git a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino deleted file mode 100644 index e646849a..00000000 --- a/examples/motion_control/torque_control/linear_hall/voltage_control/voltage_control.ino +++ /dev/null @@ -1,96 +0,0 @@ -/** - * - * Torque control example using voltage control loop. - * - * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers - * you a way to control motor torque by setting the voltage to the motor instead of the current. - * - * This makes the BLDC motor effectively a DC motor, and you can use it in a same way. - */ -#include - - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor & driver instance -//StepperMotor motor = StepperMotor(50); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// hall sensor instance -LinearHall sensor = LinearHall(A0, A1, 11); - - -// voltage set point variable -float target_voltage = 2; -// instantiate the commander -Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } - -void setup() { - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.init(); - // link driver - motor.linkDriver(&driver); - - // aligning voltage - motor.voltage_sensor_align = 3; - - // choose FOC modulation (optional) - motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - - // set motion control loop to be used - motor.controller = MotionControlType::torque; - - // use monitoring with serial - Serial.begin(115200); - // comment out if not needed - motor.useMonitoring(Serial); - - // initialize motor - motor.init(); - // initialize sensor hardware. This moves the motor to find the min/max sensor readings and - // averages them to get the center values. The motor can't move until motor.init is called, and - // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. - // You can then take the values printed to the serial monitor and pass them to sensor.init to - // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init - // is called before or after motor.init. - sensor.init(&motor); - Serial.print("LinearHall centerA: "); - Serial.print(sensor.centerA); - Serial.print(", centerB: "); - Serial.println(sensor.centerB); - // link the motor to the sensor - motor.linkSensor(&sensor); - // align sensor and start FOC - motor.initFOC(); - - // add target command T - command.add('T', doTarget, "target voltage"); - - Serial.println(F("Motor ready.")); - Serial.println(F("Set the target voltage using serial terminal:")); - _delay(1000); -} - - -void loop() { - - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // 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); - - // user communication - command.run(); -} \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino deleted file mode 100644 index 870a0899..00000000 --- a/examples/motion_control/velocity_motion_control/linear_hall/velocity_control/velocity_control.ino +++ /dev/null @@ -1,109 +0,0 @@ -/** - * - * Velocity motion control example - * Steps: - * 1) Configure the motor and sensor - * 2) Run the code - * 3) Set the target velocity (in radians per second) from serial terminal - */ -#include - -// BLDC motor & driver instance -BLDCMotor motor = BLDCMotor(11); -BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); -// Stepper motor & driver instance -//StepperMotor motor = StepperMotor(50); -//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); - -// hall sensor instance -LinearHall sensor = LinearHall(A0, A1, 11); - -// velocity set point variable -float target_velocity = 0; -// instantiate the commander -Commander command = Commander(Serial); -void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } - -void setup() { - - // driver config - // power supply voltage [V] - driver.voltage_power_supply = 12; - driver.init(); - // link the motor and the driver - motor.linkDriver(&driver); - - // aligning voltage [V] - motor.voltage_sensor_align = 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.2f; - motor.PID_velocity.I = 2; - motor.PID_velocity.D = 0; - // 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.01f; - - // use monitoring with serial - Serial.begin(115200); - // comment out if not needed - motor.useMonitoring(Serial); - - // initialize motor - motor.init(); - // initialize sensor hardware. This moves the motor to find the min/max sensor readings and - // averages them to get the center values. The motor can't move until motor.init is called, and - // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. - // You can then take the values printed to the serial monitor and pass them to sensor.init to - // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init - // is called before or after motor.init. - sensor.init(&motor); - Serial.print("LinearHall centerA: "); - Serial.print(sensor.centerA); - Serial.print(", centerB: "); - Serial.println(sensor.centerB); - // link the motor to the sensor - motor.linkSensor(&sensor); - // align sensor and start FOC - motor.initFOC(); - - // add target command T - command.add('T', doTarget, "target voltage"); - - Serial.println(F("Motor ready.")); - Serial.println(F("Set the target velocity using serial terminal:")); - _delay(1000); -} - - -void loop() { - // main FOC algorithm function - // the faster you run this function the better - // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz - motor.loopFOC(); - - // Motion control function - // velocity, position or voltage (defined in motor.controller) - // 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_velocity); - - // 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 From e81af5dbdf97e49efc0893654d179e3b03ea27f8 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 29 Jun 2024 18:22:52 -0500 Subject: [PATCH 5/6] Fix for #415 sin/cos integer overflow on 16-bit CPUs Terrible bug in the fast sin/cos added last year, resulting in rough sine waves and a spike from +1 to -1 at pi/2 --- src/common/foc_utils.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index fa937d15..7ae372f7 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,20 +9,21 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256/_2PI)); - int t1, t2, frac = i & 0xff; + int32_t t1, t2; + unsigned int i = (unsigned int)(a * (64*4*256.0f/_2PI)); + int frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { - t1 = sine_array[i]; t2 = sine_array[i+1]; + t1 = (int32_t)sine_array[i]; t2 = (int32_t)sine_array[i+1]; } else if(i < 128) { - t1 = sine_array[128 - i]; t2 = sine_array[127 - i]; + t1 = (int32_t)sine_array[128 - i]; t2 = (int32_t)sine_array[127 - i]; } else if(i < 192) { - t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i]; + t1 = -(int32_t)sine_array[-128 + i]; t2 = -(int32_t)sine_array[-127 + i]; } else { - t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i]; + t1 = -(int32_t)sine_array[256 - i]; t2 = -(int32_t)sine_array[255 - i]; } return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8)); } From bf75d206ad11d0c0e56f5074395e17f545702d69 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Sat, 29 Jun 2024 18:32:51 -0500 Subject: [PATCH 6/6] Removed accidental addition of LinearHall.h I thought I discarded this change after moving LinearHall back to drivers, but somehow it got included anyway --- src/SimpleFOC.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 9e1c80c6..4e6815e5 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -104,7 +104,6 @@ void loop() { #include "sensors/MagneticSensorAnalog.h" #include "sensors/MagneticSensorPWM.h" #include "sensors/HallSensor.h" -#include "sensors/LinearHall.h" #include "sensors/GenericSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h"