SimpleFOC -位置控制代码

#include 

#include 

// 无刷直流电机及驱动器实例
BLDCMotor motor = BLDCMotor(6);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);

// 编码器对象
Encoder encoder = Encoder(2, 3, 4096);
// 通道 A 和 B 回调函数
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}

// 电流传感器
//InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);

// 初始目标角度为0
float target_angle = 0;      


// 命令通信实例
Commander command = Commander(Serial);
// 串口控制指令：目标值
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } 
// 串口控制指令：电机
void onMotor(char* cmd){ command.motor(&motor, cmd); }

void setup() {
	
	// 初始化编码器传感器硬件
	encoder.init();
	encoder.enableInterrupts(doA, doB);
	// 连接电机与编码器
	motor.linkSensor(&encoder);
	
	// 驱动设置
	// 电源电压 [V]
	driver.voltage_power_supply = 12;
	driver.init();
	// 连接驱动器
	motor.linkDriver(&driver);
	
	// 设置要使用的控制回路类型
	//motor.torque_controller = TorqueControlType::foc_current;
	// 选择调制方式为SVPWM
	//motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
	motor.controller = MotionControlType::angle;
	
	// 基于控件类型的控制器配置
	motor.PID_velocity.P = 0.3;
	motor.PID_velocity.I = 10;
	motor.PID_velocity.D = 0;
	motor.P_angle.P = 0.5;
	// 最大电压
	motor.voltage_limit = 12;
	
	// 速度的滤波时间常数
	motor.LPF_velocity.Tf = 0.01;
	
	// 角环控制器
	motor.P_angle.P = 20;
	// 最大速度，rad/s
	motor.velocity_limit = 20;
	
	// 对电机初始化使用串行监控
	// 监控端口
	Serial.begin(115200);
	// 使用串口监视器
	motor.useMonitoring(Serial);
	//motor.monitor_downsample = 0; // 最初禁用实时监视器
	
	// 当前传感器初始化和链接
	//current_sense.init();
	//motor.linkCurrentSense(¤t_sense);
	
	// 初始化电机
	motor.init();
	// 校准传感器并启动FOC
	motor.initFOC();
	
	// 设定初始目标值
	//motor.target = 2;
	
	//提交电机命令
	command.add('T', doTarget, "target angle");
	command.add('M', onMotor, "my motor");
	
	 Serial.println(F("Motor ready."));
	 Serial.println(F("Set the target angle using serial terminal:"));
	_delay(1000);
}

void loop() {
	// 循环设置FOC相电压
	motor.loopFOC();
	
	// 设置目标的外循环函数
	motor.move(target_angle);
	
	// 运动监测 ,使用simpleFOC Studio上位机设置的时候，这句一定要打开。但是会影响程序执行速度
	motor.monitor(); 
	// 用户通信
	command.run();
}