After finally getting my motor tuned and running predictably I went to work on the Reaction wheel example (Antun!). At first, with base code the wheel would oscillate back at forth about 1 rad at a rate of 3 oscillations per second. Seemed the loop timing was too fast with the ESP32. Changing the loop time to 300 helped but it still seemed to reverse direction at the wrong moment so I added a statement forcing the swing loop to only change the target_voltage if the velocity of the pendulum was below .2 (ie, at the apogee of a swing). That helped as well but I had to speed up the loop to 20. It still takes several tries before it has enough inertia to reach the top. Once at the top, the hold commands are too strong and it shakes one or two times and falls. My plan was to adjust the weights given to the three parameters in the controllerLQR call:
40pendulum_angle + 7pendulum.getVelocity() + 0.3*motor.shaftVelocity();
To get an idea of the effect this would produce I went back to the “full control” example, set MotionControlType::torque and TorqueControlType::voltage.
I notice my motor setup results in the reaction wheel spinning at about 38 rad/s with any “Target_voltage” over 8. Which is what the set voltage call produces most of the time. My next plan is to adjust the “weights” and see what results I get.
Keshka
ps, here is my current script:
#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);
// define BLDC driver
BLDCDriver3PWM driver = BLDCDriver3PWM(16,17,18,19);
//Commander command = Commander(Serial);
//void onMotor(char* cmd){ command.motor(&motor, cmd); }
// include commander interface
//Commander command = Commander(Serial);
//void doMotor(char* cmd) { command.motor(&motor, cmd); }
void setup() {
sensor.init();
motor.linkSensor(&sensor);
motor.linkDriver(&driver);
driver.voltage_power_supply = 16;
driver.init();
Wire1.begin(33, 32, (uint32_t)400000); //Keshka put sda on pin 33 and scl on 32 for second sensor
pendulum.init(&Wire1);
motor.controller = MotionControlType::torque;
motor.torque_controller = TorqueControlType::voltage;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.PID_velocity.P = 3.0f;
motor.PID_velocity.I = 40;
motor.PID_velocity.D = 0;
motor.voltage_limit = 6;
motor.LPF_velocity.Tf = 0.01f;
motor.P_angle.P = 20; //.8
motor.velocity_limit = 50; //20
motor.init();
motor.initFOC();
Serial.begin(115200);
//motor.useMonitoring(Serial);
//command.add('A', onMotor, "motor");
}
// loop down-sampling counter
long loop_count = 0;
void loop() {
motor.loopFOC();
pendulum.update();
float target_voltage;
if(loop_count++ > 20){
float pendulum_angle = constrainAngle(pendulum.getAngle() + _PI);
if( abs(pendulum_angle) < 0.5 ) // if angle small enough stabilize
{
target_voltage = controllerLQR(pendulum_angle, pendulum.getVelocity(), motor.shaft_velocity);
// Serial.print("hold: ");
motor.move(target_voltage); // set the target voltage to the motor
}
else // else do swing-up
{
if(abs(pendulum.getVelocity()) < .2)
{
target_voltage = -sign(pendulum.getVelocity())*driver.voltage_power_supply*0.5; // sets 40% voltage to swing up
// Serial.print("swing: ");
motor.move(target_voltage); // set the target voltage to the motor
}
else
{
// Serial.print("-----: ");
}
}
/*
Serial.print(pendulum_angle);
Serial.print(" : ");
Serial.print(target_voltage);
Serial.print(" : ");
Serial.print(abs(pendulum.getVelocity()));
Serial.print(" : ");
Serial.println(-sign(pendulum.getVelocity()));
*/
loop_count=0;
//motor.monitor();
//command.run();
}
}
float constrainAngle(float x){ // limit angle +/- 180
x = fmod(x + _PI, _2PI);
if (x < 0)
x += _2PI;
return x - _PI;
}
float controllerLQR(float p_angle, float p_vel, float m_vel){ // calculating voltage needed for the motor to stabilize the pendulum
float u = 40*p_angle + 7*p_vel + 0.3*m_vel;
if(abs(u) > driver.voltage_power_supply*0.7) u = sign(u)*driver.voltage_power_supply*0.7;
return u;
}```