#include 

// 磁传感器实例 - SPI
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
//TwoWire I2Cone = TwoWire(0);
TwoWire I2CTwo = TwoWire(1);
// 无刷直流电机及驱动器实例
BLDCMotor motor = BLDCMotor(7);
//BLDCDriver3PWM driver = BLDCDriver3PWM(32,33,25,22);
BLDCDriver3PWM driver = BLDCDriver3PWM(26,27,14,12);
// commander接口
Commander command = Commander(Serial);
void onMotor(char* cmd){ command.motor(&motor, cmd); }

void setup() {

//I2Cone.begin(19,18, 400000UL); 
I2CTwo.begin(23,5, 400000UL); 
// 初始化磁传感器硬件
//sensor.init(&I2Cone);
sensor.init(&I2CTwo);
// 连接电机和传感器
  motor.linkSensor(&sensor);
 // 配置驱动器
  // 电源电压 [V]
  driver.voltage_power_supply = 12;
  driver.init();
 // 连接驱动器
  motor.linkDriver(&driver);
  // 设置控制环类型
  motor.controller = MotionControlType::angle;
  // 基于控制环类型配置控制器 
  motor.PID_velocity.P = 0.05;
  motor.PID_velocity.I = 1;
  motor.PID_velocity.D = 0;
  // 默认为电源电压
  motor.voltage_limit = 3.3;
  // 速度低通滤波时间常数
  motor.LPF_velocity.Tf = 0.01;
  // 角度环控制器
  motor.P_angle.P = 6.28;
 // 角度环速度限制
  motor.velocity_limit = 50;
  // 使用串口监视电机初始化
  // 监视点
  Serial.begin(115200);
  // 如果不需要，可以注释掉此行
  motor.useMonitoring(Serial);
  // 初始化电机
  motor.init();
  // 校准编码器，启用FOC
  motor.initFOC();
  // 设置初始目标值
  motor.target = 0;
  // 定义电机 id
  command.add('M', onMotor, "motor");
  // 运行用户命令配置电机（完整命令列表见docs.simplefoc.com）
  Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 0V."));
  _delay(1000);
}

void loop() {

  // 迭代设置FOC相电压
  motor.loopFOC();
  // 设置外部环目标的迭代函数
  // 速度，位置或电压
  // 如果在参数中未设置目标，则使用电机目标变量
  motor.move();
  motor.monitor();
  // 用户通信
  command.run();
}