diff --git a/examples/motion_control/torque_control/linear_hall_sensor/voltage_control/voltage_control.ino b/examples/motion_control/torque_control/linear_hall_sensor/voltage_control/voltage_control.ino new file mode 100644 index 00000000..6e170af7 --- /dev/null +++ b/examples/motion_control/torque_control/linear_hall_sensor/voltage_control/voltage_control.ino @@ -0,0 +1,95 @@ +/** + * + * 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 + +int16_t HALL_A_MIN = 98; +int16_t HALL_A_MAX = 577; + +int16_t HALL_B_MIN = 80; +int16_t HALL_B_MAX = 591; + +// BLDC motor & driver instance +//BLDCMotor motor = BLDCMotor(11); +//BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +BLDCMotor motor = BLDCMotor(4); +BLDCDriver3PWM driver = BLDCDriver3PWM(5, 6, 10, 8); + +// Stepper motor & driver instance +//StepperMotor motor = StepperMotor(50); +//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); + +// Linear hall sensor instance +LinearHallSensor sensor = LinearHallSensor(A4, A5); + +// 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() { + + /* use monitoring with serial */ + Serial.begin(115200); + /* comment out if not needed */ + motor.useMonitoring(Serial); + + // initialize encoder sensor hardware + sensor.init(/* HALL_A_MIN, HALL_A_MAX, HALL_B_MIN, HALL_B_MAX*/); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // 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; + + // initialize motor + motor.init(); + // 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(); +} diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4e6815e5..ce4bc5cd 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/LinearHallSensor.h" #include "sensors/GenericSensor.h" #include "drivers/BLDCDriver3PWM.h" #include "drivers/BLDCDriver6PWM.h" diff --git a/src/sensors/LinearHallSensor.cpp b/src/sensors/LinearHallSensor.cpp new file mode 100644 index 00000000..cfcf20b2 --- /dev/null +++ b/src/sensors/LinearHallSensor.cpp @@ -0,0 +1,194 @@ +#include "LinearHallSensor.h" +#include "./communication/SimpleFOCDebug.h" + +/** LinearHallSensor(uint8_t _pinAnalog, int _min, int _max) + * @param _pinAnalog the pin that is reading the pwm from magnetic sensor + * @param _min_raw_count the smallest expected reading. Whilst you might expect it to be 0 it is often ~15. Getting this wrong results in a small click once per revolution + * @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) + */ +LinearHallSensor::LinearHallSensor(uint8_t _pinAnalog_a, uint8_t _pinAnalog_b) +{ + pinAnalog_a = _pinAnalog_a; + pinAnalog_b = _pinAnalog_b; + + if (pullup == Pullup::USE_INTERN) + { + pinMode(pinAnalog_a, INPUT_PULLUP); + pinMode(pinAnalog_b, INPUT_PULLUP); + } + else + { + pinMode(pinAnalog_a, INPUT); + pinMode(pinAnalog_b, INPUT); + } +} + +/** + * @brief 线性传感器初始化 + * + */ +void LinearHallSensor::init(int _min_raw_count_a, int _max_raw_count_a, + int _min_raw_count_b, int _max_raw_count_b) +{ + raw_count_a = getRawCountA(); + raw_count_b = getRawCountB(); + + if (_isset(_min_raw_count_a) && _isset(_max_raw_count_a) && _isset(_min_raw_count_b) && _isset(_max_raw_count_b)) + { + /** LinearHall A */ + min_raw_count_a = _min_raw_count_a; + max_raw_count_a = _max_raw_count_a; + /** LinearHall B */ + min_raw_count_b = _min_raw_count_b; + max_raw_count_b = _max_raw_count_b; + } + else + { + /** 开始旋转电机一周校准线性霍尔的最大最小值 */ + int minA, maxA, minB, maxB, centerA, centerB; + + minA = raw_count_a; + maxA = minA; + centerA = (minA + maxA) / 2; + minB = raw_count_b; + maxB = minB; + centerB = (minB + maxB) / 2; + + SIMPLEFOC_DEBUG("Sensor start init"); + + // move one electrical revolution forward + for (int i = 0; i <= 5000; i++) + { + // float angle = _3PI_2 + _2PI * i / 500.0f; + // BLDCMotor::setPhaseVoltage(FOCMotor::voltage_sensor_align, 0, angle); + + //手动旋转电机一周来检测霍尔最大最小值 + int tempA = getRawCountA(); + if (tempA < minA) + minA = tempA; + if (tempA > maxA) + maxA = tempA; + centerA = (minA + maxA) / 2; + + int tempB = getRawCountB(); + if (tempB < minB) + minB = tempB; + if (tempB > maxB) + maxB = tempB; + centerB = (minB + maxB) / 2; + + if (i % 500 == 0) + { + static int num = 10; + SIMPLEFOC_DEBUG("Count down: ", num); + SIMPLEFOC_DEBUG("A: ", centerA); + SIMPLEFOC_DEBUG("B: ", centerB); + SIMPLEFOC_DEBUG("min A: ", minA); + SIMPLEFOC_DEBUG("max A: ", maxA); + SIMPLEFOC_DEBUG("min B: ", minB); + SIMPLEFOC_DEBUG("max B: ", maxB); + + num--; + } + + _delay(2); + } + + min_raw_count_a = minA; + max_raw_count_a = maxA; + + min_raw_count_b = minB; + max_raw_count_b = maxB; + + SIMPLEFOC_DEBUG("Sensor init ok:"); + SIMPLEFOC_DEBUG("A: ", centerA); + SIMPLEFOC_DEBUG("B: ", centerB); + SIMPLEFOC_DEBUG("min A: ", minA); + SIMPLEFOC_DEBUG("max A: ", maxA); + SIMPLEFOC_DEBUG("min B: ", minB); + SIMPLEFOC_DEBUG("max B: ", maxB); + } + + cpr = max_raw_count_a - min_raw_count_a; + + this->Sensor::init(); // call base class init +} + +/** + * @brief Shaft angle calculation + * + * @return angle is in radians [rad] float [0,2PI] + */ +float LinearHallSensor::getSensorAngle() +{ + /* 1.获取双霍尔原始模拟值 */ + raw_count_a = getRawCountA(); + raw_count_b = getRawCountB(); + + /* 2.双霍尔模拟值滤波 */ + + /* 3.双霍尔滤波后值映射到[-1,1] */ + this->hall_filtered_a = map_float(raw_count_a, min_raw_count_a, max_raw_count_a, -1, 1); + this->hall_filtered_b = map_float(raw_count_b, min_raw_count_b, max_raw_count_b, -1, 1); + + /* 4.反正切函数计算轴角度[-180,180] */ + float angle = atan2(hall_filtered_a, hall_filtered_b) * 180 / PI; + + /* 5.轴角转为弧度制[0,2PI] */ + return (angle + 180) * PI / 180.0f; +} + +/** + * @brief Reading the raw counter of the linear hall sensor A + * + * @return int + */ +int LinearHallSensor::getRawCountA() +{ + return analogRead(pinAnalog_a); +} + +/** + * @brief Reading the raw counter of the linear hall sensor B + * + * @return int + */ +int LinearHallSensor::getRawCountB() +{ + return analogRead(pinAnalog_b); +} + +/** + * @brief 数据映射 float + * + * @param in_x 输入值 + * @param in_min 输入值的最小幅值 + * @param in_max 输入值的最大幅值 + * @param out_min 输出值的最小幅值 + * @param out_max 输出值的最大幅值 + * @return float 映射后的输出值 + */ +float LinearHallSensor::map_float(float in_x, float in_min, float in_max, float out_min, float out_max) +{ + return (in_x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +/** + * @brief 获取线性霍尔A滤波后的值 + * + * @return float [-1,1] + */ +float LinearHallSensor::getHallFiledA() +{ + return this->hall_filtered_a; +} + +/** + * @brief 获取线性霍尔B滤波后的值 + * + * @return float [-1,1] + */ +float LinearHallSensor::getHallFiledB() +{ + return this->hall_filtered_b; +} diff --git a/src/sensors/LinearHallSensor.h b/src/sensors/LinearHallSensor.h new file mode 100644 index 00000000..cd870acb --- /dev/null +++ b/src/sensors/LinearHallSensor.h @@ -0,0 +1,69 @@ +#ifndef LINEARHALLSENSOR_LIB_H +#define LINEARHALLSENSOR_LIB_H + +#include "Arduino.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * This sensor has LinearHallSensor. + * This approach is very simple. + */ +class LinearHallSensor : public Sensor +{ +public: + /** + * LinearHallSensor class constructor + * @param _pinAnalog_a the pin to read the HALL A signal + * @param _pinAnalog_b the pin to read the HALL B signal + */ + LinearHallSensor(uint8_t _pinAnalog_a, uint8_t _pinAnalog_b); + + /** sensor initialise pins */ + void init(int _min_raw_count_a = NOT_SET, int _max_raw_count_a = NOT_SET, + int _min_raw_count_b = NOT_SET, int _max_raw_count_b = NOT_SET); + + uint8_t pinAnalog_a; //!< Linear Hall pin A + uint8_t pinAnalog_b; //!< Linear Hall pin B + + /** Encoder configuration */ + Pullup pullup; + + /** implementation of abstract functions of the Sensor class */ + /** get current angle (rad) */ + float getSensorAngle() override; + /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ + int raw_count_a; + int raw_count_b; + + /** 获取霍尔滤波后的值[-1,1] */ + float getHallFiledA(); + float getHallFiledB(); + + /** 数据映射 */ + float map_float(float in_x, float in_min, float in_max, float out_min, float out_max); + +private: + /** LinearHall 实际输出模拟值范围 */ + int min_raw_count_a; + int max_raw_count_a; + int min_raw_count_b; + int max_raw_count_b; + + /** LinearHall 滤波后值映射到[-1,1] */ + float hall_filtered_a; + float hall_filtered_b; + + int cpr; + int read(); + + /** + * Function getting current angle register value + * it uses angle_register variable + */ + int getRawCountA(); + int getRawCountB(); +}; + +#endif diff --git "a/\345\217\214\347\272\277\346\200\247\351\234\215\345\260\224\350\247\222\345\272\246\350\256\241\347\256\227.png" "b/\345\217\214\347\272\277\346\200\247\351\234\215\345\260\224\350\247\222\345\272\246\350\256\241\347\256\227.png" new file mode 100644 index 00000000..8cb53cab Binary files /dev/null and "b/\345\217\214\347\272\277\346\200\247\351\234\215\345\260\224\350\247\222\345\272\246\350\256\241\347\256\227.png" differ