Hello i have a SimpleFOC programm and i want to send the command froma a Computer to the Esp32 it works fine but when i want to send the Command M10.52 50 2.1. So 10.52 rad, 50 rad/sec and 2.1 Tourque the motor just doesn’t move at all.
Here is the code i use.
/**
*
* Position/angle motion control example
* Steps:
* 1) Configure the motor and magnetic sensor
* 2) Run the code
* 3) Set the target angle (in radians) from serial terminal
*
*/
#include <SimpleFOC.h>
#include <Wire.h>
#define I2C_SDA 21
#define I2C_SCL 45
MotionControlType prev_controller;
float prev_voltage_limit;
float prev_velocity_limit;
float desired_angle = -11.80903703704;
// magnetic sensor instance - SPI
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
// magnetic sensor instance - MagneticSensorI2C
//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
// magnetic sensor instance - analog output
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
InlineCurrentSense current_sense = InlineCurrentSense(0.004f, 20.0f, 6, 7);
// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(15, 0.502, 18.1, 0.0007758);
BLDCDriver3PWM driver = BLDCDriver3PWM(48, 47, 46);
// Stepper motor & driver instance
//StepperMotor motor = StepperMotor(50);
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
// instantiate the commander
Commander command = Commander(Serial);
void doMotor(char* cmd) { command.motor(&motor, cmd); }
void Cali_Start(char* cmd) {
prev_controller = motor.controller;
prev_voltage_limit = motor.voltage_limit;
prev_velocity_limit = motor.velocity_limit;
motor.controller = MotionControlType::torque;
// Limits setzen
motor.voltage_limit = 12; // max Torque
motor.velocity_limit = 2.0; // max Speed
// Ziel setzen
motor.move(-2.8);
}
void Cali_End(char* cmd) {
motor.sensor_offset = motor.shaft_angle - desired_angle;
motor.controller = prev_controller;
motor.voltage_limit = prev_voltage_limit;
motor.velocity_limit = prev_velocity_limit;
motor.target = desired_angle;
}
void setup() {
// use monitoring with serial
Serial.begin(115200);
// enable more verbose output for debugging
// comment out if not needed
SimpleFOCDebug::enable(&Serial);
Wire.setClock(400000);
Wire.begin(I2C_SDA, I2C_SCL);
sensor.init(&Wire);
// link the motor to the sensor
motor.linkSensor(&sensor);
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 12;
driver.pwm_frequency = 20000;
if(!driver.init()){
Serial.println("Driver init failed!");
return;
}
// link the motor and the driver
motor.linkDriver(&driver);
current_sense.linkDriver(&driver);
if(!current_sense.init()){
Serial.println("Current sense init failed!");
return;
}
motor.linkCurrentSense(¤t_sense);
// choose FOC modulation (optional)
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// set motion control loop to be used
motor.torque_controller = TorqueControlType::voltage;
motor.controller = MotionControlType::angle;
// contoller configuration
// default parameters in defaults.h
// maximal voltage to be set to the motor
motor.voltage_limit = 12;
motor.current_limit = 200;
motor.velocity_limit = 200;
motor.PID_velocity.P = 3.0;
motor.PID_velocity.I = 1.0;
motor.PID_velocity.D = 0.002;
motor.LPF_velocity.Tf = 0.05;
motor.PID_velocity.limit = motor.velocity_limit;
motor.P_angle.limit = 200.0;
motor.P_angle.P = 2.0;
motor.P_angle.I = 0.025;
motor.P_angle.D = 0;
motor.LPF_angle.Tf = 0.05;
motor.PID_current_d.limit = motor.current_limit;
motor.PID_current_q.limit = motor.current_limit;
// comment out if not needed
motor.useMonitoring(Serial);
// initialize motor
if(!motor.init()){
Serial.println("Motor init failed!");
return;
}
// align sensor and start FOC
if(!motor.initFOC()){
Serial.println("FOC init failed!");
return;
}
// add target command M
command.add('M', doMotor, "Motor");
command.add('CS', Cali_Start, "Calibration_Start");
command.add('CE', Cali_End, "Calibration_End");
motor.useMonitoring(Serial);
motor.monitor_downsample = 1000; // disable monitor at first - optional
Serial.println(F("Motor ready."));
Serial.println(F("Set the target angle using serial terminal:"));
_delay(1000);
}
void loop() {
// main FOC algorithm function
// the faster you run this function the better
// Arduino UNO loop ~1kHz
// Bluepill loop ~10kHz
motor.loopFOC();
// Motion control function
// velocity, position or voltage (defined in motor.controller)
// this function can be run at much lower frequency than loopFOC() function
// You can also use motor.move() and set the motor.target in the code
motor.move();
// user communication
command.run();
//motor.monitor();
}