Reaction wheel inverted pendulum with two AS5600

developement
1

Hi! I´m working in my own reaction wheel with two AS5600 in analogue mode, i just modified the original code of the project, when I start the project the pendulum just get energized and “tremble” but any move. Some recommendantion or help? this is the code:

#include <SimpleFOC.h>

// BLDC motor initialization
BLDCMotor motor = BLDCMotor(11);
// BLDC driver initialization
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);

// Motor sensor initialization (AS5600 via Analog)
MagneticSensorAnalog motorSensor = MagneticSensorAnalog(A0, 14, 1020);

// Pendulum sensor initialization (AS5600 via Analog)
MagneticSensorAnalog pendulumSensor = MagneticSensorAnalog(A1, 14, 1020);

void setup() {
  // Initialize motor sensor hardware
  motorSensor.init();

  // Initialize pendulum sensor hardware
  pendulumSensor.init();

  // Driver configuration
  driver.voltage_power_supply = 12;
  driver.init();

  // Motor configuration
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::torque;

  // Link the motor to the sensor
  motor.linkSensor(&motorSensor);

  // Link the motor to the driver
  motor.linkDriver(&driver);

  // Initialize motor
  motor.init();

  // Align sensor and start FOC
  motor.initFOC();
}

// Loop down-sampling counter
long loop_count = 0;

void loop() {
  // ~1ms
  motor.loopFOC();

  // Pendulum sensor read
  pendulumSensor.update();

  // Control loop each ~25ms
  if (loop_count++ > 25) {
    // Calculate the pendulum angle
    float pendulum_angle = constrainAngle(pendulumSensor.getAngle() + _PI);

    float target_voltage;
    if (abs(pendulum_angle) < 0.5) { // If angle small enough stabilize
      target_voltage = controllerLQR(pendulum_angle, pendulumSensor.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(pendulumSensor.getVelocity()) * driver.voltage_power_supply * 0.4;
    }

    // Set the target voltage to the motor
    motor.move(target_voltage);

    // Restart the counter
    loop_count = 0;
  }
}

// Function constraining the angle between -pi and pi
float constrainAngle(float x) {
  x = fmod(x + _PI, _2PI);
  if (x < 0)
    x += _2PI;
  return x - _PI;
}

// LQR stabilization controller functions
// Calculating the voltage 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;
}

// Function to determine the sign of a number
float sign(float value) {
  if (value > 0) return 1.0;
  if (value < 0) return -1.0;
  return 0.0;
}
2

Have you tested whether the motor is working normally in closed loop mode?