diff --git a/LICENSE.TXT b/LICENSE.TXT new file mode 100644 index 0000000000000000000000000000000000000000..2bd96d02ba1fe90af169cef446e5e74e49d52d57 --- /dev/null +++ b/LICENSE.TXT @@ -0,0 +1,17 @@ +Copyright (c) + +All rights reserved. + +MIT License + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation +files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, +modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software +is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED *AS IS*, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES +OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS +BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT +OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. \ No newline at end of file diff --git a/arduinoC/_images/featured.png b/arduinoC/_images/featured.png new file mode 100644 index 0000000000000000000000000000000000000000..b76bec7ea6a34d3e2e3ff107ef8161ba98a0cfdf Binary files /dev/null and b/arduinoC/_images/featured.png differ diff --git a/arduinoC/_images/icon.svg b/arduinoC/_images/icon.svg new file mode 100644 index 0000000000000000000000000000000000000000..ef32c8638863cb8fecada40588ff16f957001180 --- /dev/null +++ b/arduinoC/_images/icon.svg @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/arduinoC/_menus/arduino.json b/arduinoC/_menus/arduino.json new file mode 100644 index 0000000000000000000000000000000000000000..7a8784962a7e8990412df4ad90658f1481664b05 --- /dev/null +++ b/arduinoC/_menus/arduino.json @@ -0,0 +1,7 @@ +{ + "PINS": { + "menu": [["P0", "33"], ["P1", "32"], ["P6", "16"], ["P7", "17"], ["P8", "26"], ["P9", "25"], ["P13", "18"], ["P14", "19"], ["P15", "21"], ["P16", "5"], ["P19", "22"], ["P20", "23"]], + "default_TX": "0", + "default_RX": "1" + } +} diff --git a/arduinoC/_menus/esp32.json b/arduinoC/_menus/esp32.json new file mode 100644 index 0000000000000000000000000000000000000000..7a8784962a7e8990412df4ad90658f1481664b05 --- /dev/null +++ b/arduinoC/_menus/esp32.json @@ -0,0 +1,7 @@ +{ + "PINS": { + "menu": [["P0", "33"], ["P1", "32"], ["P6", "16"], ["P7", "17"], ["P8", "26"], ["P9", "25"], ["P13", "18"], ["P14", "19"], ["P15", "21"], ["P16", "5"], ["P19", "22"], ["P20", "23"]], + "default_TX": "0", + "default_RX": "1" + } +} diff --git a/arduinoC/_menus/firebeetleesp32e.json b/arduinoC/_menus/firebeetleesp32e.json new file mode 100644 index 0000000000000000000000000000000000000000..7a8784962a7e8990412df4ad90658f1481664b05 --- /dev/null +++ b/arduinoC/_menus/firebeetleesp32e.json @@ -0,0 +1,7 @@ +{ + "PINS": { + "menu": [["P0", "33"], ["P1", "32"], ["P6", "16"], ["P7", "17"], ["P8", "26"], ["P9", "25"], ["P13", "18"], ["P14", "19"], ["P15", "21"], ["P16", "5"], ["P19", "22"], ["P20", "23"]], + "default_TX": "0", + "default_RX": "1" + } +} diff --git a/arduinoC/libraries/mb_md40/em_check.h b/arduinoC/libraries/mb_md40/em_check.h new file mode 100644 index 0000000000000000000000000000000000000000..1f856509e31881ca3ea5be88514ed6e269ebc9cc --- /dev/null +++ b/arduinoC/libraries/mb_md40/em_check.h @@ -0,0 +1,168 @@ +#pragma once +#ifndef _EM_CHECK_H_ +#define _EM_CHECK_H_ + +#include + +#ifdef ARDUINO_ARCH_ESP32 +#include +#endif + +/** + * @file em_check.h + */ + +/** + * @~Chinese + * @brief 断言失败处理函数 + * @param expr 断言失败的表达式字符串 + * @param function 发生断言的函数名 + * @param file 发生断言的源文件名 + * @param line 发生断言的行号 + * @details 当断言失败时，此函数会输出错误信息并停止程序运行 + * - 在ESP32平台上使用printf输出并调用abort() + * - 在其他Arduino平台上使用Serial输出并进入死循环 + */ +/** + * @~English + * @brief Assertion failure handling function + * @param expr The expression string that failed the assertion + * @param function The function name where assertion occurred + * @param file The source file name where assertion occurred + * @param line The line number where assertion occurred + * @details When an assertion fails, this function outputs error information and stops program execution + * - On ESP32 platform, uses printf output and calls abort() + * - On other Arduino platforms, uses Serial output and enters an infinite loop + */ +static inline void AssertFailHandle(const char* expr, const char* function, const char* file, const int line) { +#ifdef ARDUINO_ARCH_ESP32 + printf("\nassert failed: %s %s:%d (%s)\n", function, file, line, expr); + abort(); +#else + Serial.print(F("\nassert failed: ")); + Serial.print(function); + Serial.print(F(" ")); + Serial.print(file); + Serial.print(F(":")); + Serial.print(line); + Serial.print(F(" (")); + Serial.print(expr); + Serial.println(F(")")); + Serial.flush(); + noInterrupts(); + while (true) { + } +#endif +} + +/** + * @~Chinese + * @brief 基本断言检查宏 + * @param expr 要检查的表达式 + * @details 如果表达式为假，则触发断言失败处理 + */ +/** + * @~English + * @brief Basic assertion check macro + * @param expr Expression to check + * @details If the expression is false, triggers assertion failure handling + */ +#define EM_CHECK(expr) ((expr) ? (void)0 : AssertFailHandle(#expr, __PRETTY_FUNCTION__, __FILE__, __LINE__)) + +/** + * @~Chinese + * @brief 相等断言检查宏 + * @param a 第一个比较值 + * @param b 第二个比较值 + * @details 如果a不等于b，则触发断言失败处理 + */ +/** + * @~English + * @brief Equality assertion check macro + * @param a First comparison value + * @param b Second comparison value + * @details If a is not equal to b, triggers assertion failure handling + */ +#define EM_CHECK_EQ(a, b) ((a) == (b) ? (void)0 : AssertFailHandle(#a " == " #b, __PRETTY_FUNCTION__, __FILE__, __LINE__)) + +/** + * @~Chinese + * @brief 不相等断言检查宏 + * @param a 第一个比较值 + * @param b 第二个比较值 + * @details 如果a等于b，则触发断言失败处理 + */ +/** + * @~English + * @brief Inequality assertion check macro + * @param a First comparison value + * @param b Second comparison value + * @details If a is equal to b, triggers assertion failure handling + */ +#define EM_CHECK_NE(a, b) ((a) != (b) ? (void)0 : AssertFailHandle(#a " != " #b, __PRETTY_FUNCTION__, __FILE__, __LINE__)) + +/** + * @~Chinese + * @brief 大于断言检查宏 + * @param a 第一个比较值 + * @param b 第二个比较值 + * @details 如果a不大于b，则触发断言失败处理 + */ +/** + * @~English + * @brief Greater than assertion check macro + * @param a First comparison value + * @param b Second comparison value + * @details If a is not greater than b, triggers assertion failure handling + */ +#define EM_CHECK_GT(a, b) ((a) > (b) ? (void)0 : AssertFailHandle(#a " > " #b, __PRETTY_FUNCTION__, __FILE__, __LINE__)) + +/** + * @~Chinese + * @brief 小于断言检查宏 + * @param a 第一个比较值 + * @param b 第二个比较值 + * @details 如果a不小于b，则触发断言失败处理 + */ +/** + * @~English + * @brief Less than assertion check macro + * @param a First comparison value + * @param b Second comparison value + * @details If a is not less than b, triggers assertion failure handling + */ +#define EM_CHECK_LT(a, b) ((a) < (b) ? (void)0 : AssertFailHandle(#a " < " #b, __PRETTY_FUNCTION__, __FILE__, __LINE__)) + +/** + * @~Chinese + * @brief 大于等于断言检查宏 + * @param a 第一个比较值 + * @param b 第二个比较值 + * @details 如果a不大于等于b，则触发断言失败处理 + */ +/** + * @~English + * @brief Greater than or equal assertion check macro + * @param a First comparison value + * @param b Second comparison value + * @details If a is not greater than or equal to b, triggers assertion failure handling + */ +#define EM_CHECK_GE(a, b) ((a) >= (b) ? (void)0 : AssertFailHandle(#a " >= " #b, __PRETTY_FUNCTION__, __FILE__, __LINE__)) + +/** + * @~Chinese + * @brief 小于等于断言检查宏 + * @param a 第一个比较值 + * @param b 第二个比较值 + * @details 如果a不小于等于b，则触发断言失败处理 + */ +/** + * @~English + * @brief Less than or equal assertion check macro + * @param a First comparison value + * @param b Second comparison value + * @details If a is not less than or equal to b, triggers assertion failure handling + */ +#define EM_CHECK_LE(a, b) ((a) <= (b) ? (void)0 : AssertFailHandle(#a " <= " #b, __PRETTY_FUNCTION__, __FILE__, __LINE__)) + +#endif \ No newline at end of file diff --git a/arduinoC/libraries/mb_md40/md40.cpp b/arduinoC/libraries/mb_md40/md40.cpp new file mode 100644 index 0000000000000000000000000000000000000000..81f4819e3d2049696ae4661ca9b164573e1c28f9 --- /dev/null +++ b/arduinoC/libraries/mb_md40/md40.cpp @@ -0,0 +1,579 @@ +/** + * @file md40.cpp + */ + +#include "md40.h" + +namespace em { + +namespace { +constexpr uint8_t kMotorStateOffset = 0x20; +constexpr uint8_t kI2cEndTransmissionSuccess = 0; + +enum Command : uint8_t { + kSetup = 1, + kReset = 2, + kSetSpeedPidP = 3, + kSetSpeedPidI = 4, + kSetSpeedPidD = 5, + kSetPositionPidP = 6, + kSetPositionPidI = 7, + kSetPositionPidD = 8, + kSetPosition = 9, + kSetPulseCount = 10, + kStop = 11, + kRunPwmDuty = 12, + kRunSpeed = 13, + kMoveTo = 14, + kMove = 15, +}; + +enum MemoryAddress : uint8_t { + kDeviceId = 0x00, + kMajorVersion = 0x01, + kMinorVersion = 0x02, + kPatchVersion = 0x03, + kName = 0x04, + kCommandType = 0x11, + kCommandIndex = 0x12, + kCommandParam = 0x13, + kCommandExecute = 0x23, + kState = 0x24, + kSpeedP = 0x26, + kSpeedI = 0x28, + kSpeedD = 0x2A, + kPositionP = 0x2C, + kPositionI = 0x2E, + kPositionD = 0x30, + kSpeed = 0x34, + kPosition = 0x38, + kPulseCount = 0x3C, + kPwmDuty = 0x40, +}; +} // namespace + +Md40::Md40(const uint8_t i2c_address, TwoWire &wire) : i2c_address_(i2c_address), wire_(wire) { + for (uint8_t i = 0; i < kMotorNum; i++) { + motors_[i] = new Motor(i, i2c_address, wire); + } +} + +Md40::Motor &Md40::operator[](const uint8_t index) { + EM_CHECK_LT(index, kMotorNum); + return *motors_[index]; +} + +void Md40::Init() { + for (auto motor : motors_) { + motor->Reset(); + } +} + +String Md40::firmware_version() { + wire_.beginTransmission(i2c_address_); + wire_.write(kMajorVersion); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + uint8_t version[3] = {0}; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(version))), sizeof(version)); + + uint8_t offset = 0; + while (offset < sizeof(version)) { + if (wire_.available() > 0) { + version[offset++] = wire_.read(); + } + } + + return String(version[0]) + "." + String(version[1]) + "." + String(version[2]); +} + +uint8_t Md40::device_id() { + wire_.beginTransmission(i2c_address_); + wire_.write(kDeviceId); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(1)), 1); + + while (wire_.available() == 0); + + return wire_.read(); +} + +String Md40::name() { + wire_.beginTransmission(i2c_address_); + wire_.write(kName); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + constexpr uint8_t kLength = 8; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, kLength), kLength); + + String result; + while (wire_.available() > 0) { + result += static_cast(wire_.read()); + } + + return result; +} + +Md40::Motor::Motor(const uint8_t index, const uint8_t i2c_address, TwoWire &wire) : index_(index), i2c_address_(i2c_address), wire_(wire) { +} + +void Md40::Motor::ExecuteCommand() { + wire_.beginTransmission(i2c_address_); + wire_.write(kCommandExecute); + wire_.write(0x01); + + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + WaitCommandEmptied(); +} + +void Md40::Motor::WaitCommandEmptied() { + uint8_t result = 0xFF; + do { + wire_.beginTransmission(i2c_address_); + wire_.write(kCommandExecute); + + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(result))), sizeof(result)); + + while (wire_.available() == 0); + + result = wire_.read(); + } while (result != 0); +} + +void Md40::Motor::WriteCommand(const uint8_t command, const uint8_t *data, const uint16_t length) { + wire_.beginTransmission(i2c_address_); + wire_.write(kCommandType); + wire_.write(command); + wire_.write(index_); + if (data != nullptr && length > 0) { + wire_.write(data, length); + } + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); +} + +void Md40::Motor::Reset() { + WaitCommandEmptied(); + + WriteCommand(kReset, nullptr, 0); + + ExecuteCommand(); +} + +void Md40::Motor::SetEncoderMode(const uint16_t ppr, const uint16_t reduction_ratio, const uint8_t phase_relation) { + // 保存参数到成员变量 + saved_ppr_ = ppr; + saved_reduction_ratio_ = reduction_ratio; + saved_phase_relation_ = phase_relation; + encoder_params_saved_ = true; + + WaitCommandEmptied(); + + wire_.beginTransmission(i2c_address_); + wire_.write(kCommandType); + wire_.write(kSetup); + wire_.write(index_); + wire_.write(reinterpret_cast(&ppr), sizeof(ppr)); + wire_.write(reinterpret_cast(&reduction_ratio), sizeof(reduction_ratio)); + wire_.write(static_cast(phase_relation)); + + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + ExecuteCommand(); +} + +void Md40::Motor::SetEncoderMode() { + // 检查是否已经保存了参数 + EM_CHECK_EQ(encoder_params_saved_, true); + + WaitCommandEmptied(); + + wire_.beginTransmission(i2c_address_); + wire_.write(kCommandType); + wire_.write(kSetup); + wire_.write(index_); + wire_.write(reinterpret_cast(&saved_ppr_), sizeof(saved_ppr_)); + wire_.write(reinterpret_cast(&saved_reduction_ratio_), sizeof(saved_reduction_ratio_)); + wire_.write(static_cast(saved_phase_relation_)); + + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + ExecuteCommand(); +} + +void Md40::Motor::SetDcMode() { + WaitCommandEmptied(); + + wire_.beginTransmission(i2c_address_); + wire_.write(kCommandType); + wire_.write(kSetup); + wire_.write(index_); + wire_.write(0); + wire_.write(0); + wire_.write(0); + + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + ExecuteCommand(); +} + +float Md40::Motor::speed_pid_p() { + wire_.beginTransmission(i2c_address_); + wire_.write(static_cast(kSpeedP + index_ * kMotorStateOffset)); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + uint16_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available()) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + + return data / 100.0f; +} + +void Md40::Motor::set_speed_pid_p(const float value) { + WaitCommandEmptied(); + + const uint16_t int_value = static_cast(value * 100); + WriteCommand(kSetSpeedPidP, reinterpret_cast(&int_value), sizeof(int_value)); + + ExecuteCommand(); +} + +float Md40::Motor::speed_pid_i() { + wire_.beginTransmission(i2c_address_); + wire_.write(static_cast(kSpeedI + index_ * kMotorStateOffset)); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + uint16_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available()) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + + return data / 100.0f; +} + +void Md40::Motor::set_speed_pid_i(const float value) { + WaitCommandEmptied(); + + const uint16_t int_value = static_cast(value * 100); + WriteCommand(kSetSpeedPidI, reinterpret_cast(&int_value), sizeof(int_value)); + + ExecuteCommand(); +} + +float Md40::Motor::speed_pid_d() { + wire_.beginTransmission(i2c_address_); + wire_.write(static_cast(kSpeedD + index_ * kMotorStateOffset)); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + uint16_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available()) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + + return data / 100.0f; +} + +void Md40::Motor::set_speed_pid_d(const float value) { + WaitCommandEmptied(); + + const uint16_t int_value = static_cast(value * 100); + WriteCommand(kSetSpeedPidD, reinterpret_cast(&int_value), sizeof(int_value)); + + ExecuteCommand(); +} + +float Md40::Motor::position_pid_p() { + wire_.beginTransmission(i2c_address_); + wire_.write(static_cast(kPositionP + index_ * kMotorStateOffset)); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + uint16_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available()) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + + return data / 100.0f; +} + +void Md40::Motor::set_position_pid_p(const float value) { + WaitCommandEmptied(); + + const uint16_t int_value = static_cast(value * 100); + WriteCommand(kSetPositionPidP, reinterpret_cast(&int_value), sizeof(int_value)); + + ExecuteCommand(); +} + +float Md40::Motor::position_pid_i() { + wire_.beginTransmission(i2c_address_); + wire_.write(static_cast(kPositionI + index_ * kMotorStateOffset)); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + uint16_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available()) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + + return data / 100.0f; +} + +void Md40::Motor::set_position_pid_i(const float value) { + WaitCommandEmptied(); + + const uint16_t int_value = static_cast(value * 100); + WriteCommand(kSetPositionPidI, reinterpret_cast(&int_value), sizeof(int_value)); + + ExecuteCommand(); +} + +float Md40::Motor::position_pid_d() { + wire_.beginTransmission(i2c_address_); + wire_.write(static_cast(kPositionD + index_ * kMotorStateOffset)); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + uint16_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available()) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + + return data / 100.0f; +} + +void Md40::Motor::set_position_pid_d(const float value) { + WaitCommandEmptied(); + + const uint16_t int_value = static_cast(value * 100); + WriteCommand(kSetPositionPidD, reinterpret_cast(&int_value), sizeof(int_value)); + + ExecuteCommand(); +} + +void Md40::Motor::set_position(const int32_t position) { + WaitCommandEmptied(); + + WriteCommand(kSetPosition, reinterpret_cast(&position), sizeof(position)); + + ExecuteCommand(); +} + +void Md40::Motor::set_pulse_count(const int32_t pulse_count) { + WaitCommandEmptied(); + + WriteCommand(kSetPulseCount, reinterpret_cast(&pulse_count), sizeof(pulse_count)); + + ExecuteCommand(); +} + +void Md40::Motor::Stop() { + WaitCommandEmptied(); + + WriteCommand(kStop, nullptr, 0); + + ExecuteCommand(); +} + +void Md40::Motor::RunSpeed(const int32_t rpm) { + WaitCommandEmptied(); + + WriteCommand(kRunSpeed, reinterpret_cast(&rpm), sizeof(rpm)); + + ExecuteCommand(); +} + +void Md40::Motor::RunPwmDuty(const int16_t pwm_duty) { + WaitCommandEmptied(); + + WriteCommand(kRunPwmDuty, reinterpret_cast(&pwm_duty), sizeof(pwm_duty)); + + ExecuteCommand(); +} + +void Md40::Motor::MoveTo(const int32_t position, const int32_t speed) { + WaitCommandEmptied(); + + const int32_t data[] = {position, speed}; + WriteCommand(kMoveTo, reinterpret_cast(data), sizeof(data)); + + ExecuteCommand(); +} + +void Md40::Motor::Move(const int32_t offset, const int32_t speed) { + WaitCommandEmptied(); + + const int32_t data[] = {offset, speed}; + WriteCommand(kMove, reinterpret_cast(data), sizeof(data)); + + ExecuteCommand(); +} + +int32_t Md40::Motor::state() { + const uint8_t address = kState + index_ * kMotorStateOffset; + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + wire_.write(0); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(1)), 1); + + while (wire_.available()) { + return static_cast(wire_.read()); + } + +} + +bool Md40::Motor::is_state(const String stateCn) { + int32_t my_state = state(); + if (stateCn == "FREE") { + return 0 == my_state; + } + if (stateCn == "RUNNING") { + return 1 == my_state || 2 == my_state || 3 == my_state; + } + if (stateCn == "COMPLETED") { + return 4 == my_state; + } + return false; +} + + + +int32_t Md40::Motor::speed() { + const uint8_t address = kSpeed + index_ * kMotorStateOffset; + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + wire_.write(0); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + int32_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available()) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + return data; +} + +int32_t Md40::Motor::position() { + const uint8_t address = kPosition + index_ * kMotorStateOffset; + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + wire_.write(0); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + int32_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available() > 0) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + + return data; +} + +int32_t Md40::Motor::pulse_count() { + const uint8_t address = kPulseCount + index_ * kMotorStateOffset; + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + wire_.write(0); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + int32_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available()) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + + return data; +} + +int16_t Md40::Motor::pwm_duty() { + const uint8_t address = kPwmDuty + index_ * kMotorStateOffset; + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + wire_.write(0); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + wire_.beginTransmission(i2c_address_); + wire_.write(address); + EM_CHECK_EQ(wire_.endTransmission(), kI2cEndTransmissionSuccess); + + int16_t data = 0; + EM_CHECK_EQ(wire_.requestFrom(i2c_address_, static_cast(sizeof(data))), sizeof(data)); + + uint8_t offset = 0; + while (offset < sizeof(data)) { + if (wire_.available()) { + reinterpret_cast(&data)[offset++] = wire_.read(); + } + } + + data = static_cast(data * 100 / 1023); + + return data; +} +} // namespace em \ No newline at end of file diff --git a/arduinoC/libraries/mb_md40/md40.h b/arduinoC/libraries/mb_md40/md40.h new file mode 100644 index 0000000000000000000000000000000000000000..cd1cd001d09af21525bb3640c7a6a654b6ba7fa3 --- /dev/null +++ b/arduinoC/libraries/mb_md40/md40.h @@ -0,0 +1,623 @@ +#pragma once + +#ifndef _EM_MD40_H_ +#define _EM_MD40_H_ + +#include +#include + +#include "em_check.h" + +/** + * @file md40.h + */ + +namespace em { + +/** + * @~Chinese + * @class Md40 + * @brief Md40是一个用于控制MD40模块的驱动类，用来驱动电机。 + */ +/** + * @~English + * @class Md40 + * @brief Md40 is a driver class used to control the MD40 module for motor driving. + */ +class Md40 { + public: + /** + * @~Chinese + * @brief 默认I2C地址。 + */ + /** + * @~English + * @brief Default I2C address. + */ + static constexpr uint8_t kDefaultI2cAddress = 0x16; + /** + * @~Chinese + * @brief 电机数量。 + */ + /** + * @~English + * @brief Number of motors. + */ + static constexpr uint8_t kMotorNum = 4; + + /** + * @~Chinese + * @class Md40::Motor + * @brief Motor类代表一个电机对象，提供对单个电机的控制功能，如速度、位置和PID参数设置。 + */ + /** + * @~English + * @class Md40::Motor + * @brief The Motor class represents a motor object, providing control functions for a single motor, such as speed, position, and PID parameters + * setting. + */ + class Motor { + public: + /** + * @~Chinese + * @brief 用于明确电机正转时编码器AB相的相位关系，以便在脉冲计数及后续速度计算等操作中依据正确的相位关系进行处理。 + */ + /** + * @~English + * @brief Used to clarify the phase relationship between phase A and phase B of the encoder when the motor is rotating + * forward, so that the correct phase relationship can be used in operations such as pulse counting and subsequent speed + * calculation. + */ + enum class PhaseRelation : uint8_t { + /** + * @~Chinese + * @brief 表示电机正转时A相领先于B相。 + */ + /** + * @~English + * @brief Represents the situation where phase A leads phase B when the motor is rotating forward. + */ + kAPhaseLeads = 0, + + /** + * @~Chinese + * @brief 表示电机正转时B相领先于A相。 + */ + /** + * @~English + * @brief Represents the situation where phase B leads phase A when the motor is rotating forward. + */ + kBPhaseLeads = 1, + }; + + /** + * @~Chinese + * @brief 电机状态枚举。 + */ + /** + * @~English + * @brief Motor state enumeration. + */ + enum class State : uint8_t { + /** + * @~Chinese + * @brief 表示电机处于空闲状态。 + */ + /** + * @~English + * @brief Indicates that the motor is in idle state. + */ + kIdle = 0, + + /** + * @~Chinese + * @brief 表示电机处于PWM占空比模式运行。 + */ + /** + * @~English + * @brief Indicates that the motor is running in PWM duty mode. + */ + kRuningWithPwmDuty = 1, + + /** + * @~Chinese + * @brief 表示电机处于速度模式运行。 + */ + /** + * @~English + * @brief Indicates that the motor is running in speed mode. + */ + kRuningWithSpeed = 2, + + /** + * @~Chinese + * @brief 表示电机正在执行位置闭环运动，向目标位置运行中。 + */ + /** + * @~English + * @brief Indicates that the motor is executing position closed-loop motion and moving towards the target position. + */ + kRuningToPosition = 3, + + /** + * @~Chinese + * @brief 表示电机到达目标位置状态。 + */ + /** + * @~English + * @brief Indicates that the motor has reached the target position. + */ + kReachedPosition = 4, + }; + + /** + * @~Chinese + * @brief 构造函数。 + * @param[in] index 电机索引。 + * @param[in] i2c_address I2C地址。 + * @param[in] wire TwoWire 对象引用。 + */ + /** + * @~English + * @brief Constructor. + * @param[in] index Motor index. + * @param[in] i2c_address I2C address. + * @param[in] wire TwoWire object reference. + */ + Motor(const uint8_t index, const uint8_t i2c_address, TwoWire &wire); + + /** + * @~Chinese + * @brief 重置电机。 + */ + /** + * @~English + * @brief Reset the motor. + */ + void Reset(); + + /** + * @~Chinese + * @brief 设置电机为编码器模式。 + * @param[in] ppr 每转脉冲数。 + * @param[in] reduction_ratio 减速比。 + * @param[in] phase_relation 相位关系（A相领先或B相领先，指电机正转时的情况），参数说明请查阅： @ref PhaseRelation 。 + */ + /** + * @~English + * @brief Set the motor to encoder mode. + * @param[in] ppr Pulses per revolution. + * @param[in] reduction_ratio Reduction ratio. + * @param[in] phase_relation Phase relationship (A phase leads or B phase leads, referring to the situation when the motor is + * rotating forward), for parameter descriptions, please refer to: @ref PhaseRelation. + */ + void SetEncoderMode(const uint16_t ppr, const uint16_t reduction_ratio, const uint8_t phase_relation); + + /** + * @~Chinese + * @brief 设置电机为编码器模式（使用之前保存的参数）。 + * @note 必须先调用带参数的SetEncoderMode方法保存参数后才能使用此方法。 + */ + /** + * @~English + * @brief Set the motor to encoder mode (using previously saved parameters). + * @note Must call the parameterized SetEncoderMode method first to save parameters before using this method. + */ + void SetEncoderMode(); + + /** + * @~Chinese + * @brief 设置电机为直流模式。 + */ + /** + * @~English + * @brief Set the motor to DC mode. + */ + void SetDcMode(); + + /** + * @~Chinese + * @brief 获取速度PID控制器的比例（P）值。 + * @return 速度PID控制器的比例（P）值。 + */ + /** + * @~English + * @brief Get the proportional (P) value of the speed PID controller. + * @return The proportional (P) value of the speed PID controller. + */ + float speed_pid_p(); + + /** + * @~Chinese + * @brief 设置速度PID控制器的比例（P）值。 + * @param[in] value 速度PID控制器的比例（P）值。 + */ + /** + * @~English + * @brief Set the proportional (P) value of the speed PID controller. + * @param[in] value The proportional (P) value of the speed PID controller. + */ + void set_speed_pid_p(const float value); + + /** + * @~Chinese + * @brief 获取速度PID控制器的积分（I）值。 + * @return 速度PID控制器的积分（I）值。 + */ + /** + * @~English + * @brief Get the integral (I) value of the speed PID controller. + * @return The integral (I) value of the speed PID controller. + */ + float speed_pid_i(); + + /** + * @~Chinese + * @brief 设置速度PID控制器的积分（I）值。 + * @param[in] value 速度PID控制器的积分（I）值。 + */ + /** + * @~English + * @brief Set the integral (I) value of the speed PID controller. + * @param[in] value The integral (I) value of the speed PID controller. + */ + void set_speed_pid_i(const float value); + + /** + * @~Chinese + * @brief 获取速度PID控制器的微分（D）值。 + * @return 速度PID控制器的微分（D）值。 + */ + /** + * @~English + * @brief Get the derivative (D) value of the speed PID controller. + * @return The derivative (D) value of the speed PID controller. + */ + float speed_pid_d(); + + /** + * @~Chinese + * @brief 设置速度PID控制器的微分（D）值。 + * @param[in] value 速度PID控制器的微分（D）值。 + */ + /** + * @~English + * @brief Set the derivative (D) value of the speed PID controller. + * @param[in] value The derivative (D) value of the speed PID controller. + */ + void set_speed_pid_d(const float value); + + /** + * @~Chinese + * @brief 获取位置PID控制器的比例（P）值。 + * @return 位置PID控制器的比例（P）值。 + */ + /** + * @~English + * @brief Get the proportional (P) value of the position PID controller. + * @return The proportional (P) value of the position PID controller. + */ + float position_pid_p(); + + /** + * @~Chinese + * @brief 设置位置PID控制器的比例（P）值。 + * @param[in] value 位置PID控制器的比例（P）值。 + */ + /** + * @~English + * @brief Set the proportional (P) value of the position PID controller. + * @param[in] value The proportional (P) value of the position PID controller. + */ + void set_position_pid_p(const float value); + + /** + * @~Chinese + * @brief 获取位置PID控制器的积分（I）值。 + * @return 位置PID控制器的积分（I）值。 + */ + /** + * @~English + * @brief Get the integral (I) value of the position PID controller. + * @return The integral (I) value of the position PID controller. + */ + float position_pid_i(); + + /** + * @~Chinese + * @brief 设置位置PID控制器的积分（I）值。 + * @param[in] value 位置PID控制器的积分（I）值。 + */ + /** + * @~English + * @brief Set the integral (I) value of the position PID controller. + * @param[in] value The integral (I) value of the position PID controller. + */ + void set_position_pid_i(const float value); + + /** + * @~Chinese + * @brief 获取位置PID控制器的微分（D）值。 + * @return 位置PID控制器的微分（D）值。 + */ + /** + * @~English + * @brief Get the derivative (D) value of the position PID controller. + * @return The derivative (D) value of the position PID controller. + */ + float position_pid_d(); + + /** + * @~Chinese + * @brief 设置位置PID控制器的微分（D）值。 + * @param[in] value 位置PID控制器的微分（D）值。 + */ + /** + * @~English + * @brief Set the derivative (D) value of the position PID controller. + * @param[in] value The derivative (D) value of the position PID controller. + */ + void set_position_pid_d(const float value); + + /** + * @~Chinese + * @brief 设定电机输出轴的位置值，单位为角度(°)。（电机输出轴累计角度值，例如：360度表示正转1整圈，-360度表示反转一整圈） + * @param[in] position 位置设定值，单位为角度(°)，表示从零位开始的累计角度。 + */ + /** + * @~English + * @brief Set the position value of the motor output shaft unit degrees (°). (Accumulated angle value of motor output shaft, for example: 360 + * degrees represents 1 full circle of forward rotation, -360 degrees represents 1 full circle of reverse rotation) + * @param[in] position Position setting value, unit degree (°), represents the cumulative angle from zero position. + */ + void set_position(const int32_t position); + + /** + * @~Chinese + * @brief 设定电机的编码器脉冲计数。该计数值是在A相下降沿的时候计数，如果是正转会加一，反转则减一。 + * @param[in] pulse_count 编码器脉冲数。 + */ + /** + * @~English + * @brief Set the encoder pulse count for the motor. The count value is counted at the falling edge of phase A. If it is positive, add one; if it + * is negative, subtract one. + * @param[in] pulse_count Encoder pulse count. + */ + void set_pulse_count(const int32_t pulse_count); + + /** + * @~Chinese + * @brief 停止电机运行。 + */ + /** + * @~English + * @brief Stop the motor. + */ + void Stop(); + + /** + * @~Chinese + * @brief 以设定的速度值（RPM）设置电机输出轴转速。正数代表正转，负数代表反转。 + * @param[in] rpm 速度设定值（RPM）。正数代表正转，负数代表反转。 + */ + /** + * @~English + * @brief Set the motor output shaft speed to the set speed value (RPM). Positive numbers represent forward rotation, while negative numbers + * represent reverse rotation. + * @param[in] rpm Speed setting value (RPM). Positive numbers represent forward rotation, while negative numbers represent reverse rotation. + */ + void RunSpeed(const int32_t rpm); + + /** + * @~Chinese + * @brief 以设定的PWM占空比运行电机。正数代表正转，负数代表反转。 + * @param[in] pwm_duty PWM占空比（取值范围 -1023到1023）。正数代表正转，负数代表反转。 + */ + /** + * @~English + * @brief Run the motor with the set PWM duty. Positive numbers represent forward rotation, while negative numbers represent reverse + * rotation. + * @param[in] pwm_duty PWM duty (value range -1023 to 1023). Positive values represent forward rotation, negative values represent reverse + * rotation. + */ + void RunPwmDuty(const int16_t pwm_duty); + + /** + * @~Chinese + * @brief 将电机输出轴转动到指定位置，单位为角度(°)。 + * @param[in] position 目标位置设定值，单位为角度(°)，累计角度值。 + * @param[in] speed 电机输出轴运行速度设定值（RPM）。 + */ + /** + * @~English + * @brief Rotate the motor output shaft to the designated position, unit degrees (°). + * @param[in] position Target position setting value, unit: angle (°), cumulative angle value. + * @param[in] speed Motor output shaft operating speed set value (RPM). + */ + void MoveTo(const int32_t position, const int32_t speed); + + /** + * @~Chinese + * @brief 电机输出轴相对转动指定角度，单位为角度(°)。 + * @param[in] offset 相对位移设定值，单位为角度(°)，基于当前位置的相对角度。 + * @param[in] speed 电机输出轴运行速度设定值（RPM）。 + */ + /** + * @~English + * @brief The motor output shaft rotates relative to the specified angle, unit degrees (°). + * @param[in] offset Relative displacement setting value, unit degrees (°), based on the relative angle at the current position. + * @param[in] speed Motor output shaft operating speed set value (RPM). + */ + void Move(const int32_t offset, const int32_t speed); + + /** + * @~Chinese + * @brief 获取电机当前状态。 + * @return 电机当前状态。 + */ + /** + * @~English + * @brief Get the current state of the motor. + * @return The current state of the motor. + */ + int32_t state(); + + bool is_state(const String stateCn); + + /** + * @~Chinese + * @brief 获取电机输出轴当前的转速（RPM）。正数代表正转，负数代表反转。 + * @return 电机输出轴当前的转速（RPM）。正数代表正转，负数代表反转。 + */ + /** + * @~English + * @brief Get the current RPM of the motor output shaft. Positive numbers represent forward rotation, while negative numbers represent reverse rotation. + * @return The current speed (RPM) of the motor output shaft. Positive numbers represent forward rotation, while negative numbers represent + * reverse rotation. + */ + int32_t speed(); + + /** + * @~Chinese + * @brief 获取电机输出轴的位置值，单位为角度(°)。（电机输出轴累计角度值，例如：360度表示正转1整圈，-360度表示反转一整圈） + * @return 电机输出轴当前位置，单位为角度(°)，表示从零位开始的累计角度。 + */ + /** + * @~English + * @brief Get the position value of the motor output shaft, unit degrees (°). (Accumulated angle value of motor output shaft, for example: 360 degrees represents 1 full circle of forward rotation, -360 degrees represents 1 full circle of reverse rotation) + * @return The current position of the motor output shaft, unit degrees (°), represents the cumulative angle from zero position. + */ + int32_t position(); + + /** + * @~Chinese + * @brief 获取电机当前的编码器脉冲计数。该计数值是在A相下降沿的时候计数，如果是正转会加一，反转则减一。 + * @return 电机当前的编码器脉冲数。 + */ + /** + * @~English + * @brief Get the current encoder pulse count of the motor. The count value is counted at the falling edge of phase A. If it is positive, add one; + * if it is negative, subtract one. + * @return The current encoder pulse count of the motor. + */ + int32_t pulse_count(); + + /** + * @~Chinese + * @brief 获取电机当前的PWM占空比。正数代表正转，负数代表反转。 + * @return 电机当前的PWM占空比（取值范围 -1023到1023）。正数代表正转，负数代表反转。 + */ + /** + * @~English + * @brief Get the current PWM duty of the motor. Positive numbers represent forward rotation, while negative numbers represent reverse + * rotation. + * @return The current PWM duty of the motor (value range -1023 to 1023). Positive values represent forward rotation, negative values + * represent reverse rotation. + */ + int16_t pwm_duty(); + + private: + Motor(const Motor &) = delete; + Motor &operator=(const Motor &) = delete; + + void ExecuteCommand(); + + void WaitCommandEmptied(); + + void WriteCommand(const uint8_t command, const uint8_t *data, const uint16_t length); + + const uint8_t index_ = 0; + TwoWire &wire_ = Wire; + const uint8_t i2c_address_ = kDefaultI2cAddress; + + // 保存编码器模式的参数 + uint16_t saved_ppr_ = 0; + uint16_t saved_reduction_ratio_ = 0; + uint8_t saved_phase_relation_ = 0; + bool encoder_params_saved_ = false; + }; + + /** + * @~Chinese + * @brief 构造函数。 + * @param[in] i2c_address I2C地址。 + * @param[in] wire TwoWire 对象引用。 + */ + /** + * @~English + * @brief Constructor. + * @param[in] i2c_address I2C address. + * @param[in] wire TwoWire object reference. + */ + Md40(const uint8_t i2c_address, TwoWire &wire); + + /** + * @~Chinese + * @brief 获取指定索引的电机对象。 + * @param[in] index 电机索引。 + * @return 电机对象引用。 + */ + /** + * @~English + * @brief Get the motor object with specified index. + * @param[in] index Motor index. + * @return Motor object reference. + */ + Motor &operator[](uint8_t index); + + /** + * @~Chinese + * @brief 初始化。 + */ + /** + * @~English + * @brief Initialize. + */ + void Init(); + + /** + * @~Chinese + * @brief 获取固件版本。 + * @return 固件版本。 + */ + /** + * @~English + * @brief Get firmware version. + * @return Firmware version. + */ + String firmware_version(); + + /** + * @~Chinese + * @brief 获取设备ID。 + * @return 设备ID。 + */ + /** + * @~English + * @brief Get device ID. + * @return Device ID. + */ + uint8_t device_id(); + + /** + * @~Chinese + * @brief 获取设备名称。 + * @return 设备名称。 + */ + /** + * @~English + * @brief Get device name. + * @return Device name. + */ + String name(); + + private: + Md40(const Md40 &) = delete; + Md40 &operator=(const Md40 &) = delete; + + const uint8_t i2c_address_ = kDefaultI2cAddress; + TwoWire &wire_ = Wire; + Motor *motors_[kMotorNum] = {nullptr}; +}; +} // namespace em +#endif \ No newline at end of file diff --git a/arduinoC/libraries/mb_md40/md40_lib.h b/arduinoC/libraries/mb_md40/md40_lib.h new file mode 100644 index 0000000000000000000000000000000000000000..c16b60e6fef54da6383b442f4a4e884309661d39 --- /dev/null +++ b/arduinoC/libraries/mb_md40/md40_lib.h @@ -0,0 +1,60 @@ +#pragma once + +#ifndef _EM_MD40_LIB_H_ +#define _EM_MD40_LIB_H_ + +#include + +/** + * @file md40_lib.h + */ + +namespace em { +namespace md40_lib { + +/** + * @~Chinese + * @brief 主版本号。 + */ +/** + * @~English + * @brief Major version number. + */ +constexpr uint8_t kVersionMajor = 1; + +/** + * @~Chinese + * @brief 次版本号。 + */ +/** + * @~English + * @brief Minor version number. + */ +constexpr uint8_t kVersionMinor = 0; + +/** + * @~Chinese + * @brief 修订版本号。 + */ +/** + * @~Englishs + * @brief Patch version number. + */ +constexpr uint8_t kVersionPatch = 0; + +/** + * @~Chinese + * @brief 获取版本号字符串。 + * @return 版本号字符串，格式为 major.minor.patch。 + */ +/** + * @~English + * @brief Get the version number string. + * @return The version number string in the format of major.minor.patch. + */ +String Version() { + return String(kVersionMajor) + '.' + kVersionMinor + '.' + kVersionPatch; +} +} // namespace md40_lib +} // namespace em +#endif \ No newline at end of file diff --git a/arduinoC/main.ts b/arduinoC/main.ts new file mode 100644 index 0000000000000000000000000000000000000000..f97094857fc8dd7cf26878d59bb1ba8df921da5e --- /dev/null +++ b/arduinoC/main.ts @@ -0,0 +1,379 @@ +enum E_ALL_VARS { + //% block="全部" + all, + //% block="E0" + e0, + //% block="E1" + e1, + //% block="E2" + e2, + //% block="E3" + e3, +} + +enum E_VARS { + //% block="E0" + e0, + //% block="E1" + e1, + //% block="E2" + e2, + //% block="E3" + e3, +} + +enum D_ALL_VARS { + //% block="全部" + all, + //% block="M0" + m0, + //% block="M1" + m1, + //% block="M2" + m2, + //% block="M3" + m3, +} + + +enum D_VARS { + //% block="M0" + m0, + //% block="M1" + m1, + //% block="M2" + m2, + //% block="M3" + m3, +} + +enum CLOCKWISEE { + //% block="正转" + True, + //% block="反转" + False +} + +enum PHASE_RELATIONS { + //% block="A" + A_LEAD, + //% block="B" + B_LEAD, +} + +enum D_STATE { + //% block="空闲" + FREE, + //% block="运行" + RUNNING, + //% block="完成" + COMPLETED, +} + +enum PID_OPTIONS { + P, + I, + D, +} + + +//% color="#2494F4" iconWidth=50 iconHeight=40 +namespace MicroblueMd40{ + + //% block="初始 I2C 地址[ADDR]" blockType="command" + //% ADDR.shadow="number" ADDR.defl="0x16" + export function md40_init(parameter: any, block: any) { + let addr = parameter.ADDR.code; + Generator.addInclude("wire", "#include ", true); + Generator.addInclude("md40",`#include "md40.h"`, true); + Generator.addObject("g_md40","em::Md40",`g_md40(${addr}, Wire)`); + Generator.addSetup("Wire.begin", `Wire.begin();`); + Generator.addSetup("g_md40.Init", `g_md40.Init();`); + // Generator.addInitHeader('md40', `md40 = Md40(sda=${sda}, scl=${scl})`, true); + } + + + //% block="初始[V]编码电机减速比[REDUCTION_RATION] [PPR]脉冲/转 [PHASE_RELATION]相领先" blockType="command" + //% V.shadow="dropdownRound" V.options="E_ALL_VARS" V.defl="E_ALL_VARS.all" + //% PPR.shadow="number" PPR.min=12 PPR.max=24 PPR.defl=12 + //% REDUCTION_RATION.shadow="number" REDUCTION_RATION.min=1 REDUCTION_RATION.max=180 REDUCTION_RATION.defl=90 + //% PHASE_RELATION.shadow="dropdownRound" PHASE_RELATION.options="PHASE_RELATIONS" PHASE_RELATION.defl="PHASE_RELATIONS.A_LEAD" + export function md40_motor_init_encoder(parameter: any, block: any) { + let v = parameter.V.code; + let ppr = parameter.PPR.code; + let reduction_ration = parameter.REDUCTION_RATION.code; + let phase_relation = parameter.PHASE_RELATION.code; + if(phase_relation == "A_LEAD") { + phase_relation = "0"; + } else { + phase_relation = "1"; + } + if(v == 'all') { + Generator.addSetup('e0', `g_md40[0].SetEncoderMode(${ppr}, ${reduction_ration}, ${phase_relation});`, true); + Generator.addSetup('e1', `g_md40[1].SetEncoderMode(${ppr}, ${reduction_ration}, ${phase_relation});`, true); + Generator.addSetup('e2', `g_md40[2].SetEncoderMode(${ppr}, ${reduction_ration}, ${phase_relation});`, true); + Generator.addSetup('e3', `g_md40[3].SetEncoderMode(${ppr}, ${reduction_ration}, ${phase_relation});`, true); + } else { + Generator.addSetup(v, `g_md40[${v[1]}].SetEncoderMode(${ppr}, ${reduction_ration}, ${phase_relation});`, true); + } + } + + + //% block="将[V]编码电机[DIR]速度设为[SPEED]%" blockType="command" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.e0" + //% SPEED.shadow="number" SPEED.min=0 SPEED.max=100 SPEED.defl=100 + //% DIR.shadow="dropdownRound" DIR.options="CLOCKWISEE" DIR.defl="CLOCKWISEE.True" + export function md40_encoder_run_pwm_duty(parameter: any, block: any) { + let v = parameter.V.code; + let speed=parameter.SPEED.code; + speed = (speed * 1023 / 100).toFixed(0); + let direction=parameter.DIR.code; + if(direction == 'False') { + Generator.addCode(`g_md40[${v[1]}].RunPwmDuty(-${speed});`); + } else { + Generator.addCode(`g_md40[${v[1]}].RunPwmDuty(${speed});`); + } + } + + + //% block="将[V]编码电机[DIR]速度设为[SPEED]RPM" blockType="command" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.e0" + //% SPEED.shadow="number" SPEED.min=0 SPEED.max=100 SPEED.defl=100 + //% DIR.shadow="dropdownRound" DIR.options="CLOCKWISEE" DIR.defl="CLOCKWISEE.True" + export function md40_run_speed(parameter: any, block: any) { + let v = parameter.V.code; + let speed=parameter.SPEED.code; + let direction=parameter.DIR.code; + if(direction == 'False') { + // Generator.addCode(`Serial.println(g_md40.firmware_version());`); + Generator.addCode(`g_md40[${v[1]}].RunSpeed(-${speed});`); + } else { + // Generator.addCode(`Serial.println(g_md40.firmware_version());`); + Generator.addCode(`g_md40[${v[1]}].RunSpeed(${speed});`); + } + } + + //% block="将[V]编码电机以[SPEED]RPM 速度转[ANGLE]°" blockType="command" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.m0" + //% ANGLE.shadow="number" ANGLE.defl=15 + //% SPEED.shadow="number" SPEED.min=0 SPEED.max=100 SPEED.defl=100 + export function md40_move(parameter: any, block: any) { + let v = parameter.V.code; + let angle=parameter.ANGLE.code; + let speed=parameter.SPEED.code; + Generator.addCode(`g_md40[${v[1]}].Move(${angle}, ${speed});`); + } + + //% block="将[V]编码电机以[SPEED]RPM 速度转到[ANGLE]°" blockType="command" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.m0" + //% ANGLE.shadow="number" ANGLE.defl=90 + //% SPEED.shadow="number" SPEED.min=0 SPEED.max=100 SPEED.defl=100 + export function md40_move_to(parameter: any, block: any) { + let v = parameter.V.code; + let angle=parameter.ANGLE.code; + let speed=parameter.SPEED.code; + Generator.addCode(`g_md40[${v[1]}].MoveTo(${angle}, ${speed});`); + } + + //% block="重置[V]编码电机" blockType="command" + //% V.shadow="dropdownRound" V.options="E_ALL_VARS" V.defl="E_ALL_VARS.all" + export function md40_reset(parameter: any, block: any) { + let v = parameter.V.code; + if(v == 'all') { + Generator.addCode(`g_md40[0].Reset();`); + Generator.addCode(`g_md40[0].SetEncoderMode();`); + Generator.addCode(`g_md40[1].Reset();`); + Generator.addCode(`g_md40[1].SetEncoderMode();`); + Generator.addCode(`g_md40[2].Reset();`); + Generator.addCode(`g_md40[2].SetEncoderMode();`); + Generator.addCode(`g_md40[3].Reset();`); + Generator.addCode(`g_md40[3].SetEncoderMode();`); + } else { + Generator.addCode(`g_md40[${v[1]}].Reset();`); + Generator.addCode(`g_md40[${v[1]}].SetEncoderMode();`); + } + } + + //% block="停止[V]编码电机" blockType="command" + //% V.shadow="dropdownRound" V.options="E_ALL_VARS" V.defl="E_ALL_VARS.all" + export function md40_encoder_stop(parameter: any, block: any) { + let v = parameter.V.code; + if(v == 'all') { + Generator.addCode(`g_md40[0].Stop();`); + Generator.addCode(`g_md40[1].Stop();`); + Generator.addCode(`g_md40[2].Stop();`); + Generator.addCode(`g_md40[3].Stop();`); + } else { + Generator.addCode(`g_md40[${v[1]}].Stop();`); + } + } + + //% block="将[V]编码电机角度 PID 控制器参数设为 P:[P] I:[I] D:[D]" blockType="command" + //% V.shadow="dropdownRound" V.options="E_ALL_VARS" V.defl="E_ALL_VARS.all" + //% P.shadow="number" P.min=0 P.max=10 P.defl=10 + //% I.shadow="number" I.min=0 I.max=5 I.defl=1 + //% D.shadow="number" D.min=0 D.max=2 D.defl=1 + export function md40_set_position_pid(parameter: any, block: any) { + let v = parameter.V.code; + let p = parameter.P.code; + let i = parameter.I.code; + let d = parameter.D.code; + if(v == 'all') { + Generator.addCode(`g_md40[0].set_position_pid_p(${p});`); + Generator.addCode(`g_md40[0].set_position_pid_i(${i});`); + Generator.addCode(`g_md40[0].set_position_pid_d(${d});`); + Generator.addCode(`g_md40[1].set_position_pid_p(${p});`); + Generator.addCode(`g_md40[1].set_position_pid_i(${i});`); + Generator.addCode(`g_md40[1].set_position_pid_d(${d});`); + Generator.addCode(`g_md40[2].set_position_pid_p(${p});`); + Generator.addCode(`g_md40[2].set_position_pid_i(${i});`); + Generator.addCode(`g_md40[2].set_position_pid_d(${d});`); + Generator.addCode(`g_md40[3].set_position_pid_p(${p});`); + Generator.addCode(`g_md40[3].set_position_pid_i(${i});`); + Generator.addCode(`g_md40[3].set_position_pid_d(${d});`); + } else { + Generator.addCode(`g_md40[${v[1]}].set_position_pid_p(${p});`); + Generator.addCode(`g_md40[${v[1]}].set_position_pid_i(${i});`); + Generator.addCode(`g_md40[${v[1]}].set_position_pid_d(${d});`); + } + } + + //% block="将[V]编码电机速度 PID 控制器参数设为 P:[P] I:[I] D:[D]" blockType="command" + //% V.shadow="dropdownRound" V.options="E_ALL_VARS" V.defl="E_ALL_VARS.all" + //% P.shadow="number" P.min=0 P.max=10 P.defl=1.5 + //% I.shadow="number" I.min=0 I.max=5 I.defl=1.5 + //% D.shadow="number" D.min=0 D.max=2 D.defl=1 + export function md40_set_speed_pid(parameter: any, block: any) { + let v = parameter.V.code; + let p = parameter.P.code; + let i = parameter.I.code; + let d = parameter.D.code; + if(v == 'all') { + Generator.addCode(`g_md40[0].set_speed_pid_p(${p});`); + Generator.addCode(`g_md40[0].set_speed_pid_i(${i});`); + Generator.addCode(`g_md40[0].set_speed_pid_d(${d});`); + Generator.addCode(`g_md40[1].set_speed_pid_p(${p});`); + Generator.addCode(`g_md40[1].set_speed_pid_i(${i});`); + Generator.addCode(`g_md40[1].set_speed_pid_d(${d});`); + Generator.addCode(`g_md40[2].set_speed_pid_p(${p});`); + Generator.addCode(`g_md40[2].set_speed_pid_i(${i});`); + Generator.addCode(`g_md40[2].set_speed_pid_d(${d});`); + Generator.addCode(`g_md40[3].set_speed_pid_p(${p});`); + Generator.addCode(`g_md40[3].set_speed_pid_i(${i});`); + Generator.addCode(`g_md40[3].set_speed_pid_d(${d});`); + } else { + Generator.addCode(`g_md40[${v[1]}].set_speed_pid_p(${p});`); + Generator.addCode(`g_md40[${v[1]}].set_speed_pid_i(${i});`); + Generator.addCode(`g_md40[${v[1]}].set_speed_pid_d(${d});`); + } + } + + //% block="[V]编码电机角度 PID 控制器[PID_OPTION]" blockType="reporter" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.m0" + //% PID_OPTION.shadow="dropdownRound" PID_OPTION.options="PID_OPTIONS" PID_OPTION.defl="PID_OPTIONS.P" + export function md40_get_position_pid(parameter: any, block: any) { + let v = parameter.V.code; + let pid_option = parameter.PID_OPTION.code; + if(pid_option == 'P') { + Generator.addCode(`g_md40[${v[1]}].position_pid_p()`); + } else if(pid_option == 'I') { + Generator.addCode(`g_md40[${v[1]}].position_pid_i()`); + } else if(pid_option == 'D') { + Generator.addCode(`g_md40[${v[1]}].position_pid_d()`); + } + } + + //% block="[V]编码电机速度 PID 控制器[PID_OPTION]" blockType="reporter" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.m0" + //% PID_OPTION.shadow="dropdownRound" PID_OPTION.options="PID_OPTIONS" PID_OPTION.defl="PID_OPTIONS.P" + export function md40_get_speed_pid(parameter: any, block: any) { + let v = parameter.V.code; + let pid_option = parameter.PID_OPTION.code; + if(pid_option == 'P') { + Generator.addCode(`g_md40[${v[1]}].speed_pid_p()`); + } else if(pid_option == 'I') { + Generator.addCode(`g_md40[${v[1]}].speed_pid_i()`); + } else if(pid_option == 'D') { + Generator.addCode(`g_md40[${v[1]}].speed_pid_d()`); + } + } + + + //% block="[V]编码电机速度（%）" blockType="reporter" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.m0" + export function md40_get_pwm_duty(parameter: any, block: any) { + let v = parameter.V.code; + Generator.addCode(`g_md40[${v[1]}].pwm_duty()`); + } + + + + //% block="[V]编码电机速度（RPM）" blockType="reporter" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.m0" + export function md40_get_speed(parameter: any, block: any) { + let v = parameter.V.code; + Generator.addCode(`g_md40[${v[1]}].speed()`); + } + + //% block="[V]编码电机角度" blockType="reporter" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.m0" + export function md40_get_position(parameter: any, block: any) { + let v = parameter.V.code; + Generator.addCode(`g_md40[${v[1]}].position()`); + } + + //% block="[V]编码电机脉冲数" blockType="reporter" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.m0" + export function md40_get_pulse_count(parameter: any, block: any) { + let v = parameter.V.code; + Generator.addCode(`g_md40[${v[1]}].pulse_count()`); + } + + //% block="[V]编码电机状态[STATE]?" blockType="boolean" + //% V.shadow="dropdownRound" V.options="E_VARS" V.defl="E_VARS.m0" + //% STATE.shadow="dropdownRound" STATE.options="D_STATE" STATE.defl="D_STATE.FREE" + export function md40_get_state(parameter: any, block: any) { + let v = parameter.V.code; + let state = parameter.STATE.code; + Generator.addCode(`g_md40[${v[1]}].is_state("${state}")`); + } + + + //% block="将[V]电机[DIR]速度设为[SPEED]%" blockType="command" + //% V.shadow="dropdownRound" V.options="D_VARS" V.defl="D_VARS.d0" + //% SPEED.shadow="number" SPEED.min=0 SPEED.max=100 SPEED.defl=100 + //% DIR.shadow="dropdownRound" DIR.options="CLOCKWISEE" DIR.defl="CLOCKWISEE.True" + export function md40_dc_run_pwm_duty(parameter: any, block: any) { + let v = parameter.V.code; + let speed=parameter.SPEED.code; + let direction=parameter.DIR.code; + speed = (speed * 1023 / 100).toFixed(0); + if(direction == 'False') { + Generator.addSetup(`m${v[1]}`, `g_md40[${v[1]}].SetDcMode();`, true); + Generator.addCode(`g_md40[${v[1]}].RunPwmDuty(-${speed});`); + } else { + Generator.addSetup(`m${v[1]}`, `g_md40[${v[1]}].SetDcMode();`, true); + Generator.addCode(`g_md40[${v[1]}].RunPwmDuty(${speed});`); + } + } + + + //% block="停止[V]电机" blockType="command" + //% V.shadow="dropdownRound" V.options="D_ALL_VARS" V.defl="D_ALL_VARS.all" + export function md40_dc_stop(parameter: any, block: any) { + let v = parameter.V.code; + if(v == 'all') { + Generator.addSetup(`m0`, `g_md40[0].SetDcMode();`, true); + Generator.addCode(`g_md40[0].Stop();`); + Generator.addSetup(`m1`, `g_md40[1].SetDcMode();`, true); + Generator.addCode(`g_md40[1].Stop();`); + Generator.addSetup(`m2`, `g_md40[2].SetDcMode();`, true); + Generator.addCode(`g_md40[2].Stop();`); + Generator.addSetup(`m3`, `g_md40[3].SetDcMode();`, true); + Generator.addCode(`g_md40[3].Stop();`); + } else { + Generator.addSetup(`m${v[1]}`, `g_md40[${v[1]}].SetDcMode();`, true); + Generator.addCode(`g_md40[${v[1]}].Stop();`); + } + } +} diff --git a/config.json b/config.json new file mode 100644 index 0000000000000000000000000000000000000000..fc32a59aae9cd1fd7b89ff149a4ce2687e872e73 --- /dev/null +++ b/config.json @@ -0,0 +1,30 @@ +{ + "name": { + "zh-cn": "MD40编码电机模块", + "en": " Four-Channel Encoder Driver" + }, + "description": { + "zh-cn": "PID算法，速度闭环和位置闭环控制", + "en": "PID Algorithm, Speed Closed-Loop and Position Closed-Loop Control​​​" + }, + "author": "Emakefun", + "email": "micro_blue_room@163.com", + "license": "MIT", + "isBoard": false, + "id": "emMd40", + "platform": ["win","mac","web","linux"], + "version": "1.0.0", + "asset": { + "arduinoC": { + "dir": "arduinoC/", + "version": "1.0.0", + "board": [ + "esp32", + "arduino", + "arduinonano", + "firebeetleesp32e" + ], + "main": "main.ts" + } + } +} \ No newline at end of file diff --git a/emakefun-emmd40-thirdex-V1.0.0.mpext b/emakefun-emmd40-thirdex-V1.0.0.mpext new file mode 100644 index 0000000000000000000000000000000000000000..a12d6f39925cc1c00ef127f6c8a644d5a2a0fefb Binary files /dev/null and b/emakefun-emmd40-thirdex-V1.0.0.mpext differ