|
#include <Arduino.h>
|
|
#include <SimpleFOC.h>
|
|
/* =====================================================================
|
|
* SimpleFOC 开环步进 — Nucleo-G474RE + A4950 + 42步进
|
|
* 兼容: motor_control.py Commander + Industrial_ui gimbaldll
|
|
*
|
|
* gimbaldll 串口习惯（从 DLL 字符串还原）:
|
|
* open: AE1 / BE1
|
|
* speed: %cE1 %cC1 %c<vel>
|
|
* pos: %cC2 %c<pos> <speed?>
|
|
* Commander: C1=velocity → 映射 velocity_openloop
|
|
* C2=angle → 映射 angle_openloop（无编码器）
|
|
*
|
|
* A4950: AIN1=5 AIN2=6 BIN1=9 BIN2=10；A/B 前缀同一台电机
|
|
* 串口: VCP USART2 PA2/PA3 → COM14 @ 115200
|
|
* ===================================================================== */
|
|
|
|
const uint8_t PIN_AIN1 = 5;
|
|
const uint8_t PIN_AIN2 = 6;
|
|
const uint8_t PIN_BIN1 = 9;
|
|
const uint8_t PIN_BIN2 = 10;
|
|
|
|
const int STEPPER_STEPS = 200;
|
|
const float SUPPLY_VOLTAGE = 12.0f;
|
|
const float VOLTAGE_LIMIT = 4.5f;
|
|
const float MAX_TARGET_VEL = 20.0f;
|
|
const float TARGET_RAMP_RPS = 8.0f;
|
|
|
|
StepperMotor motor(STEPPER_STEPS);
|
|
StepperDriver4PWM driver(PIN_AIN1, PIN_AIN2, PIN_BIN1, PIN_BIN2);
|
|
Commander command = Commander(Serial);
|
|
|
|
float cmdTarget = 0.0f;
|
|
float rampedTarget = 0.0f;
|
|
bool velRampEnable = true; // 仅速度模式做斜坡
|
|
|
|
static void forceOpenLoop() {
|
|
// 无 sensor：把闭环模式改成对应开环，否则 move() 不驱动
|
|
if (motor.controller == MotionControlType::velocity)
|
|
motor.controller = MotionControlType::velocity_openloop;
|
|
else if (motor.controller == MotionControlType::angle)
|
|
motor.controller = MotionControlType::angle_openloop;
|
|
}
|
|
|
|
static bool isVelMode() {
|
|
return motor.controller == MotionControlType::velocity ||
|
|
motor.controller == MotionControlType::velocity_openloop;
|
|
}
|
|
|
|
void onMotorCmd(char* cmd) {
|
|
command.motor(&motor, cmd);
|
|
forceOpenLoop();
|
|
velRampEnable = isVelMode();
|
|
if (velRampEnable) {
|
|
cmdTarget = motor.target;
|
|
}
|
|
// 角度模式：斜坡状态与目标对齐，避免回切速度时突跳
|
|
rampedTarget = motor.target;
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
delay(400);
|
|
|
|
Serial.println();
|
|
Serial.println(F("========== SimpleFOC STEPPER + A4950 =========="));
|
|
Serial.println(F("open-loop + gimbal Commander (C1/C2 remap)"));
|
|
Serial.println(F("cmd: AE1 A2 AC2 +A0.17 or Industrial_ui"));
|
|
|
|
driver.voltage_power_supply = SUPPLY_VOLTAGE;
|
|
driver.voltage_limit = VOLTAGE_LIMIT;
|
|
driver.pwm_frequency = 20000;
|
|
driver.init();
|
|
|
|
motor.linkDriver(&driver);
|
|
motor.controller = MotionControlType::velocity_openloop;
|
|
motor.voltage_limit = VOLTAGE_LIMIT;
|
|
motor.velocity_limit = MAX_TARGET_VEL;
|
|
|
|
motor.PID_velocity.P = 0.2f;
|
|
motor.PID_velocity.I = 1.0f;
|
|
motor.PID_velocity.D = 0.0f;
|
|
motor.PID_velocity.output_ramp = 50.0f;
|
|
motor.LPF_velocity.Tf = 0.02f;
|
|
|
|
motor.init();
|
|
motor.disable();
|
|
|
|
command.add('A', onMotorCmd, (char*)"stepper");
|
|
command.add('B', onMotorCmd, (char*)"stepper_alias");
|
|
|
|
Serial.println(F("Ready. AE1 then A2 / industrial SingleToPosition"));
|
|
Serial.println(F("========================================"));
|
|
}
|
|
|
|
void loop() {
|
|
static uint32_t lastUs = micros();
|
|
uint32_t now = micros();
|
|
float dt = (now - lastUs) * 1e-6f;
|
|
lastUs = now;
|
|
if (dt > 0.05f) dt = 0.05f;
|
|
|
|
if (velRampEnable && isVelMode()) {
|
|
float maxStep = TARGET_RAMP_RPS * dt;
|
|
float err = cmdTarget - rampedTarget;
|
|
if (err > maxStep) err = maxStep;
|
|
if (err < -maxStep) err = -maxStep;
|
|
rampedTarget += err;
|
|
motor.target = rampedTarget;
|
|
}
|
|
// 角度模式：保留 Commander 写入的 motor.target，不做速度斜坡覆盖
|
|
|
|
motor.move();
|
|
command.run();
|
|
}
|