Motor2040 Running PID loop while sending data at high frequency on I2C or Serial

Support
1

I’m trying to control the Motor2040 from an external master controller, which sends velocity setpoints and also reads motor positions and currents via I2C at most every 200ms, but preferably lower. Whenever I try to request data from the Motor2040 at that frequency, the motor jerks around and no longer spins smoothly.

I’ve tried a lot of different things, switching from serial to I2C, running the PID loop on core 1. It seems the actual act of trying to request data is causing the problem, because if I run the program without any I2C requests (as in just put the data into the write buffer but never have the master controller read it) it runs fine. Sending data is OK too, I can send the velocity setpoints once a millisecond and it doesn’t bat an eye!

I’ve posted my code below. Thanks!

#include <cstdio>
#include <array>
#include <algorithm>
#include <cmath>
#include <limits>
#include <cstring>

// #include "hardware/uart.h"
// #include "hardware/irq.h"
// #include <vector>
#include "hardware/timer.h"

#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "pico/util/queue.h"
#include "pico/i2c_slave.h"

#include "hardware/i2c.h"

#include "motor2040.hpp"
#include "analogmux.hpp"
#include "analog.hpp"
#include "button.hpp"
#include "pid.hpp"

using namespace motor;
using namespace encoder;
using namespace std;

const array<pin_pair, 4> motor_pins = {motor2040::MOTOR_A, motor2040::MOTOR_B, motor2040::MOTOR_C, motor2040::MOTOR_D};
const array<pin_pair, 4> encoder_pins = {motor2040::ENCODER_A, motor2040::ENCODER_B, motor2040::ENCODER_C, motor2040::ENCODER_D};

enum homing_flag
{
  NOT_HOMING,
  HOMING,
  FINISHED_HOMING
};

class MotorController
{
public:
  MotorController(uint top_motor, uint left_motor, uint right_motor, float kp, float ki, float kd, float gear_ratio, float update_rate)
  {

    array<uint, 3> index = {top_motor, left_motor, right_motor};

    update_rate_ = update_rate;

    cur_adc_ = new Analog(motor2040::SHARED_ADC, motor2040::CURRENT_GAIN,
                          motor2040::SHUNT_RESISTOR, motor2040::CURRENT_OFFSET);
    mux_ = new AnalogMux(motor2040::ADC_ADDR_0, motor2040::ADC_ADDR_1, motor2040::ADC_ADDR_2,
                         PIN_UNUSED, motor2040::SHARED_ADC);

    for (int i = 0; i < 3; i++)
    {
      motors_[i] = new Motor(motor_pins[index[i]], NORMAL_DIR, 1.0f, 0.0f, 0.05f, 25000.0f, SLOW_DECAY, false);
      motors_[i]->init();
      encoders_[i] = new Encoder(pio0, i, encoder_pins[index[i]], PIN_UNUSED, NORMAL_DIR, MMME_CPR * gear_ratio, true);
      encoders_[i]->init();
      pids_[i] = PID(kp, ki, kd, update_rate);
      motors_[i]->enable();
    }

    sleep_ms(2000);
  }
  void updatePID(array<float, 3> velocities)
  {
    Encoder::Capture captures[3];
    for (int i = 0; i < 3; i++)
    {
      captures[i] = encoders_[i]->capture();
      velocities_[i] = captures[i].radians_per_second();
    }
    if (homing_ == NOT_HOMING)
    {
      for (int i = 0; i < 3; i++)
      {
        if ((min_pos_ < captures[i].radians() || signbit(velocities[i]) == false) && (captures[i].radians() < max_pos_ || signbit(velocities[i]) == true))
        {
          pids_[i].setpoint = CLAMP(velocities[i], -max_vel_, max_vel_);
        }
        else
        {
          pids_[i].setpoint = 0.0f;
        }
      }
      for (int i = 0; i < 3; i++)
      {
        // Encoder::Capture capture = encoders_[i]->capture();
        float accel = pids_[i].calculate(captures[i].revolutions_per_second());
        motors_[i]->speed(motors_[i]->speed() + (accel * update_rate_));
      }
    }
    else if (homing_ == HOMING)
    {
      array<bool, 3> homed = {false, false, false};
      for (int i = 0; i < 3; i++)
      {
        if (abs(captures[i].radians()) > 0.0001f)
        {
          motors_[i]->stop();
          homed[i] = true;
        }
      }
      if (all_of(homed.begin(), homed.end(), [](bool v)
                 { return v; }))
      {
        homing_ = FINISHED_HOMING;
      }
    }
  }
  array<float, 3> &get_positions(array<float, 3> &positions)
  {
    for (int i = 0; i < 3; i++)
    {
      Encoder::Capture capture = encoders_[i]->capture();
      positions[i] = capture.radians();
    }
    return positions;
  }
  array<float, 3> &get_velocities(array<float, 3> &velocities)
  {
    // for (int i = 0; i < 3; i++)
    // {
    //   Encoder::Capture capture = encoders_[i]->capture();
    //   velocities[i] = capture.radians_per_second();
    // }
    velocities = velocities_;
    return velocities;
  }
  array<float, 3> &get_currents(array<float, 3> &currents)
  {
    for (int i = 0; i < 3; i++)
    {
      mux_->select(motor2040::CURRENT_SENSE_A_ADDR + i);
      currents[i] = cur_adc_->read_current();
    }
    return currents;
  }
  void stop()
  {
    for (int i = 0; i < 3; i++)
    {
      motors_[i]->stop();
    }
    // printf("Motors Stopped.\n");
  }
  void emergency_stop()
  {
    for (int i = 0; i < 3; i++)
    {
      motors_[i]->brake();
    }
    // printf("Emergency Stop.\n");
  }
  void disable()
  {
    for (int i = 0; i < 3; i++)
    {
      motors_[i]->disable();
    }
  }
  void home()
  {
    homing_ = HOMING;
    for (int i = 0; i < 3; i++)
    {
      Encoder::Capture capture = encoders_[i]->capture();
      if (abs(capture.radians()) > 0.0001f)
      {
        motors_[i]->speed(-copysign(1.0f, capture.radians()));
      }
    }

    homing_ = FINISHED_HOMING;
  }
  void set_max_vel(float max_vel)
  {
    max_vel_ = max_vel;
  }
  void set_min_max_pos(float min_pos, float max_pos)
  {
    min_pos_ = min_pos;
    max_pos_ = max_pos;
  }
  void zero()
  {
    for (int i = 0; i < 3; i++)
    {
      encoders_[i]->zero();
    }
  }
  homing_flag get_homing_status()
  {
    if (homing_ == FINISHED_HOMING)
    {
      homing_flag finished = homing_;
      homing_ = NOT_HOMING;
      return finished;
    }
    else
    {
      return homing_;
    }
  }

private:
  array<Motor *, 3> motors_;
  array<Encoder *, 3> encoders_;
  AnalogMux *mux_;
  Analog *cur_adc_;
  array<PID, 3> pids_;
  float update_rate_;
  float max_vel_ = numeric_limits<float>::infinity();
  float min_pos_ = -numeric_limits<float>::infinity();
  float max_pos_ = numeric_limits<float>::infinity();
  homing_flag homing_ = NOT_HOMING;
  array<float, 3> velocities_ = {0.0f, 0.0f, 0.0f};
};

