├── DengFOC 库 ├── V0.1 电压力矩 位置闭环 │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ └── 例程 │ │ ├── 力位闭环（闭环位置）.ino │ │ └── 电压力矩环.ino ├── V0.2 位置 速度 电压力矩闭环 │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ ├── DengFOC_Lib_Lesson7_CloseLoop_Vel.ino │ ├── lowpass_filter.cpp │ ├── lowpass_filter.h │ ├── pid.cpp │ ├── pid.h │ └── 例程 │ │ ├── 位置闭环（角度-速度-力矩）.ino │ │ ├── 电压力矩环.ino │ │ ├── 速度闭环.ino │ │ ├── 闭环位置（力位）-方式1.ino │ │ └── 闭环位置（力位）-方式2.ino ├── V0.4 用电流环改进了 位置 速度 闭环 │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ ├── InlineCurrent.cpp │ ├── InlineCurrent.h │ ├── lowpass_filter.cpp │ ├── lowpass_filter.h │ ├── pid.cpp │ ├── pid.h │ └── 例程 │ │ ├── 位置闭环（角度-速度-力矩）.ino │ │ ├── 用电流环改进位置、速度、力矩.ino │ │ ├── 电压力矩环.ino │ │ ├── 电流力矩闭环.ino │ │ ├── 速度闭环.ino │ │ ├── 闭环位置（力位）-方式1.ino │ │ └── 闭环位置（力位）-方式2.ino ├── V0.5 双路电机驱动 │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ ├── InlineCurrent.cpp │ ├── InlineCurrent.h │ ├── lowpass_filter.cpp │ ├── lowpass_filter.h │ ├── pid.cpp │ ├── pid.h │ └── 例程 │ │ ├── 位置闭环（角度-速度-力矩）.ino │ │ ├── 双电机闭环位置、速度、力矩.ino │ │ ├── 用电流环改进位置、速度、力矩.ino │ │ ├── 电压力矩环.ino │ │ ├── 电流力矩闭环.ino │ │ ├── 速度闭环.ino │ │ ├── 闭环位置（力位）-方式1.ino │ │ └── 闭环位置（力位）-方式2.ino ├── V0.6 支持SVPWM算法 │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ ├── InlineCurrent.cpp │ ├── InlineCurrent.h │ ├── lowpass_filter.cpp │ ├── lowpass_filter.h │ ├── pid.cpp │ ├── pid.h │ └── 例程 │ │ ├── 位置闭环（角度-速度-力矩）.ino │ │ ├── 双电机闭环位置、速度、力矩.ino │ │ ├── 用电流环改进位置、速度、力矩.ino │ │ ├── 电压力矩环.ino │ │ ├── 电流力矩闭环.ino │ │ ├── 速度闭环.ino │ │ ├── 闭环位置（力位）-方式1.ino │ │ └── 闭环位置（力位）-方式2.ino └── v0.3 支持电流环 │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ ├── InlineCurrent.cpp │ ├── InlineCurrent.h │ ├── lowpass_filter.cpp │ ├── lowpass_filter.h │ ├── pid.cpp │ ├── pid.h │ └── 例程 │ ├── 位置闭环（角度-速度-力矩）.ino │ ├── 电压力矩环.ino │ ├── 电流力矩闭环.ino │ ├── 速度闭环.ino │ ├── 闭环位置（力位）-方式1.ino │ └── 闭环位置（力位）-方式2.ino ├── LICENSE ├── README.md ├── pic └── mainTitle.png └── 【手把手教些FOC算法】系列课程代码 ├── 第七章a+b 闭环速度FOC代码-DengFOC库v0.2 └── 1 DengFOC库V0.2+实例 │ └── DengFOC_Lib_Lesson7_CloseLoop_Vel │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ ├── DengFOC_Lib_Lesson7_CloseLoop_Vel.ino │ ├── lowpass_filter.cpp │ ├── lowpass_filter.h │ ├── pid.cpp │ └── pid.h ├── 第九章a+b 位置闭环，速度闭环升级版（加上电流环） └── 1 DengFOC库V0.4+实例 │ └── DengFOC_Lib_Lesson9_Current_Sense │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ ├── DengFOC_Lib_Lesson9_Current_Sense.ino │ ├── InlineCurrent.cpp │ ├── InlineCurrent.h │ ├── lowpass_filter.cpp │ ├── lowpass_filter.h │ ├── pid.cpp │ └── pid.h ├── 第五章a+b 开环速度FOC代码 └── DengFOC_Lib_Lesson5_OpenLoop_Vel │ └── DengFOC_Lib_Lesson5_OpenLoop_Vel.ino ├── 第八章a+b 电流力矩闭环 └── 1 DengFOC库V0.3+实例 │ └── DengFOC_Lib_Lesson8_Current_Sense │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ ├── DengFOC_Lib_Lesson8_Current_Sense.ino │ ├── InlineCurrent.cpp │ ├── InlineCurrent.h │ ├── lowpass_filter.cpp │ ├── lowpass_filter.h │ ├── pid.cpp │ └── pid.h ├── 第六章a+b 闭环位置FOC代码-DengFOC库v0.1 ├── 1 DengFOC库V0.1+实例（6b课程后半部分） │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ └── DengFOC_Lib_Lesson6_CloseLoop_Pos.ino └── 2 单文件实现闭环代码（6b课程前半部分） │ ├── AS5600.cpp │ ├── AS5600.h │ └── DengFOC_Lib_Lesson6_CloseLoop_Pos.ino ├── 第十一课a+b SVPWM的算法实现思路和框架 └── DengFOC_Lib_Lesson11_SVPWM │ ├── AS5600.cpp │ ├── AS5600.h │ ├── DengFOC.cpp │ ├── DengFOC.h │ ├── DengFOC_Lib_Lesson10_Dual_Motor_SVPWM.ino │ ├── InlineCurrent.cpp │ ├── InlineCurrent.h │ ├── lowpass_filter.cpp │ ├── lowpass_filter.h │ ├── pid.cpp │ └── pid.h └── 第十课a+b 双电机闭环FOC代码 └── DengFOC_Lib_Lesson10_Dual_Motor ├── AS5600.cpp ├── AS5600.h ├── DengFOC.cpp ├── DengFOC.h ├── DengFOC_Lib_Lesson10_Dual_Motor.ino ├── InlineCurrent.cpp ├── InlineCurrent.h ├── lowpass_filter.cpp ├── lowpass_filter.h ├── pid.cpp └── pid.h /DengFOC 库/V0.1 电压力矩 位置闭环/AS5600.cpp: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | 8 | #include "Wire.h" 9 | #include "AS5600.h" 10 | #include 11 | 12 | int _raw_ang_hi = 0x0c; 13 | int _raw_ang_lo = 0x0d; 14 | int _ams5600_Address = 0x36; 15 | int ledtime = 0; 16 | int32_t full_rotations=0; // full rotation tracking; 17 | float angle_prev=0; 18 | 19 | void BeginSensor() { 20 | Wire.begin(19,18, 400000UL); 21 | delay(1000); 22 | } 23 | //readTwoBytes(int in_adr_hi, int in_adr_lo)这段代码是一个函数，其目的是从I2C设备（在代码中的变量名为_ams5600_Address）中读取两个字节数据，并将其合并成一个16位的无符号整数返回。 24 | //具体来说，函数接受两个整型参数in_adr_hi和in_adr_lo，它们用于指定需要读取的两个字节数据的地址。函数中首先通过Wire库开始I2C传输，向设备写入in_adr_lo和in_adr_hi分别作为数据地址，然后读取相应的字节数据。 25 | //在每个Wire.requestFrom()调用之后，通过一个while循环等待数据接收完毕。然后读取接收到的低字节和高字节，并使用位运算将它们合并成一个16位的无符号整数。 26 | //最后，返回合并后的整数。如果读取过程中出现错误或者函数没有成功读取到数据，则函数返回-1。 27 | word readTwoBytes(int in_adr_hi, int in_adr_lo) 28 | { 29 | word retVal = -1; 30 | 31 | /* 读低位 */ 32 | Wire.beginTransmission(_ams5600_Address); 33 | Wire.write(in_adr_lo); 34 | Wire.endTransmission(); 35 | Wire.requestFrom(_ams5600_Address, 1); 36 | while(Wire.available() == 0); 37 | int low = Wire.read(); 38 | 39 | /* 读高位 */ 40 | Wire.beginTransmission(_ams5600_Address); 41 | Wire.write(in_adr_hi); 42 | Wire.endTransmission(); 43 | Wire.requestFrom(_ams5600_Address, 1); 44 | while(Wire.available() == 0); 45 | int high = Wire.read(); 46 | 47 | retVal = (high << 8) | low; 48 | 49 | return retVal; 50 | } 51 | 52 | word getRawAngle() 53 | { 54 | return readTwoBytes(_raw_ang_hi, _raw_ang_lo); 55 | } 56 | 57 | float getAngle_Without_track() 58 | { 59 | return getRawAngle()*0.08789* PI / 180; //得到弧度制的角度 60 | } 61 | 62 | float getAngle() 63 | { 64 | float val = getAngle_Without_track(); 65 | float d_angle = val - angle_prev; 66 | //计算旋转的总圈数 67 | //通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出，如果发生了，则将full_rotations增加1（如果d_angle小于0）或减少1（如果d_angle大于0）。 68 | if(abs(d_angle) > (0.8f*6.28318530718f) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 69 | angle_prev = val; 70 | return (float)full_rotations * 6.28318530718f + angle_prev; 71 | 72 | } 73 | -------------------------------------------------------------------------------- /DengFOC 库/V0.1 电压力矩 位置闭环/AS5600.h: -------------------------------------------------------------------------------- 1 | void BeginSensor(); 2 | float getAngle(); 3 | float getAngle_Without_track(); 4 | -------------------------------------------------------------------------------- /DengFOC 库/V0.1 电压力矩 位置闭环/DengFOC.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "AS5600.h" 3 | 4 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 5 | float voltage_power_supply; 6 | float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0; 7 | #define _3PI_2 4.71238898038f 8 | float zero_electric_angle=0; 9 | int PP=1,DIR=1; 10 | int pwmA = 32; 11 | int pwmB = 33; 12 | int pwmC = 25; 13 | 14 | //初始变量及函数定义 15 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 16 | //宏定义实现的一个约束函数,用于限制一个值的范围。 17 | //具体来说，该宏定义的名称为 _constrain，接受三个参数 amt、low 和 high，分别表示要限制的值、最小值和最大值。该宏定义的实现使用了三元运算符，根据 amt 是否小于 low 或大于 high，返回其中的最大或最小值，或者返回原值。 18 | //换句话说，如果 amt 小于 low，则返回 low；如果 amt 大于 high，则返回 high；否则返回 amt。这样，_constrain(amt, low, high) 就会将 amt 约束在 [low, high] 的范围内。1 19 | 20 | 21 | // 归一化角度到 [0,2PI] 22 | float _normalizeAngle(float angle){ 23 | float a = fmod(angle, 2*PI); //取余运算可以用于归一化，列出特殊值例子算便知 24 | return a >= 0 ? a : (a + 2*PI); 25 | //三目运算符。格式：condition ? expr1 : expr2 26 | //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。 27 | //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。 28 | //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。 29 | } 30 | 31 | 32 | // 设置PWM到控制器输出 33 | void setPwm(float Ua, float Ub, float Uc) { 34 | 35 | // 计算占空比 36 | // 限制占空比从0到1 37 | float dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); 38 | float dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); 39 | float dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); 40 | 41 | //写入PWM到PWM 0 1 2 通道 42 | ledcWrite(0, dc_a*255); 43 | ledcWrite(1, dc_b*255); 44 | ledcWrite(2, dc_c*255); 45 | } 46 | 47 | void setTorque(float Uq,float angle_el) { 48 | Uq=_constrain(Uq,-voltage_power_supply/2,voltage_power_supply/2); 49 | float Ud=0; 50 | angle_el = _normalizeAngle(angle_el); 51 | // 帕克逆变换 52 | Ualpha = -Uq*sin(angle_el); 53 | Ubeta = Uq*cos(angle_el); 54 | 55 | // 克拉克逆变换 56 | Ua = Ualpha + voltage_power_supply/2; 57 | Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2; 58 | Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2; 59 | setPwm(Ua,Ub,Uc); 60 | } 61 | 62 | void DFOC_Vbus(float power_supply) 63 | { 64 | voltage_power_supply=power_supply; 65 | pinMode(pwmA, OUTPUT); 66 | pinMode(pwmB, OUTPUT); 67 | pinMode(pwmC, OUTPUT); 68 | ledcSetup(0, 30000, 8); //pwm频道, 频率, 精度 69 | ledcSetup(1, 30000, 8); //pwm频道, 频率, 精度 70 | ledcSetup(2, 30000, 8); //pwm频道, 频率, 精度 71 | ledcAttachPin(pwmA, 0); 72 | ledcAttachPin(pwmB, 1); 73 | ledcAttachPin(pwmC, 2); 74 | Serial.println("完成PWM初始化设置"); 75 | BeginSensor(); 76 | } 77 | 78 | 79 | float _electricalAngle(){ 80 | return _normalizeAngle((float)(DIR * PP) * getAngle_Without_track()-zero_electric_angle); 81 | } 82 | 83 | 84 | void DFOC_alignSensor(int _PP,int _DIR) 85 | { 86 | PP=_PP; 87 | DIR=_DIR; 88 | setTorque(3, _3PI_2); 89 | delay(3000); 90 | zero_electric_angle=_electricalAngle(); 91 | setTorque(0, _3PI_2); 92 | Serial.print("0电角度：");Serial.println(zero_electric_angle); 93 | } 94 | 95 | float DFOC_M0_Angle() 96 | { 97 | return getAngle(); 98 | } 99 | 100 | //==============串口接收============== 101 | float motor_target; 102 | int commaPosition; 103 | String serialReceiveUserCommand() { 104 | 105 | // a string to hold incoming data 106 | static String received_chars; 107 | 108 | String command = ""; 109 | 110 | while (Serial.available()) { 111 | // get the new byte: 112 | char inChar = (char)Serial.read(); 113 | // add it to the string buffer: 114 | received_chars += inChar; 115 | 116 | // end of user input 117 | if (inChar == '\n') { 118 | 119 | // execute the user command 120 | command = received_chars; 121 | 122 | commaPosition = command.indexOf('\n');//检测字符串中的逗号 123 | if(commaPosition != -1)//如果有逗号存在就向下执行 124 | { 125 | motor_target = command.substring(0,commaPosition).toDouble(); //电机角度 126 | Serial.println(motor_target); 127 | } 128 | // reset the command buffer 129 | received_chars = ""; 130 | } 131 | } 132 | return command; 133 | } 134 | 135 | float serial_motor_target() 136 | { 137 | return motor_target; 138 | } 139 | -------------------------------------------------------------------------------- /DengFOC 库/V0.1 电压力矩 位置闭环/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | void setPwm(float Ua, float Ub, float Uc); 9 | float setTorque(float Uq,float angle_el); 10 | float _normalizeAngle(float angle); 11 | void DFOC_Vbus(float power_supply); 12 | void DFOC_alignSensor(int _PP,int _DIR); 13 | float _electricalAngle(); 14 | float DFOC_M0_Angle(); 15 | float serial_motor_target(); 16 | String serialReceiveUserCommand(); 17 | -------------------------------------------------------------------------------- /DengFOC 库/V0.1 电压力矩 位置闭环/例程/力位闭环（闭环位置）.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(Kp*(serial_motor_target()-Sensor_DIR*Sensor_Angle)*180/PI,_electricalAngle()); //位置闭环 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.1 电压力矩 位置闭环/例程/电压力矩环.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(serial_motor_target(),_electricalAngle()); //电压力矩 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/DengFOC.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "AS5600.h" 3 | #include "lowpass_filter.h" 4 | #include "pid.h" 5 | 6 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 7 | float voltage_power_supply; 8 | float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0; 9 | #define _3PI_2 4.71238898038f 10 | float zero_electric_angle=0; 11 | int PP=1,DIR=1; 12 | int pwmA = 32; 13 | int pwmB = 33; 14 | int pwmC = 25; 15 | 16 | //低通滤波初始化 17 | LowPassFilter M0_Vel_Flt = LowPassFilter(0.01); // Tf = 10ms //M0速度环 18 | //PID 19 | PIDController vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2}; 20 | PIDController angle_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = 100}; 21 | 22 | //AS5600 23 | Sensor_AS5600 S0=Sensor_AS5600(0); 24 | TwoWire S0_I2C = TwoWire(0); 25 | 26 | //=================PID 设置函数================= 27 | //速度PID 28 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp) //M0角度环PID设置 29 | { 30 | vel_loop_M0.P=P; 31 | vel_loop_M0.I=I; 32 | vel_loop_M0.D=D; 33 | vel_loop_M0.output_ramp=ramp; 34 | } 35 | //角度PID 36 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp) //M0角度环PID设置 37 | { 38 | angle_loop_M0.P=P; 39 | angle_loop_M0.I=I; 40 | angle_loop_M0.D=D; 41 | angle_loop_M0.output_ramp=ramp; 42 | } 43 | 44 | 45 | //M0速度PID接口 46 | float DFOC_M0_VEL_PID(float error) //M0速度环 47 | { 48 | return vel_loop_M0(error); 49 | 50 | } 51 | //M0角度PID接口 52 | float DFOC_M0_ANGLE_PID(float error) 53 | { 54 | return angle_loop_M0(error); 55 | } 56 | 57 | 58 | //初始变量及函数定义 59 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 60 | //宏定义实现的一个约束函数,用于限制一个值的范围。 61 | //具体来说，该宏定义的名称为 _constrain，接受三个参数 amt、low 和 high，分别表示要限制的值、最小值和最大值。该宏定义的实现使用了三元运算符，根据 amt 是否小于 low 或大于 high，返回其中的最大或最小值，或者返回原值。 62 | //换句话说，如果 amt 小于 low，则返回 low；如果 amt 大于 high，则返回 high；否则返回 amt。这样，_constrain(amt, low, high) 就会将 amt 约束在 [low, high] 的范围内。1 63 | 64 | 65 | // 归一化角度到 [0,2PI] 66 | float _normalizeAngle(float angle){ 67 | float a = fmod(angle, 2*PI); //取余运算可以用于归一化，列出特殊值例子算便知 68 | return a >= 0 ? a : (a + 2*PI); 69 | //三目运算符。格式：condition ? expr1 : expr2 70 | //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。 71 | //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。 72 | //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。 73 | } 74 | 75 | 76 | // 设置PWM到控制器输出 77 | void setPwm(float Ua, float Ub, float Uc) { 78 | // 限制上限 79 | Ua = _constrain(Ua, 0.0f, voltage_power_supply); 80 | Ub = _constrain(Ub, 0.0f, voltage_power_supply); 81 | Uc = _constrain(Uc, 0.0f, voltage_power_supply); 82 | // 计算占空比 83 | // 限制占空比从0到1 84 | float dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); 85 | float dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); 86 | float dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); 87 | 88 | //写入PWM到PWM 0 1 2 通道 89 | ledcWrite(0, dc_a*255); 90 | ledcWrite(1, dc_b*255); 91 | ledcWrite(2, dc_c*255); 92 | } 93 | 94 | void setTorque(float Uq,float angle_el) { 95 | S0.Sensor_update(); //更新传感器数值 96 | Uq=_constrain(Uq,-(voltage_power_supply)/2,(voltage_power_supply)/2); 97 | float Ud=0; 98 | angle_el = _normalizeAngle(angle_el); 99 | // 帕克逆变换 100 | Ualpha = -Uq*sin(angle_el); 101 | Ubeta = Uq*cos(angle_el); 102 | 103 | // 克拉克逆变换 104 | Ua = Ualpha + voltage_power_supply/2; 105 | Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2; 106 | Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2; 107 | setPwm(Ua,Ub,Uc); 108 | } 109 | 110 | void DFOC_Vbus(float power_supply) 111 | { 112 | voltage_power_supply=power_supply; 113 | pinMode(pwmA, OUTPUT); 114 | pinMode(pwmB, OUTPUT); 115 | pinMode(pwmC, OUTPUT); 116 | ledcSetup(0, 30000, 8); //pwm频道, 频率, 精度 117 | ledcSetup(1, 30000, 8); //pwm频道, 频率, 精度 118 | ledcSetup(2, 30000, 8); //pwm频道, 频率, 精度 119 | ledcAttachPin(pwmA, 0); 120 | ledcAttachPin(pwmB, 1); 121 | ledcAttachPin(pwmC, 2); 122 | Serial.println("完成PWM初始化设置"); 123 | 124 | //AS5600 125 | S0_I2C.begin(19,18, 400000UL); 126 | S0.Sensor_init(&S0_I2C); //初始化编码器0 127 | Serial.println("编码器加载完毕"); 128 | 129 | //PID 加载 130 | vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2}; 131 | } 132 | 133 | 134 | float _electricalAngle(){ 135 | return _normalizeAngle((float)(DIR * PP) * S0.getMechanicalAngle()-zero_electric_angle); 136 | } 137 | 138 | 139 | void DFOC_alignSensor(int _PP,int _DIR) 140 | { 141 | PP=_PP; 142 | DIR=_DIR; 143 | setTorque(3, _3PI_2); //起劲 144 | delay(1000); 145 | S0.Sensor_update(); //更新角度，方便下面电角度读取 146 | zero_electric_angle=_electricalAngle(); 147 | setTorque(0, _3PI_2); //松劲（解除校准） 148 | Serial.print("0电角度：");Serial.println(zero_electric_angle); 149 | } 150 | 151 | float DFOC_M0_Angle() 152 | { 153 | return DIR*S0.getAngle(); 154 | } 155 | 156 | //无滤波 157 | //float DFOC_M0_Velocity() 158 | //{ 159 | // return DIR*S0.getVelocity(); 160 | //} 161 | 162 | //有滤波 163 | float DFOC_M0_Velocity() 164 | { 165 | //获取速度数据并滤波 166 | float vel_M0_ori=S0.getVelocity(); 167 | float vel_M0_flit=M0_Vel_Flt(DIR*vel_M0_ori); 168 | return vel_M0_flit; //考虑方向 169 | } 170 | 171 | //==============串口接收============== 172 | float motor_target; 173 | int commaPosition; 174 | String serialReceiveUserCommand() { 175 | 176 | // a string to hold incoming data 177 | static String received_chars; 178 | 179 | String command = ""; 180 | 181 | while (Serial.available()) { 182 | // get the new byte: 183 | char inChar = (char)Serial.read(); 184 | // add it to the string buffer: 185 | received_chars += inChar; 186 | 187 | // end of user input 188 | if (inChar == '\n') { 189 | 190 | // execute the user command 191 | command = received_chars; 192 | 193 | commaPosition = command.indexOf('\n');//检测字符串中的逗号 194 | if(commaPosition != -1)//如果有逗号存在就向下执行 195 | { 196 | motor_target = command.substring(0,commaPosition).toDouble(); //电机角度 197 | Serial.println(motor_target); 198 | } 199 | // reset the command buffer 200 | received_chars = ""; 201 | } 202 | } 203 | return command; 204 | } 205 | 206 | 207 | float serial_motor_target() 208 | { 209 | return motor_target; 210 | } 211 | 212 | 213 | 214 | //================简易接口函数================ 215 | void DFOC_M0_set_Velocity_Angle(float Target) 216 | { 217 | setTorque(DFOC_M0_VEL_PID(DFOC_M0_ANGLE_PID((Target-DFOC_M0_Angle())*180/PI)),_electricalAngle()); //角度闭环 218 | } 219 | 220 | void DFOC_M0_setVelocity(float Target) 221 | { 222 | setTorque(DFOC_M0_VEL_PID((serial_motor_target()-DFOC_M0_Velocity())*180/PI),_electricalAngle()); //速度闭环 223 | } 224 | 225 | void DFOC_M0_set_Force_Angle(float Target) //力位 226 | { 227 | setTorque(DFOC_M0_ANGLE_PID((Target-DFOC_M0_Angle())*180/PI),_electricalAngle()); 228 | } 229 | 230 | void DFOC_M0_setTorque(float Target) 231 | { 232 | setTorque(Target,_electricalAngle()); 233 | } 234 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | void setPwm(float Ua, float Ub, float Uc); 9 | float setTorque(float Uq,float angle_el); 10 | float _normalizeAngle(float angle); 11 | void DFOC_Vbus(float power_supply); 12 | void DFOC_alignSensor(int _PP,int _DIR); 13 | float _electricalAngle(); 14 | 15 | float serial_motor_target(); 16 | String serialReceiveUserCommand(); 17 | //传感器读取 18 | float DFOC_M0_Velocity(); 19 | float DFOC_M0_Angle(); 20 | //PID 21 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp); 22 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp); 23 | float DFOC_M0_VEL_PID(float error); 24 | float DFOC_M0_ANGLE_PID(float error); 25 | //接口函数 26 | void DFOC_M0_set_Velocity_Angle(float Target); 27 | void DFOC_M0_setVelocity(float Target); 28 | void DFOC_M0_set_Force_Angle(float Target); 29 | void DFOC_M0_setTorque(float Target); 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/DengFOC_Lib_Lesson7_CloseLoop_Vel.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | 20 | void loop() 21 | { 22 | //设置速度环PID 23 | DFOC_M0_SET_VEL_PID(0.005,0.00,0,0); 24 | //设置速度 25 | DFOC_M0_setVelocity(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/例程/位置闭环（角度-速度-力矩）.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | void loop() 20 | { 21 | //设置PID 22 | DFOC_M0_SET_ANGLE_PID(0.5,0,0,0); 23 | DFOC_M0_SET_VEL_PID(0.01,0.00,0,0); 24 | //给库设定速度 25 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/例程/电压力矩环.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(serial_motor_target(),_electricalAngle()); //电压力矩 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/例程/速度闭环.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | 20 | void loop() 21 | { 22 | //设置速度环PID 23 | DFOC_M0_SET_VEL_PID(0.005,0.00,0,0); 24 | //设置速度 25 | DFOC_M0_setVelocity(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/例程/闭环位置（力位）-方式1.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | int count_output=0; 19 | 20 | void loop() 21 | { 22 | 23 | //设置PID 24 | DFOC_M0_SET_ANGLE_PID(0.3,0,0,0); 25 | DFOC_M0_set_Force_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } -------------------------------------------------------------------------------- /DengFOC 库/V0.2 位置 速度 电压力矩闭环/例程/闭环位置（力位）-方式2.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(Kp*(serial_motor_target()-Sensor_DIR*Sensor_Angle)*180/PI,_electricalAngle()); //位置闭环 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | void setPwm(float Ua, float Ub, float Uc); 9 | float setTorque(float Uq,float angle_el); 10 | float _normalizeAngle(float angle); 11 | void DFOC_Vbus(float power_supply); 12 | void DFOC_alignSensor(int _PP,int _DIR); 13 | float _electricalAngle(); 14 | 15 | float serial_motor_target(); 16 | String serialReceiveUserCommand(); 17 | //传感器读取 18 | float DFOC_M0_Velocity(); 19 | float DFOC_M0_Angle(); 20 | float DFOC_M0_Current(); 21 | //PID 22 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 23 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 24 | void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp); 25 | float DFOC_M0_VEL_PID(float error); 26 | float DFOC_M0_ANGLE_PID(float error); 27 | 28 | //接口函数 29 | void DFOC_M0_set_Velocity_Angle(float Target); 30 | void DFOC_M0_setVelocity(float Target); 31 | void DFOC_M0_set_Force_Angle(float Target); 32 | void DFOC_M0_setTorque(float Target); 33 | //runFOC 循环函数 34 | void runFOC(); 35 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/InlineCurrent.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "InlineCurrent.h" 3 | 4 | 5 | // - shunt_resistor - 分流电阻值 6 | // - gain - 电流检测运算放大器增益 7 | // - phA - A 相 adc 引脚 8 | // - phB - B 相 adc 引脚 9 | // - phC - C 相 adc 引脚（可选） 10 | 11 | 12 | #define _ADC_VOLTAGE 3.3f //ADC 电压 13 | #define _ADC_RESOLUTION 4095.0f //ADC 分辨率 14 | 15 | // ADC 计数到电压转换比率求解 16 | #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) 17 | 18 | #define NOT_SET -12345.0 19 | #define _isset(a) ( (a) != (NOT_SET) ) 20 | 21 | CurrSense::CurrSense(int Mot_Num) 22 | { 23 | if(Mot_Num==0) 24 | { 25 | pinA = 39; 26 | pinB = 36; 27 | //int pinC; 28 | _shunt_resistor = 0.01; 29 | amp_gain = 50; 30 | 31 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 32 | 33 | // gains for each phase 34 | gain_a = volts_to_amps_ratio*-1; 35 | gain_b = volts_to_amps_ratio*-1; 36 | gain_c = volts_to_amps_ratio; 37 | } 38 | if(Mot_Num==1) 39 | { 40 | pinA = 35; 41 | pinB = 34; 42 | //int pinC; 43 | _shunt_resistor = 0.01; 44 | amp_gain = 50; 45 | 46 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 47 | 48 | // gains for each phase 49 | gain_a = volts_to_amps_ratio*-1; 50 | gain_b = volts_to_amps_ratio*-1; 51 | gain_c = volts_to_amps_ratio; 52 | } 53 | } 54 | float CurrSense::readADCVoltageInline(const int pinA){ 55 | uint32_t raw_adc = analogRead(pinA); 56 | return raw_adc * _ADC_CONV; 57 | } 58 | void CurrSense::configureADCInline(const int pinA,const int pinB, const int pinC){ 59 | pinMode(pinA, INPUT); 60 | pinMode(pinB, INPUT); 61 | if( _isset(pinC) ) pinMode(pinC, INPUT); 62 | } 63 | 64 | // 查找 ADC 零偏移量的函数 65 | void CurrSense::calibrateOffsets(){ 66 | const int calibration_rounds = 1000; 67 | 68 | // 查找0电流时候的电压 69 | offset_ia = 0; 70 | offset_ib = 0; 71 | offset_ic = 0; 72 | // 读数1000次 73 | for (int i = 0; i < calibration_rounds; i++) { 74 | offset_ia += readADCVoltageInline(pinA); 75 | offset_ib += readADCVoltageInline(pinB); 76 | if(_isset(pinC)) offset_ic += readADCVoltageInline(pinC); 77 | delay(1); 78 | } 79 | // 求平均，得到误差 80 | offset_ia = offset_ia / calibration_rounds; 81 | offset_ib = offset_ib / calibration_rounds; 82 | if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; 83 | } 84 | 85 | void CurrSense::init(){ 86 | // 配置函数 87 | configureADCInline(pinA,pinB,pinC); 88 | // 校准 89 | calibrateOffsets(); 90 | } 91 | 92 | 93 | // 读取全部三相电流 94 | 95 | void CurrSense::getPhaseCurrents(){ 96 | current_a = (readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps 97 | current_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps 98 | current_c = (!_isset(pinC)) ? 0 : (readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps 99 | } 100 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/InlineCurrent.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class CurrSense 4 | { 5 | public: 6 | CurrSense(int Mot_Num); 7 | float readADCVoltageInline(const int pinA); 8 | void configureADCInline(const int pinA,const int pinB, const int pinC); 9 | void calibrateOffsets(); 10 | void init(); 11 | void getPhaseCurrents(); 12 | float current_a,current_b,current_c; 13 | int pinA; 14 | int pinB; 15 | int pinC; 16 | float offset_ia; 17 | float offset_ib; 18 | float offset_ic; 19 | float _shunt_resistor; 20 | float amp_gain; 21 | 22 | float volts_to_amps_ratio; 23 | 24 | float gain_a; 25 | float gain_b; 26 | float gain_c; 27 | private: 28 | int _Mot_Num; 29 | }; 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/例程/位置闭环（角度-速度-力矩）.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | void loop() 20 | { 21 | //设置PID 22 | DFOC_M0_SET_ANGLE_PID(0.5,0,0,0); 23 | DFOC_M0_SET_VEL_PID(0.01,0.00,0,0); 24 | //给库设定速度 25 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/例程/用电流环改进位置、速度、力矩.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | int count=0; 20 | void loop() 21 | { 22 | runFOC(); 23 | 24 | //力位（加入电流环后） 25 | // DFOC_M0_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 26 | // DFOC_M0_SET_CURRENT_PID(1.25,50,0,100000); 27 | // DFOC_M0_set_Force_Angle(serial_motor_target()); 28 | 29 | //速度（加入电流环后） 30 | // DFOC_M0_SET_VEL_PID(3,2,0,100000,0.5); 31 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 32 | // DFOC_M0_setVelocity(serial_motor_target()); 33 | 34 | //位置-速度-力（加入电流环后） 35 | DFOC_M0_SET_ANGLE_PID(1,0,0,100000,30); 36 | DFOC_M0_SET_VEL_PID(0.02,1,0,100000,0.5); 37 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 38 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 39 | 40 | //电流力矩 41 | // DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 42 | // DFOC_M0_setTorque(serial_motor_target()); 43 | 44 | count++; 45 | if(count>30) 46 | { 47 | count=0; 48 | //Serial.printf("%f\n", DFOC_M0_Current()); 49 | Serial.printf("%f,%f\n", DFOC_M0_Current(), DFOC_M0_Velocity()); 50 | } 51 | //接收串口 52 | serialReceiveUserCommand(); 53 | 54 | } 55 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/例程/电压力矩环.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(serial_motor_target(),_electricalAngle()); //电压力矩 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/例程/电流力矩闭环.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | int count=0; 20 | void loop() 21 | { 22 | runFOC(); 23 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 24 | DFOC_M0_setTorque(serial_motor_target()); 25 | count++; 26 | if(count>30) 27 | { 28 | count=0; 29 | Serial.printf("%f\n", DFOC_M0_Current()); 30 | } 31 | //接收串口 32 | serialReceiveUserCommand(); 33 | 34 | } 35 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/例程/速度闭环.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | 20 | void loop() 21 | { 22 | //设置速度环PID 23 | DFOC_M0_SET_VEL_PID(0.005,0.00,0,0); 24 | //设置速度 25 | DFOC_M0_setVelocity(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/例程/闭环位置（力位）-方式1.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | int count_output=0; 19 | 20 | void loop() 21 | { 22 | 23 | //设置PID 24 | DFOC_M0_SET_ANGLE_PID(0.3,0,0,0); 25 | DFOC_M0_set_Force_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } -------------------------------------------------------------------------------- /DengFOC 库/V0.4 用电流环改进了 位置 速度 闭环/例程/闭环位置（力位）-方式2.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(Kp*(serial_motor_target()-Sensor_DIR*Sensor_Angle)*180/PI,_electricalAngle()); //位置闭环 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | 9 | void M0_setPwm(float Ua, float Ub, float Uc); 10 | void M1_setPwm(float Ua, float Ub, float Uc); 11 | void DFOC_enable(); 12 | void DFOC_disable(); 13 | void M0_setTorque(float Uq,float angle_el); 14 | void M1_setTorque(float Uq,float angle_el); 15 | float _normalizeAngle(float angle); 16 | void DFOC_Vbus(float power_supply); 17 | void DFOC_M0_alignSensor(int _PP,int _DIR); 18 | void DFOC_M1_alignSensor(int _PP,int _DIR); 19 | float S0_electricalAngle(); 20 | float S1_electricalAngle(); 21 | float cal_Iq_Id(float current_a,float current_b,float angle_el); 22 | float serial_motor_target(); 23 | String serialReceiveUserCommand(); 24 | //传感器读取 25 | float DFOC_M0_Velocity(); 26 | float DFOC_M1_Velocity(); 27 | float DFOC_M0_Angle(); 28 | float DFOC_M1_Angle(); 29 | float DFOC_M0_Current(); 30 | float DFOC_M1_Current(); 31 | //PID 32 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 33 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 34 | void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp); 35 | void DFOC_M1_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 36 | void DFOC_M1_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 37 | void DFOC_M1_SET_CURRENT_PID(float P,float I,float D,float ramp); 38 | float DFOC_M0_VEL_PID(float error); 39 | float DFOC_M0_ANGLE_PID(float error); 40 | float DFOC_M1_VEL_PID(float error); 41 | float DFOC_M1_ANGLE_PID(float error); 42 | 43 | //接口函数 44 | void DFOC_M0_set_Velocity_Angle(float Target); 45 | void DFOC_M0_setVelocity(float Target); 46 | void DFOC_M0_set_Force_Angle(float Target); 47 | void DFOC_M1_set_Velocity_Angle(float Target); 48 | void DFOC_M1_setVelocity(float Target); 49 | void DFOC_M1_set_Force_Angle(float Target); 50 | void DFOC_M0_setTorque(float Target); 51 | void DFOC_M1_setTorque(float Target); 52 | //runFOC 循环函数 53 | void runFOC(); 54 | //测试函数 55 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/InlineCurrent.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "InlineCurrent.h" 3 | 4 | 5 | // - shunt_resistor - 分流电阻值 6 | // - gain - 电流检测运算放大器增益 7 | // - phA - A 相 adc 引脚 8 | // - phB - B 相 adc 引脚 9 | // - phC - C 相 adc 引脚（可选） 10 | 11 | 12 | #define _ADC_VOLTAGE 3.3f //ADC 电压 13 | #define _ADC_RESOLUTION 4095.0f //ADC 分辨率 14 | 15 | // ADC 计数到电压转换比率求解 16 | #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) 17 | 18 | #define NOT_SET -12345.0 19 | #define _isset(a) ( (a) != (NOT_SET) ) 20 | 21 | CurrSense::CurrSense(int Mot_Num) 22 | { 23 | if(Mot_Num==0) 24 | { 25 | pinA = 39; 26 | pinB = 36; 27 | //int pinC; 28 | _shunt_resistor = 0.01; 29 | amp_gain = 50; 30 | 31 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 32 | 33 | // DengFOC V3P 34 | gain_a = volts_to_amps_ratio*-1; 35 | gain_b = volts_to_amps_ratio*-1; 36 | 37 | // DengFOC V4 38 | // gain_a = volts_to_amps_ratio; 39 | // gain_b = volts_to_amps_ratio; 40 | 41 | 42 | gain_c = volts_to_amps_ratio; 43 | } 44 | if(Mot_Num==1) 45 | { 46 | pinA = 35; 47 | pinB = 34; 48 | //int pinC; 49 | _shunt_resistor = 0.01; 50 | amp_gain = 50; 51 | 52 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 53 | 54 | // DengFOC V3P 55 | gain_a = volts_to_amps_ratio*-1; 56 | gain_b = volts_to_amps_ratio*-1; 57 | 58 | // DengFOC V4 59 | // gain_a = volts_to_amps_ratio; 60 | // gain_b = volts_to_amps_ratio; 61 | 62 | gain_c = volts_to_amps_ratio; 63 | } 64 | } 65 | float CurrSense::readADCVoltageInline(const int pinA){ 66 | uint32_t raw_adc = analogRead(pinA); 67 | return raw_adc * _ADC_CONV; 68 | } 69 | void CurrSense::configureADCInline(const int pinA,const int pinB, const int pinC){ 70 | pinMode(pinA, INPUT); 71 | pinMode(pinB, INPUT); 72 | if( _isset(pinC) ) pinMode(pinC, INPUT); 73 | } 74 | 75 | // 查找 ADC 零偏移量的函数 76 | void CurrSense::calibrateOffsets(){ 77 | const int calibration_rounds = 1000; 78 | 79 | // 查找0电流时候的电压 80 | offset_ia = 0; 81 | offset_ib = 0; 82 | offset_ic = 0; 83 | // 读数1000次 84 | for (int i = 0; i < calibration_rounds; i++) { 85 | offset_ia += readADCVoltageInline(pinA); 86 | offset_ib += readADCVoltageInline(pinB); 87 | if(_isset(pinC)) offset_ic += readADCVoltageInline(pinC); 88 | delay(1); 89 | } 90 | // 求平均，得到误差 91 | offset_ia = offset_ia / calibration_rounds; 92 | offset_ib = offset_ib / calibration_rounds; 93 | if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; 94 | } 95 | 96 | void CurrSense::init(){ 97 | // 配置函数 98 | configureADCInline(pinA,pinB,pinC); 99 | // 校准 100 | calibrateOffsets(); 101 | } 102 | 103 | 104 | // 读取全部三相电流 105 | 106 | void CurrSense::getPhaseCurrents(){ 107 | current_a = (readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps 108 | current_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps 109 | current_c = (!_isset(pinC)) ? 0 : (readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps 110 | } 111 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/InlineCurrent.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class CurrSense 4 | { 5 | public: 6 | CurrSense(int Mot_Num); 7 | float readADCVoltageInline(const int pinA); 8 | void configureADCInline(const int pinA,const int pinB, const int pinC); 9 | void calibrateOffsets(); 10 | void init(); 11 | void getPhaseCurrents(); 12 | float current_a,current_b,current_c; 13 | int pinA; 14 | int pinB; 15 | int pinC; 16 | float offset_ia; 17 | float offset_ib; 18 | float offset_ic; 19 | float _shunt_resistor; 20 | float amp_gain; 21 | 22 | float volts_to_amps_ratio; 23 | 24 | float gain_a; 25 | float gain_b; 26 | float gain_c; 27 | private: 28 | int _Mot_Num; 29 | }; 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/例程/位置闭环（角度-速度-力矩）.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | void loop() 20 | { 21 | //设置PID 22 | DFOC_M0_SET_ANGLE_PID(0.5,0,0,0); 23 | DFOC_M0_SET_VEL_PID(0.01,0.00,0,0); 24 | //给库设定速度 25 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/例程/双电机闭环位置、速度、力矩.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | 11 | void setup() { 12 | Serial.begin(115200); 13 | DFOC_enable(); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_M0_alignSensor(7,-1); 16 | DFOC_M1_alignSensor(7,-1); 17 | //DFOC_M1_alignSensor(7,1); 18 | 19 | } 20 | 21 | int count=0; 22 | void loop() 23 | { 24 | runFOC(); 25 | // DFOC_M0_setTorque(1); 26 | // DFOC_M1_setTorque(1); 27 | 28 | //力位（加入电流环后） 29 | // DFOC_M0_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 30 | // DFOC_M0_SET_CURRENT_PID(1.25,50,0,100000); 31 | // DFOC_M0_set_Force_Angle(serial_motor_target()); 32 | // DFOC_M1_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 33 | // DFOC_M1_SET_CURRENT_PID(1.25,50,0,100000); 34 | // DFOC_M1_set_Force_Angle(serial_motor_target()); 35 | 36 | //速度（加入电流环后） 37 | // DFOC_M0_SET_VEL_PID(3,2,0,100000,0.5); 38 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 39 | // DFOC_M0_setVelocity(serial_motor_target()); 40 | // DFOC_M0_SET_VEL_PID(0.1,2,0,100000,0.5); 41 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 42 | // DFOC_M0_setVelocity(serial_motor_target()); 43 | 44 | //位置-速度-力（加入电流环后） 45 | DFOC_M0_SET_ANGLE_PID(1,0,0,100000,30); 46 | DFOC_M0_SET_VEL_PID(0.02,1,0,100000,0.5); 47 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 48 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 49 | 50 | //位置-速度-力（加入电流环后） 51 | DFOC_M1_SET_ANGLE_PID(1,0,0,100000,30); 52 | DFOC_M1_SET_VEL_PID(0.02,1,0,100000,0.5); 53 | DFOC_M1_SET_CURRENT_PID(5,200,0,100000); 54 | DFOC_M1_set_Velocity_Angle(serial_motor_target()); 55 | //电流力矩 56 | // DFOC_M1_SET_CURRENT_PID(5,200,0,100000); 57 | // DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 58 | 59 | // DFOC_M0_setTorque(serial_motor_target()); 60 | // DFOC_M1_setTorque(serial_motor_target()); 61 | 62 | 63 | 64 | count++; 65 | if(count>100) 66 | { 67 | count=0; 68 | //Serial.printf("%f\n", DFOC_M0_Current()); 69 | Serial.printf("%f,%f,%f,%f\n", DFOC_M0_Current(),DFOC_M0_Angle(), DFOC_M0_Velocity(),serial_motor_target()); 70 | // Serial.printf("%f,%f,%f\n", DFOC_M0_Angle(), S0_electricalAngle(),S1_electricalAngle()); 71 | // Serial.printf("%f,%f,%f\n", DFOC_M0_Current(), DFOC_M1_Current(),serial_motor_target()); 72 | } 73 | //接收串口 74 | serialReceiveUserCommand(); 75 | 76 | } 77 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/例程/用电流环改进位置、速度、力矩.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | int count=0; 20 | void loop() 21 | { 22 | runFOC(); 23 | 24 | //力位（加入电流环后） 25 | // DFOC_M0_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 26 | // DFOC_M0_SET_CURRENT_PID(1.25,50,0,100000); 27 | // DFOC_M0_set_Force_Angle(serial_motor_target()); 28 | 29 | //速度（加入电流环后） 30 | // DFOC_M0_SET_VEL_PID(3,2,0,100000,0.5); 31 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 32 | // DFOC_M0_setVelocity(serial_motor_target()); 33 | 34 | //位置-速度-力（加入电流环后） 35 | DFOC_M0_SET_ANGLE_PID(1,0,0,100000,30); 36 | DFOC_M0_SET_VEL_PID(0.02,1,0,100000,0.5); 37 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 38 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 39 | 40 | //电流力矩 41 | // DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 42 | // DFOC_M0_setTorque(serial_motor_target()); 43 | 44 | count++; 45 | if(count>30) 46 | { 47 | count=0; 48 | //Serial.printf("%f\n", DFOC_M0_Current()); 49 | Serial.printf("%f,%f\n", DFOC_M0_Current(), DFOC_M0_Velocity()); 50 | } 51 | //接收串口 52 | serialReceiveUserCommand(); 53 | 54 | } 55 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/例程/电压力矩环.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(serial_motor_target(),_electricalAngle()); //电压力矩 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/例程/电流力矩闭环.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | int count=0; 20 | void loop() 21 | { 22 | runFOC(); 23 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 24 | DFOC_M0_setTorque(serial_motor_target()); 25 | count++; 26 | if(count>30) 27 | { 28 | count=0; 29 | Serial.printf("%f\n", DFOC_M0_Current()); 30 | } 31 | //接收串口 32 | serialReceiveUserCommand(); 33 | 34 | } 35 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/例程/速度闭环.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | 20 | void loop() 21 | { 22 | //设置速度环PID 23 | DFOC_M0_SET_VEL_PID(0.005,0.00,0,0); 24 | //设置速度 25 | DFOC_M0_setVelocity(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/例程/闭环位置（力位）-方式1.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | int count_output=0; 19 | 20 | void loop() 21 | { 22 | 23 | //设置PID 24 | DFOC_M0_SET_ANGLE_PID(0.3,0,0,0); 25 | DFOC_M0_set_Force_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } -------------------------------------------------------------------------------- /DengFOC 库/V0.5 双路电机驱动/例程/闭环位置（力位）-方式2.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(Kp*(serial_motor_target()-Sensor_DIR*Sensor_Angle)*180/PI,_electricalAngle()); //位置闭环 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | 9 | void M0_setPwm(float Ua, float Ub, float Uc); 10 | void M1_setPwm(float Ua, float Ub, float Uc); 11 | void DFOC_enable(); 12 | void DFOC_disable(); 13 | void M0_setTorque(float Uq,float angle_el); 14 | void M1_setTorque(float Uq,float angle_el); 15 | float _normalizeAngle(float angle); 16 | void DFOC_Vbus(float power_supply); 17 | void DFOC_M0_alignSensor(int _PP,int _DIR); 18 | void DFOC_M1_alignSensor(int _PP,int _DIR); 19 | float S0_electricalAngle(); 20 | float S1_electricalAngle(); 21 | float cal_Iq_Id(float current_a,float current_b,float angle_el); 22 | float serial_motor_target(); 23 | String serialReceiveUserCommand(); 24 | //传感器读取 25 | float DFOC_M0_Velocity(); 26 | float DFOC_M1_Velocity(); 27 | float DFOC_M0_Angle(); 28 | float DFOC_M1_Angle(); 29 | float DFOC_M0_Current(); 30 | float DFOC_M1_Current(); 31 | //PID 32 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 33 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 34 | void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp); 35 | void DFOC_M1_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 36 | void DFOC_M1_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 37 | void DFOC_M1_SET_CURRENT_PID(float P,float I,float D,float ramp); 38 | float DFOC_M0_VEL_PID(float error); 39 | float DFOC_M0_ANGLE_PID(float error); 40 | float DFOC_M1_VEL_PID(float error); 41 | float DFOC_M1_ANGLE_PID(float error); 42 | 43 | //接口函数 44 | void DFOC_M0_set_Velocity_Angle(float Target); 45 | void DFOC_M0_setVelocity(float Target); 46 | void DFOC_M0_set_Force_Angle(float Target); 47 | void DFOC_M1_set_Velocity_Angle(float Target); 48 | void DFOC_M1_setVelocity(float Target); 49 | void DFOC_M1_set_Force_Angle(float Target); 50 | void DFOC_M0_setTorque(float Target); 51 | void DFOC_M1_setTorque(float Target); 52 | //runFOC 循环函数 53 | void runFOC(); 54 | //测试函数 55 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/InlineCurrent.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "InlineCurrent.h" 3 | 4 | 5 | // - shunt_resistor - 分流电阻值 6 | // - gain - 电流检测运算放大器增益 7 | // - phA - A 相 adc 引脚 8 | // - phB - B 相 adc 引脚 9 | // - phC - C 相 adc 引脚（可选） 10 | 11 | 12 | #define _ADC_VOLTAGE 3.3f //ADC 电压 13 | #define _ADC_RESOLUTION 4095.0f //ADC 分辨率 14 | 15 | // ADC 计数到电压转换比率求解 16 | #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) 17 | 18 | #define NOT_SET -12345.0 19 | #define _isset(a) ( (a) != (NOT_SET) ) 20 | 21 | CurrSense::CurrSense(int Mot_Num) 22 | { 23 | if(Mot_Num==0) 24 | { 25 | pinA = 39; 26 | pinB = 36; 27 | //int pinC; 28 | _shunt_resistor = 0.01; 29 | amp_gain = 50; 30 | 31 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 32 | 33 | // DengFOC V3P 34 | // gain_a = volts_to_amps_ratio*-1; 35 | // gain_b = volts_to_amps_ratio*-1; 36 | 37 | // DengFOC V4 38 | gain_a = volts_to_amps_ratio; 39 | gain_b = volts_to_amps_ratio; 40 | 41 | 42 | gain_c = volts_to_amps_ratio; 43 | } 44 | if(Mot_Num==1) 45 | { 46 | pinA = 35; 47 | pinB = 34; 48 | //int pinC; 49 | _shunt_resistor = 0.01; 50 | amp_gain = 50; 51 | 52 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 53 | 54 | // DengFOC V3P 55 | // gain_a = volts_to_amps_ratio*-1; 56 | // gain_b = volts_to_amps_ratio*-1; 57 | 58 | // DengFOC V4 59 | gain_a = volts_to_amps_ratio; 60 | gain_b = volts_to_amps_ratio; 61 | 62 | gain_c = volts_to_amps_ratio; 63 | } 64 | } 65 | float CurrSense::readADCVoltageInline(const int pinA){ 66 | uint32_t raw_adc = analogRead(pinA); 67 | return raw_adc * _ADC_CONV; 68 | } 69 | void CurrSense::configureADCInline(const int pinA,const int pinB, const int pinC){ 70 | pinMode(pinA, INPUT); 71 | pinMode(pinB, INPUT); 72 | if( _isset(pinC) ) pinMode(pinC, INPUT); 73 | } 74 | 75 | // 查找 ADC 零偏移量的函数 76 | void CurrSense::calibrateOffsets(){ 77 | const int calibration_rounds = 1000; 78 | 79 | // 查找0电流时候的电压 80 | offset_ia = 0; 81 | offset_ib = 0; 82 | offset_ic = 0; 83 | // 读数1000次 84 | for (int i = 0; i < calibration_rounds; i++) { 85 | offset_ia += readADCVoltageInline(pinA); 86 | offset_ib += readADCVoltageInline(pinB); 87 | if(_isset(pinC)) offset_ic += readADCVoltageInline(pinC); 88 | delay(1); 89 | } 90 | // 求平均，得到误差 91 | offset_ia = offset_ia / calibration_rounds; 92 | offset_ib = offset_ib / calibration_rounds; 93 | if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; 94 | } 95 | 96 | void CurrSense::init(){ 97 | // 配置函数 98 | configureADCInline(pinA,pinB,pinC); 99 | // 校准 100 | calibrateOffsets(); 101 | } 102 | 103 | 104 | // 读取全部三相电流 105 | 106 | void CurrSense::getPhaseCurrents(){ 107 | current_a = (readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps 108 | current_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps 109 | current_c = (!_isset(pinC)) ? 0 : (readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps 110 | } 111 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/InlineCurrent.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class CurrSense 4 | { 5 | public: 6 | CurrSense(int Mot_Num); 7 | float readADCVoltageInline(const int pinA); 8 | void configureADCInline(const int pinA,const int pinB, const int pinC); 9 | void calibrateOffsets(); 10 | void init(); 11 | void getPhaseCurrents(); 12 | float current_a,current_b,current_c; 13 | int pinA; 14 | int pinB; 15 | int pinC; 16 | float offset_ia; 17 | float offset_ib; 18 | float offset_ic; 19 | float _shunt_resistor; 20 | float amp_gain; 21 | 22 | float volts_to_amps_ratio; 23 | 24 | float gain_a; 25 | float gain_b; 26 | float gain_c; 27 | private: 28 | int _Mot_Num; 29 | }; 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/例程/位置闭环（角度-速度-力矩）.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | void loop() 20 | { 21 | //设置PID 22 | DFOC_M0_SET_ANGLE_PID(0.5,0,0,0); 23 | DFOC_M0_SET_VEL_PID(0.01,0.00,0,0); 24 | //给库设定速度 25 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/例程/双电机闭环位置、速度、力矩.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | 11 | void setup() { 12 | Serial.begin(115200); 13 | DFOC_enable(); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_M0_alignSensor(7,-1); 16 | DFOC_M1_alignSensor(7,-1); 17 | //DFOC_M1_alignSensor(7,1); 18 | 19 | } 20 | 21 | int count=0; 22 | void loop() 23 | { 24 | runFOC(); 25 | // DFOC_M0_setTorque(1); 26 | // DFOC_M1_setTorque(1); 27 | 28 | //力位（加入电流环后） 29 | // DFOC_M0_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 30 | // DFOC_M0_SET_CURRENT_PID(1.25,50,0,100000); 31 | // DFOC_M0_set_Force_Angle(serial_motor_target()); 32 | // DFOC_M1_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 33 | // DFOC_M1_SET_CURRENT_PID(1.25,50,0,100000); 34 | // DFOC_M1_set_Force_Angle(serial_motor_target()); 35 | 36 | //速度（加入电流环后） 37 | // DFOC_M0_SET_VEL_PID(3,2,0,100000,0.5); 38 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 39 | // DFOC_M0_setVelocity(serial_motor_target()); 40 | // DFOC_M0_SET_VEL_PID(0.1,2,0,100000,0.5); 41 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 42 | // DFOC_M0_setVelocity(serial_motor_target()); 43 | 44 | //位置-速度-力（加入电流环后） 45 | DFOC_M0_SET_ANGLE_PID(1,0,0,100000,30); 46 | DFOC_M0_SET_VEL_PID(0.02,1,0,100000,0.5); 47 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 48 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 49 | 50 | //位置-速度-力（加入电流环后） 51 | DFOC_M1_SET_ANGLE_PID(1,0,0,100000,30); 52 | DFOC_M1_SET_VEL_PID(0.02,1,0,100000,0.5); 53 | DFOC_M1_SET_CURRENT_PID(5,200,0,100000); 54 | DFOC_M1_set_Velocity_Angle(serial_motor_target()); 55 | //电流力矩 56 | // DFOC_M1_SET_CURRENT_PID(5,200,0,100000); 57 | // DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 58 | 59 | // DFOC_M0_setTorque(serial_motor_target()); 60 | // DFOC_M1_setTorque(serial_motor_target()); 61 | 62 | 63 | 64 | count++; 65 | if(count>100) 66 | { 67 | count=0; 68 | //Serial.printf("%f\n", DFOC_M0_Current()); 69 | Serial.printf("%f,%f,%f,%f\n", DFOC_M0_Current(),DFOC_M0_Angle(), DFOC_M0_Velocity(),serial_motor_target()); 70 | // Serial.printf("%f,%f,%f\n", DFOC_M0_Angle(), S0_electricalAngle(),S1_electricalAngle()); 71 | // Serial.printf("%f,%f,%f\n", DFOC_M0_Current(), DFOC_M1_Current(),serial_motor_target()); 72 | } 73 | //接收串口 74 | serialReceiveUserCommand(); 75 | 76 | } 77 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/例程/用电流环改进位置、速度、力矩.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | int count=0; 20 | void loop() 21 | { 22 | runFOC(); 23 | 24 | //力位（加入电流环后） 25 | // DFOC_M0_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 26 | // DFOC_M0_SET_CURRENT_PID(1.25,50,0,100000); 27 | // DFOC_M0_set_Force_Angle(serial_motor_target()); 28 | 29 | //速度（加入电流环后） 30 | // DFOC_M0_SET_VEL_PID(3,2,0,100000,0.5); 31 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 32 | // DFOC_M0_setVelocity(serial_motor_target()); 33 | 34 | //位置-速度-力（加入电流环后） 35 | DFOC_M0_SET_ANGLE_PID(1,0,0,100000,30); 36 | DFOC_M0_SET_VEL_PID(0.02,1,0,100000,0.5); 37 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 38 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 39 | 40 | //电流力矩 41 | // DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 42 | // DFOC_M0_setTorque(serial_motor_target()); 43 | 44 | count++; 45 | if(count>30) 46 | { 47 | count=0; 48 | //Serial.printf("%f\n", DFOC_M0_Current()); 49 | Serial.printf("%f,%f\n", DFOC_M0_Current(), DFOC_M0_Velocity()); 50 | } 51 | //接收串口 52 | serialReceiveUserCommand(); 53 | 54 | } 55 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/例程/电压力矩环.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(serial_motor_target(),_electricalAngle()); //电压力矩 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/例程/电流力矩闭环.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | int count=0; 20 | void loop() 21 | { 22 | runFOC(); 23 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 24 | DFOC_M0_setTorque(serial_motor_target()); 25 | count++; 26 | if(count>30) 27 | { 28 | count=0; 29 | Serial.printf("%f\n", DFOC_M0_Current()); 30 | } 31 | //接收串口 32 | serialReceiveUserCommand(); 33 | 34 | } 35 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/例程/速度闭环.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | 20 | void loop() 21 | { 22 | //设置速度环PID 23 | DFOC_M0_SET_VEL_PID(0.005,0.00,0,0); 24 | //设置速度 25 | DFOC_M0_setVelocity(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/例程/闭环位置（力位）-方式1.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | int count_output=0; 19 | 20 | void loop() 21 | { 22 | 23 | //设置PID 24 | DFOC_M0_SET_ANGLE_PID(0.3,0,0,0); 25 | DFOC_M0_set_Force_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } -------------------------------------------------------------------------------- /DengFOC 库/V0.6 支持SVPWM算法/例程/闭环位置（力位）-方式2.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(Kp*(serial_motor_target()-Sensor_DIR*Sensor_Angle)*180/PI,_electricalAngle()); //位置闭环 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | void setPwm(float Ua, float Ub, float Uc); 9 | float setTorque(float Uq,float angle_el); 10 | float _normalizeAngle(float angle); 11 | void DFOC_Vbus(float power_supply); 12 | void DFOC_alignSensor(int _PP,int _DIR); 13 | float _electricalAngle(); 14 | 15 | float serial_motor_target(); 16 | String serialReceiveUserCommand(); 17 | //传感器读取 18 | float DFOC_M0_Velocity(); 19 | float DFOC_M0_Angle(); 20 | float DFOC_M0_Current(); 21 | //PID 22 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp); 23 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp); 24 | void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp); 25 | float DFOC_M0_VEL_PID(float error); 26 | float DFOC_M0_ANGLE_PID(float error); 27 | 28 | //接口函数 29 | void DFOC_M0_set_Velocity_Angle(float Target); 30 | void DFOC_M0_setVelocity(float Target); 31 | void DFOC_M0_set_Force_Angle(float Target); 32 | void DFOC_M0_setTorque(float Target); 33 | //runFOC 循环函数 34 | void runFOC(); 35 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/InlineCurrent.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "InlineCurrent.h" 3 | 4 | 5 | // - shunt_resistor - 分流电阻值 6 | // - gain - 电流检测运算放大器增益 7 | // - phA - A 相 adc 引脚 8 | // - phB - B 相 adc 引脚 9 | // - phC - C 相 adc 引脚（可选） 10 | 11 | 12 | #define _ADC_VOLTAGE 3.3f //ADC 电压 13 | #define _ADC_RESOLUTION 4095.0f //ADC 分辨率 14 | 15 | // ADC 计数到电压转换比率求解 16 | #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) 17 | 18 | #define NOT_SET -12345.0 19 | #define _isset(a) ( (a) != (NOT_SET) ) 20 | 21 | CurrSense::CurrSense(int Mot_Num) 22 | { 23 | if(Mot_Num==0) 24 | { 25 | pinA = 39; 26 | pinB = 36; 27 | //int pinC; 28 | _shunt_resistor = 0.01; 29 | amp_gain = 50; 30 | 31 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 32 | 33 | // gains for each phase 34 | gain_a = volts_to_amps_ratio*-1; 35 | gain_b = volts_to_amps_ratio*-1; 36 | gain_c = volts_to_amps_ratio; 37 | } 38 | if(Mot_Num==1) 39 | { 40 | pinA = 35; 41 | pinB = 34; 42 | //int pinC; 43 | _shunt_resistor = 0.01; 44 | amp_gain = 50; 45 | 46 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 47 | 48 | // gains for each phase 49 | gain_a = volts_to_amps_ratio*-1; 50 | gain_b = volts_to_amps_ratio*-1; 51 | gain_c = volts_to_amps_ratio; 52 | } 53 | } 54 | float CurrSense::readADCVoltageInline(const int pinA){ 55 | uint32_t raw_adc = analogRead(pinA); 56 | return raw_adc * _ADC_CONV; 57 | } 58 | void CurrSense::configureADCInline(const int pinA,const int pinB, const int pinC){ 59 | pinMode(pinA, INPUT); 60 | pinMode(pinB, INPUT); 61 | if( _isset(pinC) ) pinMode(pinC, INPUT); 62 | } 63 | 64 | // 查找 ADC 零偏移量的函数 65 | void CurrSense::calibrateOffsets(){ 66 | const int calibration_rounds = 1000; 67 | 68 | // 查找0电流时候的电压 69 | offset_ia = 0; 70 | offset_ib = 0; 71 | offset_ic = 0; 72 | // 读数1000次 73 | for (int i = 0; i < calibration_rounds; i++) { 74 | offset_ia += readADCVoltageInline(pinA); 75 | offset_ib += readADCVoltageInline(pinB); 76 | if(_isset(pinC)) offset_ic += readADCVoltageInline(pinC); 77 | delay(1); 78 | } 79 | // 求平均，得到误差 80 | offset_ia = offset_ia / calibration_rounds; 81 | offset_ib = offset_ib / calibration_rounds; 82 | if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; 83 | } 84 | 85 | void CurrSense::init(){ 86 | // 配置函数 87 | configureADCInline(pinA,pinB,pinC); 88 | // 校准 89 | calibrateOffsets(); 90 | } 91 | 92 | 93 | // 读取全部三相电流 94 | 95 | void CurrSense::getPhaseCurrents(){ 96 | current_a = (readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps 97 | current_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps 98 | current_c = (!_isset(pinC)) ? 0 : (readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps 99 | } 100 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/InlineCurrent.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class CurrSense 4 | { 5 | public: 6 | CurrSense(int Mot_Num); 7 | float readADCVoltageInline(const int pinA); 8 | void configureADCInline(const int pinA,const int pinB, const int pinC); 9 | void calibrateOffsets(); 10 | void init(); 11 | void getPhaseCurrents(); 12 | float current_a,current_b,current_c; 13 | int pinA; 14 | int pinB; 15 | int pinC; 16 | float offset_ia; 17 | float offset_ib; 18 | float offset_ic; 19 | float _shunt_resistor; 20 | float amp_gain; 21 | 22 | float volts_to_amps_ratio; 23 | 24 | float gain_a; 25 | float gain_b; 26 | float gain_c; 27 | private: 28 | int _Mot_Num; 29 | }; 30 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/例程/位置闭环（角度-速度-力矩）.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | void loop() 20 | { 21 | //设置PID 22 | DFOC_M0_SET_ANGLE_PID(0.5,0,0,0); 23 | DFOC_M0_SET_VEL_PID(0.01,0.00,0,0); 24 | //给库设定速度 25 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/例程/电压力矩环.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(serial_motor_target(),_electricalAngle()); //电压力矩 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/例程/电流力矩闭环.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | int count=0; 20 | void loop() 21 | { 22 | runFOC(); 23 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 24 | DFOC_M0_setTorque(serial_motor_target()); 25 | count++; 26 | if(count>30) 27 | { 28 | count=0; 29 | Serial.printf("%f\n", DFOC_M0_Current()); 30 | } 31 | //接收串口 32 | serialReceiveUserCommand(); 33 | 34 | } 35 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/例程/速度闭环.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | 20 | void loop() 21 | { 22 | //设置速度环PID 23 | DFOC_M0_SET_VEL_PID(0.005,0.00,0,0); 24 | //设置速度 25 | DFOC_M0_setVelocity(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/例程/闭环位置（力位）-方式1.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | int count_output=0; 19 | 20 | void loop() 21 | { 22 | 23 | //设置PID 24 | DFOC_M0_SET_ANGLE_PID(0.3,0,0,0); 25 | DFOC_M0_set_Force_Angle(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } -------------------------------------------------------------------------------- /DengFOC 库/v0.3 支持电流环/例程/闭环位置（力位）-方式2.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(Kp*(serial_motor_target()-Sensor_DIR*Sensor_Angle)*180/PI,_electricalAngle()); //位置闭环 27 | serialReceiveUserCommand(); 28 | } 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DengFOC 极简FOC库 -- 开源 2 | 3 | 4 | # 课程教案 和 库文档,请访问：dengfoc.com 5 | 6 | ## 1 概述 7 | 8 | DengFOC 库是灯哥开源的，基于 Arduino 和 ESP32 硬件开发的开源 FOC 库。 9 | 10 | ![mainTitle](pic/mainTitle.png) 11 | 12 | 本FOC库参考了 SimpleFOC 的部分设计，但更秉承极简主义和实用主意，拥有比SimpleFOC更简易但更丰富的二次开发和应用特性： 13 | 14 | 1. **资源占用低**：内存占用比SImpleFOC少2/3 15 | 2. **开放度更广**：库把基本的FOC算法接口（如电角度、Iaplha 、IBeta）等以一种及其简单的方式向用户开放，使得用户可以直接接触到 FOC 算法实现的全过程，易于学习和在此基础上更进一步的开发 FOC 算法。 16 | 3. **外接能力强**：支持与OpenMV、树莓派、Python等多种硬件方案和软件语言直接的对接和互相调用，可以以比SimpleFOC更快的方式完成无刷电机应用的开发。 17 | 4. **即插即用，无需校准**：先进的参数自识别功能可以使得用户无需配置任何参数，直接插入电机和编码器即可跑FOC 18 | 5. **无线控制支持**：高速UDP，ESPNow 通讯，无需信号线即可控制电机 19 | 6. **脚本支持**：库内建强大的脚本语言 Lua，可以在不编译的情况下快速建立FOC应用 20 | 7. **强大的工具链支持**：支持与 Matlab 、Simulink 、ROS 等系统直接通讯，秒速建立机器人应用 21 | 8. **高精度减速器应用支持**：支持双编码器减速机应用 22 | 23 | ## 2 重要链接 24 | 25 | 本 FOC 库与 DengFOC 硬件联合组成一整套完整可用的 FOC 电机驱动方案，资料链接： 26 | 27 | 1 [灯哥开源 淘宝店--一站配齐DengFOC板](https://shop564514875.taobao.com/) 您的支持就是我们持续做开源内容和课程的动力，项目收益将用于后续开发DengFOC和做课程~ 28 | 29 | 2 [DengFOC硬件 Github](https://github.com/ToanTech/Deng-s-foc-controller) 30 | 31 | 3 [DengFOC官网](http://dengfoc.com/#/) 包含课程文字版讲义，DengFOC使用文档，库使用方法等。 32 | 33 | ## 2 社区 34 | 35 | 本FOC板子社区唯四Q群，欢迎加入：**开源FOC无刷驱动交流群 灯哥开源 群号 778255240（1群） 735755513（2群）471832283（3群）834835665（4群）247431752(5群)** 36 | 37 | 任何使用问题和 DIY 问题 都会在这里做直接的讨论解答 38 | 39 | ## 3 怎样写 DengFOC 代码 40 | 41 | 灯哥在设计DengFOC库的时候，秉承极简和低内存占用的设计思路。因此，所有的功能块都是走**即用即引入，不用不引入**的原则，以此获得极低的资源占用率。 42 | 43 | 目前库已经有的模块有： 44 | 45 | 1. DengFOC 主库 -- 包含DengFOC基础运行函数 46 | 2. AS5600 编码器库 -- 传感器库 47 | 3. InlineCurrent 库 -- INA240 电流传感器库 48 | 3. 低通滤波库 49 | 50 | 目前已实现的功能： 51 | 52 | 1. 开环速度 53 | 1. 闭环速度 54 | 2. AS5600传感器读取 55 | 3. 在线电流检测 56 | 4. 角度闭环（力位闭环） 57 | 5. 力角度闭环（位置+速度+力闭环） 58 | 5. 电流力矩闭环 59 | 8. 双路电机完整三环驱动（位置，速度，力矩） 60 | 61 | 下面用力+位置闭环代码（力位闭环，电流力矩环作为内环，位置作为外环），来示范 DengFOC 库的代码编写，更多有关于库函数的说明，请查看[DengFOC官网](dengfoc.com) 。 62 | 63 | ### 3.1 电机力位闭环（基于电流力矩环） 64 | 65 | ```c++ 66 | #include "DengFOC.h" 67 | 68 | int Sensor_DIR=-1; //传感器方向 69 | int Motor_PP=7; //电机极对数 70 | 71 | void setup() { 72 | Serial.begin(115200); 73 | DFOC_Vbus(12.6); //设定驱动器供电电压 74 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 75 | } 76 | 77 | void loop() 78 | { 79 | 80 | //设置PID 81 | DFOC_M0_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); //角度PID 82 | DFOC_M0_SET_CURRENT_PID(1.25,50,0,100000); //电流环PID 83 | DFOC_M0_set_Force_Angle(serial_motor_target()); //力位控制指令 84 | 85 | //接收串口 86 | serialReceiveUserCommand(); 87 | 88 | } 89 | ``` 90 | 91 | ## 4 免费手把手教写FOC算法原理课 92 | 93 | 为了方便大家更进一步的理解FOC的算法和原理，我做了手把手教些FOC算法原理课，通过这些原理课你能够快速的从原理角度理解FOC知识并尝试写出自己的FOC基本功能库，视频链接： 94 | 95 | 1 [【手把手教写FOC算法】1_起源，无刷电机概念与控制原理](https://www.bilibili.com/video/BV1dy4y1X7yx) 96 | 97 | 2 [【手把手教写FOC算法】2_克拉克变换，建立简化电机数学模型](https://www.bilibili.com/video/BV1x84y1V76u/) 98 | 99 | 3 [【手把手教写FOC算法】3_等幅值变换与克拉克逆变换](https://www.bilibili.com/video/BV13s4y1Z7Tg/) 100 | 101 | 4 [【手把手教写FOC算法】4_帕克变换](https://www.bilibili.com/video/BV1t24y1u7do/) 102 | 103 | 5a [【手把手教写FOC算法】5a_撰写开环速度代码的前置知识](https://www.bilibili.com/video/BV1Pc411s7mP/) 104 | 105 | 5b [【手把手教写FOC算法】5b_开环速度代码编程+硬件调试教学](https://www.bilibili.com/video/BV16X4y167XZ/) 106 | 107 | 6a [【手把手教写FOC算法】6a_撰写闭环位置代码的前置知识](https://www.bilibili.com/video/BV1Rm4y1871K/) 108 | 109 | 6b [【手把手教写FOC算法】6b_闭环位置代码编程+硬件调试教学](https://www.bilibili.com/video/BV1yh4y1J7Xx/) 110 | 111 | 7a1 [【手把手教写FOC算法】7a1_写闭环速度代码的前置知识](https://www.bilibili.com/video/BV18m4y1v75j/?spm_id_from=333.788&vd_source=365d5478018f3e4c39b71b3dd6a7dd0a) 112 | 113 | 7a2[【手把手教写FOC算法】7a2_速度低通滤波器](https://www.bilibili.com/video/BV16h4y1X7gZ/?spm_id_from=333.999.0.0) 114 | 115 | 7a3 [【手把手教写FOC算法】7a3_速度PI控制器](https://www.bilibili.com/video/BV17s4y1y767/?spm_id_from=333.999.0.0) 116 | 117 | 7b [【手把手教写FOC算法】7b_闭环速度代码编程+硬件调试教学](https://www.bilibili.com/video/BV1tu4y1o7WU/?spm_id_from=333.999.0.0) 118 | 119 | 8a [【手把手教写FOC算法】8a_撰写电流环代码的前置知识](https://www.bilibili.com/video/BV1z14y1v7yS/?spm_id_from=333.999.0.0) 120 | 121 | 8b [【手把手教写FOC算法】8b_精确电流力矩环代码编程+硬件调试教学](https://www.bilibili.com/video/BV1Sh4y1Q7ue/?spm_id_from=333.999.0.0) 122 | 123 | 9a[【手把手教写FOC算法】9a_用电流环改进速度和位置闭环-前置知识](https://www.bilibili.com/video/BV1Zy4y1A7pL/) 124 | 125 | 9b[【手把手教写FOC算法】9b_电流环改进速度和位置闭环代码编程+硬件调试教学](https://www.bilibili.com/video/BV1dy4y1N798/) 126 | 127 | ## 5 DengFOC爱好者拓展支持 128 | 129 | DengFOC是开源项目，欢迎各位爱好者做二次开发。优秀的项目我将会在这里收录，如果你基于它做了有意思的项目，欢迎在群里联系我。 130 | 131 | 目前，已经有爱好者把项目移植到stm32平台，链接如下： 132 | 133 | https://github.com/haotianh9/DengFOC_on_STM32 134 | 135 | -------------------------------------------------------------------------------- /pic/mainTitle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ToanTech/DengFOC_Lib/8494ec97ea240691472a36fe3282c00fefcf32e2/pic/mainTitle.png -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第七章a+b 闭环速度FOC代码-DengFOC库v0.2/1 DengFOC库V0.2+实例/DengFOC_Lib_Lesson7_CloseLoop_Vel/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第七章a+b 闭环速度FOC代码-DengFOC库v0.2/1 DengFOC库V0.2+实例/DengFOC_Lib_Lesson7_CloseLoop_Vel/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第七章a+b 闭环速度FOC代码-DengFOC库v0.2/1 DengFOC库V0.2+实例/DengFOC_Lib_Lesson7_CloseLoop_Vel/DengFOC.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "AS5600.h" 3 | #include "lowpass_filter.h" 4 | #include "pid.h" 5 | 6 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 7 | float voltage_power_supply; 8 | float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0; 9 | #define _3PI_2 4.71238898038f 10 | float zero_electric_angle=0; 11 | int PP=1,DIR=1; 12 | int pwmA = 32; 13 | int pwmB = 33; 14 | int pwmC = 25; 15 | 16 | //低通滤波初始化 17 | LowPassFilter M0_Vel_Flt = LowPassFilter(0.01); // Tf = 10ms //M0速度环 18 | //PID 19 | PIDController vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2}; 20 | PIDController angle_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = 100}; 21 | 22 | //AS5600 23 | Sensor_AS5600 S0=Sensor_AS5600(0); 24 | TwoWire S0_I2C = TwoWire(0); 25 | 26 | //=================PID 设置函数================= 27 | //速度PID 28 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp) //M0角度环PID设置 29 | { 30 | vel_loop_M0.P=P; 31 | vel_loop_M0.I=I; 32 | vel_loop_M0.D=D; 33 | vel_loop_M0.output_ramp=ramp; 34 | } 35 | //角度PID 36 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp) //M0角度环PID设置 37 | { 38 | angle_loop_M0.P=P; 39 | angle_loop_M0.I=I; 40 | angle_loop_M0.D=D; 41 | angle_loop_M0.output_ramp=ramp; 42 | } 43 | 44 | 45 | //M0速度PID接口 46 | float DFOC_M0_VEL_PID(float error) //M0速度环 47 | { 48 | return vel_loop_M0(error); 49 | 50 | } 51 | //M0角度PID接口 52 | float DFOC_M0_ANGLE_PID(float error) 53 | { 54 | return angle_loop_M0(error); 55 | } 56 | 57 | 58 | //初始变量及函数定义 59 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 60 | //宏定义实现的一个约束函数,用于限制一个值的范围。 61 | //具体来说，该宏定义的名称为 _constrain，接受三个参数 amt、low 和 high，分别表示要限制的值、最小值和最大值。该宏定义的实现使用了三元运算符，根据 amt 是否小于 low 或大于 high，返回其中的最大或最小值，或者返回原值。 62 | //换句话说，如果 amt 小于 low，则返回 low；如果 amt 大于 high，则返回 high；否则返回 amt。这样，_constrain(amt, low, high) 就会将 amt 约束在 [low, high] 的范围内。1 63 | 64 | 65 | // 归一化角度到 [0,2PI] 66 | float _normalizeAngle(float angle){ 67 | float a = fmod(angle, 2*PI); //取余运算可以用于归一化，列出特殊值例子算便知 68 | return a >= 0 ? a : (a + 2*PI); 69 | //三目运算符。格式：condition ? expr1 : expr2 70 | //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。 71 | //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。 72 | //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。 73 | } 74 | 75 | 76 | // 设置PWM到控制器输出 77 | void setPwm(float Ua, float Ub, float Uc) { 78 | // 限制上限 79 | Ua = _constrain(Ua, 0.0f, voltage_power_supply); 80 | Ub = _constrain(Ub, 0.0f, voltage_power_supply); 81 | Uc = _constrain(Uc, 0.0f, voltage_power_supply); 82 | // 计算占空比 83 | // 限制占空比从0到1 84 | float dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); 85 | float dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); 86 | float dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); 87 | 88 | //写入PWM到PWM 0 1 2 通道 89 | ledcWrite(0, dc_a*255); 90 | ledcWrite(1, dc_b*255); 91 | ledcWrite(2, dc_c*255); 92 | } 93 | 94 | void setTorque(float Uq,float angle_el) { 95 | S0.Sensor_update(); //更新传感器数值 96 | Uq=_constrain(Uq,-(voltage_power_supply)/2,(voltage_power_supply)/2); 97 | float Ud=0; 98 | angle_el = _normalizeAngle(angle_el); 99 | // 帕克逆变换 100 | Ualpha = -Uq*sin(angle_el); 101 | Ubeta = Uq*cos(angle_el); 102 | 103 | // 克拉克逆变换 104 | Ua = Ualpha + voltage_power_supply/2; 105 | Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2; 106 | Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2; 107 | setPwm(Ua,Ub,Uc); 108 | } 109 | 110 | void DFOC_Vbus(float power_supply) 111 | { 112 | voltage_power_supply=power_supply; 113 | pinMode(pwmA, OUTPUT); 114 | pinMode(pwmB, OUTPUT); 115 | pinMode(pwmC, OUTPUT); 116 | ledcSetup(0, 30000, 8); //pwm频道, 频率, 精度 117 | ledcSetup(1, 30000, 8); //pwm频道, 频率, 精度 118 | ledcSetup(2, 30000, 8); //pwm频道, 频率, 精度 119 | ledcAttachPin(pwmA, 0); 120 | ledcAttachPin(pwmB, 1); 121 | ledcAttachPin(pwmC, 2); 122 | Serial.println("完成PWM初始化设置"); 123 | 124 | //AS5600 125 | S0_I2C.begin(19,18, 400000UL); 126 | S0.Sensor_init(&S0_I2C); //初始化编码器0 127 | Serial.println("编码器加载完毕"); 128 | 129 | //PID 加载 130 | vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2}; 131 | } 132 | 133 | 134 | float _electricalAngle(){ 135 | return _normalizeAngle((float)(DIR * PP) * S0.getMechanicalAngle()-zero_electric_angle); 136 | } 137 | 138 | 139 | void DFOC_alignSensor(int _PP,int _DIR) 140 | { 141 | PP=_PP; 142 | DIR=_DIR; 143 | setTorque(3, _3PI_2); //起劲 144 | delay(1000); 145 | S0.Sensor_update(); //更新角度，方便下面电角度读取 146 | zero_electric_angle=_electricalAngle(); 147 | setTorque(0, _3PI_2); //松劲（解除校准） 148 | Serial.print("0电角度：");Serial.println(zero_electric_angle); 149 | } 150 | 151 | float DFOC_M0_Angle() 152 | { 153 | return DIR*S0.getAngle(); 154 | } 155 | 156 | //无滤波 157 | //float DFOC_M0_Velocity() 158 | //{ 159 | // return DIR*S0.getVelocity(); 160 | //} 161 | 162 | //有滤波 163 | float DFOC_M0_Velocity() 164 | { 165 | //获取速度数据并滤波 166 | float vel_M0_ori=S0.getVelocity(); 167 | float vel_M0_flit=M0_Vel_Flt(DIR*vel_M0_ori); 168 | return vel_M0_flit; //考虑方向 169 | } 170 | 171 | //==============串口接收============== 172 | float motor_target; 173 | int commaPosition; 174 | String serialReceiveUserCommand() { 175 | 176 | // a string to hold incoming data 177 | static String received_chars; 178 | 179 | String command = ""; 180 | 181 | while (Serial.available()) { 182 | // get the new byte: 183 | char inChar = (char)Serial.read(); 184 | // add it to the string buffer: 185 | received_chars += inChar; 186 | 187 | // end of user input 188 | if (inChar == '\n') { 189 | 190 | // execute the user command 191 | command = received_chars; 192 | 193 | commaPosition = command.indexOf('\n');//检测字符串中的逗号 194 | if(commaPosition != -1)//如果有逗号存在就向下执行 195 | { 196 | motor_target = command.substring(0,commaPosition).toDouble(); //电机角度 197 | Serial.println(motor_target); 198 | } 199 | // reset the command buffer 200 | received_chars = ""; 201 | } 202 | } 203 | return command; 204 | } 205 | 206 | 207 | float serial_motor_target() 208 | { 209 | return motor_target; 210 | } 211 | 212 | 213 | 214 | //================简易接口函数================ 215 | void DFOC_M0_set_Velocity_Angle(float Target) 216 | { 217 | setTorque(DFOC_M0_VEL_PID(DFOC_M0_ANGLE_PID((Target-DFOC_M0_Angle())*180/PI)),_electricalAngle()); //角度闭环 218 | } 219 | 220 | void DFOC_M0_setVelocity(float Target) 221 | { 222 | setTorque(DFOC_M0_VEL_PID((serial_motor_target()-DFOC_M0_Velocity())*180/PI),_electricalAngle()); //速度闭环 223 | } 224 | 225 | void DFOC_M0_set_Force_Angle(float Target) //力位 226 | { 227 | setTorque(DFOC_M0_ANGLE_PID((Target-DFOC_M0_Angle())*180/PI),_electricalAngle()); 228 | } 229 | 230 | void DFOC_M0_setTorque(float Target) 231 | { 232 | setTorque(Target,_electricalAngle()); 233 | } 234 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第七章a+b 闭环速度FOC代码-DengFOC库v0.2/1 DengFOC库V0.2+实例/DengFOC_Lib_Lesson7_CloseLoop_Vel/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | void setPwm(float Ua, float Ub, float Uc); 9 | float setTorque(float Uq,float angle_el); 10 | float _normalizeAngle(float angle); 11 | void DFOC_Vbus(float power_supply); 12 | void DFOC_alignSensor(int _PP,int _DIR); 13 | float _electricalAngle(); 14 | 15 | float serial_motor_target(); 16 | String serialReceiveUserCommand(); 17 | //传感器读取 18 | float DFOC_M0_Velocity(); 19 | float DFOC_M0_Angle(); 20 | //PID 21 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp); 22 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp); 23 | float DFOC_M0_VEL_PID(float error); 24 | float DFOC_M0_ANGLE_PID(float error); 25 | //接口函数 26 | void DFOC_M0_set_Velocity_Angle(float Target); 27 | void DFOC_M0_setVelocity(float Target); 28 | void DFOC_M0_set_Force_Angle(float Target); 29 | void DFOC_M0_setTorque(float Target); 30 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第七章a+b 闭环速度FOC代码-DengFOC库v0.2/1 DengFOC库V0.2+实例/DengFOC_Lib_Lesson7_CloseLoop_Vel/DengFOC_Lib_Lesson7_CloseLoop_Vel.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | 20 | void loop() 21 | { 22 | //设置速度环PID 23 | DFOC_M0_SET_VEL_PID(0.005,0.00,0,0); 24 | //设置速度 25 | DFOC_M0_setVelocity(serial_motor_target()); 26 | //接收串口 27 | serialReceiveUserCommand(); 28 | 29 | } 30 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第七章a+b 闭环速度FOC代码-DengFOC库v0.2/1 DengFOC库V0.2+实例/DengFOC_Lib_Lesson7_CloseLoop_Vel/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第七章a+b 闭环速度FOC代码-DengFOC库v0.2/1 DengFOC库V0.2+实例/DengFOC_Lib_Lesson7_CloseLoop_Vel/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第七章a+b 闭环速度FOC代码-DengFOC库v0.2/1 DengFOC库V0.2+实例/DengFOC_Lib_Lesson7_CloseLoop_Vel/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第七章a+b 闭环速度FOC代码-DengFOC库v0.2/1 DengFOC库V0.2+实例/DengFOC_Lib_Lesson7_CloseLoop_Vel/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | void setPwm(float Ua, float Ub, float Uc); 9 | float setTorque(float Uq,float angle_el); 10 | float _normalizeAngle(float angle); 11 | void DFOC_Vbus(float power_supply); 12 | void DFOC_alignSensor(int _PP,int _DIR); 13 | float _electricalAngle(); 14 | 15 | float serial_motor_target(); 16 | String serialReceiveUserCommand(); 17 | //传感器读取 18 | float DFOC_M0_Velocity(); 19 | float DFOC_M0_Angle(); 20 | float DFOC_M0_Current(); 21 | //PID 22 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 23 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 24 | void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp); 25 | float DFOC_M0_VEL_PID(float error); 26 | float DFOC_M0_ANGLE_PID(float error); 27 | 28 | //接口函数 29 | void DFOC_M0_set_Velocity_Angle(float Target); 30 | void DFOC_M0_setVelocity(float Target); 31 | void DFOC_M0_set_Force_Angle(float Target); 32 | void DFOC_M0_setTorque(float Target); 33 | //runFOC 循环函数 34 | void runFOC(); 35 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/DengFOC_Lib_Lesson9_Current_Sense.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | int count=0; 20 | void loop() 21 | { 22 | runFOC(); 23 | 24 | //力位（加入电流环后） 25 | // DFOC_M0_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 26 | // DFOC_M0_SET_CURRENT_PID(1.25,50,0,100000); 27 | // DFOC_M0_set_Force_Angle(serial_motor_target()); 28 | 29 | //速度（加入电流环后） 30 | // DFOC_M0_SET_VEL_PID(3,2,0,100000,0.5); 31 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 32 | // DFOC_M0_setVelocity(serial_motor_target()); 33 | 34 | //位置-速度-力（加入电流环后） 35 | DFOC_M0_SET_ANGLE_PID(1,0,0,100000,30); 36 | DFOC_M0_SET_VEL_PID(0.02,1,0,100000,0.5); 37 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 38 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 39 | 40 | //电流力矩 41 | // DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 42 | // DFOC_M0_setTorque(serial_motor_target()); 43 | 44 | count++; 45 | if(count>30) 46 | { 47 | count=0; 48 | //Serial.printf("%f\n", DFOC_M0_Current()); 49 | Serial.printf("%f,%f\n", DFOC_M0_Current(), DFOC_M0_Velocity()); 50 | } 51 | //接收串口 52 | serialReceiveUserCommand(); 53 | 54 | } 55 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/InlineCurrent.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "InlineCurrent.h" 3 | 4 | 5 | // - shunt_resistor - 分流电阻值 6 | // - gain - 电流检测运算放大器增益 7 | // - phA - A 相 adc 引脚 8 | // - phB - B 相 adc 引脚 9 | // - phC - C 相 adc 引脚（可选） 10 | 11 | 12 | #define _ADC_VOLTAGE 3.3f //ADC 电压 13 | #define _ADC_RESOLUTION 4095.0f //ADC 分辨率 14 | 15 | // ADC 计数到电压转换比率求解 16 | #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) 17 | 18 | #define NOT_SET -12345.0 19 | #define _isset(a) ( (a) != (NOT_SET) ) 20 | 21 | CurrSense::CurrSense(int Mot_Num) 22 | { 23 | if(Mot_Num==0) 24 | { 25 | pinA = 39; 26 | pinB = 36; 27 | //int pinC; 28 | _shunt_resistor = 0.01; 29 | amp_gain = 50; 30 | 31 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 32 | 33 | // gains for each phase 34 | gain_a = volts_to_amps_ratio*-1; 35 | gain_b = volts_to_amps_ratio*-1; 36 | gain_c = volts_to_amps_ratio; 37 | } 38 | if(Mot_Num==1) 39 | { 40 | pinA = 35; 41 | pinB = 34; 42 | //int pinC; 43 | _shunt_resistor = 0.01; 44 | amp_gain = 50; 45 | 46 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 47 | 48 | // gains for each phase 49 | gain_a = volts_to_amps_ratio*-1; 50 | gain_b = volts_to_amps_ratio*-1; 51 | gain_c = volts_to_amps_ratio; 52 | } 53 | } 54 | float CurrSense::readADCVoltageInline(const int pinA){ 55 | uint32_t raw_adc = analogRead(pinA); 56 | return raw_adc * _ADC_CONV; 57 | } 58 | void CurrSense::configureADCInline(const int pinA,const int pinB, const int pinC){ 59 | pinMode(pinA, INPUT); 60 | pinMode(pinB, INPUT); 61 | if( _isset(pinC) ) pinMode(pinC, INPUT); 62 | } 63 | 64 | // 查找 ADC 零偏移量的函数 65 | void CurrSense::calibrateOffsets(){ 66 | const int calibration_rounds = 1000; 67 | 68 | // 查找0电流时候的电压 69 | offset_ia = 0; 70 | offset_ib = 0; 71 | offset_ic = 0; 72 | // 读数1000次 73 | for (int i = 0; i < calibration_rounds; i++) { 74 | offset_ia += readADCVoltageInline(pinA); 75 | offset_ib += readADCVoltageInline(pinB); 76 | if(_isset(pinC)) offset_ic += readADCVoltageInline(pinC); 77 | delay(1); 78 | } 79 | // 求平均，得到误差 80 | offset_ia = offset_ia / calibration_rounds; 81 | offset_ib = offset_ib / calibration_rounds; 82 | if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; 83 | } 84 | 85 | void CurrSense::init(){ 86 | // 配置函数 87 | configureADCInline(pinA,pinB,pinC); 88 | // 校准 89 | calibrateOffsets(); 90 | } 91 | 92 | 93 | // 读取全部三相电流 94 | 95 | void CurrSense::getPhaseCurrents(){ 96 | current_a = (readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps 97 | current_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps 98 | current_c = (!_isset(pinC)) ? 0 : (readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps 99 | } 100 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/InlineCurrent.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class CurrSense 4 | { 5 | public: 6 | CurrSense(int Mot_Num); 7 | float readADCVoltageInline(const int pinA); 8 | void configureADCInline(const int pinA,const int pinB, const int pinC); 9 | void calibrateOffsets(); 10 | void init(); 11 | void getPhaseCurrents(); 12 | float current_a,current_b,current_c; 13 | int pinA; 14 | int pinB; 15 | int pinC; 16 | float offset_ia; 17 | float offset_ib; 18 | float offset_ic; 19 | float _shunt_resistor; 20 | float amp_gain; 21 | 22 | float volts_to_amps_ratio; 23 | 24 | float gain_a; 25 | float gain_b; 26 | float gain_c; 27 | private: 28 | int _Mot_Num; 29 | }; 30 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第九章a+b 位置闭环，速度闭环升级版（加上电流环）/1 DengFOC库V0.4+实例/DengFOC_Lib_Lesson9_Current_Sense/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第五章a+b 开环速度FOC代码/DengFOC_Lib_Lesson5_OpenLoop_Vel/DengFOC_Lib_Lesson5_OpenLoop_Vel.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，转载请著名出处 2 | //仅在DengFOC上测试过 3 | //PWM输出引脚定义 4 | int pwmA = 32; 5 | int pwmB = 33; 6 | int pwmC = 25; 7 | 8 | //初始变量及函数定义 9 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 10 | //宏定义实现的一个约束函数,用于限制一个值的范围。 11 | //具体来说，该宏定义的名称为 _constrain，接受三个参数 amt、low 和 high，分别表示要限制的值、最小值和最大值。该宏定义的实现使用了三元运算符，根据 amt 是否小于 low 或大于 high，返回其中的最大或最小值，或者返回原值。 12 | //换句话说，如果 amt 小于 low，则返回 low；如果 amt 大于 high，则返回 high；否则返回 amt。这样，_constrain(amt, low, high) 就会将 amt 约束在 [low, high] 的范围内。 13 | float voltage_power_supply=12.6; 14 | float shaft_angle=0,open_loop_timestamp=0; 15 | float zero_electric_angle=0,Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0,dc_a=0,dc_b=0,dc_c=0; 16 | 17 | 18 | void setup() { 19 | // put your setup code here, to run once: 20 | Serial.begin(115200); 21 | //PWM设置 22 | pinMode(pwmA, OUTPUT); 23 | pinMode(pwmB, OUTPUT); 24 | pinMode(pwmC, OUTPUT); 25 | ledcSetup(0, 30000, 8); //pwm频道, 频率, 精度 26 | ledcSetup(1, 30000, 8); //pwm频道, 频率, 精度 27 | ledcSetup(2, 30000, 8); //pwm频道, 频率, 精度 28 | ledcAttachPin(pwmA, 0); 29 | ledcAttachPin(pwmB, 1); 30 | ledcAttachPin(pwmC, 2); 31 | Serial.println("完成PWM初始化设置"); 32 | delay(3000); 33 | 34 | } 35 | 36 | // 电角度求解 37 | float _electricalAngle(float shaft_angle, int pole_pairs) { 38 | return (shaft_angle * pole_pairs); 39 | } 40 | 41 | // 归一化角度到 [0,2PI] 42 | float _normalizeAngle(float angle){ 43 | float a = fmod(angle, 2*PI); //取余运算可以用于归一化，列出特殊值例子算便知 44 | return a >= 0 ? a : (a + 2*PI); 45 | //三目运算符。格式：condition ? expr1 : expr2 46 | //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。 47 | //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。 48 | //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。 49 | } 50 | 51 | 52 | // 设置PWM到控制器输出 53 | void setPwm(float Ua, float Ub, float Uc) { 54 | 55 | // 计算占空比 56 | // 限制占空比从0到1 57 | dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); 58 | dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); 59 | dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); 60 | 61 | //写入PWM到PWM 0 1 2 通道 62 | ledcWrite(0, dc_a*255); 63 | ledcWrite(1, dc_b*255); 64 | ledcWrite(2, dc_c*255); 65 | } 66 | 67 | void setPhaseVoltage(float Uq,float Ud, float angle_el) { 68 | angle_el = _normalizeAngle(angle_el + zero_electric_angle); 69 | // 帕克逆变换 70 | Ualpha = -Uq*sin(angle_el); 71 | Ubeta = Uq*cos(angle_el); 72 | 73 | // 克拉克逆变换 74 | Ua = Ualpha + voltage_power_supply/2; 75 | Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2; 76 | Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2; 77 | setPwm(Ua,Ub,Uc); 78 | } 79 | 80 | 81 | //开环速度函数 82 | float velocityOpenloop(float target_velocity){ 83 | unsigned long now_us = micros(); //获取从开启芯片以来的微秒数，它的精度是 4 微秒。 micros() 返回的是一个无符号长整型（unsigned long）的值 84 | 85 | //计算当前每个Loop的运行时间间隔 86 | float Ts = (now_us - open_loop_timestamp) * 1e-6f; 87 | 88 | //由于 micros() 函数返回的时间戳会在大约 70 分钟之后重新开始计数，在由70分钟跳变到0时，TS会出现异常，因此需要进行修正。如果时间间隔小于等于零或大于 0.5 秒，则将其设置为一个较小的默认值，即 1e-3f 89 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 90 | 91 | 92 | // 通过乘以时间间隔和目标速度来计算需要转动的机械角度，存储在 shaft_angle 变量中。在此之前，还需要对轴角度进行归一化，以确保其值在 0 到 2π 之间。 93 | shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); 94 | //以目标速度为 10 rad/s 为例，如果时间间隔是 1 秒，则在每个循环中需要增加 10 * 1 = 10 弧度的角度变化量，才能使电机转动到目标速度。 95 | //如果时间间隔是 0.1 秒，那么在每个循环中需要增加的角度变化量就是 10 * 0.1 = 1 弧度，才能实现相同的目标速度。因此，电机轴的转动角度取决于目标速度和时间间隔的乘积。 96 | 97 | // 使用早前设置的voltage_power_supply的1/3作为Uq值，这个值会直接影响输出力矩 98 | // 最大只能设置为Uq = voltage_power_supply/2，否则ua,ub,uc会超出供电电压限幅 99 | float Uq = voltage_power_supply/3; 100 | 101 | setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, 7)); 102 | 103 | open_loop_timestamp = now_us; //用于计算下一个时间间隔 104 | 105 | return Uq; 106 | } 107 | 108 | 109 | void loop() { 110 | // put your main code here, to run repeatedly: 111 | velocityOpenloop(5); 112 | } 113 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | void setPwm(float Ua, float Ub, float Uc); 9 | float setTorque(float Uq,float angle_el); 10 | float _normalizeAngle(float angle); 11 | void DFOC_Vbus(float power_supply); 12 | void DFOC_alignSensor(int _PP,int _DIR); 13 | float _electricalAngle(); 14 | 15 | float serial_motor_target(); 16 | String serialReceiveUserCommand(); 17 | //传感器读取 18 | float DFOC_M0_Velocity(); 19 | float DFOC_M0_Angle(); 20 | float DFOC_M0_Current(); 21 | //PID 22 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp); 23 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp); 24 | void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp); 25 | float DFOC_M0_VEL_PID(float error); 26 | float DFOC_M0_ANGLE_PID(float error); 27 | 28 | //接口函数 29 | void DFOC_M0_set_Velocity_Angle(float Target); 30 | void DFOC_M0_setVelocity(float Target); 31 | void DFOC_M0_set_Force_Angle(float Target); 32 | void DFOC_M0_setTorque(float Target); 33 | //runFOC 循环函数 34 | void runFOC(); 35 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/DengFOC_Lib_Lesson8_Current_Sense.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | int Sensor_DIR=-1; //传感器方向 11 | int Motor_PP=7; //电机极对数 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | DFOC_Vbus(12.6); //设定驱动器供电电压 16 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 17 | } 18 | 19 | int count=0; 20 | void loop() 21 | { 22 | runFOC(); 23 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 24 | DFOC_M0_setTorque(serial_motor_target()); 25 | count++; 26 | if(count>30) 27 | { 28 | count=0; 29 | Serial.printf("%f\n", DFOC_M0_Current()); 30 | } 31 | //接收串口 32 | serialReceiveUserCommand(); 33 | 34 | } 35 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/InlineCurrent.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "InlineCurrent.h" 3 | 4 | 5 | // - shunt_resistor - 分流电阻值 6 | // - gain - 电流检测运算放大器增益 7 | // - phA - A 相 adc 引脚 8 | // - phB - B 相 adc 引脚 9 | // - phC - C 相 adc 引脚（可选） 10 | 11 | 12 | #define _ADC_VOLTAGE 3.3f //ADC 电压 13 | #define _ADC_RESOLUTION 4095.0f //ADC 分辨率 14 | 15 | // ADC 计数到电压转换比率求解 16 | #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) 17 | 18 | #define NOT_SET -12345.0 19 | #define _isset(a) ( (a) != (NOT_SET) ) 20 | 21 | CurrSense::CurrSense(int Mot_Num) 22 | { 23 | if(Mot_Num==0) 24 | { 25 | pinA = 39; 26 | pinB = 36; 27 | //int pinC; 28 | _shunt_resistor = 0.01; 29 | amp_gain = 50; 30 | 31 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 32 | 33 | // gains for each phase 34 | gain_a = volts_to_amps_ratio*-1; 35 | gain_b = volts_to_amps_ratio*-1; 36 | gain_c = volts_to_amps_ratio; 37 | } 38 | if(Mot_Num==1) 39 | { 40 | pinA = 35; 41 | pinB = 34; 42 | //int pinC; 43 | _shunt_resistor = 0.01; 44 | amp_gain = 50; 45 | 46 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 47 | 48 | // gains for each phase 49 | gain_a = volts_to_amps_ratio*-1; 50 | gain_b = volts_to_amps_ratio*-1; 51 | gain_c = volts_to_amps_ratio; 52 | } 53 | } 54 | float CurrSense::readADCVoltageInline(const int pinA){ 55 | uint32_t raw_adc = analogRead(pinA); 56 | return raw_adc * _ADC_CONV; 57 | } 58 | void CurrSense::configureADCInline(const int pinA,const int pinB, const int pinC){ 59 | pinMode(pinA, INPUT); 60 | pinMode(pinB, INPUT); 61 | if( _isset(pinC) ) pinMode(pinC, INPUT); 62 | } 63 | 64 | // 查找 ADC 零偏移量的函数 65 | void CurrSense::calibrateOffsets(){ 66 | const int calibration_rounds = 1000; 67 | 68 | // 查找0电流时候的电压 69 | offset_ia = 0; 70 | offset_ib = 0; 71 | offset_ic = 0; 72 | // 读数1000次 73 | for (int i = 0; i < calibration_rounds; i++) { 74 | offset_ia += readADCVoltageInline(pinA); 75 | offset_ib += readADCVoltageInline(pinB); 76 | if(_isset(pinC)) offset_ic += readADCVoltageInline(pinC); 77 | delay(1); 78 | } 79 | // 求平均，得到误差 80 | offset_ia = offset_ia / calibration_rounds; 81 | offset_ib = offset_ib / calibration_rounds; 82 | if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; 83 | } 84 | 85 | void CurrSense::init(){ 86 | // 配置函数 87 | configureADCInline(pinA,pinB,pinC); 88 | // 校准 89 | calibrateOffsets(); 90 | } 91 | 92 | 93 | // 读取全部三相电流 94 | 95 | void CurrSense::getPhaseCurrents(){ 96 | current_a = (readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps 97 | current_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps 98 | current_c = (!_isset(pinC)) ? 0 : (readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps 99 | } 100 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/InlineCurrent.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class CurrSense 4 | { 5 | public: 6 | CurrSense(int Mot_Num); 7 | float readADCVoltageInline(const int pinA); 8 | void configureADCInline(const int pinA,const int pinB, const int pinC); 9 | void calibrateOffsets(); 10 | void init(); 11 | void getPhaseCurrents(); 12 | float current_a,current_b,current_c; 13 | int pinA; 14 | int pinB; 15 | int pinC; 16 | float offset_ia; 17 | float offset_ib; 18 | float offset_ic; 19 | float _shunt_resistor; 20 | float amp_gain; 21 | 22 | float volts_to_amps_ratio; 23 | 24 | float gain_a; 25 | float gain_b; 26 | float gain_c; 27 | private: 28 | int _Mot_Num; 29 | }; 30 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第八章a+b 电流力矩闭环/1 DengFOC库V0.3+实例/DengFOC_Lib_Lesson8_Current_Sense/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第六章a+b 闭环位置FOC代码-DengFOC库v0.1/1 DengFOC库V0.1+实例（6b课程后半部分）/AS5600.cpp: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | 8 | #include "Wire.h" 9 | #include "AS5600.h" 10 | #include 11 | 12 | int _raw_ang_hi = 0x0c; 13 | int _raw_ang_lo = 0x0d; 14 | int _ams5600_Address = 0x36; 15 | int ledtime = 0; 16 | int32_t full_rotations=0; // full rotation tracking; 17 | float angle_prev=0; 18 | 19 | void BeginSensor() { 20 | Wire.begin(19,18, 400000UL); 21 | delay(1000); 22 | } 23 | //readTwoBytes(int in_adr_hi, int in_adr_lo)这段代码是一个函数，其目的是从I2C设备（在代码中的变量名为_ams5600_Address）中读取两个字节数据，并将其合并成一个16位的无符号整数返回。 24 | //具体来说，函数接受两个整型参数in_adr_hi和in_adr_lo，它们用于指定需要读取的两个字节数据的地址。函数中首先通过Wire库开始I2C传输，向设备写入in_adr_lo和in_adr_hi分别作为数据地址，然后读取相应的字节数据。 25 | //在每个Wire.requestFrom()调用之后，通过一个while循环等待数据接收完毕。然后读取接收到的低字节和高字节，并使用位运算将它们合并成一个16位的无符号整数。 26 | //最后，返回合并后的整数。如果读取过程中出现错误或者函数没有成功读取到数据，则函数返回-1。 27 | word readTwoBytes(int in_adr_hi, int in_adr_lo) 28 | { 29 | word retVal = -1; 30 | 31 | /* 读低位 */ 32 | Wire.beginTransmission(_ams5600_Address); 33 | Wire.write(in_adr_lo); 34 | Wire.endTransmission(); 35 | Wire.requestFrom(_ams5600_Address, 1); 36 | while(Wire.available() == 0); 37 | int low = Wire.read(); 38 | 39 | /* 读高位 */ 40 | Wire.beginTransmission(_ams5600_Address); 41 | Wire.write(in_adr_hi); 42 | Wire.endTransmission(); 43 | Wire.requestFrom(_ams5600_Address, 1); 44 | while(Wire.available() == 0); 45 | int high = Wire.read(); 46 | 47 | retVal = (high << 8) | low; 48 | 49 | return retVal; 50 | } 51 | 52 | word getRawAngle() 53 | { 54 | return readTwoBytes(_raw_ang_hi, _raw_ang_lo); 55 | } 56 | 57 | float getAngle_Without_track() 58 | { 59 | return getRawAngle()*0.08789* PI / 180; //得到弧度制的角度 60 | } 61 | 62 | float getAngle() 63 | { 64 | float val = getAngle_Without_track(); 65 | float d_angle = val - angle_prev; 66 | //计算旋转的总圈数 67 | //通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出，如果发生了，则将full_rotations增加1（如果d_angle小于0）或减少1（如果d_angle大于0）。 68 | if(abs(d_angle) > (0.8f*6.28318530718f) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 69 | angle_prev = val; 70 | return (float)full_rotations * 6.28318530718f + angle_prev; 71 | 72 | } 73 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第六章a+b 闭环位置FOC代码-DengFOC库v0.1/1 DengFOC库V0.1+实例（6b课程后半部分）/AS5600.h: -------------------------------------------------------------------------------- 1 | void BeginSensor(); 2 | float getAngle(); 3 | float getAngle_Without_track(); 4 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第六章a+b 闭环位置FOC代码-DengFOC库v0.1/1 DengFOC库V0.1+实例（6b课程后半部分）/DengFOC.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "AS5600.h" 3 | 4 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 5 | float voltage_power_supply; 6 | float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0; 7 | #define _3PI_2 4.71238898038f 8 | float zero_electric_angle=0; 9 | int PP=1,DIR=1; 10 | int pwmA = 32; 11 | int pwmB = 33; 12 | int pwmC = 25; 13 | 14 | //初始变量及函数定义 15 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 16 | //宏定义实现的一个约束函数,用于限制一个值的范围。 17 | //具体来说，该宏定义的名称为 _constrain，接受三个参数 amt、low 和 high，分别表示要限制的值、最小值和最大值。该宏定义的实现使用了三元运算符，根据 amt 是否小于 low 或大于 high，返回其中的最大或最小值，或者返回原值。 18 | //换句话说，如果 amt 小于 low，则返回 low；如果 amt 大于 high，则返回 high；否则返回 amt。这样，_constrain(amt, low, high) 就会将 amt 约束在 [low, high] 的范围内。1 19 | 20 | 21 | // 归一化角度到 [0,2PI] 22 | float _normalizeAngle(float angle){ 23 | float a = fmod(angle, 2*PI); //取余运算可以用于归一化，列出特殊值例子算便知 24 | return a >= 0 ? a : (a + 2*PI); 25 | //三目运算符。格式：condition ? expr1 : expr2 26 | //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。 27 | //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。 28 | //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。 29 | } 30 | 31 | 32 | // 设置PWM到控制器输出 33 | void setPwm(float Ua, float Ub, float Uc) { 34 | 35 | // 计算占空比 36 | // 限制占空比从0到1 37 | float dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); 38 | float dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); 39 | float dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); 40 | 41 | //写入PWM到PWM 0 1 2 通道 42 | ledcWrite(0, dc_a*255); 43 | ledcWrite(1, dc_b*255); 44 | ledcWrite(2, dc_c*255); 45 | } 46 | 47 | void setTorque(float Uq,float angle_el) { 48 | Uq=_constrain(Uq,-voltage_power_supply/2,voltage_power_supply/2); 49 | float Ud=0; 50 | angle_el = _normalizeAngle(angle_el); 51 | // 帕克逆变换 52 | Ualpha = -Uq*sin(angle_el); 53 | Ubeta = Uq*cos(angle_el); 54 | 55 | // 克拉克逆变换 56 | Ua = Ualpha + voltage_power_supply/2; 57 | Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2; 58 | Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2; 59 | setPwm(Ua,Ub,Uc); 60 | } 61 | 62 | void DFOC_Vbus(float power_supply) 63 | { 64 | voltage_power_supply=power_supply; 65 | pinMode(pwmA, OUTPUT); 66 | pinMode(pwmB, OUTPUT); 67 | pinMode(pwmC, OUTPUT); 68 | ledcSetup(0, 30000, 8); //pwm频道, 频率, 精度 69 | ledcSetup(1, 30000, 8); //pwm频道, 频率, 精度 70 | ledcSetup(2, 30000, 8); //pwm频道, 频率, 精度 71 | ledcAttachPin(pwmA, 0); 72 | ledcAttachPin(pwmB, 1); 73 | ledcAttachPin(pwmC, 2); 74 | Serial.println("完成PWM初始化设置"); 75 | BeginSensor(); 76 | } 77 | 78 | 79 | float _electricalAngle(){ 80 | return _normalizeAngle((float)(DIR * PP) * getAngle_Without_track()-zero_electric_angle); 81 | } 82 | 83 | 84 | void DFOC_alignSensor(int _PP,int _DIR) 85 | { 86 | PP=_PP; 87 | DIR=_DIR; 88 | setTorque(3, _3PI_2); 89 | delay(3000); 90 | zero_electric_angle=_electricalAngle(); 91 | setTorque(0, _3PI_2); 92 | Serial.print("0电角度：");Serial.println(zero_electric_angle); 93 | } 94 | 95 | float DFOC_M0_Angle() 96 | { 97 | return getAngle(); 98 | } 99 | 100 | //==============串口接收============== 101 | float motor_target; 102 | int commaPosition; 103 | String serialReceiveUserCommand() { 104 | 105 | // a string to hold incoming data 106 | static String received_chars; 107 | 108 | String command = ""; 109 | 110 | while (Serial.available()) { 111 | // get the new byte: 112 | char inChar = (char)Serial.read(); 113 | // add it to the string buffer: 114 | received_chars += inChar; 115 | 116 | // end of user input 117 | if (inChar == '\n') { 118 | 119 | // execute the user command 120 | command = received_chars; 121 | 122 | commaPosition = command.indexOf('\n');//检测字符串中的逗号 123 | if(commaPosition != -1)//如果有逗号存在就向下执行 124 | { 125 | motor_target = command.substring(0,commaPosition).toDouble(); //电机角度 126 | Serial.println(motor_target); 127 | } 128 | // reset the command buffer 129 | received_chars = ""; 130 | } 131 | } 132 | return command; 133 | } 134 | 135 | float serial_motor_target() 136 | { 137 | return motor_target; 138 | } 139 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第六章a+b 闭环位置FOC代码-DengFOC库v0.1/1 DengFOC库V0.1+实例（6b课程后半部分）/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | void setPwm(float Ua, float Ub, float Uc); 9 | float setTorque(float Uq,float angle_el); 10 | float _normalizeAngle(float angle); 11 | void DFOC_Vbus(float power_supply); 12 | void DFOC_alignSensor(int _PP,int _DIR); 13 | float _electricalAngle(); 14 | float DFOC_M0_Angle(); 15 | float serial_motor_target(); 16 | String serialReceiveUserCommand(); 17 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第六章a+b 闭环位置FOC代码-DengFOC库v0.1/1 DengFOC库V0.1+实例（6b课程后半部分）/DengFOC_Lib_Lesson6_CloseLoop_Pos.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | #include "DengFOC.h" 8 | 9 | int Sensor_DIR=-1; //传感器方向 10 | int Motor_PP=7; //电机极对数 11 | 12 | void setup() { 13 | Serial.begin(115200); 14 | DFOC_Vbus(12.6); //设定驱动器供电电压 15 | DFOC_alignSensor(Motor_PP,Sensor_DIR); 16 | } 17 | 18 | void loop() 19 | { 20 | //输出角度值 21 | //Serial.print("当前角度："); 22 | //Serial.println(DFOC_M0_Angle()); 23 | //输出角度值 24 | float Kp=0.133; 25 | float Sensor_Angle=DFOC_M0_Angle(); 26 | setTorque(Kp*(serial_motor_target()-Sensor_DIR*Sensor_Angle)*180/PI,_electricalAngle()); //位置闭环 27 | //setTorque(serial_motor_target(),_electricalAngle()); //电压力矩 28 | serialReceiveUserCommand(); 29 | } 30 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第六章a+b 闭环位置FOC代码-DengFOC库v0.1/2 单文件实现闭环代码（6b课程前半部分）/AS5600.cpp: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | 7 | 8 | #include "Wire.h" 9 | #include "AS5600.h" 10 | #include 11 | 12 | int _raw_ang_hi = 0x0c; 13 | int _raw_ang_lo = 0x0d; 14 | int _ams5600_Address = 0x36; 15 | int ledtime = 0; 16 | int32_t full_rotations=0; // full rotation tracking; 17 | float angle_prev=0; 18 | 19 | void BeginSensor() { 20 | Wire.begin(19,18, 400000UL); 21 | delay(1000); 22 | } 23 | //readTwoBytes(int in_adr_hi, int in_adr_lo)这段代码是一个函数，其目的是从I2C设备（在代码中的变量名为_ams5600_Address）中读取两个字节数据，并将其合并成一个16位的无符号整数返回。 24 | //具体来说，函数接受两个整型参数in_adr_hi和in_adr_lo，它们用于指定需要读取的两个字节数据的地址。函数中首先通过Wire库开始I2C传输，向设备写入in_adr_lo和in_adr_hi分别作为数据地址，然后读取相应的字节数据。 25 | //在每个Wire.requestFrom()调用之后，通过一个while循环等待数据接收完毕。然后读取接收到的低字节和高字节，并使用位运算将它们合并成一个16位的无符号整数。 26 | //最后，返回合并后的整数。如果读取过程中出现错误或者函数没有成功读取到数据，则函数返回-1。 27 | word readTwoBytes(int in_adr_hi, int in_adr_lo) 28 | { 29 | word retVal = -1; 30 | 31 | /* 读低位 */ 32 | Wire.beginTransmission(_ams5600_Address); 33 | Wire.write(in_adr_lo); 34 | Wire.endTransmission(); 35 | Wire.requestFrom(_ams5600_Address, 1); 36 | while(Wire.available() == 0); 37 | int low = Wire.read(); 38 | 39 | /* 读高位 */ 40 | Wire.beginTransmission(_ams5600_Address); 41 | Wire.write(in_adr_hi); 42 | Wire.endTransmission(); 43 | Wire.requestFrom(_ams5600_Address, 1); 44 | while(Wire.available() == 0); 45 | int high = Wire.read(); 46 | 47 | retVal = (high << 8) | low; 48 | 49 | return retVal; 50 | } 51 | 52 | word getRawAngle() 53 | { 54 | return readTwoBytes(_raw_ang_hi, _raw_ang_lo); 55 | } 56 | 57 | float getAngle_Without_track() 58 | { 59 | return getRawAngle()*0.08789* PI / 180; //得到弧度制的角度 60 | } 61 | 62 | float getAngle() 63 | { 64 | float val = getAngle_Without_track(); 65 | float d_angle = val - angle_prev; 66 | //计算旋转的总圈数 67 | //通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出，如果发生了，则将full_rotations增加1（如果d_angle小于0）或减少1（如果d_angle大于0）。 68 | if(abs(d_angle) > (0.8f*6.28318530718f) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 69 | angle_prev = val; 70 | return (float)full_rotations * 6.28318530718f + angle_prev; 71 | 72 | } 73 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第六章a+b 闭环位置FOC代码-DengFOC库v0.1/2 单文件实现闭环代码（6b课程前半部分）/AS5600.h: -------------------------------------------------------------------------------- 1 | void BeginSensor(); 2 | float getAngle(); 3 | float getAngle_Without_track(); 4 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第六章a+b 闭环位置FOC代码-DengFOC库v0.1/2 单文件实现闭环代码（6b课程前半部分）/DengFOC_Lib_Lesson6_CloseLoop_Pos.ino: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //PWM输出引脚定义 7 | 8 | #include "AS5600.h" 9 | 10 | int pwmA = 32; 11 | int pwmB = 33; 12 | int pwmC = 25; 13 | 14 | //初始变量及函数定义 15 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 16 | //宏定义实现的一个约束函数,用于限制一个值的范围。 17 | //具体来说，该宏定义的名称为 _constrain，接受三个参数 amt、low 和 high，分别表示要限制的值、最小值和最大值。该宏定义的实现使用了三元运算符，根据 amt 是否小于 low 或大于 high，返回其中的最大或最小值，或者返回原值。 18 | //换句话说，如果 amt 小于 low，则返回 low；如果 amt 大于 high，则返回 high；否则返回 amt。这样，_constrain(amt, low, high) 就会将 amt 约束在 [low, high] 的范围内。 19 | float voltage_limit=12.6; 20 | float voltage_power_supply=12.6; 21 | float shaft_angle=0,open_loop_timestamp=0; 22 | float zero_electric_angle=0,Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0,dc_a=0,dc_b=0,dc_c=0; 23 | #define _3PI_2 4.71238898038f 24 | 25 | int PP=7,DIR=-1; 26 | float _electricalAngle(){ 27 | return _normalizeAngle((float)(DIR * PP) * getAngle_Without_track()-zero_electric_angle); 28 | } 29 | 30 | 31 | // 归一化角度到 [0,2PI] 32 | float _normalizeAngle(float angle){ 33 | float a = fmod(angle, 2*PI); //取余运算可以用于归一化，列出特殊值例子算便知 34 | return a >= 0 ? a : (a + 2*PI); 35 | //三目运算符。格式：condition ? expr1 : expr2 36 | //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。 37 | //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。 38 | //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。 39 | } 40 | 41 | 42 | // 设置PWM到控制器输出 43 | void setPwm(float Ua, float Ub, float Uc) { 44 | 45 | // 限制上限 46 | Ua = _constrain(Ua, 0.0f, voltage_limit); 47 | Ub = _constrain(Ub, 0.0f, voltage_limit); 48 | Uc = _constrain(Uc, 0.0f, voltage_limit); 49 | // 计算占空比 50 | // 限制占空比从0到1 51 | dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); 52 | dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); 53 | dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); 54 | 55 | //写入PWM到PWM 0 1 2 通道 56 | ledcWrite(0, dc_a*255); 57 | ledcWrite(1, dc_b*255); 58 | ledcWrite(2, dc_c*255); 59 | } 60 | 61 | void setPhaseVoltage(float Uq,float Ud, float angle_el) { 62 | angle_el = _normalizeAngle(angle_el); 63 | // 帕克逆变换 64 | Ualpha = -Uq*sin(angle_el); 65 | Ubeta = Uq*cos(angle_el); 66 | 67 | // 克拉克逆变换 68 | Ua = Ualpha + voltage_power_supply/2; 69 | Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2; 70 | Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2; 71 | setPwm(Ua,Ub,Uc); 72 | } 73 | 74 | void setup() { 75 | // put your setup code here, to run once: 76 | Serial.begin(115200); 77 | //PWM设置 78 | pinMode(pwmA, OUTPUT); 79 | pinMode(pwmB, OUTPUT); 80 | pinMode(pwmC, OUTPUT); 81 | ledcSetup(0, 30000, 8); //pwm频道, 频率, 精度 82 | ledcSetup(1, 30000, 8); //pwm频道, 频率, 精度 83 | ledcSetup(2, 30000, 8); //pwm频道, 频率, 精度 84 | ledcAttachPin(pwmA, 0); 85 | ledcAttachPin(pwmB, 1); 86 | ledcAttachPin(pwmC, 2); 87 | Serial.println("完成PWM初始化设置"); 88 | BeginSensor(); 89 | setPhaseVoltage(3, 0,_3PI_2); 90 | delay(3000); 91 | zero_electric_angle=_electricalAngle(); 92 | setPhaseVoltage(0, 0,_3PI_2); 93 | Serial.print("0电角度：");Serial.println(zero_electric_angle); 94 | 95 | 96 | } 97 | 98 | 99 | //==============串口接收============== 100 | float motor_target; 101 | int commaPosition; 102 | String serialReceiveUserCommand() { 103 | 104 | // a string to hold incoming data 105 | static String received_chars; 106 | 107 | String command = ""; 108 | 109 | while (Serial.available()) { 110 | // get the new byte: 111 | char inChar = (char)Serial.read(); 112 | // add it to the string buffer: 113 | received_chars += inChar; 114 | 115 | // end of user input 116 | if (inChar == '\n') { 117 | 118 | // execute the user command 119 | command = received_chars; 120 | 121 | commaPosition = command.indexOf('\n');//检测字符串中的逗号 122 | if(commaPosition != -1)//如果有逗号存在就向下执行 123 | { 124 | motor_target = command.substring(0,commaPosition).toDouble(); //电机角度 125 | Serial.println(motor_target); 126 | } 127 | // reset the command buffer 128 | received_chars = ""; 129 | } 130 | } 131 | return command; 132 | } 133 | void loop() { 134 | // put your main code here, to run repeatedly: 135 | Serial.println(getAngle()); 136 | float Sensor_Angle=getAngle(); 137 | float Kp=0.133; 138 | setPhaseVoltage(_constrain(Kp*(motor_target-DIR*Sensor_Angle)*180/PI,-6,6),0,_electricalAngle()); 139 | serialReceiveUserCommand(); 140 | } -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | 9 | void M0_setPwm(float Ua, float Ub, float Uc); 10 | void M1_setPwm(float Ua, float Ub, float Uc); 11 | void DFOC_enable(); 12 | void DFOC_disable(); 13 | void M0_setTorque(float Uq,float angle_el); 14 | void M1_setTorque(float Uq,float angle_el); 15 | float _normalizeAngle(float angle); 16 | void DFOC_Vbus(float power_supply); 17 | void DFOC_M0_alignSensor(int _PP,int _DIR); 18 | void DFOC_M1_alignSensor(int _PP,int _DIR); 19 | float S0_electricalAngle(); 20 | float S1_electricalAngle(); 21 | float cal_Iq_Id(float current_a,float current_b,float angle_el); 22 | float serial_motor_target(); 23 | String serialReceiveUserCommand(); 24 | //传感器读取 25 | float DFOC_M0_Velocity(); 26 | float DFOC_M1_Velocity(); 27 | float DFOC_M0_Angle(); 28 | float DFOC_M1_Angle(); 29 | float DFOC_M0_Current(); 30 | float DFOC_M1_Current(); 31 | //PID 32 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 33 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 34 | void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp); 35 | void DFOC_M1_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 36 | void DFOC_M1_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 37 | void DFOC_M1_SET_CURRENT_PID(float P,float I,float D,float ramp); 38 | float DFOC_M0_VEL_PID(float error); 39 | float DFOC_M0_ANGLE_PID(float error); 40 | float DFOC_M1_VEL_PID(float error); 41 | float DFOC_M1_ANGLE_PID(float error); 42 | 43 | //接口函数 44 | void DFOC_M0_set_Velocity_Angle(float Target); 45 | void DFOC_M0_setVelocity(float Target); 46 | void DFOC_M0_set_Force_Angle(float Target); 47 | void DFOC_M1_set_Velocity_Angle(float Target); 48 | void DFOC_M1_setVelocity(float Target); 49 | void DFOC_M1_set_Force_Angle(float Target); 50 | void DFOC_M0_setTorque(float Target); 51 | void DFOC_M1_setTorque(float Target); 52 | //runFOC 循环函数 53 | void runFOC(); 54 | //测试函数 55 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/DengFOC_Lib_Lesson10_Dual_Motor_SVPWM.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | 11 | void setup() { 12 | 13 | Serial.begin(115200); 14 | 15 | DFOC_enable(); //放在校准前 16 | DFOC_Vbus(12.6); //设定驱动器供电电压 17 | DFOC_M0_alignSensor(7, 1); 18 | DFOC_M1_alignSensor(7, 1); 19 | } 20 | 21 | int count = 0; 22 | void loop() { 23 | runFOC(); 24 | // DFOC_M0_setTorque(1); 25 | // DFOC_M1_setTorque(1); 26 | 27 | //力位（加入电流环后） 28 | // DFOC_M0_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 29 | // DFOC_M0_SET_CURRENT_PID(1.25,50,0,100000); 30 | // DFOC_M0_set_Force_Angle(serial_motor_target()); 31 | // DFOC_M1_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 32 | // DFOC_M1_SET_CURRENT_PID(1.25,50,0,100000); 33 | // DFOC_M1_set_Force_Angle(serial_motor_target()); 34 | 35 | //速度（加入电流环后） 36 | // DFOC_M0_SET_VEL_PID(3,2,0,100000,0.5); 37 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 38 | // DFOC_M0_setVelocity(serial_motor_target()); 39 | // DFOC_M0_SET_VEL_PID(0.1,2,0,100000,0.5); 40 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 41 | // DFOC_M0_setVelocity(serial_motor_target()); 42 | 43 | // //位置-速度-力（加入电流环后） 44 | // DFOC_M0_SET_ANGLE_PID(1, 0, 0, 100000, 30); 45 | // DFOC_M0_SET_VEL_PID(0.02, 1, 0, 100000, 0.5); 46 | // DFOC_M0_SET_CURRENT_PID(5, 200, 0, 100000); 47 | // DFOC_M0_set_Velocity_Angle(serial_motor_target()); 48 | 49 | // //位置-速度-力（加入电流环后） 50 | // DFOC_M1_SET_ANGLE_PID(1, 0, 0, 100000, 30); 51 | // DFOC_M1_SET_VEL_PID(0.02, 1, 0, 100000, 0.5); 52 | // DFOC_M1_SET_CURRENT_PID(5, 200, 0, 100000); 53 | // DFOC_M1_set_Velocity_Angle(serial_motor_target()); 54 | //电流力矩 55 | DFOC_M1_SET_CURRENT_PID(5, 200, 0, 100000); 56 | DFOC_M0_SET_CURRENT_PID(5, 200, 0, 100000); 57 | 58 | DFOC_M0_setTorque(serial_motor_target()); 59 | DFOC_M1_setTorque(serial_motor_target()); 60 | 61 | 62 | 63 | count++; 64 | if (count > 100) { 65 | count = 0; 66 | //Serial.printf("%f\n", DFOC_M0_Current()); 67 | Serial.printf("%f,%f,%f,%f,%f\n", DFOC_M0_Current(), DFOC_M1_Current(), DFOC_M0_Angle(), DFOC_M0_Velocity(), serial_motor_target()); 68 | // Serial.printf("%f,%f,%f\n", DFOC_M0_Angle(), S0_electricalAngle(),S1_electricalAngle()); 69 | // Serial.printf("%f,%f,%f\n", DFOC_M0_Current(), DFOC_M1_Current(),serial_motor_target()); 70 | } 71 | //接收串口 72 | serialReceiveUserCommand(); 73 | } 74 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/InlineCurrent.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "InlineCurrent.h" 3 | 4 | 5 | // - shunt_resistor - 分流电阻值 6 | // - gain - 电流检测运算放大器增益 7 | // - phA - A 相 adc 引脚 8 | // - phB - B 相 adc 引脚 9 | // - phC - C 相 adc 引脚（可选） 10 | 11 | 12 | #define _ADC_VOLTAGE 3.3f //ADC 电压 13 | #define _ADC_RESOLUTION 4095.0f //ADC 分辨率 14 | 15 | // ADC 计数到电压转换比率求解 16 | #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) 17 | 18 | #define NOT_SET -12345.0 19 | #define _isset(a) ( (a) != (NOT_SET) ) 20 | 21 | CurrSense::CurrSense(int Mot_Num) 22 | { 23 | if(Mot_Num==0) 24 | { 25 | pinA = 39; 26 | pinB = 36; 27 | //int pinC; 28 | _shunt_resistor = 0.01; 29 | amp_gain = 50; 30 | 31 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 32 | 33 | // DengFOC V3P 34 | // gain_a = volts_to_amps_ratio*-1; 35 | // gain_b = volts_to_amps_ratio*-1; 36 | 37 | // DengFOC V4 38 | gain_a = volts_to_amps_ratio; 39 | gain_b = volts_to_amps_ratio; 40 | 41 | 42 | gain_c = volts_to_amps_ratio; 43 | } 44 | if(Mot_Num==1) 45 | { 46 | pinA = 35; 47 | pinB = 34; 48 | //int pinC; 49 | _shunt_resistor = 0.01; 50 | amp_gain = 50; 51 | 52 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 53 | 54 | // DengFOC V3P 55 | // gain_a = volts_to_amps_ratio*-1; 56 | // gain_b = volts_to_amps_ratio*-1; 57 | 58 | // DengFOC V4 59 | gain_a = volts_to_amps_ratio; 60 | gain_b = volts_to_amps_ratio; 61 | 62 | gain_c = volts_to_amps_ratio; 63 | } 64 | } 65 | float CurrSense::readADCVoltageInline(const int pinA){ 66 | uint32_t raw_adc = analogRead(pinA); 67 | return raw_adc * _ADC_CONV; 68 | } 69 | void CurrSense::configureADCInline(const int pinA,const int pinB, const int pinC){ 70 | pinMode(pinA, INPUT); 71 | pinMode(pinB, INPUT); 72 | if( _isset(pinC) ) pinMode(pinC, INPUT); 73 | } 74 | 75 | // 查找 ADC 零偏移量的函数 76 | void CurrSense::calibrateOffsets(){ 77 | const int calibration_rounds = 1000; 78 | 79 | // 查找0电流时候的电压 80 | offset_ia = 0; 81 | offset_ib = 0; 82 | offset_ic = 0; 83 | // 读数1000次 84 | for (int i = 0; i < calibration_rounds; i++) { 85 | offset_ia += readADCVoltageInline(pinA); 86 | offset_ib += readADCVoltageInline(pinB); 87 | if(_isset(pinC)) offset_ic += readADCVoltageInline(pinC); 88 | delay(1); 89 | } 90 | // 求平均，得到误差 91 | offset_ia = offset_ia / calibration_rounds; 92 | offset_ib = offset_ib / calibration_rounds; 93 | if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; 94 | } 95 | 96 | void CurrSense::init(){ 97 | // 配置函数 98 | configureADCInline(pinA,pinB,pinC); 99 | // 校准 100 | calibrateOffsets(); 101 | } 102 | 103 | 104 | // 读取全部三相电流 105 | 106 | void CurrSense::getPhaseCurrents(){ 107 | current_a = (readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps 108 | current_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps 109 | current_c = (!_isset(pinC)) ? 0 : (readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps 110 | } 111 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/InlineCurrent.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class CurrSense 4 | { 5 | public: 6 | CurrSense(int Mot_Num); 7 | float readADCVoltageInline(const int pinA); 8 | void configureADCInline(const int pinA,const int pinB, const int pinC); 9 | void calibrateOffsets(); 10 | void init(); 11 | void getPhaseCurrents(); 12 | float current_a,current_b,current_c; 13 | int pinA; 14 | int pinB; 15 | int pinC; 16 | float offset_ia; 17 | float offset_ib; 18 | float offset_ic; 19 | float _shunt_resistor; 20 | float amp_gain; 21 | 22 | float volts_to_amps_ratio; 23 | 24 | float gain_a; 25 | float gain_b; 26 | float gain_c; 27 | private: 28 | int _Mot_Num; 29 | }; 30 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十一课a+b SVPWM的算法实现思路和框架/DengFOC_Lib_Lesson11_SVPWM/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/AS5600.cpp: -------------------------------------------------------------------------------- 1 | #include "AS5600.h" 2 | #include "Wire.h" 3 | #include 4 | 5 | #define _2PI 6.28318530718f 6 | 7 | 8 | 9 | // AS5600 相关 10 | double Sensor_AS5600::getSensorAngle() { 11 | uint8_t angle_reg_msb = 0x0C; 12 | 13 | byte readArray[2]; 14 | uint16_t readValue = 0; 15 | 16 | wire->beginTransmission(0x36); 17 | wire->write(angle_reg_msb); 18 | wire->endTransmission(false); 19 | 20 | 21 | wire->requestFrom(0x36, (uint8_t)2); 22 | for (byte i=0; i < 2; i++) { 23 | readArray[i] = wire->read(); 24 | } 25 | int _bit_resolution=12; 26 | int _bits_used_msb=11-7; 27 | float cpr = pow(2, _bit_resolution); 28 | int lsb_used = _bit_resolution - _bits_used_msb; 29 | 30 | uint8_t lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); 31 | uint8_t msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); 32 | 33 | readValue = ( readArray[1] & lsb_mask ); 34 | readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); 35 | return (readValue/ (float)cpr) * _2PI; 36 | 37 | } 38 | 39 | //AS5600 相关 40 | 41 | //=========角度处理相关============= 42 | Sensor_AS5600::Sensor_AS5600(int Mot_Num) { 43 | _Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用 44 | 45 | } 46 | void Sensor_AS5600::Sensor_init(TwoWire* _wire) { 47 | wire=_wire; 48 | wire->begin(); //电机Sensor 49 | delay(500); 50 | getSensorAngle(); 51 | delayMicroseconds(1); 52 | vel_angle_prev = getSensorAngle(); 53 | vel_angle_prev_ts = micros(); 54 | delay(1); 55 | getSensorAngle(); 56 | delayMicroseconds(1); 57 | angle_prev = getSensorAngle(); 58 | angle_prev_ts = micros(); 59 | } 60 | void Sensor_AS5600::Sensor_update() { 61 | float val = getSensorAngle(); 62 | angle_prev_ts = micros(); 63 | float d_angle = val - angle_prev; 64 | // 圈数检测 65 | if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 66 | angle_prev = val; 67 | } 68 | 69 | float Sensor_AS5600::getMechanicalAngle() { 70 | return angle_prev; 71 | } 72 | 73 | float Sensor_AS5600::getAngle(){ 74 | return (float)full_rotations * _2PI + angle_prev; 75 | } 76 | 77 | float Sensor_AS5600::getVelocity() { 78 | // 计算采样时间 79 | float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; 80 | // 快速修复奇怪的情况（微溢出） 81 | if(Ts <= 0) Ts = 1e-3f; 82 | // 速度计算 83 | float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts; 84 | // 保存变量以待将来使用 85 | vel_angle_prev = angle_prev; 86 | vel_full_rotations = full_rotations; 87 | vel_angle_prev_ts = angle_prev_ts; 88 | return vel; 89 | } 90 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/AS5600.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Wire.h" 3 | 4 | class Sensor_AS5600 5 | { 6 | public: 7 | Sensor_AS5600(int Mot_Num); 8 | void Sensor_init(TwoWire* _wire = &Wire); 9 | void Sensor_update(); 10 | float getAngle(); 11 | float getVelocity(); 12 | float getMechanicalAngle(); 13 | double getSensorAngle(); 14 | private: 15 | int _Mot_Num; 16 | //AS5600 变量定义 17 | //int sensor_direction=1; //编码器旋转方向定义 18 | float angle_prev=0; // 最后一次调用 getSensorAngle() 的输出结果，用于得到完整的圈数和速度 19 | long angle_prev_ts=0; // 上次调用 getAngle 的时间戳 20 | float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度 21 | long vel_angle_prev_ts=0; // 最后速度计算时间戳 22 | int32_t full_rotations=0; // 总圈数计数 23 | int32_t vel_full_rotations=0; //用于速度计算的先前完整旋转圈数 24 | TwoWire* wire; 25 | }; 26 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/DengFOC.h: -------------------------------------------------------------------------------- 1 | //灯哥开源，遵循GNU协议，转载请著名版权！ 2 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 3 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 4 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 5 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 6 | //函数声明 7 | 8 | 9 | void M0_setPwm(float Ua, float Ub, float Uc); 10 | void M1_setPwm(float Ua, float Ub, float Uc); 11 | void DFOC_enable(); 12 | void DFOC_disable(); 13 | void M0_setTorque(float Uq,float angle_el); 14 | void M1_setTorque(float Uq,float angle_el); 15 | float _normalizeAngle(float angle); 16 | void DFOC_Vbus(float power_supply); 17 | void DFOC_M0_alignSensor(int _PP,int _DIR); 18 | void DFOC_M1_alignSensor(int _PP,int _DIR); 19 | float S0_electricalAngle(); 20 | float S1_electricalAngle(); 21 | float cal_Iq_Id(float current_a,float current_b,float angle_el); 22 | float serial_motor_target(); 23 | String serialReceiveUserCommand(); 24 | //传感器读取 25 | float DFOC_M0_Velocity(); 26 | float DFOC_M1_Velocity(); 27 | float DFOC_M0_Angle(); 28 | float DFOC_M1_Angle(); 29 | float DFOC_M0_Current(); 30 | float DFOC_M1_Current(); 31 | //PID 32 | void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 33 | void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 34 | void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp); 35 | void DFOC_M1_SET_ANGLE_PID(float P,float I,float D,float ramp,float limit); 36 | void DFOC_M1_SET_VEL_PID(float P,float I,float D,float ramp,float limit); 37 | void DFOC_M1_SET_CURRENT_PID(float P,float I,float D,float ramp); 38 | float DFOC_M0_VEL_PID(float error); 39 | float DFOC_M0_ANGLE_PID(float error); 40 | float DFOC_M1_VEL_PID(float error); 41 | float DFOC_M1_ANGLE_PID(float error); 42 | 43 | //接口函数 44 | void DFOC_M0_set_Velocity_Angle(float Target); 45 | void DFOC_M0_setVelocity(float Target); 46 | void DFOC_M0_set_Force_Angle(float Target); 47 | void DFOC_M1_set_Velocity_Angle(float Target); 48 | void DFOC_M1_setVelocity(float Target); 49 | void DFOC_M1_set_Force_Angle(float Target); 50 | void DFOC_M0_setTorque(float Target); 51 | void DFOC_M1_setTorque(float Target); 52 | //runFOC 循环函数 53 | void runFOC(); 54 | //测试函数 55 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/DengFOC_Lib_Lesson10_Dual_Motor.ino: -------------------------------------------------------------------------------- 1 | //DengFOC V0.2 2 | //灯哥开源，遵循GNU协议，转载请著名版权！ 3 | //GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 4 | //该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 5 | //仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 6 | //你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 7 | 8 | #include "DengFOC.h" 9 | 10 | 11 | void setup() { 12 | Serial.begin(115200); 13 | DFOC_Vbus(12.6); //设定驱动器供电电压 14 | DFOC_M0_alignSensor(7,-1); 15 | DFOC_M1_alignSensor(7,-1); 16 | //DFOC_M1_alignSensor(7,1); 17 | DFOC_enable(); 18 | 19 | } 20 | 21 | int count=0; 22 | void loop() 23 | { 24 | runFOC(); 25 | // DFOC_M0_setTorque(1); 26 | // DFOC_M1_setTorque(1); 27 | 28 | //力位（加入电流环后） 29 | // DFOC_M0_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 30 | // DFOC_M0_SET_CURRENT_PID(1.25,50,0,100000); 31 | // DFOC_M0_set_Force_Angle(serial_motor_target()); 32 | // DFOC_M1_SET_ANGLE_PID(0.5,0,0.003,100000,0.1); 33 | // DFOC_M1_SET_CURRENT_PID(1.25,50,0,100000); 34 | // DFOC_M1_set_Force_Angle(serial_motor_target()); 35 | 36 | //速度（加入电流环后） 37 | // DFOC_M0_SET_VEL_PID(3,2,0,100000,0.5); 38 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 39 | // DFOC_M0_setVelocity(serial_motor_target()); 40 | // DFOC_M0_SET_VEL_PID(0.1,2,0,100000,0.5); 41 | // DFOC_M0_SET_CURRENT_PID(0.5,50,0,100000); 42 | // DFOC_M0_setVelocity(serial_motor_target()); 43 | 44 | //位置-速度-力（加入电流环后） 45 | DFOC_M0_SET_ANGLE_PID(1,0,0,100000,30); 46 | DFOC_M0_SET_VEL_PID(0.02,1,0,100000,0.5); 47 | DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 48 | DFOC_M0_set_Velocity_Angle(serial_motor_target()); 49 | 50 | //位置-速度-力（加入电流环后） 51 | DFOC_M1_SET_ANGLE_PID(1,0,0,100000,30); 52 | DFOC_M1_SET_VEL_PID(0.02,1,0,100000,0.5); 53 | DFOC_M1_SET_CURRENT_PID(5,200,0,100000); 54 | DFOC_M1_set_Velocity_Angle(serial_motor_target()); 55 | //电流力矩 56 | // DFOC_M1_SET_CURRENT_PID(5,200,0,100000); 57 | // DFOC_M0_SET_CURRENT_PID(5,200,0,100000); 58 | 59 | // DFOC_M0_setTorque(serial_motor_target()); 60 | // DFOC_M1_setTorque(serial_motor_target()); 61 | 62 | 63 | 64 | count++; 65 | if(count>100) 66 | { 67 | count=0; 68 | //Serial.printf("%f\n", DFOC_M0_Current()); 69 | Serial.printf("%f,%f,%f,%f\n", DFOC_M0_Current(),DFOC_M0_Angle(), DFOC_M0_Velocity(),serial_motor_target()); 70 | // Serial.printf("%f,%f,%f\n", DFOC_M0_Angle(), S0_electricalAngle(),S1_electricalAngle()); 71 | // Serial.printf("%f,%f,%f\n", DFOC_M0_Current(), DFOC_M1_Current(),serial_motor_target()); 72 | } 73 | //接收串口 74 | serialReceiveUserCommand(); 75 | 76 | } 77 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/InlineCurrent.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "InlineCurrent.h" 3 | 4 | 5 | // - shunt_resistor - 分流电阻值 6 | // - gain - 电流检测运算放大器增益 7 | // - phA - A 相 adc 引脚 8 | // - phB - B 相 adc 引脚 9 | // - phC - C 相 adc 引脚（可选） 10 | 11 | 12 | #define _ADC_VOLTAGE 3.3f //ADC 电压 13 | #define _ADC_RESOLUTION 4095.0f //ADC 分辨率 14 | 15 | // ADC 计数到电压转换比率求解 16 | #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) 17 | 18 | #define NOT_SET -12345.0 19 | #define _isset(a) ( (a) != (NOT_SET) ) 20 | 21 | CurrSense::CurrSense(int Mot_Num) 22 | { 23 | if(Mot_Num==0) 24 | { 25 | pinA = 39; 26 | pinB = 36; 27 | //int pinC; 28 | _shunt_resistor = 0.01; 29 | amp_gain = 50; 30 | 31 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 32 | 33 | // DengFOC V3P 34 | gain_a = volts_to_amps_ratio*-1; 35 | gain_b = volts_to_amps_ratio*-1; 36 | 37 | // DengFOC V4 38 | // gain_a = volts_to_amps_ratio; 39 | // gain_b = volts_to_amps_ratio; 40 | 41 | 42 | gain_c = volts_to_amps_ratio; 43 | } 44 | if(Mot_Num==1) 45 | { 46 | pinA = 35; 47 | pinB = 34; 48 | //int pinC; 49 | _shunt_resistor = 0.01; 50 | amp_gain = 50; 51 | 52 | volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps 53 | 54 | // DengFOC V3P 55 | gain_a = volts_to_amps_ratio*-1; 56 | gain_b = volts_to_amps_ratio*-1; 57 | 58 | // DengFOC V4 59 | // gain_a = volts_to_amps_ratio; 60 | // gain_b = volts_to_amps_ratio; 61 | 62 | gain_c = volts_to_amps_ratio; 63 | } 64 | } 65 | float CurrSense::readADCVoltageInline(const int pinA){ 66 | uint32_t raw_adc = analogRead(pinA); 67 | return raw_adc * _ADC_CONV; 68 | } 69 | void CurrSense::configureADCInline(const int pinA,const int pinB, const int pinC){ 70 | pinMode(pinA, INPUT); 71 | pinMode(pinB, INPUT); 72 | if( _isset(pinC) ) pinMode(pinC, INPUT); 73 | } 74 | 75 | // 查找 ADC 零偏移量的函数 76 | void CurrSense::calibrateOffsets(){ 77 | const int calibration_rounds = 1000; 78 | 79 | // 查找0电流时候的电压 80 | offset_ia = 0; 81 | offset_ib = 0; 82 | offset_ic = 0; 83 | // 读数1000次 84 | for (int i = 0; i < calibration_rounds; i++) { 85 | offset_ia += readADCVoltageInline(pinA); 86 | offset_ib += readADCVoltageInline(pinB); 87 | if(_isset(pinC)) offset_ic += readADCVoltageInline(pinC); 88 | delay(1); 89 | } 90 | // 求平均，得到误差 91 | offset_ia = offset_ia / calibration_rounds; 92 | offset_ib = offset_ib / calibration_rounds; 93 | if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; 94 | } 95 | 96 | void CurrSense::init(){ 97 | // 配置函数 98 | configureADCInline(pinA,pinB,pinC); 99 | // 校准 100 | calibrateOffsets(); 101 | } 102 | 103 | 104 | // 读取全部三相电流 105 | 106 | void CurrSense::getPhaseCurrents(){ 107 | current_a = (readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps 108 | current_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps 109 | current_c = (!_isset(pinC)) ? 0 : (readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps 110 | } 111 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/InlineCurrent.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class CurrSense 4 | { 5 | public: 6 | CurrSense(int Mot_Num); 7 | float readADCVoltageInline(const int pinA); 8 | void configureADCInline(const int pinA,const int pinB, const int pinC); 9 | void calibrateOffsets(); 10 | void init(); 11 | void getPhaseCurrents(); 12 | float current_a,current_b,current_c; 13 | int pinA; 14 | int pinB; 15 | int pinC; 16 | float offset_ia; 17 | float offset_ib; 18 | float offset_ic; 19 | float _shunt_resistor; 20 | float amp_gain; 21 | 22 | float volts_to_amps_ratio; 23 | 24 | float gain_a; 25 | float gain_b; 26 | float gain_c; 27 | private: 28 | int _Mot_Num; 29 | }; 30 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "lowpass_filter.h" 2 | 3 | LowPassFilter::LowPassFilter(float time_constant) 4 | : Tf(time_constant) 5 | , y_prev(0.0f) 6 | { 7 | timestamp_prev = micros(); 8 | } 9 | 10 | 11 | float LowPassFilter::operator() (float x) 12 | { 13 | unsigned long timestamp = micros(); 14 | float dt = (timestamp - timestamp_prev)*1e-6f; 15 | 16 | if (dt < 0.0f ) dt = 1e-3f; 17 | else if(dt > 0.3f) { 18 | y_prev = x; 19 | timestamp_prev = timestamp; 20 | return x; 21 | } 22 | 23 | float alpha = Tf/(Tf + dt); 24 | float y = alpha*y_prev + (1.0f - alpha)*x; 25 | y_prev = y; 26 | timestamp_prev = timestamp; 27 | return y; 28 | } 29 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LOWPASS_FILTER_H 4 | #define LOWPASS_FILTER_H 5 | 6 | /** 7 | * 低通滤波器类 8 | */ 9 | 10 | class LowPassFilter 11 | { 12 | public: 13 | /** 14 | * @Tf - 低通滤波时间常数 15 | */ 16 | LowPassFilter(float Tf); 17 | ~LowPassFilter() = default; 18 | 19 | float operator() (float x); 20 | float Tf; //!< 低通滤波时间常数 21 | 22 | protected: 23 | unsigned long timestamp_prev; //!< 最后执行时间戳 24 | float y_prev; //!< 上一个循环中的过滤后的值 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include 3 | #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 4 | 5 | 6 | PIDController::PIDController(float P, float I, float D, float ramp, float limit) 7 | : P(P) 8 | , I(I) 9 | , D(D) 10 | , output_ramp(ramp) // PID控制器加速度限幅 11 | , limit(limit) // PID控制器输出限幅 12 | , error_prev(0.0f) 13 | , output_prev(0.0f) 14 | , integral_prev(0.0f) 15 | { 16 | timestamp_prev = micros(); 17 | } 18 | 19 | // PID 控制器函数 20 | float PIDController::operator() (float error){ 21 | // 计算两次循环中间的间隔时间 22 | unsigned long timestamp_now = micros(); 23 | float Ts = (timestamp_now - timestamp_prev) * 1e-6f; 24 | if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; 25 | 26 | // P环 27 | float proportional = P * error; 28 | // Tustin 散点积分（I环） 29 | float integral = integral_prev + I*Ts*0.5f*(error + error_prev); 30 | integral = _constrain(integral, -limit, limit); 31 | // D环（微分环节） 32 | float derivative = D*(error - error_prev)/Ts; 33 | 34 | // 将P,I,D三环的计算值加起来 35 | float output = proportional + integral + derivative; 36 | output = _constrain(output, -limit, limit); 37 | 38 | if(output_ramp > 0){ 39 | // 对PID的变化速率进行限制 40 | float output_rate = (output - output_prev)/Ts; 41 | if (output_rate > output_ramp) 42 | output = output_prev + output_ramp*Ts; 43 | else if (output_rate < -output_ramp) 44 | output = output_prev - output_ramp*Ts; 45 | } 46 | // 保存值（为了下一次循环） 47 | integral_prev = integral; 48 | output_prev = output; 49 | error_prev = error; 50 | timestamp_prev = timestamp_now; 51 | return output; 52 | } 53 | -------------------------------------------------------------------------------- /【手把手教些FOC算法】系列课程代码/第十课a+b 双电机闭环FOC代码/DengFOC_Lib_Lesson10_Dual_Motor/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PIDController 5 | { 6 | public: 7 | PIDController(float P, float I, float D, float ramp, float limit); 8 | ~PIDController() = default; 9 | 10 | float operator() (float error); 11 | 12 | float P; //!< 比例增益(P环增益) 13 | float I; //!< 积分增益（I环增益） 14 | float D; //!< 微分增益（D环增益） 15 | float output_ramp; 16 | float limit; 17 | protected: 18 | float error_prev; //!< 最后的跟踪误差值 19 | float output_prev; //!< 最后一个 pid 输出值 20 | float integral_prev; //!< 最后一个积分分量值 21 | unsigned long timestamp_prev; //!< 上次执行时间戳 22 | }; 23 | 24 | #endif 25 | --------------------------------------------------------------------------------