mbedによるロボット制御

sudo apt install python3-venv

curl -fsSL https://raw.githubusercontent.com/platformio/platformio-core/develop/platformio/assets/system/99-platformio-udev.rules | sudo tee /etc/udev/rules.d/99-platformio-udev.rules

#include <mbed.h>

DigitalIn button{BUTTON1};
DigitalOut led{LED1};

int main() {
  // put your setup code here, to run once:
  printf("setup\n");

  while(1) {
    // put your main code here, to run repeatedly:
    if(!button) {
      led = !led;
      ThisThread::sleep_for(100ms);
    }
  }
}

#include <mbed.h>

DigitalIn button{BUTTON1};
DigitalOut led{LED1};

bool pre_button = false;

int main() {
  // put your setup code here, to run once:
  printf("setup\n");

  while(1) {
    // put your main code here, to run repeatedly:
    if(!pre_button && button) {
      float out = led.read() + 0.1;
      if(out > 1) {
        out = 0;
      }
      led = out;
      ThisThread::sleep_for(100ms);
    }
  }
}

#include <mbed.h>

// CAN1を使う場合
CAN can{PA_11, PA_12, (int)1e6};
// (通信速度を1MHzに設定)

// CAN2を使う場合はこっち
// CAN can{PB_12, PB_13, (int)1e6};

int main() {
  // put your setup code here, to run once:
  printf("setup\n");

  while(1) {
    // put your main code here, to run repeatedly:
    auto now = HighResClock::now();
    static auto pre = now;

    // CANメッセージの受信
    CANMessage msg;
    if(can.read(msg)) {
      // IDの確認
      printf("%d\n", msg.id);
      // データは`msg.data[0]〜[7]`に入っている。
      printf("%d\n", msg.data[0]);
    }

    // 10msごとに実行
    if(now - pre > 10ms) {

      // 送信データの作成
      uint32_t id = 0x200;
      uint8_t data[8] = {};
      // CANMessage構造体の作成
      CANMessage msg{id, data, sizeof(data)};
      // メッセージの送信
      can.write(msg);

      pre = now;
    }
  }
}

#ifndef RCT_C620_HPP
#define RCT_C620_HPP
/// @file
/// @brief Provides the C620 class for controlling the motor driver for M3508.
/// @copyright Copyright (c) 2024 Yoshikawa Teru
/// @license This project is released under the MIT License.

#include <mbed.h>

#include <array>

/// @brief The C620 motor driver class for M3508.
struct C620 {
  static constexpr int max = 16384;

  void set_current(float current) {
    raw_current_ = max / 20.0 * current;
  }
  void set_raw_current(int16_t raw_current) {
    raw_current_ = raw_current;
  }
  uint16_t get_angle() const {
    return rx_.angle;
  }
  int16_t get_rpm() const {
    return rx_.rpm;
  }
  float get_actual_current() const {
    return 20.0 / max * rx_.actual_current;
  }
  int16_t get_actual_raw_current() const {
    return rx_.actual_current;
  }
  uint8_t get_temp() const {
    return rx_.temp;
  }
  int16_t get_raw_current() const {
    return raw_current_;
  }
  void parse_packet(const uint8_t data[8]) {
    rx_.angle = uint16_t(data[0] << 8 | data[1]);
    rx_.rpm = int16_t(data[2] << 8 | data[3]);
    rx_.ampere = int16_t(data[4] << 8 | data[5]);
    rx_.temp = data[6];
  }

 private:
  /// @brief The packet structure of the C620 motor driver.
  struct C620Packet {
    uint16_t angle;
    int16_t rpm;
    int16_t actual_current;
    uint8_t temp;
  } rx_ = {};
  int16_t raw_current_ = {};
};

/// @brief The C620 motor driver array for M3508.
struct C620Array {
  void parse_packet(const CANMessage& msg) {
    if(msg.format == CANStandard && msg.type == CANData && msg.len == 8 && 0x200 <= msg.id && msg.id <= 0x208) {
      arr_[msg.id - 0x201u].parse(msg.data);
    }
  }
  auto to_msgs() -> std::array<CANMessage, 2> const {
    uint8_t buf[16];
    for(int i = 0; i < 8; i++) {
      buf[i] = arr_[i].get_raw_current() >> 8;
      buf[i + 1] = arr_[i].get_raw_current() & 0xff;
    }
    return {CANMessage{0x200, buf}, CANMessage{0x1FF, buf + 8}};
  }
  auto& operator[](int index) {
    return arr_[index];
  }
  auto begin() {
    return std::begin(arr_);
  }
  auto end() {
    return std::end(arr_);
  }