const uint updates = 100;
constexpr float update_rate = 1.0f / (float)updates;

const array<float, 3> zero_setpoint = {0.0f, 0.0f, 0.0f};

static const uint I2C_SLAVE_ADDRESS = 0x7;
static const uint I2C_BAUDRATE = 100000; // 100 kHz

static const uint I2C_SLAVE_SDA_PIN = 20; // 20
static const uint I2C_SLAVE_SCL_PIN = 21; // 21

MotorController controller = MotorController(0, 1, 2, 30.0f, 0.0f, 0.4f, 298.0f, update_rate);
array<float, 3> setpoints = {0.0f, 0.0f, 0.0f};

Button user_sw(motor2040::USER_SW);

// queue_t setpoint_queue;

uint8_t i2c_command;

static void send_float_array_to_bytes(array<float, 3> floats)
{
  uint8_t bytes[12];
  for (int i = 0; i < 3; i++)
  {
    uint8_t px[4];
    memcpy(&px, &floats[i], 4);
    reverse(begin(px), end(px));
    for (int j = 0; j < 4; j++)
    {
      bytes[(i * 4) + j] = px[j];
    }
  }
  i2c_write_raw_blocking(i2c_default, bytes, 12);
}

// static array<float, 3> get_float_array_from_bytes(array<float, 3> &floats)
// {
//   array<uint8_t, 12> bytes;
//   i2c_read_raw_blocking(i2c_default, *bytes, 12);
//   // for(int i = 0; i < 12; i++)
//   // {
//   //   bytes[i] = i2c_read_byte_raw(i2c);
//   // }
//   for (int i = 0; i < 3; i++)
//   {
//     array<uint8_t, 4> slice = {bytes[(i * 4)], bytes[(i * 4) + 1], bytes[(i * 4) + 2], bytes[(i * 4) + 3]};
//     memcpy(&slice, &floats[i], 4);
//   }
//   return floats;
// }

