I made mistake in earlier code here. I guess mks v3.2 dual foc board (rip off ones from aliexpress) use InlineCurrentSense. Below is code for dual 2804 gimbal motor holding 0 position with an idle timeout.
// Using lib_deps = askuric/Simple FOC@^2.4.0
#include <Arduino.h>
#include <SimpleFOC.h>
#include <Wire.h>
// ======================= Hardware pins =======================
// Motor 0 (I2C port 0)
constexpr int M0_I2C_SDA = 18;
constexpr int M0_I2C_SCL = 19;
constexpr int M0_PWM_A = 33;
constexpr int M0_PWM_B = 25;
constexpr int M0_PWM_C = 26;
constexpr int M0_CS_A = 39;
constexpr int M0_CS_B = 36;
// Motor 1 (I2C port 1)
constexpr int M1_I2C_SDA = 22;
constexpr int M1_I2C_SCL = 23;
constexpr int M1_PWM_A = 27;
constexpr int M1_PWM_B = 14;
constexpr int M1_PWM_C = 12;
constexpr int M1_CS_A = 32;
constexpr int M1_CS_B = 35;
// ======================= Motor specs & limits =======================
// These values are for 2804 gimbal motor
constexpr int POLE_PAIRS = 7;
constexpr float PHASE_RES = 0.25f; // [Ohm]
constexpr float KV = 360.0f; // [rpm/V]
constexpr int VOLT_SUPPLY = 24; // power supply [V]
constexpr float VOLT_LIMIT = 4.5f; // [V]
constexpr float VOLT_ALIGN = 2.0f; // sensor/motor align [V]
constexpr float CURR_LIMIT = 2.5f; // [A]
constexpr float VEL_LIMIT = 3.28f; // [rad/s]
// ======================= Control mode =======================
const TorqueControlType TORQUE_MODE = TorqueControlType::foc_current;
const MotionControlType MOTION_MODE = MotionControlType::angle;
const FOCModulationType FOC_MODE = FOCModulationType::SpaceVectorPWM;
// ======================= PID / filter tuning =======================
constexpr float CUR_PID_Q_P = 2.0f, CUR_PID_Q_I = 50.0f;
constexpr float CUR_PID_D_P = 2.0f, CUR_PID_D_I = 50.0f;
constexpr float CUR_LPF_TF = 0.005f;
// Quiet, damped profile (no velocity-D noise amplification).
constexpr float VEL_PID_P = 0.075f, VEL_PID_I = 0.25f, VEL_PID_D = 0.0f;
constexpr float ANG_P = 10.0f, ANG_I = 0.0f, ANG_D = 0.0f;
constexpr float VEL_LPF_TF = 0.005f;
constexpr float VEL_RAMP = 80.0f;
// ======================= Demo behavior =======================
constexpr float POSITION_DEADBAND = 0.05f; // ~3 deg; stop hunting on encoder noise at rest
constexpr int IDLE_TIMEOUT_MS = 1000; // release motor after this long settled
constexpr int PRINT_PERIOD_MS = 1000; // print cadence [ms]
// ======================= Hardware objects =======================
TwoWire I2Cone(0);
MagneticSensorI2C sensor0(AS5600_I2C);
BLDCDriver3PWM driver0(M0_PWM_A, M0_PWM_B, M0_PWM_C);
InlineCurrentSense current0(0.01f, 50.0f, M0_CS_A, M0_CS_B, NOT_SET);
BLDCMotor motor0(POLE_PAIRS, PHASE_RES, KV);
TwoWire I2Ctwo(1);
MagneticSensorI2C sensor1(AS5600_I2C);
BLDCDriver3PWM driver1(M1_PWM_A, M1_PWM_B, M1_PWM_C);
InlineCurrentSense current1(0.01f, 50.0f, M1_CS_A, M1_CS_B, NOT_SET);
BLDCMotor motor1(POLE_PAIRS, PHASE_RES, KV);
// ======================= Per-motor config + init (shared) =======================
// Both motors are configured identically except their I2C bus pins and the
// zero electric angle, so a single helper drives both setups.
void initMotor(BLDCMotor& motor, BLDCDriver3PWM& driver, MagneticSensorI2C& sensor,
InlineCurrentSense& cs, TwoWire& i2c, int i2cSDA, int i2cSCL,
float zeroElectricAngle) {
i2c.begin(i2cSDA, i2cSCL, 400000UL);
sensor.init(&i2c);
driver.voltage_power_supply = VOLT_SUPPLY;
driver.init();
motor.linkSensor(&sensor);
motor.linkDriver(&driver);
cs.skip_align = false;
cs.linkDriver(&driver);
cs.init();
motor.linkCurrentSense(&cs);
motor.voltage_limit = VOLT_LIMIT;
motor.voltage_sensor_align = VOLT_ALIGN;
motor.current_limit = CURR_LIMIT;
motor.velocity_limit = VEL_LIMIT;
motor.PID_current_q.P = CUR_PID_Q_P;
motor.PID_current_q.I = CUR_PID_Q_I;
motor.PID_current_d.P = CUR_PID_D_P;
motor.PID_current_d.I = CUR_PID_D_I;
motor.LPF_current_q.Tf = CUR_LPF_TF;
motor.LPF_current_d.Tf = CUR_LPF_TF;
motor.torque_controller = TORQUE_MODE;
motor.controller = MOTION_MODE;
motor.foc_modulation = FOC_MODE;
motor.PID_velocity.P = VEL_PID_P;
motor.PID_velocity.I = VEL_PID_I;
motor.PID_velocity.D = VEL_PID_D;
motor.P_angle.P = ANG_P;
motor.P_angle.I = ANG_I;
motor.P_angle.D = ANG_D;
motor.LPF_velocity.Tf = VEL_LPF_TF;
motor.PID_velocity.output_ramp = VEL_RAMP;
// Uncomment once you have made sure values in setup() are set for your configuration
// motor.sensor_direction = Direction::CCW;
// motor.zero_electric_angle = zeroElectricAngle;
motor.init();
motor.initFOC();
}
// ======================= Per-loop FOC step + idle auto-disable =======================
void runMotor(BLDCMotor& motor, unsigned long& idleTimer, float target) {
// Still moving? Reset the idle timer so an active motor stays enabled.
if (fabsf(motor.shaftAngle() - target) > POSITION_DEADBAND) {
idleTimer = millis();
}
motor.move(target);
motor.loopFOC();
// Release once settled for IDLE_TIMEOUT_MS; re-enable if released early.
if (millis() > idleTimer + IDLE_TIMEOUT_MS) {
motor.disable();
} else if (!motor.enabled) {
motor.enable();
}
}
// ======================= Setup =======================
void setup() {
Serial.begin(115200);
delay(200);
SimpleFOCDebug::enable(&Serial);
initMotor(motor0, driver0, sensor0, current0, I2Cone, M0_I2C_SDA, M0_I2C_SCL, 1.25f);
initMotor(motor1, driver1, sensor1, current1, I2Ctwo, M1_I2C_SDA, M1_I2C_SCL, 4.15f);
motor0.enable();
motor1.enable();
}
// ======================= Loop =======================
unsigned long motor0_idle = 0;
unsigned long motor1_idle = 0;
void loop() {
static unsigned long lastPrint = 0;
if (millis() - lastPrint >= PRINT_PERIOD_MS) {
lastPrint = millis();
Serial.printf("M0: %6.2f A / %6.2f A | M1: %6.2f A / %6.2f A\n",
motor0.current.d, motor0.current.q,
motor1.current.d, motor1.current.q);
}
runMotor(motor0, motor0_idle, 0.0f);
runMotor(motor1, motor1_idle, 0.0f);
}
