该例程主要是对角度信息进行上电初始化,确定初始电角度偏移量。这里采用的是磁传感器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信号输出
}