M3508

main.cpp

#include "InterfaceCAN.h"
#include "PinNames.h"
#include "mbed.h"
#include <cstdint>

#if !DEVICE_CAN
#error [NOT_SUPPORTED] CAN not supported for this target
#endif

CAN can(PB_8, PB_9, 1000 * 1000);

// 定数の定義
constexpr int16_t MAX_POWER = 600;
constexpr int16_t MIN_POWER = -600;

struct motor_param {
    int16_t SET_SPEED;
    int16_t speed;
    int16_t power;
    int16_t p_gain; // 比例ゲイン
    int16_t i_gain; // 積分ゲイン
    int16_t d_gain; // 微分ゲイン
    int16_t err_last;
    int16_t integral_err;

    // PID制御の計算
    void calculatePID() {
        auto err = SET_SPEED - speed;
        auto p_out = err / p_gain;
        integral_err += err;
        auto i_out = integral_err / i_gain;
        auto d_out = (err - err_last) * d_gain;
        power += p_out + i_out + d_out;

        // 飽和処理
        if (power > MAX_POWER) power = MAX_POWER;
        else if (power < MIN_POWER) power = MIN_POWER;

        // エラーの更新
        err_last = err;
    }
} motors[4];

Ticker flipper;
volatile bool newCanMessage = false;

void flip() {
    for(auto& motor : motors) {
        motor.calculatePID();
    }
    newCanMessage = true;
}

void processCANMessage(const CANMessage& msg) {
    if (msg.id >= 0x201 && msg.id <= 0x204) {
        int motorIndex = msg.id - 0x201;
        motors[motorIndex].speed = static_cast<int16_t>((msg.data[2] << 8) | msg.data[3]);
    }
}

int main() {
    printf("CAN Motor Control Initialized\n");

    // モーターの初期化
    for (auto& motor : motors) {
        motor.p_gain = 50;
        motor.i_gain = 10;
        motor.d_gain = 4;
        motor.err_last = 0;
        motor.integral_err = 0;
    }

    flipper.attach(&flip, 2ms);

    // モーターの目標速度設定
    motors[0].SET_SPEED = 3000;
    motors[1].SET_SPEED = 2000;
    motors[2].SET_SPEED = 2000;
    motors[3].SET_SPEED = 2000;
    while (true) {
        CANMessage rcv_msg;
        if (can.read(rcv_msg)) {
            processCANMessage(rcv_msg);
        }

        if (newCanMessage) {
            unsigned char send_data[8];
            for (int i = 0; i < 4; i++) {
                send_data[i * 2] = (motors[i].power >> 8) & 0xFF;
                send_data[i * 2 + 1] = motors[i].power & 0xFF;
            }
            CANMessage send_msg(0x200, send_data, 8);
            can.write(send_msg);
            newCanMessage = false;
        }
    }
}