手写最简单的simplefoc的svpwm算法（便于理解）

原创

mb6376218bdd5cf 2022-11-18 11:38:00 ©著作权

文章标签 arduino simplefoc svpwm git #define 文章分类 数据结构与算法人工智能

©著作权归作者所有：来自51CTO博客作者mb6376218bdd5cf的原创作品，请联系作者获取转载授权，否则将追究法律责任

此帖子用于记录学习过程中写的程序（半成品）。

1.arduino代码：

test.ino

//      (010)U2   II   U6(110)
//             *********
//      III   * *     * *   I
//           *   *   *   *
//  (011)U3 ***************  U4(100)
//           *   *   *   *
//       IV   * *     * *   VI
//             *********
//      (001)U1    V     U5(101)
//author:liuzhitong
//mail:953598974@qq.com

#include "svpwm.h"
#include <SimpleFOC.h>

#define sl 500
#define pi 3.1415926
#define f 30

Motor motor1;

MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Cone = TwoWire(0);
long t;

void setup() {
  Serial.begin(115200);
  motor1.init();
  I2Cone.begin(18, 5, 400000); 
  sensor.init(&I2Cone);
  t=millis();
}


void loop() {
  //if((motor1.angle_c!=motor1.angle_go)&&((millis()-motor1.old_time)>1))
  if((millis()-motor1.old_time)>25)
  {
    motor1.calculate();
  }
  if((millis()-t)>200)
  {
    sensor.update();
    Serial.println(sensor.getVelocity()); 
    t=millis();
  }
  motor1.svpwm(); 
  
}

svpwm.cpp

#define sqrt_3 1.7320
#define pi 3.1415926
#define T 200

#define enable_pinA 33
#define enable_pinB 32
#define enable_pinC 23

#define piA 25
#define piB 19
#define piC 22

class Motor{
  public:
    double angle_c=0.0;//默认开头是0度,电机角度在U4处
    double velocity_m=10.0;
    long angle_go=41400;//目标角度
    int T0=0,T1=0,T2=0,T7=0;
    int sector;//sector为扇区
    float U_dc=12.0,U_ref=6.0;
    float oumiga=pi;
    int cal_flag=1;//计算扇区和T0,T1,T2,T7的标志位，需要计算时为1，不需要计算时为0
    long old_time=0;//用于判断速度
    
    void init();
    void calculate();
    void svpwm();
};
//计算标志位的函数
void Motor::init()
{
  pinMode(22,OUTPUT);
  digitalWrite(22,HIGH);
  pinMode(32,OUTPUT);
  pinMode(33,OUTPUT);
  pinMode(25,OUTPUT);
  //默认开启的时候在第一区间
  digitalWrite(32,1);
  digitalWrite(33,0);
  digitalWrite(25,0);
  delay(1000);
  old_time=millis();
}
//T=1ms=1us;
//计算标志位的函数
void Motor::calculate()
{
  int angleInSector=int(angle_c)%60;
  sector=(int(angle_c)%360)/60+1;
  T1=sqrt_3*U_ref/U_dc*T*sin(pi*(60-angleInSector)/180);
  T2=sqrt_3*U_ref/U_dc*T*sin(pi*angleInSector/180);
  T0=(T-T1-T2)/2;
  T7=T0;
//  if(angle_c<angle_go)
//  {
    velocity_m=396.0*oumiga/(10.0*pi);
    angle_c=angle_c+2.5*velocity_m;
//  }
  old_time=millis();
//  Serial.println("-");
//  Serial.println(angle_c);
//  Serial.println(sector);
//  Serial.println(T0);
//  Serial.println(T1);
//  Serial.println(T2);
//  Serial.println(T7);
}

void Motor::svpwm()
{
  switch(sector)
  {
    case 1:
    {
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      digitalWrite(32,1);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T1/2);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,0);
      delayMicroseconds(T2/2);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T7);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,0);
      delayMicroseconds(T2/2);
      digitalWrite(32,1);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T1/2);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      break;
    }
    case 2:
    {
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,0);
      delayMicroseconds(T1/2);
      digitalWrite(32,0);
      digitalWrite(33,1);
      digitalWrite(25,0);
      delayMicroseconds(T2/2);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T7);
      digitalWrite(32,0);
      digitalWrite(33,1);
      digitalWrite(25,0);
      delayMicroseconds(T2/2);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,0);
      delayMicroseconds(T1/2);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      break;
    }
    case 3:
    {
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      digitalWrite(32,0);
      digitalWrite(33,1);
      digitalWrite(25,0);
      delayMicroseconds(T1/2);
      digitalWrite(32,0);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T2/2);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T7);
      digitalWrite(32,0);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T2/2);
      digitalWrite(32,0);
      digitalWrite(33,1);
      digitalWrite(25,0);
      delayMicroseconds(T1/2);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      break;
    }
    case 4:
    {
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      digitalWrite(32,0);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T1/2);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,1);
      delayMicroseconds(T2/2);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T7);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,1);
      delayMicroseconds(T2/2);
      digitalWrite(32,0);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T1/2);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      break;
    }
    case 5:
    {
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,1);
      delayMicroseconds(T1/2);
      digitalWrite(32,1);
      digitalWrite(33,0);
      digitalWrite(25,1);
      delayMicroseconds(T2/2);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T7);
      digitalWrite(32,1);
      digitalWrite(33,0);
      digitalWrite(25,1);
      delayMicroseconds(T2/2);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,1);
      delayMicroseconds(T1/2);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      break;
    }
    case 6:
    {
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      digitalWrite(32,1);
      digitalWrite(33,0);
      digitalWrite(25,1);
      delayMicroseconds(T1/2);
      digitalWrite(32,1);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T2/2);
      digitalWrite(32,1);
      digitalWrite(33,1);
      digitalWrite(25,1);
      delayMicroseconds(T7);
      digitalWrite(32,1);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T2/2);
      digitalWrite(32,1);
      digitalWrite(33,0);
      digitalWrite(25,1);
      delayMicroseconds(T1/2);
      digitalWrite(32,0);
      digitalWrite(33,0);
      digitalWrite(25,0);
      delayMicroseconds(T0/2);
      break;
    }
  }
}