c6x0.hpp

[詳解]

#pragma once
 
#include <array>
#include <cstddef>
#include <cstdint>
 
#include "tutrcos/peripheral/can_base.hpp"
#include "tutrcos/utility.hpp"
 
#include "encoder_base.hpp"
 
namespace tutrcos {
namespace module {
 
class C6x0 : public EncoderBase {
public:
  enum class Type {
    C610,
    C620,
  };
 
  enum class ID {
    _1,
    _2,
    _3,
    _4,
    _5,
    _6,
    _7,
    _8,
  };
 
  class Manager {
  public:
    Manager(peripheral::CANBase &can) : can_{can} {}
 
    bool update() {
      peripheral::CANBase::Message msg;
      while (can_.receive(msg, 0)) {
        if (msg.id >= 0x201 && msg.id < 0x201 + 8) {
          C6x0 *motor = motors_[msg.id - 0x201];
          if (motor) {
            int16_t count =
                static_cast<int16_t>(msg.data[0] << 8 | msg.data[1]);
            int16_t delta = count - motor->prev_count_;
            if (delta > 4096) {
              delta -= 8192;
            } else if (delta < -4096) {
              delta += 8192;
            }
            motor->set_count(motor->get_count() + delta);
            motor->prev_count_ = count;
 
            motor->rpm_ = static_cast<int16_t>(msg.data[2] << 8 | msg.data[3]);
            motor->current_ =
                static_cast<int16_t>(msg.data[4] << 8 | msg.data[5]);
          }
        }
      }
 
      msg.data.fill(0);
      msg.id_type = peripheral::CANBase::IDType::STANDARD;
      msg.id = 0x200;
      msg.dlc = 8;
      for (size_t i = 0; i < 4; ++i) {
        if (motors_[i]) {
          msg.data[i * 2] = motors_[i]->target_current_ >> 8;
          msg.data[i * 2 + 1] = motors_[i]->target_current_;
        }
      }
      if (!can_.transmit(msg, 0)) {
        return false;
      }
 
      msg.data.fill(0);
      msg.id = 0x1FF;
      for (size_t i = 0; i < 4; ++i) {
        if (motors_[i + 4]) {
          msg.data[i * 2] = motors_[i + 4]->target_current_ >> 8;
          msg.data[i * 2 + 1] = motors_[i + 4]->target_current_;
        }
      }
      return can_.transmit(msg, 0);
    }
 
  private:
    peripheral::CANBase &can_;
    std::array<C6x0 *, 8> motors_{};
 
    friend C6x0;
  };
 
  C6x0(Manager &manager, Type type, ID id)
      : EncoderBase{8192}, manager_{manager}, type_{type}, id_{id} {
    TUTRCOS_VERIFY(manager_.motors_[utility::to_underlying(id_)] == nullptr);
    manager_.motors_[utility::to_underlying(id_)] = this;
  }
 
  ~C6x0() { manager_.motors_[utility::to_underlying(id_)] = nullptr; }
 
  float get_rps() override { return get_rpm() / 60; }
 
  float get_rpm() override { return rpm_; }
 
  int16_t get_current() {
    switch (type_) {
    case Type::C610:
      return current_;
    case Type::C620:
      return current_ * 25000 / 16384;
    }
  }
 
  void set_current(int16_t current) {
    switch (type_) {
    case Type::C610:
      target_current_ = current;
      break;
    case Type::C620:
      target_current_ = current * 16384 / 25000;
      break;
    }
  }
 
private:
  Manager &manager_;
  Type type_;
  ID id_;
 
  int16_t prev_count_ = 0;
  int16_t rpm_ = 0;
  int16_t current_ = 0;
  int16_t target_current_ = 0;
};
 
} // namespace module
} // namespace tutrcos