## OpenCR controller

- CPU 為 STM32F746ZGT6，時脈 216Mhz
- 具有 MPU9250 可做為 IMU

## Arduino IDE 中需要安裝的 libraries

這是在底盤中有用到的兩個額外套件，需要自行安裝 library

#### micro-ros

git clone https://github.com/micro-ROS/micro_ros_arduino.git -b humble

#### PID_v1

git clone https://github.com/br3ttb/Arduino-PID-Library.git

## AMR 底盤控制系統

整體完整的系統程式碼在 [Github](https://github.com/ming0071/ROS2-3D-lidar-AMR/tree/main/module/chassic) 上

#### Encoder

這一個函式是主要讀取 encoder 資料的結構，流程為
1. 藉由 A 相的數值變化(2倍脈衝)啟動中斷，記錄下第一個脈衝的時間，並由 B 相的變化來得知正、反轉
2. 當到第 PULSE_THRESHOLD 個脈衝時，由第一個脈衝到現在的時間差計算出直線速度
$$
速度 (Speed) = 方向 \times \frac{2 \pi \times 半徑 (Radius) \times \text{PULSE_THRESHOLD}}{\text{PPR} \times \text{時間間隔 (TimeElapsed)}}
$$
3. 數值歸零

void encoderISR(EncoderData &encoder, int pinA, int pinB)
{
    encoder.state = digitalRead(pinA); // Read the current state of the encoder pin A

    // Detect state change (pulse detected)
    if (encoder.state != encoder.lastState)
    {
        // Update pulse count based on direction determined by pin B
        if (digitalRead(pinB) != encoder.state)
        {
            encoder.value += 1; // Increment pulse count
        }
        else
        {
            encoder.value -= 1; // Decrement pulse count
        }
    }

    // Record start time for speed calculation if a new pulse is detected
    if (encoder.value == 1 || encoder.value == -1)
    {
        encoder.previousMicros = micros(); // Save the time of the first pulse
    }

    // Calculate speed when pulse count reaches the threshold
    if (abs(encoder.value) > PULSE_THRESHOLD)
    {
        // Calculate direction based on the encoder value
        if (encoder.value > 0)
        {
            encoder.direction = encoder.directionFactor; // Set direction based on directionFactor for forward rotation
        }
        else
        {
            encoder.direction = -encoder.directionFactor; // Set direction based on directionFactor for reverse rotation
        }

        encoder.currentMicros = micros();                                                   // Record the current time
        encoder.timeElapsed = (encoder.currentMicros - encoder.previousMicros) / 1000000.0; // Compute time interval in seconds

        // Compute speed and update ROS message
        encoder.speedMsg.data = encoder.direction * (2 * PI * Ridus) * (PULSE_THRESHOLD / PPR) / encoder.timeElapsed;

        // Reset values for the next calculation
        encoder.value = 0;
        encoder.direction = 0;
    }

    // Update last state for the next ISR invocation
    encoder.lastState = encoder.state;
}

#### Motor

馬達中主要的函式是這一段，得知由 `cmd_vel` 發布的直線速度、角速度後，由 Ackermann steering geometry 計算出左、右輪速，再將計算出的 pwm 脈衝作為 PID 的 target 後續計算使用。

void twistToPulseAndServo(double linearVel, double angularVel)
{
  // Ignore invalid commands for Ackermann steering
  if (linearVel == 0)
    return;

  // Kinematic calculation for Ackermann steering
  double vLeft = (1 - (wheelBase * angularVel) / (2 * linearVel)) * linearVel;
  double vRight = (1 + (wheelBase * angularVel) / (2 * linearVel)) * linearVel;

  twistTemp.leftPulsePerInterval = static_cast<int>((vLeft * (encoderResolution / (PI * wheelDiameter))) / pidRate);
  twistTemp.rightPulsePerInterval = static_cast<int>((vRight * (encoderResolution / (PI * wheelDiameter))) / pidRate);

  setTargetTicksPerFrame(twistTemp.leftPulsePerInterval, twistTemp.rightPulsePerInterval);
}

#### PID control

PID 的模組使用了先前安裝的 library，直接呼叫 function 即可使用，error 在 function 裡面才計算：
- input：encoder 的讀取結果
- output：計算後的結果
- target：由 `cmd_vel`分速
- KP、KI、KD：自行設定

PID leftPID(&leftInfo.input, &leftInfo.output, &leftInfo.target, Kp_L, Ki_L, Kd_L, DIRECT);

注意 input、output、target 提供指標，建議好好的去看 header file

#### Micro-ros-arduino

寫入一個小型的 ROS 到嵌入式系統上，使上位機可以與單晶片通訊，以下是定義出訂閱和發布節點的主要部分。

##### Publisher
以 `encoder_left` 為例，需要有 callback、node、timer

// Timer callback to publish left encoder data
void leftEncoderTimerCallback(rcl_timer_t *timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL)
  {
    encoderLeftMsg.data = leftEncoder.speedMsg.data; // Retrieve left encoder data
    RCSOFTCHECK(rcl_publish(&encoderLeftPublisher, &encoderLeftMsg, NULL));
  }
}
// General function for publisher initialization
void initializePublisher(rcl_publisher_t &publisher, const char *topic_name)
{
  RCCHECK(rclc_publisher_init_default(
      &publisher,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64),
      topic_name));
}

// General function for timer initialization
void initializeTimer(rcl_timer_t &timer, unsigned int timeout_ms, void (*callback)(rcl_timer_t *, int64_t))
{
  RCCHECK(rclc_timer_init_default(
      &timer,
      &support,
      RCL_MS_TO_NS(timeout_ms),
      callback));
  // Add the timer to the executor for periodic callbacks
  RCCHECK(rclc_executor_add_timer(&executor, &timer));
}

// Initialize encoder data publishers
initializePublisher(encoderLeftPublisher, "encoder_left");

// Initialize timers for encoder data publishing
const unsigned int timerTimeout = 50; // Timer interval in ms
initializeTimer(leftEncoderTimer, timerTimeout, leftEncoderTimerCallback);

##### Subscriber

以 `cmd_vel` 為例，需要有 callback、node

// Timer callback to subscribe cmd_vel topic
void cmdVelTimerCallback(const void *msgin)
{
  const geometry_msgs__msg__Twist *msg = (const geometry_msgs__msg__Twist *)msgin;
  double Velocity = msg->linear.x;
  double Angular = msg->angular.z;
  twistToPulseAndServo(Velocity, Angular);

  lastMotorCommand = millis(); // record the time when cmd_vel is received
}

// function for subscriber initialization
void twistSubscriber(rcl_subscription_t &subscriber, const char *topic_name,
                     void (*callback)(const void *))
{
  RCCHECK(rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      topic_name));
  // Add the subscriber to the executor for handling received messages
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &twist_msg, callback, ON_NEW_DATA));
}

// Initialize cmd_vel subscriber
twistSubscriber(cmdVelSubscriber, "cmd_vel", cmdVelTimerCallback);