【SimpleFOC】例程find_sensor_offset_and_direction阅读笔记

该例程主要是对角度信息进行上电初始化，确定初始电角度偏移量。这里采用的是磁传感器AS5600来获取角度，可以实时更新转子的绝对位置，范围0~2PI。首先需要指定无刷电机的极对数，这里电机极对数为7，则转子物理上转一圈磁场周期性变化7次，对应的电角度也周期性变化（从0到2PI）7次。我们采用FOC来驱动无刷电机需要知道准确的电角度。SimpleFOC的角度校准策略，是在上电时通过开环的SVPWM驱动将转子强行拉到电角度0度，记录此时磁传感器的绝对角度，在软件中计算此绝对角度对应的电角度，则该电角度就是初始电角度偏移。校准完成之后，每次从磁传感器获取绝对角度计算对应的电角度，都需要减去这个初始的偏移量。

/**
 * Simple example intended to help users find the zero offset and natural direction of the sensor. 
 * 
 * These values can further be used to avoid motor and sensor alignment procedure. 
 * 
 * motor.initFOC(zero_offset, sensor_direction);
 * 
 * This will only work for abosolute value sensors - magnetic sensors. 
 * Bypassing the alignment procedure is not possible for the encoders and for the current implementation of the Hall sensors. 
 * library version 1.4.2.
 * 
 */
#include <SimpleFOC.h>

//三种接口的磁传感器
// magnetic sensor instance - SPI
//MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);//SPI接口
// magnetic sensor instance - I2C
//AS5600通常用IIC驱动
MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);//IIC接口
// magnetic sensor instance - analog output
//MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);//模拟接口

// BLDC motor instance
BLDCMotor motor = BLDCMotor(7);//磁极对数，14个磁铁构成7对
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);//PWM信号分别由9、5、6号引脚输出，8号引脚作为使能引脚，如果不够，可以再额外定义两个用于使能的GPIO引脚
// Stepper motor instance
//StepperMotor motor = StepperMotor(50);
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);

void setup() {

  // power supply voltage
  driver.voltage_power_supply = 12;//电机工作电压为12V
  driver.init();//PWM引脚初始化
  motor.linkDriver(&driver);//将PWM驱动参数对接到相应的变量中

  // initialise magnetic sensor hardware
  sensor.init();//磁传感器引脚初始化
  // link the motor to the sensor
  motor.linkSensor(&sensor);//将传感器信息对接到相应的变量中

  // aligning voltage 
  motor.voltage_sensor_align = 7;//对齐电压根据需要来设置，最大不超过电源电压
  
  // set motion control loop to be used
  motor.controller = MotionControlType::torque;//这里是扭矩控制，还有角度、速度等控制

  // initialize motor
  motor.init();//这里设置了一些电压限制和电流限制
  // align sensor and start FOC
  motor.initFOC();//角度对齐、电流传感对齐（可选），一些变量如角度偏移量等在这里初始化完成

  
  Serial.begin(115200);
  Serial.println("Sensor zero offset is:");
  Serial.println(motor.zero_electric_angle, 4);//初始电角度
  Serial.println("Sensor natural direction is: ");
  Serial.println(motor.sensor_direction == 1 ? "Direction::CW" : "Direction::CCW");//电机转动方向（顺时针、逆时针）

  Serial.println("To use these values provide them to the: motor.initFOC(offset, direction)");
  _delay(1000);
  Serial.println("If motor is not moving the alignment procedure was not successfull!!");
}


void loop() {
    
  // main FOC algorithm function
  //
  motor.loopFOC();//采集传感器数据，电流闭环，控制电机转动

  // Motion control function
  motor.move(2);//如果是速度环，位置环则更新控制量，如果是开环则直接控制电机
}

下面是loopFOC函数的代码，该函数主要完成传感器数据的更新，以及电流PID闭环控制，当没有电流传感器时，直接以电压作为控制量而不需要进行PID控制，电压有越大则三相输出占空比就越大。

// Iterative function looping FOC algorithm, setting Uq on the Motor
// The faster it can be run the better
void BLDCMotor::loopFOC() {
  // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
  //                 of full rotations otherwise.
  if (sensor) sensor->update();		//更新传感器数据

  // if open-loop do nothing
  if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;			
  //如果是开环控制则直接返回
  
  // if disabled do nothing
  if(!enabled) return;

  // Needs the update() to be called first
  // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() 
  // which is in range 0-2PI
  electrical_angle = electricalAngle();
  switch (torque_controller) {
    case TorqueControlType::voltage:			//如果是电压控制，则直接跳过PID调节
      // no need to do anything really
      break;
    case TorqueControlType::dc_current:			//只调节切向分量
      if(!current_sense) return;
      // read overall current magnitude
      current.q = current_sense->getDCCurrent(electrical_angle);		//获取当前电流值
      // filter the value values
      current.q = LPF_current_q(current.q);				//低通滤波
      // calculate the phase voltage
      voltage.q = PID_current_q(current_sp - current.q);			//经过PID调节得到Uq
      voltage.d = 0;
      break;
    case TorqueControlType::foc_current:							//调节切向和径向分量
      if(!current_sense) return;
      // read dq currents
      current = current_sense->getFOCCurrents(electrical_angle);	//获取当前电流值
      // filter values
      current.q = LPF_current_q(current.q);				//低通滤波
      current.d = LPF_current_d(current.d);
      // calculate the phase voltages
      voltage.q = PID_current_q(current_sp - current.q);			//PID调节得到Uq和Ud
      voltage.d = PID_current_d(-current.d);						//目标值为0，所以误差输入量为-current.d
      break;
    default:
      // no torque control selected
      SIMPLEFOC_DEBUG("MOT: no torque control selected!");
      break;
  }
  
  // set the phase voltage - FOC heart function :)
  setPhaseVoltage(voltage.q, voltage.d, electrical_angle);			//三相占空PWM信号输出
}