Reaction wheel example theory and practice

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;
}```
1 Like

OK here is the issue, I think because I am using a stronger motor.
When the pendulum swings over the balance point, I have the “weights” adjusted to what seems a reasonable amount of force to halt the pendulum. If the pendulum is not traveling at a high rate it will receive a pulse, reversing direction as it should.

Once direction has been reversed, Some times it will over compensate and pulse back the opposite direction, This starts a rattle or oscillation over the balance point and it will stay there for a second or so but once the oscillations damp out the pendulum falls.

If I hold the pendulum at the balance point where the motor is near zero movement and release it, it will fall because not enough correction is applied.

This seems to be as close as I can get it. It seems, if I increase any of the three weights, the oscillations around the balance point are more severe. If I reduce them, it will not stop and simply passes the balance point.

I have found, increasing the weights results in the oscillations that steadily increase until it is thrown from the balance point, at lower values, the oscillations damp out and then the pendulum falls

Even with increased weights, the pendulum will still fall if released at the balance point.

What should I try next?

Well after over a week of pounding sand. I went in search of more theory. Randomly putting values in for the three “weights” in the controllerLQR function was getting me nowhere.

I found a video series by Steve Brunton on YouTube called Control Bootcamp. Steve speaks well and I was able to understand 90% given that my math has been out of use for 20 years. His videos prompted an internet search for “writing code for an inverted pendulum” which lead me to…guess who! Antun’s GitHub page and his “inverted_inertia_pendulum” project three years ago.

After watching Steve’s presentation I felt in order to make further progress I would need to use MatLab and enter parameters specific to my build and recalculate the needed values for the controllerLQR function.

I have heard of MatLab but never used it. I was able to download and install it thanks to my wife’s student status. Antun was so kind as to include his MatLab workup on the inverted pendulum (thank you!). I managed to get it loaded into Matlab and run the simulation, now I must figure out the parameter values pertinent to my system. Some of the values at a glance do not seem reasonable and I will need some help understanding. An example is:
l_cm = 0.110; % center of mass distance (Solidworks)
Isn’t that approximately the length of the pendulum arm? I is most defiantly is longer that one millimeter. (I don’t have access to Solidworks).