Controller Implementation¶

The implementation of the controller system was done using Arduino IDE. The functionality was separated into files within the Arduino IDE project. ZumoIMU and ZumoEncoders files where already explained in the sections Inertial Management Unit and Encoders.

SegwayLQR¶

SegwayLQR API¶

Global Constants¶

const uint8_t samplingPeriodMS = 20¶: Sampling Period in ms

const float samplingPeriod = samplingPeriodMS / 1000.0;: Sampling Period in s

const float samplingFrequency = 1 / samplingPeriod¶: Sampling frequency

const uint8_t statesNumber = 4¶: Number states

Global Variables¶

float angularPositionLP = 0¶: Low pass filter angular Position

float angularPosition = 0¶: Zumo’s angular position

float correctedAngularPosition = 0¶: Corrected angular position

float angularSpeed = 0¶: Zumo’s angular speed

float motorAngularPosition = 0¶: Motor’s angular position

float motorAngularSpeed = 0¶: Motor’s angular speed

int32_t speed¶: PWM signal applied to the motor’s driver 400 is 100% cycle and -400 is 100% but inverse direction

Zumo32U4ButtonA buttonA¶: A button of the zumo board

Zumo32U4Motors motors¶: Zumo robot’s motors

Functions¶

class SegwayLQR¶

void setup()¶: Segway’s setup function.

void loop()¶: Segway’s loop function.

void setActuators()¶: Set the values to the actuators.

SegwayLQR Details¶

src/SegwayLQR/SegwayLQR.ino features the main loop and setup functions of the Arduino project. Listing 16 shows the implementation of the setup function. Firstly, it setups the IMU by calling ZumoIMU::setupIMU(). Then, calibrates the IMU’s gyro by calling ZumoIMU::calibrateGyro(). After calibrating it starts a loop of sampling the gyro as frequently as possible and the accelerometer every samplingPeriod. When the buttonA is pressed the loop is exited and before starts executing SegwayLQR::loop() the encoders counters are cleared by calling ZumoEncoders::clearEncoders().

Listing 16 Setup function¶

/**
 * Setup Function
 */
void setup() {
  Wire.begin();

  Serial.begin(115200);

  // Setup the IMU
  setupIMU();

  // Calibrate the IMU (obtain the offset)
  calibrateGyro();

  // Display the angle until the user presses A.
  while (!buttonA.getSingleDebouncedRelease()) {
    // Update the angle using the gyro as often as possible.
    sampleGyro();

    // Sample accelerometer every sampling period
    static uint8_t lastCorrectionTime = 0;
    uint8_t m = millis();
    if ((uint8_t)(m - lastCorrectionTime) >= samplingPeriodMS)
    {
      lastCorrectionTime = m;
      sampleAccelerometer();
    }
  }
  delay(500);
  clearEncoders();
}

Listing 17 shows the loop function’s code. Basically does the same as in the loop in SegwayLQR::setup() but every samplingPeriod it;

ZumoIMU::sampleAccelerometer() to obtain the corrected estimation of the Zumo’s angle and angular speed, as explained in Inertial Management Unit.
ZumoEncoders::sampleEncoders() to obtain encoders position and speed, as explained in Encoders.
SegwayLQR::setActuators() calculates the new speed to be set based on the current state variables’ state and the LQR designed control law.

Listing 17 Loop function¶

/**
 * Main loop Function
 */
void loop() {
  // Update the angle using the gyro as often as possible.
  sampleGyro();

  // Every 20 ms (50 Hz), correct the angle using the
  // accelerometer, print it, and set the motor speeds.
  static byte lastCorrectionTime = 0;
  byte m = millis();
  if ((byte)(m - lastCorrectionTime) >= 20)
  {
    lastCorrectionTime = m;
    sampleAccelerometer();
    sampleEncoders();
    setActuators();
  }
}

Listing 18 shows the SegwayLQR::setActuators() function’s code. As a security measure when the angle is greater that \(45^\circ\) the speed is set to zero. Furthermore, the angle is corrected by the deviation of the COM from the actual horizontal center of the Zumo32u4. Finally the LQR::lqr() is called to apply the control law and generate the input of the system.

Listing 18 Set actuators function¶

/**
 * Control the actuators
 */
void setActuators() {
  const float targetAngle = 1.45;

  if (abs(angularPosition) > 45) {
    // If the robot is tilted more than 45 degrees, it is
    // probably going to fall over.  Stop the motors to prevent
    // it from running away.
    speed = 0;
  } else {
    correctedAngularPosition = angularPosition - targetAngle;
    lqr();
    speed = constrain(speed, -400, 400);
  }

  motors.setSpeeds(speed, speed);
}

LQR¶

LQR API¶

class LQR¶

const float K[statesNumber] = {0.55192, 8.9867, 0.194, 0.39237}¶: Control Law

const float scaleConst = 2.5¶: Correction factor gain factor

void lqr()¶: Apply LQR control law

LQR Details¶

Listing 19 shows how the LQR::lqr() is implemented.

Listing 19 Apply LQR designed control law¶

/**
 * LQR control law
 */
void lqr() {
  speed = 0;

  speed -= motorAngularPosition * K[0];
  speed -= correctedAngularPosition * K[1];
  speed -= motorAngularSpeed * K[2];
  speed -= angularSpeed * K[3];

  speed = speed*scaleConst;

}

Note

The K values are multiplied by \(-1\) in according to Fig. 4.
An additional scale factor, \(scaleConst = 2.5\), is introduce to compensate;
- Possible deviation of the actual Stall Torque with load.
- Bad estimation of the \(\beta_m\) and \(\beta_\gamma\) values.