 private:
  C620 arr_[8] = {};
};

#endif  /// RCT_C620_HPP

#ifndef VEL_PID_HPP
#define VEL_PID_HPP
/// @file
/// @brief Provides the VelPid class for velocity PID control.
/// @copyright Copyright (c) 2024 Yoshikawa Teru
/// @license This project is released under the MIT License, see [LICENSE](https://github.com/teruyamato0731/Chassis/blob/main/LICENSE).

#include <algorithm>
#include <chrono>
#include <cmath>

/// @brief Gains for a PID controller.
struct PidGain {
  float kp;
  float ki;
  float kd;
};

/// @brief Gains and limits for a PID controller.
struct PidParam {
  PidGain gain;
  float min = NAN;
  float max = NAN;
};

/// @brief Velocity Form PID controller.
template<class T = float>
class VelPid {
 public:
  /// @brief Constructor with the specified gains and limits.
  /// @param param The parameters for the PID controller.
  VelPid(const PidParam& param) : param_{param} {}

  /// @brief Calculates the output of the PID controller.
  T calc(const T target, const T actual, const std::chrono::duration<float>& dt) {
    return calc(target - actual, dt);
  }

  /// @brief Calculates the output of the PID controller.
  T calc(const T error, const std::chrono::duration<float>& dt) {
    using namespace std;
    const auto sec = dt.count();
    const auto prop = (error - pre_error_) / sec;
    const auto deriv = isnan(pre_prop_) ? 0 : (prop - pre_prop_) / sec;
    pre_error_ = error;
    pre_prop_ = prop;
    low_pass_deriv_ += deriv / 8;
    const auto du = param_.gain.kp * prop + param_.gain.ki * error + param_.gain.kd * low_pass_deriv_;
    output_ = clamp(output_ + du * sec, param_.min, param_.max);
    return output_;
  }

  /// @brief Resets the PID controller.
  void reset() {
    pre_error_ = 0.0;
    pre_prop_ = NAN;
    low_pass_deriv_ = 0.0;
    output_ = 0.0;
  }

  /// @brief Sets the parameters for the PID controller.
  void set_param(const PidParam& param) {
    param_ = param;
    reset();
  }

  /// @brief Sets the gains for the PID controller.
  void set_gain(const PidGain& gain) {
    param_.gain = gain;
    reset();
  }

  /// @brief Sets the limits for the PID controller.
  void set_limit(const float min, const float max) {
    param_.min = min;
    param_.max = max;
  }

 private:
  PidParam param_;
  T pre_error_ = 0.0;
  T pre_prop_ = NAN;
  T low_pass_deriv_ = 0.0;
  T output_ = 0.0;
};

#endif  // VEL_PID_HPP

#include <mbed.h>

InterruptIn a_phase{D4};
InterruptIn b_phase{D5};

volatile int32_t omega = 0;
int64_t pos = 0;
void increment() {
    omega = omega + 1;
}

int main() {
    a_phase.rise(increment);
    a_phase.fall(increment);
    b_phase.rise(increment);
    b_phase.fall(increment);
    while(1) {
        auto tmp = omega;
        omega = 0;
        pos + = tmp;
        printf("pos:%d vel:%d\n", pos, tmp);
        ThisThread::sleep_for(1s);
    }
}

#include <mbed.h>

uint8_t status = 0;
int64_t count = 0;

int get_increment(uint8_t status) {
    switch(status) {
        case 1: case 7: case 8: case 14:
            return 1;
        case 2: case 4: case 11: case 13:
            return -1;
        default:
            return 0;
    }
}

int main() {
    while(1) {
        status = a_phase << 3 | b_phase << 2 | status >> 2;
        count += get_increment(status);
        printf("pos:%d", pos);
    }
}

volatile int64_t counter = 0;
void a_rise() {
    if(b_phase) {
        counter += 1;
    } else {
        counter -= 1;
    }
}
void a_fall() {
    if(b_phase) {
        counter -= 1;
    } else {
        counter += 1;
    }
}

int main() {
    a_phase.rise(a_rise);
    a_phase.fall(a_fall);
    while(1) {
        printf("%d\n", counter);
    }
}