static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
{
  switch (event)
  {
  case I2C_SLAVE_RECEIVE: // master has written some data
  {
    i2c_command = i2c_read_byte_raw(i2c);
    switch (i2c_command)
    {
    case 0x0: // Set Velocities
    {
      array<float, 3> velocities;
      uint8_t bytes[12];
      i2c_read_raw_blocking(i2c_default, bytes, 12);
      // for(int i = 0; i < 12; i++)
      // {
      //   bytes[i] = i2c_read_byte_raw(i2c);
      // }
      for (int i = 0; i < 3; i++)
      {
        uint8_t slice[4] = {bytes[(i * 4) + 3], bytes[(i * 4) + 2], bytes[(i * 4) + 1], bytes[(i * 4)]};
        memcpy(&velocities[i], &slice, 4);
      }
      // queue_add_blocking(&setpoint_queue, &velocities);
      setpoints = velocities;
    }
    break;
    case 0x4: // Emergency Stop
    {
      controller.emergency_stop();
    }
    break;
    case 0x5: // Zero Encoders
    {
      controller.zero();
    }
    break;
    case 0x6: // Set Position Limits
    {
      array<float, 2> limits;
      uint8_t bytes[8];
      i2c_read_raw_blocking(i2c_default, bytes, 8);
      // for(int i = 0; i < 12; i++)
      // {
      //   bytes[i] = i2c_read_byte_raw(i2c);
      // }
      for (int i = 0; i < 2; i++)
      {
        uint8_t slice[4] = {bytes[(i * 4) + 3], bytes[(i * 4) + 2], bytes[(i * 4) + 1], bytes[(i * 4)]};
        memcpy(&limits[i], &slice, 4);
      }
      controller.set_min_max_pos(limits[0], limits[1]);
    }
    break;
    case 0x07: // Set Velocity Limit
    {
      float limit;
      uint8_t bytes[4];
      i2c_read_raw_blocking(i2c_default, bytes, 4);
      // for(int i = 0; i < 12; i++)
      // {
      //   bytes[i] = i2c_read_byte_raw(i2c);
      // }
      reverse(begin(bytes), end(bytes));
      memcpy(&limit, &bytes, 4);
      controller.set_max_vel(limit);
    }
    break;
    case 0x8: // Go Home
    {
      controller.home();
    }
    break;
    default:
      break;
    }
  }
  break;
  case I2C_SLAVE_REQUEST: // master is requesting data
  {
    switch (i2c_command)
    {
    case 0x1: // Read Velocities
    {
      array<float, 3> velocities = {0.0f, 0.0f, 0.0f};
      controller.get_velocities(velocities);
      send_float_array_to_bytes(velocities);
    }
    case 0x2: // Read Positions
    {
      array<float, 3> positions;
      controller.get_positions(positions);
      send_float_array_to_bytes(positions);
    }
    case 0x3: // Read Currents
    {
      array<float, 3> currents;
      controller.get_currents(currents);
      send_float_array_to_bytes(currents);
    }
    case 0x9: // Get Homing Status
    {
      homing_flag status = controller.get_homing_status();
      switch (status)
      {
      case NOT_HOMING:
        i2c_write_byte_raw(i2c_default, 0x0);
        break;
      case HOMING:
        i2c_write_byte_raw(i2c_default, 0x1);
        break;
      case FINISHED_HOMING:
        i2c_write_byte_raw(i2c_default, 0x2);
        break;
      }
    }
    break;
    default:
      break;
    }
  }
  break;
  case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
    break;
  default:
    break;
  }
}

void core1_entry()
{
  multicore_lockout_victim_init();

  gpio_init(I2C_SLAVE_SDA_PIN);
  gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
  gpio_pull_up(I2C_SLAVE_SDA_PIN);

  gpio_init(I2C_SLAVE_SCL_PIN);
  gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);
  gpio_pull_up(I2C_SLAVE_SCL_PIN);

  i2c_init(i2c0, I2C_BAUDRATE);

  i2c_slave_init(i2c_default, I2C_SLAVE_ADDRESS, &i2c_slave_handler);
}

int main()
{
  // set_sys_clock_khz(133000, true);
  //  stdio_init_all();

  sleep_ms(2000);

  // queue_init(&setpoint_queue, sizeof(array<float, 3>), 2);

  multicore_launch_core1(core1_entry);

  // array<float, 3> setpoints = {0.0f, 0.0f, 0.0f};

  while (!user_sw.raw())
  {
    // queue_try_remove(&setpoint_queue, &setpoints);

    multicore_lockout_start_blocking();

    controller.updatePID(setpoints);

    multicore_lockout_end_blocking();

    sleep_ms(update_rate * 1000.0f);
  }

  // printf("%f ", setpoints[0]);
  // printf("%f ", setpoints[1]);
  // printf("%f ", setpoints[2]);
  // printf("\n");

  // controller.updatePID(setpoints);

  // while (!get_absolute_time() >= delayed_by_ms(start_loop, update_rate * 1000.0f))
  // {
  //   tight_loop_contents();
  // }

  // sleep_ms(update_rate * 1000.0f);

  controller.stop();
  controller.disable();

  // delete controller;
}
2

Solved: Changing sleep_ms() to busy_wait_ms() seems to fix the issue. As power consumption is not a limitation this works for me.

I assume sleep_ms() causes both cores to sleep? Is there a better way to fix this issue?