Been playing with it today. Ran the code, the reaction wheel jitters back and forth. sometimes it damps out and stops, other times I can add a bit of friction with my finger and stop it. I never tries to spin in such a way that might result in swinging the pendulum. I add the code and installed SimpleFOCStudio and below is a trace it made.
While the motor was stationary and I was reading…maybe an hour? I noticed what looked like a motor mounting screw might have backed out but no! The motor had gotten quite HOT and was melting the 3D printer material. I don’t think I “smoked” anything but wow.
attached is my current code, most from your examples and a photo of the Studio output. Since I get stable and predictable readings from both encoders - at this point I don’t think they are causing problems. I think it is a matter of tuning but I have no idea where or how to start. I see posts that suggest PID adjustments but is there no logical approach to this? Simply changing numbers at random does not seem productive.
I feel like I need to reload the position_motion_control script and do any tuning there. Seems that must work correctly before the pendulum program could.
Here is my code, it is running on an SP32 WROOM-32 Wemos D1 LOLIN32 WIFI+BT 2.4GHz Dual Mode Developer Board:
#include <SimpleFOC.h>
#define sign(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C pendulum = MagneticSensorI2C(AS5600_I2C);
// BLDC motor init
BLDCMotor motor = BLDCMotor(11,10.9,42);
// define BLDC driver
BLDCDriver3PWM driver = BLDCDriver3PWM(16,17,18,19);
// include commander interface
Commander command = Commander(Serial);
void doMotor(char* cmd) { command.motor(&motor, cmd); }
void setup() {
// initialize motor encoder hardware
sensor.init();
motor.linkSensor(&sensor);
// driver config
driver.voltage_power_supply = 14.8;
driver.init();
// init the pendulum encoder
Wire1.begin(33, 32, (uint32_t)400000); //Keshka put sda on pin 33 and scl on 32 for second sensor
pendulum.init(&Wire1);
//PciManager.registerListener(&listenerPA);
//PciManager.registerListener(&listenerPB);
// set control loop type to be used
motor.torque_controller = TorqueControlType::voltage;
motor.controller = MotionControlType::torque;
// link the motor to the encoder
motor.linkSensor(&sensor);
// link the motor to the driver
motor.linkDriver(&driver);
// initialize motor
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 14.8;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
// choose FOC modulation (optional)
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// set motion control loop to be used
motor.controller = MotionControlType::angle;
// contoller configuration
// default parameters in defaults.h
// velocity PI controller parameters
motor.PID_velocity.P = 0.2f;
motor.PID_velocity.I = 20;
motor.PID_velocity.D = 0;
// maximal voltage to be set to the motor
motor.voltage_limit = 6;
// velocity low pass filtering time constant
// the lower the less filtered
motor.LPF_velocity.Tf = 0.01f;
// angle P controller
motor.P_angle.P = 20;
// maximal velocity of the position control
motor.velocity_limit = 5; //was 20
// use monitoring with serial
Serial.begin(115200);
// comment out if not needed
motor.useMonitoring(Serial);
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();
// add the motor to the commander interface
// The letter (here 'M') you will provide to the SimpleFOCStudio
command.add('M',doMotor,"motor");
// tell the motor to use the monitoring
motor.useMonitoring(Serial);
motor.monitor_downsample = 0; // disable monitor at first - optional
}
// loop down-sampling counter
long loop_count = 0;
void loop() {
// ~1ms
motor.loopFOC();
// pendulum sensor read
pendulum.update();
// control loop each ~25ms
if(loop_count++ > 25){
// calculate the pendulum angle
float pendulum_angle = constrainAngle(pendulum.getAngle() + _PI);
float target_voltage;
if( abs(pendulum_angle) < 0.5 ) // if angle small enough stabilize
target_voltage = controllerLQR(pendulum_angle, pendulum.getVelocity(), motor.shaft_velocity);
else // else do swing-up
// sets 40% of the maximal voltage to the motor in order to swing up
target_voltage = -_sign(pendulum.getVelocity())*driver.voltage_power_supply*0.4;
// set the target voltage to the motor
motor.move(target_voltage);
// restart the counter
loop_count=0;
}
// real-time monitoring calls
motor.monitor();
// real-time commander calls
command.run();
}
// function constraining the angle in between -pi and pi, in degrees -180 and 180
float constrainAngle(float x){
x = fmod(x + _PI, _2PI);
if (x < 0)
x += _2PI;
return x - _PI;
}
// LQR stabilization controller functions
// calculating the voltage that needs to be set to the motor in order to stabilize the pendulum
float controllerLQR(float p_angle, float p_vel, float m_vel){
// if angle controllable
// calculate the control law
// LQR controller u = k*x
// - k = [40, 7, 0.3]
// - x = [pendulum angle, pendulum velocity, motor velocity]'
float u = 40*p_angle + 7*p_vel + 0.3*m_vel;
// limit the voltage set to the motor
if(abs(u) > driver.voltage_power_supply*0.7) u = sign(u)*driver.voltage_power_supply*0.7;
return u;
}
Here is the trace: