diff --git a/.github/ISSUE_TEMPLATE/bug-report.yml b/.github/ISSUE_TEMPLATE/bug-report.yml index 594a2c5..1406bfb 100644 --- a/.github/ISSUE_TEMPLATE/bug-report.yml +++ b/.github/ISSUE_TEMPLATE/bug-report.yml @@ -76,7 +76,7 @@ body: label: Issue checklist description: Please double-check that you have done each of the following things before submitting the issue. options: - - label: I searched for previous reports in [the issue tracker](https://github.com/m5stack/M5Stack/issues?q=) + - label: I searched for previous reports in [the issue tracker](https://github.com/m5stack/StackChan-BSP/issues?q=) required: true - label: My report contains all necessary details required: true diff --git a/.github/workflows/clang-format-check.yml b/.github/workflows/clang-format-check.yml index dc9e340..980303e 100644 --- a/.github/workflows/clang-format-check.yml +++ b/.github/workflows/clang-format-check.yml @@ -39,12 +39,12 @@ jobs: strategy: matrix: path: - - check: './' # path to include - exclude: '' # path to exclude - #- check: 'src' - # exclude: '(Fonts)' # Exclude file paths containing "Fonts" - #- check: 'examples' - # exclude: '' + # - check: './' # path to include + # exclude: '' # path to exclude + - check: 'src' + exclude: 'src/(utils|drivers)' + - check: 'examples' + exclude: '' steps: - name: Checkout diff --git a/README.md b/README.md index 2be4654..2ade382 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ -# Product Name +# StackChan ## Overview -### SKU:xxx +### SKU:K151 Description of the product @@ -12,7 +12,9 @@ Description of the product ## Required Libraries: -- [Adafruit_BMP280_Library](https://github.com/adafruit/Required_Libraries_Link) +- [M5Unified](https://github.com/m5stack/M5Unified) +- [IRremoteESP8266](https://github.com/crankyoldgit/irremoteesp8266) +- [M5Unit-NFC](https://github.com/m5stack/M5Unit-NFC) ## License diff --git a/examples/INA226/INA226.ino b/examples/INA226/INA226.ino new file mode 100644 index 0000000..3613e8f --- /dev/null +++ b/examples/INA226/INA226.ino @@ -0,0 +1,29 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include +#include + +void setup() +{ + /* Init StackChan */ + M5StackChan.begin(); + + /* Setup display */ + M5StackChan.Display().setTextSize(2); + M5StackChan.Display().setTextColor(TFT_GREENYELLOW); + M5StackChan.Display().setTextScroll(true); +} + +void loop() +{ + /* Get battery info from INA226 */ + float voltage = M5StackChan.getBatteryVoltage(); + float current = M5StackChan.getBatteryCurrent() * 1000; + + M5StackChan.Display().printf("> Bat: %0.2fV %0.2fmA\n", voltage, current); + + delay(1000); +} diff --git a/examples/IR/Receive/Receive.ino b/examples/IR/Receive/Receive.ino new file mode 100644 index 0000000..e6c451d --- /dev/null +++ b/examples/IR/Receive/Receive.ino @@ -0,0 +1,77 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +// More examples on: https://github.com/crankyoldgit/IRremoteESP8266/tree/master/examples +#include +#include +#include +#include +#include +#include +#include +#include + +const uint16_t kRecvPin = 10; // IR rx pin + +const uint16_t kCaptureBufferSize = 1024; + +#if DECODE_AC +// Some A/C units have gaps in their protocols of ~40ms. e.g. Kelvinator +// A value this large may swallow repeats of some protocols +const uint8_t kTimeout = 50; +#else // DECODE_AC +// Suits most messages, while not swallowing many repeats. +const uint8_t kTimeout = 15; +#endif // DECODE_AC + +const uint16_t kMinUnknownSize = 12; +const uint8_t kTolerancePercentage = kTolerance; // kTolerance is normally 25% + +IRrecv irrecv(kRecvPin, kCaptureBufferSize, kTimeout, true); +decode_results results; + +void setup() +{ + M5StackChan.begin(); + Serial.begin(115200); + + // Perform a low level sanity checks that the compiler performs bit field + // packing as we expect and Endianness is as we expect. + assert(irutils::lowLevelSanityCheck() == 0); + +#if DECODE_HASH + // Ignore messages with less than minimum on or off pulses. + irrecv.setUnknownThreshold(kMinUnknownSize); +#endif // DECODE_HASH + irrecv.setTolerance(kTolerancePercentage); // Override the default tolerance. + irrecv.enableIRIn(); // Start the receiver +} + +void loop() +{ + // Check if the IR code has been received. + if (irrecv.decode(&results)) { + // Display a crude timestamp. + uint32_t now = millis(); + Serial.printf(D_STR_TIMESTAMP " : %06u.%03u\n", now / 1000, now % 1000); + // Check if we got an IR message that was to big for our capture buffer. + if (results.overflow) Serial.printf(D_WARN_BUFFERFULL "\n", kCaptureBufferSize); + // Display the library version the message was captured with. + Serial.println(D_STR_LIBRARY " : v" _IRREMOTEESP8266_VERSION_STR "\n"); + // Display the tolerance percentage if it has been change from the default. + if (kTolerancePercentage != kTolerance) Serial.printf(D_STR_TOLERANCE " : %d%%\n", kTolerancePercentage); + // Display the basic output of what we found. + Serial.print(resultToHumanReadableBasic(&results)); + // Display any extra A/C info if we have it. + String description = IRAcUtils::resultAcToString(&results); + if (description.length()) Serial.println(D_STR_MESGDESC ": " + description); + yield(); // Feed the WDT as the text output can take a while to print. + // Output the results as source code + Serial.println(resultToSourceCode(&results)); + Serial.println(); // Blank line between entries + yield(); // Feed the WDT (again) + } + delay(50); +} diff --git a/examples/IR/Send/Send.ino b/examples/IR/Send/Send.ino new file mode 100644 index 0000000..4086646 --- /dev/null +++ b/examples/IR/Send/Send.ino @@ -0,0 +1,47 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +// More examples on: https://github.com/crankyoldgit/IRremoteESP8266/tree/master/examples +#include +#include +#include +#include + +const uint16_t kIrLed = 5; // IR tx pin + +IRsend irsend(kIrLed); // Set the GPIO to be used to sending the message. + +// Example of data captured by IRrecvDumpV2.ino +uint16_t rawData[67] = {9000, 4500, 650, 550, 650, 1650, 600, 550, 650, 550, 600, 1650, 650, 550, + 600, 1650, 650, 1650, 650, 1650, 600, 550, 650, 1650, 650, 1650, 650, 550, + 600, 1650, 650, 1650, 650, 550, 650, 550, 650, 1650, 650, 550, 650, 550, + 650, 550, 600, 550, 650, 550, 650, 550, 650, 1650, 600, 550, 650, 1650, + 650, 1650, 650, 1650, 650, 1650, 650, 1650, 650, 1650, 600}; +// Example Samsung A/C state captured from IRrecvDumpV2.ino +uint8_t samsungState[kSamsungAcStateLength] = {0x02, 0x92, 0x0F, 0x00, 0x00, 0x00, 0xF0, + 0x01, 0xE2, 0xFE, 0x71, 0x40, 0x11, 0xF0}; + +void setup() +{ + M5StackChan.begin(); + Serial.begin(115200); + irsend.begin(); +} + +void loop() +{ + Serial.println("NEC"); + irsend.sendNEC(0x00FFE01FUL); + delay(2000); + Serial.println("Sony"); + irsend.sendSony(0xa90, 12, 2); // 12 bits & 2 repeats + delay(2000); + Serial.println("a rawData capture from IRrecvDumpV2"); + irsend.sendRaw(rawData, 67, 38); // Send a raw data capture at 38kHz. + delay(2000); + Serial.println("a Samsung A/C state from IRrecvDumpV2"); + irsend.sendSamsungAC(samsungState); + delay(2000); +} diff --git a/examples/NFC/Detect/Detect.ino b/examples/NFC/Detect/Detect.ino new file mode 100644 index 0000000..1629fb3 --- /dev/null +++ b/examples/NFC/Detect/Detect.ino @@ -0,0 +1,75 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +// More examples on: https://github.com/m5stack/M5Unit-NFC/tree/main/examples +#include +#include +#include +#include +#include +#include + +using namespace m5::nfc::a; + +namespace { +auto& lcd = M5.Display; +m5::unit::UnitUnified Units; +m5::unit::UnitNFC unit{}; // I2C +m5::nfc::NFCLayerA nfc_a{unit}; +} // namespace + +void setup() +{ + M5StackChan.begin(); + + if (!Units.add(unit, M5.In_I2C) || !Units.begin()) { + M5_LOGE("Failed to begin"); + lcd.clear(TFT_RED); + while (true) { + m5::utility::delay(10000); + } + } + M5_LOGI("M5UnitUnified has been begun"); + M5_LOGI("%s", Units.debugInfo().c_str()); + + if (lcd.width() < lcd.height()) { + lcd.setRotation(1); + } + lcd.setFont(&fonts::Font0); + lcd.fillScreen(0); + lcd.setCursor(0, 0); +} + +void loop() +{ + M5StackChan.update(); + Units.update(); + + std::vector piccs; + if (nfc_a.detect(piccs)) { + lcd.fillScreen(0); + lcd.setCursor(0, 0); + uint16_t idx{}; + for (auto&& u : piccs) { + M5.Speaker.tone(6000, 5); + // detect only performs a provisional classification based on sak, so further identification is required + if (nfc_a.identify(u)) { + M5.Log.printf("PICC:%s %s %04X/%02X %u/%u\n", u.uidAsString().c_str(), u.typeAsString().c_str(), u.atqa, + u.sak, u.userAreaSize(), u.totalSize()); + lcd.printf("[%2u]:PICC:<%s> %s\n", idx, u.uidAsString().c_str(), u.typeAsString().c_str()); + ++idx; + } else { + M5_LOGW("Failed to identify %s %s %04X/%02X %u/%u", u.uidAsString().c_str(), u.typeAsString().c_str(), + u.atqa, u.sak, u.userAreaSize(), u.totalSize()); + } + } + if (idx) { + M5.Speaker.tone(3000, 10); + lcd.printf("==> %u PICC\n", idx); + M5.Log.printf("==> %u PICC\n", idx); + } + nfc_a.deactivate(); + } +} diff --git a/examples/NFC/Emulation/Emulation.ino b/examples/NFC/Emulation/Emulation.ino new file mode 100644 index 0000000..29b81b0 --- /dev/null +++ b/examples/NFC/Emulation/Emulation.ino @@ -0,0 +1,133 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +// More examples on: https://github.com/m5stack/M5Unit-NFC/tree/main/examples +#include +#include +#include +#include +#include +#include + +using namespace m5::nfc; +using namespace m5::nfc::a; +using namespace m5::nfc::a::mifare; +using namespace m5::nfc::a::mifare::classic; + +namespace { +auto& lcd = M5.Display; +m5::unit::UnitUnified Units; +m5::unit::UnitNFC unit{}; // I2C +m5::nfc::EmulationLayerA emu_a{unit}; + +PICC picc{}; + +constexpr Type type{Type::MIFARE_Ultralight}; +constexpr uint8_t uid[] = {0x04, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0xDE}; +uint8_t picc_memory[] = { + 0x00, 0x00, 0x00, 0x00, // + 0x00, 0x00, 0x00, 0x00, // + 0x00, 0xA3, 0x00, 0x00, // + 0xE1, 0x10, 0x06, 0x00, // + 0x03, 0x25, 0x91, 0x01, // + 0x0D, 0x55, 0x04, 0x6D, // + 0x35, 0x73, 0x74, 0x61, // + 0x63, 0x6B, 0x2E, 0x63, // + 0x6F, 0x6D, 0x2F, 0x51, // + 0x01, 0x10, 0x54, 0x02, // + 0x65, 0x6E, 0x48, 0x65, // + 0x6C, 0x6C, 0x6F, 0x20, // + 0x4D, 0x35, 0x53, 0x74, // + 0x61, 0x63, 0x6B, 0xFE, // + 0x44, 0x45, 0x46, 0x00, // + 0x44, 0x45, 0x46, 0x00, // +}; + +uint8_t bcc8(const uint8_t* p, const uint8_t len, const uint8_t init = 0) +{ + uint8_t v = init; + for (uint_fast8_t i = 0; i < len; ++i) { + v ^= p[i]; + } + return v; +} + +// Correctly embed the Ultralight and NTAG UIDs into memory +void embed_uid(uint8_t mem[9], const uint8_t uid[7]) +{ + memcpy(mem, uid, 3); + mem[3] = bcc8(uid, 3, 0x88 /* CT */); + memcpy(mem + 4, uid + 3, 4); + mem[8] = bcc8(uid + 3, 4); +} + +constexpr uint16_t color_table[] = { + // None, Off, Idle, Ready, Active, Halt }; + TFT_BLACK, TFT_RED, TFT_BLUE, TFT_YELLOW, TFT_GREEN, TFT_MAGENTA}; +constexpr const char* state_table[] = {"-", "O", "I", "R", "A", "H"}; + +} // namespace + +void setup() +{ + M5StackChan.begin(); + + // Emulation settings + auto cfg = unit.config(); + cfg.emulation = true; + cfg.mode = NFC::A; + unit.config(cfg); + + if (!Units.add(unit, M5.In_I2C) || !Units.begin()) { + M5_LOGE("Failed to begin"); + lcd.clear(TFT_RED); + while (true) { + m5::utility::delay(10000); + } + } + M5_LOGI("M5UnitUnified has been begun"); + M5_LOGI("%s", Units.debugInfo().c_str()); + + if (lcd.width() < lcd.height()) { + lcd.setRotation(1); + } + lcd.setFont(&fonts::Font2); + + // + lcd.startWrite(); + lcd.fillScreen(TFT_RED); + if (picc.emulate(type, uid, sizeof(uid))) { + embed_uid(picc_memory, uid); + if (emu_a.begin(picc, picc_memory, sizeof(picc_memory))) { + lcd.fillScreen(TFT_DARKGREEN); + lcd.setCursor(0, 16); + const auto& e_picc = emu_a.emulatePICC(); + M5.Log.printf("Emulation:%s %s ATQA:%04X SAK:%u\n", e_picc.typeAsString().c_str(), + e_picc.uidAsString().c_str(), e_picc.atqa, e_picc.sak); + lcd.printf("%s\n%s\nATQA:%04X SAK:%u", e_picc.typeAsString().c_str(), e_picc.uidAsString().c_str(), + e_picc.atqa, e_picc.sak); + } + } + lcd.fillRect(0, 0, 32, 16, color_table[0]); + lcd.drawString(state_table[0], 0, 0); + lcd.endWrite(); +} + +void loop() +{ + M5StackChan.update(); + Units.update(); + emu_a.update(); // Need call in loop + + static EmulationLayerA::State latest{}; + auto state = emu_a.state(); + if (latest != state) { + latest = state; + lcd.startWrite(); + lcd.fillRect(0, 0, 32, 16, color_table[m5::stl::to_underlying(state)]); + lcd.drawString(state_table[m5::stl::to_underlying(state)], 0, 0); + lcd.endWrite(); + } +} diff --git a/examples/RGB_LED/RGB_LED.ino b/examples/RGB_LED/RGB_LED.ino new file mode 100644 index 0000000..dd8ddbe --- /dev/null +++ b/examples/RGB_LED/RGB_LED.ino @@ -0,0 +1,35 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include +#include + +struct Color_t { + uint8_t r = 0; + uint8_t g = 0; + uint8_t b = 0; +}; + +static std::vector colors = { + {0, 0, 0}, {168, 0, 0}, {0, 168, 0}, {0, 0, 168}, {168, 168, 0}, {168, 0, 168}, {0, 168, 168}, {168, 168, 168}, +}; + +void setup() +{ + /* Init StackChan */ + M5StackChan.begin(); +} + +void loop() +{ + /* There are 12 RGB LEDs, index 0-5 are on the left, 6-11 are on the right */ + for (int color_index = 0; color_index < colors.size(); color_index++) { + for (int led_index = 0; led_index < 12; led_index++) { + M5StackChan.setRgbColor(led_index, colors[color_index].r, colors[color_index].g, colors[color_index].b); + M5StackChan.refreshRgb(); + delay(1000 / 24); + } + } +} diff --git a/examples/Servo/BasicMovement/BasicMovement.ino b/examples/Servo/BasicMovement/BasicMovement.ino new file mode 100644 index 0000000..4461481 --- /dev/null +++ b/examples/Servo/BasicMovement/BasicMovement.ino @@ -0,0 +1,138 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include +#include + +void setup() +{ + /* Init StackChan */ + M5StackChan.begin(); + + /* Setup display */ + M5StackChan.Display().setTextSize(2); + M5StackChan.Display().setTextScroll(true); + M5StackChan.Display().setTextColor(TFT_YELLOW); + M5StackChan.Display().printf("> Touch the top to start\n"); + M5StackChan.Display().setTextColor(TFT_GREENYELLOW); + + // Set to false if high-frequency updates are needed + // M5StackChan.Motion.setAutoAngleSyncEnabled(false); +} + +void loop() +{ + M5StackChan.update(); + if (M5StackChan.TouchSensor.wasPressed()) { + delay(200); + + /* Angle unit: 10 = 1 degrees, Speed range: 0~1000 */ + /* Range X: -1280 ~ 1280 (-128° ~ 128°), Range Y: 0 ~ 900 (0° ~ 90°) */ + + /* Move to home position (0, 0) */ + M5StackChan.Motion.goHome(); + M5StackChan.Display().printf("> Go home\n"); + delay(2000); + + /* Move X servo to 100° */ + M5StackChan.Motion.moveX(1000, 200); + M5StackChan.Display().printf("> Turn Left (Slow: 200)\n"); + delay(2000); + + /* Move X servo to -100° */ + M5StackChan.Motion.moveX(-1000, 200); + M5StackChan.Display().printf("> Turn Right (Slow: 200)\n"); + delay(2000); + + /* Move X servo to 100° */ + M5StackChan.Motion.moveX(1000, 800); + M5StackChan.Display().printf("> Turn Left (Fast: 800)\n"); + delay(2000); + + /* Move X servo to -100° */ + M5StackChan.Motion.moveX(-1000, 800); + M5StackChan.Display().printf("> Turn Right (Fast: 800)\n"); + delay(2000); + + M5StackChan.Motion.goHome(); + M5StackChan.Display().printf("> Go home\n"); + delay(2000); + + /* Move Y servo to 90° */ + M5StackChan.Motion.moveY(900, 200); + M5StackChan.Display().printf("> Look Up (Slow: 200)\n"); + delay(2000); + + /* Move Y servo to 0° */ + M5StackChan.Motion.moveY(0, 200); + M5StackChan.Display().printf("> Look Down (Slow: 200)\n"); + delay(2000); + + /* Move Y servo to 90° */ + M5StackChan.Motion.moveY(900, 800); + M5StackChan.Display().printf("> Look Up (Fast: 800)\n"); + delay(2000); + + /* Move Y servo to 0° */ + M5StackChan.Motion.moveY(0, 800); + M5StackChan.Display().printf("> Look Down (Fast: 800)\n"); + delay(2000); + + /* Move X servo to 60°, Y servo to 70° */ + M5StackChan.Motion.move(600, 700); + M5StackChan.Display().printf("> Top Left\n"); + delay(2000); + + M5StackChan.Motion.goHome(); + M5StackChan.Display().printf("> Go home\n"); + delay(2000); + + /* Move X servo to -60°, Y servo to 70° */ + M5StackChan.Motion.move(-600, 700); + M5StackChan.Display().printf("> Top Right\n"); + delay(2000); + + M5StackChan.Motion.goHome(); + M5StackChan.Display().printf("> Go home\n"); + delay(2000); + + /* Only X axis supports continuous 360° rotation. Y axis does not. */ + /* Velocity range: -1000 ~ 1000 (Negative: CW, Positive: CCW) */ + + /* Rotate clockwise */ + M5StackChan.Motion.rotateX(-300); + M5StackChan.Display().printf("> Rotate clockwise (Slow: 300)\n"); + delay(3000); + + /* Rotate clockwise */ + M5StackChan.Motion.rotateX(-800); + M5StackChan.Display().printf("> Rotate clockwise (Fast: 800)\n"); + delay(3000); + + M5StackChan.Motion.goHome(); + M5StackChan.Display().printf("> Go home\n"); + delay(2000); + + /* Rotate counter-clockwise */ + M5StackChan.Motion.rotateX(300); + M5StackChan.Display().printf("> Rotate counter-clockwise (Slow: 300)\n"); + delay(3000); + + /* Rotate counter-clockwise */ + M5StackChan.Motion.rotateX(800); + M5StackChan.Display().printf("> Rotate counter-clockwise (Fast: 800)\n"); + delay(3000); + + M5StackChan.Motion.goHome(); + M5StackChan.Display().printf("> Go home\n"); + delay(2000); + + M5StackChan.Display().setTextColor(TFT_YELLOW); + M5StackChan.Display().printf("> Touch the top to start\n"); + M5StackChan.Display().setTextColor(TFT_GREENYELLOW); + } + + delay(100); +} diff --git a/examples/Servo/Dance/Dance.ino b/examples/Servo/Dance/Dance.ino new file mode 100644 index 0000000..752fa36 --- /dev/null +++ b/examples/Servo/Dance/Dance.ino @@ -0,0 +1,111 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include +#include + +/** + * @brief + * Angle unit: 10 = 1 degrees, Speed range: 0~1000 + * Range X: -1280 ~ 1280 (-128° ~ 128°), Range Y: 0 ~ 900 (0° ~ 90°) + * + */ +struct Keyframe_t { + int x = 0; + int y = 0; + int speed = 500; + uint32_t interval = 0; +}; + +/* Move around */ +std::vector dance_1 = { + {0, 0, 500, 1000}, // Home, 1s + {600, 200, 800, 500}, // Left, fast + {-600, 200, 800, 500}, // Right, fast + {600, 200, 800, 500}, // Left, fast + {-600, 200, 800, 500}, // Right, fast + {0, 800, 900, 400}, // Look Up + {0, 0, 900, 400}, // Look Down + {0, 800, 900, 400}, // Look Up + {0, 0, 900, 400}, // Look Down + {800, 700, 700, 500}, // Top Left + {-800, 700, 700, 500}, // Top Right + {800, 700, 700, 500}, // Top Left + {-800, 700, 700, 500}, // Top Right + {0, 0, 500, 1000} // Back Home +}; + +/* Shake */ +std::vector dance_2 = { + {0, 0, 500, 1000}, // Home + {300, 0, 750, 250}, // Shake Left + {-300, 0, 750, 250}, // Shake Right + {300, 0, 750, 250}, // Shake Left + {-300, 0, 750, 250}, // Shake Right + {300, 0, 750, 250}, // Shake Left + {-300, 0, 750, 250}, // Shake Right + {0, 0, 500, 1000} // Back Home +}; + +/* Nod */ +std::vector dance_3 = { + {0, 0, 500, 1000}, // Home + {0, 350, 900, 250}, // Nod Up + {0, 0, 900, 250}, // Nod Down + {0, 350, 900, 250}, // Nod Up + {0, 0, 900, 250}, // Nod Down + {0, 350, 900, 250}, // Nod Up + {0, 0, 900, 250}, // Nod Down + {0, 0, 500, 500} // Back Home +}; + +std::vector*> dances = { + &dance_1, + &dance_2, + &dance_3, +}; + +static int dance_index = 0; + +void setup() +{ + /* Init StackChan */ + M5StackChan.begin(); + + /* Setup display */ + M5StackChan.Display().setTextSize(2); + M5StackChan.Display().setTextScroll(true); + M5StackChan.Display().setTextColor(TFT_YELLOW); + M5StackChan.Display().printf("> Touch the top to start\n"); + M5StackChan.Display().setTextColor(TFT_GREENYELLOW); + + // Disable auto angle sync for smooth and continuous movement + M5StackChan.Motion.setAutoAngleSyncEnabled(false); +} + +void loop() +{ + M5StackChan.update(); + if (M5StackChan.TouchSensor.wasPressed()) { + delay(200); + + /* Perform dance */ + M5StackChan.Display().printf("> Dance #%d!\n", dance_index + 1); + for (const auto& frame : *dances[dance_index]) { + M5StackChan.Motion.move(frame.x, frame.y, frame.speed); + delay(frame.interval); + } + M5StackChan.Display().printf("> Finished!\n"); + + /* Next dance */ + dance_index = (dance_index + 1) % dances.size(); + + M5StackChan.Display().setTextColor(TFT_YELLOW); + M5StackChan.Display().printf("> Touch the top to start\n"); + M5StackChan.Display().setTextColor(TFT_GREENYELLOW); + } + + delay(100); +} diff --git a/examples/Servo/HomeCalibration/HomeCalibration.ino b/examples/Servo/HomeCalibration/HomeCalibration.ino new file mode 100644 index 0000000..42244b9 --- /dev/null +++ b/examples/Servo/HomeCalibration/HomeCalibration.ino @@ -0,0 +1,111 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include +#include + +namespace { + +constexpr uint16_t kBackgroundColor = TFT_BLACK; +constexpr uint16_t kBorderColor = TFT_WHITE; +constexpr uint16_t kTopButtonColor = 0x39C7; +constexpr uint16_t kTopButtonPressedColor = 0x2204; +constexpr uint16_t kBottomButtonColor = 0x03EF; +constexpr uint16_t kBottomButtonPressedColor = 0x01E8; +constexpr uint16_t kTextColor = TFT_WHITE; + +enum class ButtonZone { + None, + SetHome, + GoHome, +}; + +ButtonZone pressed_zone = ButtonZone::None; + +ButtonZone getButtonZone(const int16_t y, const int16_t height) +{ + return y < (height / 2) ? ButtonZone::SetHome : ButtonZone::GoHome; +} + +void drawButton(const int16_t x, const int16_t y, const int16_t w, const int16_t h, const uint16_t color, + const char* line_1, const char* line_2) +{ + auto& display = M5StackChan.Display(); + + display.fillRect(x, y, w, h, color); + display.drawRect(x, y, w, h, kBorderColor); + + display.setTextDatum(middle_center); + display.setTextColor(kTextColor, color); + display.setTextSize(2); + display.drawString(line_1, x + w / 2, y + h / 2 - 12); + display.drawString(line_2, x + w / 2, y + h / 2 + 12); +} + +void drawUi(ButtonZone active_zone) +{ + auto& display = M5StackChan.Display(); + const int16_t width = display.width(); + const int16_t height = display.height(); + const int16_t gap = 8; + const int16_t button_x = 8; + const int16_t button_w = width - button_x * 2; + const int16_t half_h = (height - gap) / 2; + + display.startWrite(); + display.fillScreen(kBackgroundColor); + display.fillRect(0, half_h, width, gap, kBackgroundColor); + + drawButton(button_x, 8, button_w, half_h - 12, + active_zone == ButtonZone::SetHome ? kTopButtonPressedColor : kTopButtonColor, "set current postion", + "as home"); + + drawButton(button_x, half_h + gap + 4, button_w, height - (half_h + gap + 12), + active_zone == ButtonZone::GoHome ? kBottomButtonPressedColor : kBottomButtonColor, "move to", "home"); + + display.endWrite(); +} + +} // namespace + +void setup() +{ + /* Init StackChan */ + M5StackChan.begin(); + + /* Setup display */ + M5StackChan.Display().setTextScroll(false); + drawUi(ButtonZone::None); +} + +void loop() +{ + M5StackChan.update(); + auto& display = M5StackChan.Display(); + const int16_t screen_height = display.height(); + int16_t touch_x = 0; + int16_t touch_y = 0; + const bool touching = display.getTouch(&touch_x, &touch_y); + + if (touching) { + const ButtonZone current_zone = getButtonZone(touch_y, screen_height); + if (current_zone != pressed_zone) { + pressed_zone = current_zone; + drawUi(pressed_zone); + } + } else if (pressed_zone != ButtonZone::None) { + const ButtonZone released_zone = pressed_zone; + pressed_zone = ButtonZone::None; + drawUi(ButtonZone::None); + + if (released_zone == ButtonZone::SetHome) { + M5StackChan.Motion.setCurrentPostionAsHome(); + } else if (released_zone == ButtonZone::GoHome) { + M5StackChan.Motion.goHome(); + } + } + + delay(16); +} diff --git a/examples/TouchSensor/TouchSensor.ino b/examples/TouchSensor/TouchSensor.ino new file mode 100644 index 0000000..ca7f07b --- /dev/null +++ b/examples/TouchSensor/TouchSensor.ino @@ -0,0 +1,42 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include +#include + +void setup() +{ + /* Init StackChan */ + M5StackChan.begin(); + + /* Setup display */ + M5StackChan.Display().setTextSize(2); + M5StackChan.Display().setTextScroll(true); + M5StackChan.Display().setTextColor(TFT_YELLOW); + M5StackChan.Display().printf("> Touch or swipe the top\n"); + M5StackChan.Display().setTextColor(TFT_GREENYELLOW); +} + +void loop() +{ + /* Update touch sensor */ + M5StackChan.update(); + + auto& ts = M5StackChan.TouchSensor; + + if (ts.wasClicked()) { + M5StackChan.Display().printf("> Was clicked\n"); + } + + if (ts.wasSwipedForward()) { + M5StackChan.Display().printf("> Was swiped forward\n"); + } + + if (ts.wasSwipedBackward()) { + M5StackChan.Display().printf("> Was swiped backward\n"); + } + + delay(50); +} diff --git a/library.json b/library.json new file mode 100644 index 0000000..61dd35e --- /dev/null +++ b/library.json @@ -0,0 +1,21 @@ +{ + "name": "M5StackChan", + "description": "Library for M5StackChan", + "keywords": "M5Stack,M5StackChan", + "authors": { + "name": "M5Stack" + }, + "repository": { + "type": "git", + "url": "https://github.com/M5Stack/M5StackChan.git" + }, + "dependencies": { + "M5Unified": "*", + "M5GFX": "*", + "IRremoteESP8266": "*", + "M5Unit-NFC": "*" + }, + "version": "1.0.0", + "frameworks": "arduino", + "platforms": "espressif32" +} \ No newline at end of file diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..3e90e6c --- /dev/null +++ b/library.properties @@ -0,0 +1,11 @@ +name=M5StackChan +version=1.0.0 +author=M5Stack +maintainer=M5Stack +sentence=M5StackChan is a library for M5StackChan +paragraph= +category=Device Control +url=https://github.com/m5stack/M5StackChan.git +architectures=esp32 +includes=src,src/utils +depends=M5Unified,IRremoteESP8266,M5Unit-NFC diff --git a/src/M5StackChan.cpp b/src/M5StackChan.cpp new file mode 100644 index 0000000..6940ea1 --- /dev/null +++ b/src/M5StackChan.cpp @@ -0,0 +1,323 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include "M5StackChan.h" +#include "drivers/PY32IOExpander/PY32IOExpander.hpp" +#include "drivers/SCServo_lib/src/SCSCL.h" +#include "utils/compat/make_unique.h" +#include "utils/settings/settings.h" +#include "utility/power/INA226_Class.hpp" +#include + +using namespace m5; + +M5StackChan_Class M5StackChan; + +static const char* TAG = "M5StackChan"; + +void M5StackChan_Class::begin() +{ + M5.begin(); + TouchSensor.begin(); + io_expander_init(); + servo_init(); + ina226_init(); +} + +void M5StackChan_Class::update() +{ + M5.update(); + TouchSensor.update(); +} + +/* -------------------------------------------------------------------------- */ +/* IO Expander */ +/* -------------------------------------------------------------------------- */ +std::unique_ptr _io_expander; + +void M5StackChan_Class::io_expander_init() +{ + _io_expander = std::make_unique(); + + // PY32 IO Expander may boot slowly, wait for it + uint32_t start_tick = millis(); + while (1) { + delay(200); + + if (millis() - start_tick > 1200) { + ESP_LOGE(TAG, "IO expander init timeout"); + _io_expander.reset(); + break; + } + + if (_io_expander->begin()) { + break; + } + } + + if (_io_expander) { + // VM EN + _io_expander->setDirection(0, true); // Output + _io_expander->setPullMode(0, true); // Pull-up + setServoPowerEnabled(true); + delay(200); + + // RGB + _io_expander->setDirection(13, true); // Output + _io_expander->setPullMode(13, true); // Pull-up + _io_expander->setDriveMode(13, false); // Push-pull + _io_expander->setLedCount(12); + delay(200); + showRgbColor(0, 0, 0); + delay(50); + showRgbColor(0, 0, 0); + } +} + +void M5StackChan_Class::setServoPowerEnabled(bool enabled) +{ + if (!_io_expander) { + return; + } + _io_expander->digitalWrite(0, enabled ? true : false); +} + +void M5StackChan_Class::setRgbColor(uint8_t index, uint8_t r, uint8_t g, uint8_t b) +{ + if (!_io_expander) { + return; + } + _io_expander->setLedColor(index, r, g, b); +} + +void M5StackChan_Class::refreshRgb() +{ + if (!_io_expander) { + return; + } + _io_expander->refreshLeds(); +} + +void M5StackChan_Class::showRgbColor(uint8_t r, uint8_t g, uint8_t b) +{ + for (int i = 0; i < 12; i++) { + setRgbColor(i, r, g, b); + } + refreshRgb(); +} + +/* -------------------------------------------------------------------------- */ +/* Servo */ +/* -------------------------------------------------------------------------- */ +static SCSCL _scs_bus; + +struct ServoConfig_t { + int id = -1; + int defaultZeroPos = 0; + uitk_intl::Vector2i angleLimit; + uitk_intl::Vector2i rawPosLimit; + std::string settingNs; + std::string settingZeroPositionKey; + bool enablePwmMode = false; +}; + +/** + * @brief Servo class implement + * + */ +class ScsServo : public stackchan::motion::Servo { +public: + static inline const std::string _tag = "ScsServo"; + + ScsServo(const ServoConfig_t& config) : _config(config) + { + } + + void init() override + { + set_angle_limit(_config.angleLimit); + get_zero_pos_from_nvs(); + Servo::init(); + } + + void get_zero_pos_from_nvs() + { + _zero_pos = _config.defaultZeroPos; + bool is_valid = false; + + { + Settings settings(_config.settingNs, false); + int nvs_zero_pos = settings.GetInt(_config.settingZeroPositionKey, -1); + + // Limit check + if (nvs_zero_pos >= _config.rawPosLimit.x && nvs_zero_pos <= _config.rawPosLimit.y) { + _zero_pos = nvs_zero_pos; + is_valid = true; + ESP_LOGI(TAG, "Servo ID: %d get zero pos: %d from settings", _config.id, _zero_pos); + } else { + is_valid = false; + ESP_LOGW(TAG, "Servo ID: %d get invalid zero pos: %d from settings", _config.id, nvs_zero_pos); + } + } + + if (!is_valid) { + _zero_pos = _config.defaultZeroPos; + ESP_LOGI(TAG, "Servo ID: %d override zero pos to default: %d", _config.id, _zero_pos); + + Settings settings(_config.settingNs, true); + settings.SetInt(_config.settingZeroPositionKey, _zero_pos); + } + } + + void set_angle_impl(int angle) override + { + int mapped_angle = _zero_pos + angle * 16 / 5 / 10; // 一步对应 0.3125度, 0.3125 = 5/16 + mapped_angle = uitk_intl::clamp(mapped_angle, _config.rawPosLimit.x, _config.rawPosLimit.y); + + // ESP_LOGI(TAG, "Servo ID: %d mapped angle: %d", _config.id, mapped_angle); + + check_mode(Mode::Position); + _scs_bus.WritePos(_config.id, mapped_angle, 20, 0); + } + + int getCurrentAngle() override + { + int current_pos = _scs_bus.ReadPos(_config.id); + int angle = (current_pos - _zero_pos) * 5 * 10 / 16; + angle = uitk_intl::clamp(angle, getAngleLimit().x, getAngleLimit().y); + // ESP_LOGI(TAG, "Servo ID: %d current pos: %d angle: %d", _id, current_pos, angle); + return angle; + } + + bool is_moving_impl() override + { + int moving = _scs_bus.ReadMove(_config.id); + // ESP_LOGI(TAG, "Servo ID: %d moving: %d", _id, moving); + return moving != 0; + } + + void setTorqueEnabled(bool enabled) override + { + Servo::setTorqueEnabled(enabled); + _scs_bus.EnableTorque(_config.id, enabled ? 1 : 0); + // ESP_LOGI(TAG, "Servo ID: %d set torque: %d", _id, enabled); + } + + bool getTorqueEnabled() override + { + int torque_enable = _scs_bus.ReadToqueEnable(_config.id); + // ESP_LOGI(TAG, "Servo ID: %d torque enable: %d", _id, torque_enable); + return torque_enable > 0; + } + + void setCurrentAngleAsZero() override + { + _zero_pos = _scs_bus.ReadPos(_config.id); + + Settings settings(_config.settingNs, true); + settings.SetInt(_config.settingZeroPositionKey, _zero_pos); + + ESP_LOGI(TAG, "Servo ID: %d set zero pos: %d to settings", _config.id, _zero_pos); + } + + void rotate(int velocity) override + { + velocity = uitk_intl::clamp(velocity, -1000, 1000); + + if (!_config.enablePwmMode) { + return; + } + + int mapped_velocity = uitk_intl::map_range(velocity, 0, 1000, 0, 1023); + + check_mode(Mode::PWM); + _scs_bus.WritePWM(_config.id, mapped_velocity); + } + +private: + enum class Mode { Position = 0, PWM = 1 }; + + ServoConfig_t _config; + int _zero_pos = 0; + Mode _current_mode = Mode::Position; + + void check_mode(Mode targetMode) + { + if (targetMode == _current_mode) { + return; + } + + _scs_bus.SwitchMode(_config.id, static_cast(targetMode)); + _current_mode = targetMode; + } +}; + +void M5StackChan_Class::servo_init() +{ + _scs_bus.begin(UART_NUM_1, 1000000, 6, 7); + + uitk_intl::ui_hal::on_delay([](uint32_t ms) { delay(ms); }); + uitk_intl::ui_hal::on_get_tick([]() { return millis(); }); + + ServoConfig_t yaw_servo_config; + yaw_servo_config.id = 1; + yaw_servo_config.defaultZeroPos = 460; + yaw_servo_config.angleLimit = uitk_intl::Vector2i(-1280, 1280); + yaw_servo_config.rawPosLimit = uitk_intl::Vector2i(0, 1000); + yaw_servo_config.settingNs = "servo"; + yaw_servo_config.settingZeroPositionKey = "zero_pos_1"; + yaw_servo_config.enablePwmMode = true; + + ServoConfig_t pitch_servo_config; + pitch_servo_config.id = 2; + pitch_servo_config.defaultZeroPos = 620; + pitch_servo_config.angleLimit = uitk_intl::Vector2i(0, 900); + pitch_servo_config.rawPosLimit = uitk_intl::Vector2i(0, 1000); + pitch_servo_config.settingNs = "servo"; + pitch_servo_config.settingZeroPositionKey = "zero_pos_2"; + + auto yaw_servo = std::make_unique(yaw_servo_config); + auto pitch_servo = std::make_unique(pitch_servo_config); + + Motion.init(std::move(yaw_servo), std::move(pitch_servo)); +} + +/* -------------------------------------------------------------------------- */ +/* INA226 */ +/* -------------------------------------------------------------------------- */ +std::unique_ptr _ina226; + +void M5StackChan_Class::ina226_init() +{ + _ina226 = std::make_unique(0x41); + + m5::INA226_Class::config_t config; + config.shunt_res = 0.01; + config.max_expected_current = 8.19; + _ina226->config(config); + + if (!_ina226->begin()) { + ESP_LOGE(TAG, "INA226 init failed"); + _ina226.reset(); + } +} + +float M5StackChan_Class::getBatteryVoltage() +{ + float result = 0.0f; + if (_ina226) { + result = _ina226->getBusVoltage(); + } + return result; +} + +float M5StackChan_Class::getBatteryCurrent() +{ + float result = 0.0f; + if (_ina226) { + result = _ina226->getShuntCurrent(); + } + return result; +} diff --git a/src/M5StackChan.h b/src/M5StackChan.h new file mode 100644 index 0000000..d31cf1a --- /dev/null +++ b/src/M5StackChan.h @@ -0,0 +1,87 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#pragma once +#include "utils/touch_sensor/touch_sensor.h" +#include "utils/motion/motion.h" +#include +#include +#include + +namespace m5 { + +class M5StackChan_Class { +public: + void begin(); + void update(); + + inline LGFX_Device& Display() + { + return M5.Display; + } + inline LGFX_Device& Lcd() + { + return M5.Lcd; + } + TouchSensor_Class TouchSensor; + stackchan::motion::Motion Motion; + + /** + * @brief Enable or disable servo power. + * + * @param enabled + */ + void setServoPowerEnabled(bool enabled); + + /** + * @brief Set the Rgb Color object. + * There are 12 RGB LEDs, 0-5 are on the left, 6-11 are on the right. + * + * @param index + * @param r + * @param g + * @param b + */ + void setRgbColor(uint8_t index, uint8_t r, uint8_t g, uint8_t b); + + /** + * @brief Update the RGB LEDs with the set colors. + * + */ + void refreshRgb(); + + /** + * @brief Set all RGB LEDs to the specified color. + * + * @param r + * @param g + * @param b + */ + void showRgbColor(uint8_t r, uint8_t g, uint8_t b); + + /** + * @brief Get battery voltage. + * + * @return float + */ + float getBatteryVoltage(); + + /** + * @brief Get battery current. + * Positive when discharging, negative when charging. + * + * @return float + */ + float getBatteryCurrent(); + +protected: + void io_expander_init(); + void servo_init(); + void ina226_init(); +}; + +} // namespace m5 + +extern m5::M5StackChan_Class M5StackChan; diff --git a/src/drivers/PY32IOExpander/PY32IOExpander.cpp b/src/drivers/PY32IOExpander/PY32IOExpander.cpp new file mode 100644 index 0000000..8867adf --- /dev/null +++ b/src/drivers/PY32IOExpander/PY32IOExpander.cpp @@ -0,0 +1,280 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include "PY32IOExpander.hpp" + +namespace m5 { +// Register definitions +static constexpr uint8_t REG_UID_L = 0x00; +static constexpr uint8_t REG_UID_H = 0x01; +static constexpr uint8_t REG_VERSION = 0x02; +static constexpr uint8_t REG_GPIO_M_L = 0x03; +static constexpr uint8_t REG_GPIO_M_H = 0x04; +static constexpr uint8_t REG_GPIO_O_L = 0x05; +static constexpr uint8_t REG_GPIO_O_H = 0x06; +static constexpr uint8_t REG_GPIO_I_L = 0x07; +static constexpr uint8_t REG_GPIO_I_H = 0x08; +static constexpr uint8_t REG_GPIO_PU_L = 0x09; +static constexpr uint8_t REG_GPIO_PU_H = 0x0A; +static constexpr uint8_t REG_GPIO_PD_L = 0x0B; +static constexpr uint8_t REG_GPIO_PD_H = 0x0C; +static constexpr uint8_t REG_GPIO_IE_L = 0x0D; +static constexpr uint8_t REG_GPIO_IE_H = 0x0E; +static constexpr uint8_t REG_GPIO_IT_L = 0x0F; +static constexpr uint8_t REG_GPIO_IT_H = 0x10; +static constexpr uint8_t REG_GPIO_IS_L = 0x11; +static constexpr uint8_t REG_GPIO_IS_H = 0x12; +static constexpr uint8_t REG_GPIO_DRV_L = 0x13; +static constexpr uint8_t REG_GPIO_DRV_H = 0x14; + +static constexpr uint8_t REG_ADC_CTRL = 0x15; +static constexpr uint8_t REG_ADC_D_L = 0x16; +static constexpr uint8_t REG_ADC_D_H = 0x17; + +static constexpr uint8_t REG_PWM_FREQ_L = 0x25; +static constexpr uint8_t REG_PWM_FREQ_H = 0x26; + +static constexpr uint8_t REG_LED_CFG = 0x24; +static constexpr uint8_t REG_LED_RAM_START = 0x30; + +// PWM Duty Registers +static constexpr uint8_t REG_PWM1_DUTY_L = 0x1B; +static constexpr uint8_t REG_PWM1_DUTY_H = 0x1C; +static constexpr uint8_t REG_PWM2_DUTY_L = 0x1D; +static constexpr uint8_t REG_PWM2_DUTY_H = 0x1E; +static constexpr uint8_t REG_PWM3_DUTY_L = 0x1F; +static constexpr uint8_t REG_PWM3_DUTY_H = 0x20; +static constexpr uint8_t REG_PWM4_DUTY_L = 0x21; +static constexpr uint8_t REG_PWM4_DUTY_H = 0x22; + +void PY32IOExpander_Class::_writeBit(uint8_t reg_l, uint8_t reg_h, uint8_t pin, bool value) +{ + if (pin < 8) { + if (value) + bitOn(reg_l, 1 << pin); + else + bitOff(reg_l, 1 << pin); + } else { + if (value) + bitOn(reg_h, 1 << (pin - 8)); + else + bitOff(reg_h, 1 << (pin - 8)); + } +} + +bool PY32IOExpander_Class::_readBit(uint8_t reg_l, uint8_t reg_h, uint8_t pin) +{ + if (pin < 8) { + return (readRegister8(reg_l) & (1 << pin)) != 0; + } else { + return (readRegister8(reg_h) & (1 << (pin - 8))) != 0; + } +} + +bool PY32IOExpander_Class::begin() +{ + uint8_t version = readRegister8(REG_VERSION); + if (version == 0 || version == 0xFF) return false; + return true; +} + +void PY32IOExpander_Class::setDirection(uint8_t pin, bool direction) +{ + // direction: false=input (0), true=output (1) + _writeBit(REG_GPIO_M_L, REG_GPIO_M_H, pin, direction); +} + +void PY32IOExpander_Class::enablePull(uint8_t pin, bool enablePull) +{ + if (enablePull) { + // Enable Pull Up by default if neither is set + bool pu = _readBit(REG_GPIO_PU_L, REG_GPIO_PU_H, pin); + bool pd = _readBit(REG_GPIO_PD_L, REG_GPIO_PD_H, pin); + if (!pu && !pd) { + _writeBit(REG_GPIO_PU_L, REG_GPIO_PU_H, pin, true); + } + // If one is already set, leave it. + } else { + // Disable both + _writeBit(REG_GPIO_PU_L, REG_GPIO_PU_H, pin, false); + _writeBit(REG_GPIO_PD_L, REG_GPIO_PD_H, pin, false); + } +} + +void PY32IOExpander_Class::setPullMode(uint8_t pin, bool mode) +{ + // mode: false=down, true=up + if (mode) { + // Pull Up + _writeBit(REG_GPIO_PD_L, REG_GPIO_PD_H, pin, false); + _writeBit(REG_GPIO_PU_L, REG_GPIO_PU_H, pin, true); + } else { + // Pull Down + _writeBit(REG_GPIO_PU_L, REG_GPIO_PU_H, pin, false); + _writeBit(REG_GPIO_PD_L, REG_GPIO_PD_H, pin, true); + } +} + +void PY32IOExpander_Class::setDriveMode(uint8_t pin, bool openDrain) +{ + // openDrain: false=push-pull (0), true=open-drain (1) + _writeBit(REG_GPIO_DRV_L, REG_GPIO_DRV_H, pin, openDrain); +} + +void PY32IOExpander_Class::setHighImpedance(uint8_t pin, bool enable) +{ + if (enable) { + // Input mode + setDirection(pin, false); + // Disable pulls + enablePull(pin, false); + } +} + +bool PY32IOExpander_Class::getWriteValue(uint8_t pin) +{ + return _readBit(REG_GPIO_O_L, REG_GPIO_O_H, pin); +} + +void PY32IOExpander_Class::digitalWrite(uint8_t pin, bool level) +{ + _writeBit(REG_GPIO_O_L, REG_GPIO_O_H, pin, level); +} + +bool PY32IOExpander_Class::digitalRead(uint8_t pin) +{ + return _readBit(REG_GPIO_I_L, REG_GPIO_I_H, pin); +} + +void PY32IOExpander_Class::resetIrq() +{ + // Clear all interrupts by writing 1s to IS registers + writeRegister8(REG_GPIO_IS_L, 0xFF); + writeRegister8(REG_GPIO_IS_H, 0xFF); // Only bits 0-5 used for high byte (pins 8-13) +} + +void PY32IOExpander_Class::disableIrq() +{ + // Disable all interrupts + writeRegister8(REG_GPIO_IE_L, 0x00); + writeRegister8(REG_GPIO_IE_H, 0x00); +} + +void PY32IOExpander_Class::enableIrq() +{ + // Enable all interrupts + writeRegister8(REG_GPIO_IE_L, 0xFF); + writeRegister8(REG_GPIO_IE_H, 0x3F); // Pins 8-13 +} + +uint16_t PY32IOExpander_Class::readDeviceUID() +{ + uint8_t l = readRegister8(REG_UID_L); + uint8_t h = readRegister8(REG_UID_H); + return (h << 8) | l; +} + +uint8_t PY32IOExpander_Class::readVersion() +{ + return readRegister8(REG_VERSION); +} + +uint16_t PY32IOExpander_Class::analogRead(uint8_t channel) +{ + if (channel < 1 || channel > 4) return 0; + + // Start conversion + // REG_ADC_CTRL: [7:Busy] [6:Start] [2:0:Channel] + // Channel mapping: 1->1, 2->2, 3->3, 4->4 + writeRegister8(REG_ADC_CTRL, (1 << 6) | (channel & 0x07)); + + // Wait for busy bit to clear + // Simple polling with timeout + for (int i = 0; i < 100; i++) { + uint8_t ctrl = readRegister8(REG_ADC_CTRL); + if (!(ctrl & (1 << 7))) { + break; + } + // delay? + } + + uint8_t l = readRegister8(REG_ADC_D_L); + uint8_t h = readRegister8(REG_ADC_D_H); + return (h << 8) | l; +} + +void PY32IOExpander_Class::setPwmDuty(uint8_t channel, uint8_t duty) +{ + if (channel > 3) return; + + // Calculate register address + // Channel 0 -> PWM1 (0x1B) + // Channel 1 -> PWM2 (0x1D) + // Channel 2 -> PWM3 (0x1F) + // Channel 3 -> PWM4 (0x21) + uint8_t reg_l = REG_PWM1_DUTY_L + (channel * 2); + uint8_t reg_h = reg_l + 1; + + // Duty is 8-bit percentage (0-100)? Or 0-255? + // m5_io_py32ioexpander uses percentage (0-100) or 12-bit raw. + // Let's assume 0-255 for standard Arduino style, but map to 12-bit (0-4095). + // 255 -> 4095. val * 4095 / 255 = val * 16 approx. + uint16_t duty12 = (uint16_t)duty * 16; + if (duty12 > 4095) duty12 = 4095; + + // High byte contains Enable(7) and Polarity(6) bits. + // We need to preserve them or set defaults. + // Let's enable by default, polarity normal (0). + uint8_t h_val = (duty12 >> 8) & 0x0F; + h_val |= (1 << 7); // Enable + + writeRegister8(reg_l, duty12 & 0xFF); + writeRegister8(reg_h, h_val); +} + +void PY32IOExpander_Class::setPwmFrequency(uint16_t freq) +{ + writeRegister8(REG_PWM_FREQ_L, freq & 0xFF); + writeRegister8(REG_PWM_FREQ_H, (freq >> 8) & 0xFF); +} + +void PY32IOExpander_Class::setLedCount(uint8_t count) +{ + if (count > 32) count = 32; + writeRegister8(REG_LED_CFG, count & 0x3F); +} + +void PY32IOExpander_Class::setLedColor(uint8_t index, uint16_t color565) +{ + if (index >= 32) return; + uint8_t data[2] = {(uint8_t)(color565 & 0xFF), (uint8_t)((color565 >> 8) & 0xFF)}; + writeRegister(REG_LED_RAM_START + index * 2, data, 2); +} + +void PY32IOExpander_Class::setLedColor(uint8_t index, uint8_t r, uint8_t g, uint8_t b) +{ + // RGB888 to RGB565: RRRRRGGG GGGBBBBB + uint16_t val = ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3); + setLedColor(index, val); +} + +void PY32IOExpander_Class::setLedColor(uint8_t index, uint32_t color) +{ + setLedColor(index, (uint8_t)((color >> 16) & 0xFF), (uint8_t)((color >> 8) & 0xFF), (uint8_t)(color & 0xFF)); +} + +void PY32IOExpander_Class::setLedData(const uint8_t* data, size_t len) +{ + if (!data || len == 0) return; + if (len > 64) len = 64; // Max 32 LEDs * 2 bytes + writeRegister(REG_LED_RAM_START, (uint8_t*)data, len); +} + +void PY32IOExpander_Class::refreshLeds() +{ + uint8_t val = readRegister8(REG_LED_CFG); + writeRegister8(REG_LED_CFG, val | (1 << 6)); +} +} // namespace m5 diff --git a/src/drivers/PY32IOExpander/PY32IOExpander.hpp b/src/drivers/PY32IOExpander/PY32IOExpander.hpp new file mode 100644 index 0000000..2a39dab --- /dev/null +++ b/src/drivers/PY32IOExpander/PY32IOExpander.hpp @@ -0,0 +1,77 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#ifndef __M5_PY32IOEXPANDER_CLASS_H__ +#define __M5_PY32IOEXPANDER_CLASS_H__ + +#include + +namespace m5 { +class PY32IOExpander_Class : public IOExpander_Base { +public: + static constexpr std::uint8_t DEFAULT_ADDRESS = 0x6F; + + PY32IOExpander_Class(std::uint8_t i2c_addr = DEFAULT_ADDRESS, std::uint32_t freq = 100000, + m5::I2C_Class* i2c = &m5::In_I2C) + : IOExpander_Base(i2c_addr, freq, i2c) + { + } + + bool begin(); + + // IOExpander_Base overrides + // false input, true output + void setDirection(uint8_t pin, bool direction) override; + + void enablePull(uint8_t pin, bool enablePull) override; + + // false down, true up + void setPullMode(uint8_t pin, bool mode) override; + + // false push-pull, true open-drain + void setDriveMode(uint8_t pin, bool openDrain); + + void setHighImpedance(uint8_t pin, bool enable) override; + + bool getWriteValue(uint8_t pin) override; + + void digitalWrite(uint8_t pin, bool level) override; + + bool digitalRead(uint8_t pin) override; + + void resetIrq() override; + + void disableIrq() override; + + void enableIrq() override; + + // Extended functionality + uint16_t readDeviceUID(); + uint8_t readVersion(); + + // ADC + // channel: 1-4 + uint16_t analogRead(uint8_t channel); + + // PWM + // channel: 0-3 + void setPwmDuty(uint8_t channel, uint8_t duty); + void setPwmFrequency(uint16_t freq); + + // LED + void setLedCount(uint8_t count); + void setLedColor(uint8_t index, uint16_t color565); + void setLedColor(uint8_t index, uint8_t r, uint8_t g, uint8_t b); + void setLedColor(uint8_t index, uint32_t color); + void setLedData(const uint8_t* data, size_t len); + void refreshLeds(); + +private: + void _writeBit(uint8_t reg_l, uint8_t reg_h, uint8_t pin, bool value); + bool _readBit(uint8_t reg_l, uint8_t reg_h, uint8_t pin); +}; +} // namespace m5 + +#endif diff --git a/src/drivers/SCServo_lib/CMakeLists.txt b/src/drivers/SCServo_lib/CMakeLists.txt new file mode 100644 index 0000000..3ffc85b --- /dev/null +++ b/src/drivers/SCServo_lib/CMakeLists.txt @@ -0,0 +1,12 @@ +idf_component_register( + SRCS + "src/SCS.cpp" + "src/SCSCL.cpp" + "src/SCSerial.cpp" + INCLUDE_DIRS + "src" + REQUIRES + driver + esp_timer +) + diff --git a/src/drivers/SCServo_lib/LICENSE.txt b/src/drivers/SCServo_lib/LICENSE.txt new file mode 100644 index 0000000..f288702 --- /dev/null +++ b/src/drivers/SCServo_lib/LICENSE.txt @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/src/drivers/SCServo_lib/src/INST.h b/src/drivers/SCServo_lib/src/INST.h new file mode 100644 index 0000000..187fb4b --- /dev/null +++ b/src/drivers/SCServo_lib/src/INST.h @@ -0,0 +1,40 @@ +/* + * INST.h + * FIT serial servo protocol command definition + */ + +#ifndef _INST_H +#define _INST_H + +#include + +typedef int8_t s8; +typedef uint8_t u8; +typedef uint16_t u16; +typedef int16_t s16; +typedef uint32_t u32; +typedef int32_t s32; + +#define INST_PING 0x01 +#define INST_READ 0x02 +#define INST_WRITE 0x03 +#define INST_REG_WRITE 0x04 +#define INST_REG_ACTION 0x05 +#define INST_SYNC_READ 0x82 +#define INST_SYNC_WRITE 0x83 + +// Baud rate definition +#define _1M 0 +#define _0_5M 1 +#define _250K 2 +#define _128K 3 +#define _115200 4 +#define _76800 5 +#define _57600 6 +#define _38400 7 +#define _19200 8 +#define _14400 9 +#define _9600 10 +#define _4800 11 + +#endif \ No newline at end of file diff --git a/src/drivers/SCServo_lib/src/SCS.cpp b/src/drivers/SCServo_lib/src/SCS.cpp new file mode 100644 index 0000000..7c06b5a --- /dev/null +++ b/src/drivers/SCServo_lib/src/SCS.cpp @@ -0,0 +1,369 @@ +/* + * SCS.cpp + * FIT serial servo communication layer protocol program + */ + +#include +#include "SCS.h" + +SCS::SCS() +{ + Level = 1; // All commands except broadcast commands return responses + Error = 0; +} + +SCS::SCS(u8 End) +{ + Level = 1; + this->End = End; + Error = 0; +} + +SCS::SCS(u8 End, u8 Level) +{ + this->Level = Level; + this->End = End; + Error = 0; +} + +// Split a 16-bit number into two 8-bit numbers +// DataL is the low bit, DataH is the high bit +void SCS::Host2SCS(u8 *DataL, u8 *DataH, u16 Data) +{ + if (End) { + *DataL = (Data >> 8); + *DataH = (Data & 0xff); + } else { + *DataH = (Data >> 8); + *DataL = (Data & 0xff); + } +} + +// 8-bit numbers are combined into a 16-bit number +// DataL is the low bit, DataH is the high bit +u16 SCS::SCS2Host(u8 DataL, u8 DataH) +{ + u16 Data; + if (End) { + Data = DataL; + Data <<= 8; + Data |= DataH; + } else { + Data = DataH; + Data <<= 8; + Data |= DataL; + } + return Data; +} + +void SCS::writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun) +{ + u8 msgLen = 2; + u8 bBuf[6]; + u8 CheckSum = 0; + bBuf[0] = 0xff; + bBuf[1] = 0xff; + bBuf[2] = ID; + bBuf[4] = Fun; + if (nDat) { + msgLen += nLen + 1; + bBuf[3] = msgLen; + bBuf[5] = MemAddr; + writeSCS(bBuf, 6); + + } else { + bBuf[3] = msgLen; + writeSCS(bBuf, 5); + } + CheckSum = ID + msgLen + Fun + MemAddr; + u8 i = 0; + if (nDat) { + for (i = 0; i < nLen; i++) { + CheckSum += nDat[i]; + } + writeSCS(nDat, nLen); + } + writeSCS(~CheckSum); +} + +// Normal write command +// Servo ID, MemAddr memory table address, write data, write length +int SCS::genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen) +{ + rFlushSCS(); + writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE); + wFlushSCS(); + return Ack(ID); +} + +// Asynchronous write command +// Servo ID, MemAddr memory table address, write data, write length +int SCS::regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen) +{ + rFlushSCS(); + writeBuf(ID, MemAddr, nDat, nLen, INST_REG_WRITE); + wFlushSCS(); + return Ack(ID); +} + +// Asynchronous write execution command +// Servo ID +int SCS::RegWriteAction(u8 ID) +{ + rFlushSCS(); + writeBuf(ID, 0, NULL, 0, INST_REG_ACTION); + wFlushSCS(); + return Ack(ID); +} + +// Synchronous write command +// Servo ID[] array, IDN array length, MemAddr memory table address, write data, write length +void SCS::syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen) +{ + rFlushSCS(); + u8 mesLen = ((nLen + 1) * IDN + 4); + u8 Sum = 0; + u8 bBuf[7]; + bBuf[0] = 0xff; + bBuf[1] = 0xff; + bBuf[2] = 0xfe; + bBuf[3] = mesLen; + bBuf[4] = INST_SYNC_WRITE; + bBuf[5] = MemAddr; + bBuf[6] = nLen; + writeSCS(bBuf, 7); + + Sum = 0xfe + mesLen + INST_SYNC_WRITE + MemAddr + nLen; + u8 i, j; + for (i = 0; i < IDN; i++) { + writeSCS(ID[i]); + writeSCS(nDat + i * nLen, nLen); + Sum += ID[i]; + for (j = 0; j < nLen; j++) { + Sum += nDat[i * nLen + j]; + } + } + writeSCS(~Sum); + wFlushSCS(); +} + +int SCS::writeByte(u8 ID, u8 MemAddr, u8 bDat) +{ + rFlushSCS(); + writeBuf(ID, MemAddr, &bDat, 1, INST_WRITE); + wFlushSCS(); + return Ack(ID); +} + +int SCS::writeWord(u8 ID, u8 MemAddr, u16 wDat) +{ + u8 bBuf[2]; + Host2SCS(bBuf + 0, bBuf + 1, wDat); + rFlushSCS(); + writeBuf(ID, MemAddr, bBuf, 2, INST_WRITE); + wFlushSCS(); + return Ack(ID); +} + +// Read command +// Servo ID, MemAddr memory table address, return data nData, data length nLen +int SCS::Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen) +{ + rFlushSCS(); + writeBuf(ID, MemAddr, &nLen, 1, INST_READ); + wFlushSCS(); + if (!checkHead()) { + return 0; + } + u8 bBuf[4]; + Error = 0; + if (readSCS(bBuf, 3) != 3) { + return 0; + } + int Size = readSCS(nData, nLen); + if (Size != nLen) { + return 0; + } + if (readSCS(bBuf + 3, 1) != 1) { + return 0; + } + u8 calSum = bBuf[0] + bBuf[1] + bBuf[2]; + u8 i; + for (i = 0; i < Size; i++) { + calSum += nData[i]; + } + calSum = ~calSum; + if (calSum != bBuf[3]) { + return 0; + } + Error = bBuf[2]; + return Size; +} + +// Read 1 byte, return -1 if timeout +int SCS::readByte(u8 ID, u8 MemAddr) +{ + u8 bDat; + int Size = Read(ID, MemAddr, &bDat, 1); + if (Size != 1) { + return -1; + } else { + return bDat; + } +} + +// Read 2 bytes, return -1 if timeout +int SCS::readWord(u8 ID, u8 MemAddr) +{ + u8 nDat[2]; + int Size; + u16 wDat; + Size = Read(ID, MemAddr, nDat, 2); + if (Size != 2) return -1; + wDat = SCS2Host(nDat[0], nDat[1]); + return wDat; +} + +// Ping command, return the servo ID, timeout returns -1 +int SCS::Ping(u8 ID) +{ + rFlushSCS(); + writeBuf(ID, 0, NULL, 0, INST_PING); + wFlushSCS(); + Error = 0; + if (!checkHead()) { + return -1; + } + u8 bBuf[4]; + if (readSCS(bBuf, 4) != 4) { + return -1; + } + if (bBuf[0] != ID && ID != 0xfe) { + return -1; + } + if (bBuf[1] != 2) { + return -1; + } + u8 calSum = ~(bBuf[0] + bBuf[1] + bBuf[2]); + if (calSum != bBuf[3]) { + return -1; + } + Error = bBuf[2]; + return bBuf[0]; +} + +int SCS::checkHead() +{ + u8 bDat; + u8 bBuf[2] = {0, 0}; + u8 Cnt = 0; + while (1) { + if (!readSCS(&bDat, 1)) { + return 0; + } + bBuf[1] = bBuf[0]; + bBuf[0] = bDat; + if (bBuf[0] == 0xff && bBuf[1] == 0xff) { + break; + } + Cnt++; + if (Cnt > 10) { + return 0; + } + } + return 1; +} + +int SCS::Ack(u8 ID) +{ + Error = 0; + if (ID != 0xfe && Level) { + if (!checkHead()) { + return 0; + } + u8 bBuf[4]; + if (readSCS(bBuf, 4) != 4) { + return 0; + } + if (bBuf[0] != ID) { + return 0; + } + if (bBuf[1] != 2) { + return 0; + } + u8 calSum = ~(bBuf[0] + bBuf[1] + bBuf[2]); + if (calSum != bBuf[3]) { + return 0; + } + Error = bBuf[2]; + } + return 1; +} + +int SCS::syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen) +{ + syncReadRxPacketLen = nLen; + u8 checkSum = (4 + 0xfe) + IDN + MemAddr + nLen + INST_SYNC_READ; + u8 i; + writeSCS(0xff); + writeSCS(0xff); + writeSCS(0xfe); + writeSCS(IDN + 4); + writeSCS(INST_SYNC_READ); + writeSCS(MemAddr); + writeSCS(nLen); + for (i = 0; i < IDN; i++) { + writeSCS(ID[i]); + checkSum += ID[i]; + } + checkSum = ~checkSum; + writeSCS(checkSum); + return nLen; +} + +int SCS::syncReadPacketRx(u8 ID, u8 *nDat) +{ + syncReadRxPacket = nDat; + syncReadRxPacketIndex = 0; + u8 bBuf[4]; + if (!checkHead()) { + return 0; + } + if (readSCS(bBuf, 3) != 3) { + return 0; + } + if (bBuf[0] != ID) { + return 0; + } + if (bBuf[1] != (syncReadRxPacketLen + 2)) { + return 0; + } + Error = bBuf[2]; + if (readSCS(nDat, syncReadRxPacketLen) != syncReadRxPacketLen) { + return 0; + } + return syncReadRxPacketLen; +} + +int SCS::syncReadRxPacketToByte() +{ + if (syncReadRxPacketIndex >= syncReadRxPacketLen) { + return -1; + } + return syncReadRxPacket[syncReadRxPacketIndex++]; +} + +int SCS::syncReadRxPacketToWrod(u8 negBit) +{ + if ((syncReadRxPacketIndex + 1) >= syncReadRxPacketLen) { + return -1; + } + int Word = SCS2Host(syncReadRxPacket[syncReadRxPacketIndex], syncReadRxPacket[syncReadRxPacketIndex + 1]); + syncReadRxPacketIndex += 2; + if (negBit) { + if (Word & (1 << negBit)) { + Word = -(Word & ~(1 << negBit)); + } + } + return Word; +} diff --git a/src/drivers/SCServo_lib/src/SCS.h b/src/drivers/SCServo_lib/src/SCS.h new file mode 100644 index 0000000..3ac2b84 --- /dev/null +++ b/src/drivers/SCServo_lib/src/SCS.h @@ -0,0 +1,56 @@ +/* + * SCS.h + * FIT serial servo communication layer protocol program + */ + +#ifndef _SCS_H +#define _SCS_H + +#include "INST.h" + +class SCS { +public: + SCS(); + SCS(u8 End); + SCS(u8 End, u8 Level); + int genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen); // Normal write instruction + int regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen); // Asynchronous write command + int RegWriteAction(u8 ID = 0xfe); // Asynchronous write execution instruction + void syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen); // Synchronous write instruction + int writeByte(u8 ID, u8 MemAddr, u8 bDat); // Write 1 byte + int writeWord(u8 ID, u8 MemAddr, u16 wDat); // Write 2 bytes + int Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen); // Read instruction + int readByte(u8 ID, u8 MemAddr); // Read 1 byte + int readWord(u8 ID, u8 MemAddr); // Read 2 bytes + int Ping(u8 ID); // Ping Command + int syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen); // Synchronous read instruction packet sending + int syncReadPacketRx( + u8 ID, + u8 *nDat); // Synchronous read return packet reception, successful return memory byte count, failed return 0 + int syncReadRxPacketToByte(); // Decode a byte + int syncReadRxPacketToWrod( + u8 negBit = 0); // Decode two bytes, negBit is the direction, negBit=0 means no direction +public: + u8 Level; // Servo return level + u8 End; // Processor endian structure + u8 Error; // Servo status + u8 syncReadRxPacketIndex; + u8 syncReadRxPacketLen; + u8 *syncReadRxPacket; + +protected: + virtual int writeSCS(unsigned char *nDat, int nLen) = 0; + virtual int readSCS(unsigned char *nDat, int nLen) = 0; + virtual int writeSCS(unsigned char bDat) = 0; + virtual void rFlushSCS() = 0; + virtual void wFlushSCS() = 0; + +protected: + void writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun); + void Host2SCS(u8 *DataL, u8 *DataH, u16 Data); // Split a 16-digit number into two 8-digit numbers + u16 SCS2Host(u8 DataL, u8 DataH); // Two 8-digit numbers combined into a 16-digit number + int Ack(u8 ID); // Return Response + int checkHead(); // Frame header detection +}; + +#endif diff --git a/src/drivers/SCServo_lib/src/SCSCL.cpp b/src/drivers/SCServo_lib/src/SCSCL.cpp new file mode 100644 index 0000000..bb45907 --- /dev/null +++ b/src/drivers/SCServo_lib/src/SCSCL.cpp @@ -0,0 +1,319 @@ +/* + * SCSCL.cpp + * FIT SCSCL series serial servo application layer program + */ + +#include "SCSCL.h" + +SCSCL::SCSCL() +{ + End = 1; +} + +SCSCL::SCSCL(u8 End) : SCSerial(End) +{ +} + +SCSCL::SCSCL(u8 End, u8 Level) : SCSerial(End, Level) +{ +} + +int SCSCL::WritePos(u8 ID, u16 Position, u16 Time, u16 Speed) +{ + u8 bBuf[6]; + Host2SCS(bBuf + 0, bBuf + 1, Position); + Host2SCS(bBuf + 2, bBuf + 3, Time); + Host2SCS(bBuf + 4, bBuf + 5, Speed); + + return genWrite(ID, SCSCL_GOAL_POSITION_L, bBuf, 6); +} + +int SCSCL::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC) +{ + (void)ACC; // ACC parameter is not used in this implementation + u16 Time = 0; + u8 bBuf[6]; + Host2SCS(bBuf + 0, bBuf + 1, Position); + Host2SCS(bBuf + 2, bBuf + 3, Time); + Host2SCS(bBuf + 4, bBuf + 5, Speed); + + return genWrite(ID, SCSCL_GOAL_POSITION_L, bBuf, 6); +} + +int SCSCL::RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed) +{ + u8 bBuf[6]; + Host2SCS(bBuf + 0, bBuf + 1, Position); + Host2SCS(bBuf + 2, bBuf + 3, Time); + Host2SCS(bBuf + 4, bBuf + 5, Speed); + + return regWrite(ID, SCSCL_GOAL_POSITION_L, bBuf, 6); +} + +int SCSCL::CalibrationOfs(u8 ID) +{ + return -1; +} + +void SCSCL::SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], u16 Speed[]) +{ + u8 offbuf[6 * IDN]; + for (u8 i = 0; i < IDN; i++) { + u16 T, V; + if (Time) { + T = Time[i]; + } else { + T = 0; + } + if (Speed) { + V = Speed[i]; + } else { + V = 0; + } + Host2SCS(offbuf + i * 6 + 0, offbuf + i * 6 + 1, Position[i]); + Host2SCS(offbuf + i * 6 + 2, offbuf + i * 6 + 3, T); + Host2SCS(offbuf + i * 6 + 4, offbuf + i * 6 + 5, V); + } + syncWrite(ID, IDN, SCSCL_GOAL_POSITION_L, offbuf, 6); +} + +int SCSCL::PWMMode(u8 ID) +{ + u8 bBuf[4]; + bBuf[0] = 0; + bBuf[1] = 0; + bBuf[2] = 0; + bBuf[3] = 0; + return genWrite(ID, SCSCL_MIN_ANGLE_LIMIT_L, bBuf, 4); +} + +int SCSCL::WritePWM(u8 ID, s16 pwmOut) +{ + if (pwmOut < 0) { + pwmOut = -pwmOut; + pwmOut |= (1 << 10); + } + u8 bBuf[2]; + Host2SCS(bBuf + 0, bBuf + 1, pwmOut); + + return genWrite(ID, SCSCL_GOAL_TIME_L, bBuf, 2); +} + +/** + * Switch between position mode and PWM mode 切换位置模式和PWM模式 + * ID: Servo ID + * mode: 0 for position mode, 1 for PWM mode 0表示位置模式，1表示PWM模式 + * Return: Result of the operation + */ +int SCSCL::SwitchMode(int ID, uint8_t mode) +{ + if (ID < 0 || ID > 1) { + return -1; // Invalid ID + } + if (mode > 1) { + return -2; // Invalid mode + } + + if (mode == 1) { // PWM mode + // Store current angle limits + min_angle[ID] = readWord(ID, SCSCL_MIN_ANGLE_LIMIT_L); + max_angle[ID] = readWord(ID, SCSCL_MAX_ANGLE_LIMIT_L); + + if (min_angle[ID] == -1 || max_angle[ID] == -1) { + return -3; // Failed to read angle limits + } + PWMMode(ID); // Switch to PWM mode + return 0; + } else { // Position mode (mode == 0) + if (writeWord(ID, SCSCL_MIN_ANGLE_LIMIT_L, (uint16_t)min_angle[ID]) != 1) { + return -4; // Failed to write min angle limit + } + if (writeWord(ID, SCSCL_MAX_ANGLE_LIMIT_L, (uint16_t)max_angle[ID]) != 1) { + return -5; // Failed to write max angle limit + } + return 0; + } +} + +/** + * Enable or disable torque 扭矩开关 + * ID: Servo ID + * Enable: 1 to enable, 0 to disable 2 to damping 1表示使能，0表示关闭，2表示阻尼 + * Return: Result of the operation + */ +int SCSCL::EnableTorque(u8 ID, u8 Enable) +{ + return writeByte(ID, SCSCL_TORQUE_ENABLE, Enable); +} + +int SCSCL::unLockEprom(u8 ID) +{ + return writeByte(ID, SCSCL_LOCK, 0); +} + +int SCSCL::LockEprom(u8 ID) +{ + return writeByte(ID, SCSCL_LOCK, 1); +} + +int SCSCL::FeedBack(int ID) +{ + int nLen = Read(ID, SCSCL_PRESENT_POSITION_L, Mem, sizeof(Mem)); + if (nLen != sizeof(Mem)) { + Err = 1; + return -1; + } + Err = 0; + return nLen; +} + +int SCSCL::ReadPos(int ID) +{ + int Pos = -1; + if (ID == -1) { + Pos = Mem[SCSCL_PRESENT_POSITION_L - SCSCL_PRESENT_POSITION_L]; + Pos <<= 8; + Pos |= Mem[SCSCL_PRESENT_POSITION_H - SCSCL_PRESENT_POSITION_L]; + } else { + Err = 0; + Pos = readWord(ID, SCSCL_PRESENT_POSITION_L); + if (Pos == -1) { + Err = 1; + } + } + return Pos; +} + +int SCSCL::ReadSpeed(int ID) +{ + int Speed = -1; + if (ID == -1) { + Speed = Mem[SCSCL_PRESENT_SPEED_L - SCSCL_PRESENT_POSITION_L]; + Speed <<= 8; + Speed |= Mem[SCSCL_PRESENT_SPEED_H - SCSCL_PRESENT_POSITION_L]; + } else { + Err = 0; + Speed = readWord(ID, SCSCL_PRESENT_SPEED_L); + if (Speed == -1) { + Err = 1; + return -1; + } + } + if (!Err && (Speed & (1 << 15))) { + Speed = -(Speed & ~(1 << 15)); + } + return Speed; +} + +int SCSCL::ReadLoad(int ID) +{ + int Load = -1; + if (ID == -1) { + Load = Mem[SCSCL_PRESENT_LOAD_L - SCSCL_PRESENT_POSITION_L]; + Load <<= 8; + Load |= Mem[SCSCL_PRESENT_LOAD_H - SCSCL_PRESENT_POSITION_L]; + } else { + Err = 0; + Load = readWord(ID, SCSCL_PRESENT_LOAD_L); + if (Load == -1) { + Err = 1; + } + } + if (!Err && (Load & (1 << 10))) { + Load = -(Load & ~(1 << 10)); + } + return Load; +} + +int SCSCL::ReadVoltage(int ID) +{ + int Voltage = -1; + if (ID == -1) { + Voltage = Mem[SCSCL_PRESENT_VOLTAGE - SCSCL_PRESENT_POSITION_L]; + } else { + Err = 0; + Voltage = readByte(ID, SCSCL_PRESENT_VOLTAGE); + if (Voltage == -1) { + Err = 1; + } + } + return Voltage; +} + +int SCSCL::ReadTemper(int ID) +{ + int Temper = -1; + if (ID == -1) { + Temper = Mem[SCSCL_PRESENT_TEMPERATURE - SCSCL_PRESENT_POSITION_L]; + } else { + Err = 0; + Temper = readByte(ID, SCSCL_PRESENT_TEMPERATURE); + if (Temper == -1) { + Err = 1; + } + } + return Temper; +} + +int SCSCL::ReadMove(int ID) +{ + int Move = -1; + if (ID == -1) { + Move = Mem[SCSCL_MOVING - SCSCL_PRESENT_POSITION_L]; + } else { + Err = 0; + Move = readByte(ID, SCSCL_MOVING); + if (Move == -1) { + Err = 1; + } + } + return Move; +} + +int SCSCL::ReadMode(int ID) +{ + int ValueRead = -1; + ValueRead = readWord(ID, SCSCL_MIN_ANGLE_LIMIT_L); + if (ValueRead == 0) { + return 1; + } else if (ValueRead > 0) { + return 0; + } + return ValueRead; +} + +int SCSCL::ReadToqueEnable(int ID) +{ + // return writeByte(ID, SCSCL_TORQUE_ENABLE, Enable); + int ValueRead = -1; + ValueRead = readWord(ID, SCSCL_TORQUE_ENABLE); + return ValueRead; +} + +int SCSCL::ReadInfoValue(int ID, int AddInput) +{ + int ValueRead = -1; + ValueRead = readWord(ID, AddInput); + return ValueRead; +} + +int SCSCL::ReadCurrent(int ID) +{ + int Current = -1; + if (ID == -1) { + Current = Mem[SCSCL_PRESENT_CURRENT_L - SCSCL_PRESENT_POSITION_L]; + Current <<= 8; + Current |= Mem[SCSCL_PRESENT_CURRENT_H - SCSCL_PRESENT_POSITION_L]; + } else { + Err = 0; + Current = readWord(ID, SCSCL_PRESENT_CURRENT_L); + if (Current == -1) { + Err = 1; + return -1; + } + } + if (!Err && (Current & (1 << 15))) { + Current = -(Current & ~(1 << 15)); + } + return Current; +} \ No newline at end of file diff --git a/src/drivers/SCServo_lib/src/SCSCL.h b/src/drivers/SCServo_lib/src/SCSCL.h new file mode 100644 index 0000000..6424acb --- /dev/null +++ b/src/drivers/SCServo_lib/src/SCSCL.h @@ -0,0 +1,86 @@ +/* + * SCSCL.h + * FIT SCSCL series serial servo application layer program + */ + +#ifndef _SCSCL_H +#define _SCSCL_H + +// Memory table definition +//-------EPROM (read-only)-------- +#define SCSCL_VERSION_L 3 +#define SCSCL_VERSION_H 4 + +//-------EPROM (read and write)-------- +#define SCSCL_ID 5 +#define SCSCL_BAUD_RATE 6 +#define SCSCL_MIN_ANGLE_LIMIT_L 9 +#define SCSCL_MIN_ANGLE_LIMIT_H 10 +#define SCSCL_MAX_ANGLE_LIMIT_L 11 +#define SCSCL_MAX_ANGLE_LIMIT_H 12 +#define SCSCL_CW_DEAD 26 +#define SCSCL_CCW_DEAD 27 + +//-------SRAM (read and write)-------- +#define SCSCL_TORQUE_ENABLE 40 +#define SCSCL_GOAL_POSITION_L 42 +#define SCSCL_GOAL_POSITION_H 43 +#define SCSCL_GOAL_TIME_L 44 +#define SCSCL_GOAL_TIME_H 45 +#define SCSCL_GOAL_SPEED_L 46 +#define SCSCL_GOAL_SPEED_H 47 +#define SCSCL_LOCK 48 + +//-------SRAM (read only)-------- +#define SCSCL_PRESENT_POSITION_L 56 +#define SCSCL_PRESENT_POSITION_H 57 +#define SCSCL_PRESENT_SPEED_L 58 +#define SCSCL_PRESENT_SPEED_H 59 +#define SCSCL_PRESENT_LOAD_L 60 +#define SCSCL_PRESENT_LOAD_H 61 +#define SCSCL_PRESENT_VOLTAGE 62 +#define SCSCL_PRESENT_TEMPERATURE 63 +#define SCSCL_MOVING 66 +#define SCSCL_PRESENT_CURRENT_L 69 +#define SCSCL_PRESENT_CURRENT_H 70 + +#include "SCSerial.h" + +class SCSCL : public SCSerial { +public: + SCSCL(); + SCSCL(u8 End); + SCSCL(u8 End, u8 Level); + virtual int WritePos(u8 ID, u16 Position, u16 Time, u16 Speed); // Normal write of single servo position command + virtual int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC); // Single servo position command + virtual int RegWritePos( + u8 ID, u16 Position, u16 Time, + u16 Speed = 0); // Asynchronously write a single servo position command (RegWriteAction takes effect) + virtual void SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], + u16 Speed[]); // Synchronously write multiple servo position commands + virtual int PWMMode(u8 ID); // PWM output mode + virtual int WritePWM(u8 ID, s16 pwmOut); // PWM output mode command + virtual int SwitchMode(int ID, uint8_t mode); // Switch between position mode and PWM mode + virtual int EnableTorque(u8 ID, u8 Enable); // Torque control command + virtual int unLockEprom(u8 ID); // eprom unlock + virtual int LockEprom(u8 ID); // eprom lock + virtual int FeedBack(int ID); // Feedback servo information + virtual int ReadPos(int ID); // Read position + virtual int ReadSpeed(int ID); // Read speed + virtual int ReadLoad(int ID); // Read the output voltage percentage to the motor (0~1000) + virtual int ReadVoltage(int ID); // Read voltage + virtual int ReadTemper(int ID); // Read temperature + virtual int ReadMove(int ID); // Read movement status + virtual int ReadCurrent(int ID); // read current + virtual int ReadMode(int ID); + virtual int ReadToqueEnable(int ID); + virtual int CalibrationOfs(u8 ID); + virtual int ReadInfoValue(int ID, int AddInput); + +private: + u8 Mem[SCSCL_PRESENT_CURRENT_H - SCSCL_PRESENT_POSITION_L + 1]; + int min_angle[2]; + int max_angle[2]; +}; + +#endif \ No newline at end of file diff --git a/src/drivers/SCServo_lib/src/SCSerial.cpp b/src/drivers/SCServo_lib/src/SCSerial.cpp new file mode 100644 index 0000000..1a52d19 --- /dev/null +++ b/src/drivers/SCServo_lib/src/SCSerial.cpp @@ -0,0 +1,152 @@ +/* + * SCSerial.cpp + * FIT serial servo hardware interface layer program + */ + +#include "SCSerial.h" +#include "esp_log.h" +#include "esp_timer.h" + +static const char *TAG = "SCSerial"; + +SCSerial::SCSerial() +{ + IOTimeOut = 100; + uart_num = UART_NUM_MAX; +} + +SCSerial::SCSerial(u8 End) : SCS(End) +{ + IOTimeOut = 100; + uart_num = UART_NUM_MAX; +} + +SCSerial::SCSerial(u8 End, u8 Level) : SCS(End, Level) +{ + IOTimeOut = 100; + uart_num = UART_NUM_MAX; +} + +bool SCSerial::begin(uart_port_t uart_num, int baud_rate, int tx_pin, int rx_pin, int buf_size) +{ + this->uart_num = uart_num; + + uart_config_t uart_config = { + .baud_rate = baud_rate, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + .rx_flow_ctrl_thresh = 0, + .source_clk = UART_SCLK_APB, + }; + + esp_err_t ret = uart_driver_install(uart_num, buf_size, buf_size, 0, NULL, 0); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "Failed to install UART driver: %s", esp_err_to_name(ret)); + return false; + } + + ret = uart_param_config(uart_num, &uart_config); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "Failed to configure UART: %s", esp_err_to_name(ret)); + uart_driver_delete(uart_num); + return false; + } + + ret = uart_set_pin(uart_num, tx_pin, rx_pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "Failed to set UART pins: %s", esp_err_to_name(ret)); + uart_driver_delete(uart_num); + return false; + } + + return true; +} + +void SCSerial::end() +{ + if (uart_num < UART_NUM_MAX) { + uart_driver_delete(uart_num); + uart_num = UART_NUM_MAX; + } +} + +int SCSerial::readSCS(unsigned char *nDat, int nLen) +{ + if (uart_num >= UART_NUM_MAX) { + return 0; + } + + int Size = 0; + int64_t t_begin = esp_timer_get_time() / 1000; // Convert to milliseconds + int64_t t_user; + + while (1) { + size_t available = 0; + uart_get_buffered_data_len(uart_num, &available); + + if (available > 0) { + unsigned char byte; + int len = uart_read_bytes(uart_num, &byte, 1, 0); + if (len > 0) { + if (nDat) { + nDat[Size] = byte; + } + Size++; + t_begin = esp_timer_get_time() / 1000; + } + } + + if (Size >= nLen) { + break; + } + + t_user = (esp_timer_get_time() / 1000) - t_begin; + if (t_user > IOTimeOut) { + break; + } + + vTaskDelay(1 / portTICK_PERIOD_MS); + } + + return Size; +} + +int SCSerial::writeSCS(unsigned char *nDat, int nLen) +{ + if (uart_num >= UART_NUM_MAX || nDat == NULL) { + return 0; + } + + int len = uart_write_bytes(uart_num, (const char *)nDat, nLen); + return len; +} + +int SCSerial::writeSCS(unsigned char bDat) +{ + if (uart_num >= UART_NUM_MAX) { + return 0; + } + + int len = uart_write_bytes(uart_num, (const char *)&bDat, 1); + return len; +} + +void SCSerial::rFlushSCS() +{ + if (uart_num >= UART_NUM_MAX) { + return; + } + + uart_flush_input(uart_num); +} + +void SCSerial::wFlushSCS() +{ + if (uart_num >= UART_NUM_MAX) { + return; + } + + uart_wait_tx_done(uart_num, pdMS_TO_TICKS(100)); +} \ No newline at end of file diff --git a/src/drivers/SCServo_lib/src/SCSerial.h b/src/drivers/SCServo_lib/src/SCSerial.h new file mode 100644 index 0000000..97454c7 --- /dev/null +++ b/src/drivers/SCServo_lib/src/SCSerial.h @@ -0,0 +1,46 @@ +/* + * SCSerial.h + * FIT serial servo hardware interface layer program + */ + +#ifndef _SCSERIAL_H +#define _SCSERIAL_H + +#include "driver/uart.h" +#include "driver/gpio.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include + +#include "SCS.h" + +class SCSerial : public SCS { +public: + SCSerial(); + SCSerial(u8 End); + SCSerial(u8 End, u8 Level); + + // Initialize UART + bool begin(uart_port_t uart_num, int baud_rate, int tx_pin, int rx_pin, int buf_size = 1024); + void end(); + +protected: + virtual int writeSCS(unsigned char *nDat, int nLen); // Output nLen bytes + virtual int readSCS(unsigned char *nDat, int nLen); // Input nLen bytes + virtual int writeSCS(unsigned char bDat); // Output 1 byte + virtual void rFlushSCS(); + virtual void wFlushSCS(); + +public: + unsigned long int IOTimeOut; // Input and output timeout (in milliseconds) + uart_port_t uart_num; // UART port number + int Err; + +public: + virtual int getErr() + { + return Err; + } +}; + +#endif \ No newline at end of file diff --git a/src/drivers/SCServo_lib/src/SCServo.h b/src/drivers/SCServo_lib/src/SCServo.h new file mode 100644 index 0000000..1a5c482 --- /dev/null +++ b/src/drivers/SCServo_lib/src/SCServo.h @@ -0,0 +1,11 @@ +/* + * SCServo.h + * FIT serial servo interface + */ + +#ifndef _SCSERVO_H +#define _SCSERVO_H + +#include "SCSCL.h" + +#endif \ No newline at end of file diff --git a/src/drivers/Si12T/Si12T.cpp b/src/drivers/Si12T/Si12T.cpp new file mode 100644 index 0000000..be56480 --- /dev/null +++ b/src/drivers/Si12T/Si12T.cpp @@ -0,0 +1,199 @@ +#include +#include "Si12T.h" + +static uint32_t i2c_freq = 100000; // 100kHz + +Si12T::Si12T(uint8_t sens_type, uint8_t sens_level, m5::I2C_Class* i2c) + : m5::I2C_Device(SI12T_GND_ADDRESS, i2c_freq, i2c) +{ + this->sens_type = sens_type; + this->sens_level = sens_level; + point_type[0] = OUTPUT_NONE; + point_type[1] = OUTPUT_NONE; + point_type[2] = OUTPUT_NONE; +} + +void Si12T::begin() +{ + enable_channel(); + set_Ctrl2(); + set_Ctrl1(); + SI12T_Set_Sensitivity(sens_type, sens_level); // set sensitivity level, low level means high detectivity + SI12T_Get_Sensitivity(); // get sensitivity level to check if it is set correctly +} + +bool Si12T::SI12T_Writeregister(std::uint8_t regAddr, std::uint8_t value) +{ + if (!_i2c) { + return false; + } + + return writeRegister8(regAddr, value); +} + +bool Si12T::SI12T_Readregister(std::uint8_t regAddr, std::uint8_t* value) +{ + if (!_i2c) { + return false; + } + return readRegister(regAddr, value, 1); +} + +uint8_t Si12T::set_sens(uint8_t value) +{ + SI12T_Writeregister(SI12T_SENSITIVITY1_ADDR, value); + SI12T_Writeregister(SI12T_SENSITIVITY2_ADDR, value); + SI12T_Writeregister(SI12T_SENSITIVITY3_ADDR, value); + SI12T_Writeregister(SI12T_SENSITIVITY4_ADDR, value); + SI12T_Writeregister(SI12T_SENSITIVITY5_ADDR, value); + + return 0; +} + +void Si12T::SI12T_Get_Sensitivity(void) +{ + uint8_t data = 0; + SI12T_Readregister(SI12T_SENSITIVITY1_ADDR, &data); + SI12T_Readregister(SI12T_SENSITIVITY2_ADDR, &data); + SI12T_Readregister(SI12T_SENSITIVITY3_ADDR, &data); + SI12T_Readregister(SI12T_SENSITIVITY4_ADDR, &data); + SI12T_Readregister(SI12T_SENSITIVITY5_ADDR, &data); +} + +/** + * @brief:Set the sensitivity level of the sensor + */ +int8_t Si12T::SI12T_Set_Sensitivity(uint8_t sens_type, uint8_t sens_level) +{ + if (sens_type < SI12T_Type_Low || sens_type > SI12T_Type_High) { + return -1; + } + uint8_t value = 0x00; + if (sens_type == SI12T_Type_High) { + switch (sens_level) { + case SI12T_Sensitivity_Level_0: + value = 0x88; + break; + case SI12T_Sensitivity_Level_1: + value = 0x99; + break; + case SI12T_Sensitivity_Level_2: + value = 0xAA; + break; + case SI12T_Sensitivity_Level_3: + value = 0xBB; + break; + case SI12T_Sensitivity_Level_4: + value = 0xCC; + break; + case SI12T_Sensitivity_Level_5: + value = 0xDD; + break; + case SI12T_Sensitivity_Level_6: + value = 0xEE; + break; + case SI12T_Sensitivity_Level_7: + value = 0xFF; + break; + default: + break; + } + } else { + switch (sens_level) { + case SI12T_Sensitivity_Level_0: + value = 0x00; + break; + case SI12T_Sensitivity_Level_1: + value = 0x11; + break; + case SI12T_Sensitivity_Level_2: + value = 0x22; + break; + case SI12T_Sensitivity_Level_3: + value = 0x33; + break; + case SI12T_Sensitivity_Level_4: + value = 0x44; + break; + case SI12T_Sensitivity_Level_5: + value = 0x55; + break; + case SI12T_Sensitivity_Level_6: + value = 0x66; + break; + case SI12T_Sensitivity_Level_7: + value = 0x77; + break; + default: + break; + } + } + + log_d("value: 0x%02x", value); + set_sens(value); + return 0; +} + +void Si12T::set_Ctrl1(void) +{ + // sends register data, Auto Moe,FTC=01, Interrupt(Middle,High), Response 4 (2+2) + uint8_t test; + SI12T_Writeregister(SI12T_CTRL1_ADDR, 0x22); + SI12T_Readregister(SI12T_CTRL1_ADDR, &test); +} + +void Si12T::set_Ctrl2(void) +{ + uint8_t test; + // S/W Reset Enable, Sleep Mode Enable + SI12T_Writeregister(SI12T_CTRL2_ADDR, 0x0F); + SI12T_Writeregister(SI12T_CTRL2_ADDR, 0x07); + + SI12T_Readregister(SI12T_CTRL2_ADDR, &test); +} + +void Si12T::sleep_enable(void) +{ + SI12T_Writeregister(SI12T_CTRL2_ADDR, 0x07); // S/W Reset Enable, Sleep Mode Enable +} + +void Si12T::sleep_disable(void) +{ + SI12T_Writeregister(SI12T_CTRL2_ADDR, 0x03); // S/W Reset Enable, Sleep Mode Enable +} + +void Si12T::enable_channel(void) +{ + SI12T_Writeregister(SI12T_REF_RST1_ADDR, 0x00); // channel 1-8 enable reference calibration + SI12T_Writeregister(SI12T_REF_RST2_ADDR, 0x00); // channel 9 enable reference calibration + + SI12T_Writeregister(SI12T_CH_HOLD1_ADDR, 0x00); // channel 1-8 enable + SI12T_Writeregister(SI12T_CH_HOLD2_ADDR, 0x00); // channel 9 enable + + SI12T_Writeregister(SI12T_CAL_HOLD1_ADDR, 0x00); // channel 1-8 enable reference calibration + SI12T_Writeregister(SI12T_CAL_HOLD2_ADDR, 0x00); // channel 9 enable reference calibration + + uint8_t data = 1; + SI12T_Readregister(SI12T_REF_RST1_ADDR, &data); + SI12T_Readregister(SI12T_REF_RST2_ADDR, &data); + SI12T_Readregister(SI12T_CH_HOLD1_ADDR, &data); + SI12T_Readregister(SI12T_CH_HOLD2_ADDR, &data); + SI12T_Readregister(SI12T_CAL_HOLD1_ADDR, &data); + SI12T_Readregister(SI12T_CAL_HOLD2_ADDR, &data); +} + +void Si12T::read_touch_result() +{ + readRegister(SI12T_OUTPUT1_ADDR, &this->touch_result, 1); +} + +void Si12T::parse_touch_result() +{ + int index = 0; + memset(this->point_type, 0, sizeof(this->point_type)); + + for (int j = 0; j < 6; j += 2) { + this->point_type[index] = (this->touch_result >> j) & 0x03; + index++; + } +} diff --git a/src/drivers/Si12T/Si12T.h b/src/drivers/Si12T/Si12T.h new file mode 100644 index 0000000..7e09cb9 --- /dev/null +++ b/src/drivers/Si12T/Si12T.h @@ -0,0 +1,75 @@ +#ifndef __SI12T_H__ +#define __SI12T_H__ + +#include +#include + +#define SI12T_VERSION "0.0.1" + +/*LTR-507 SEL pin is "GND"*/ +#define SI12T_GND_ADDRESS 0x68 // 7bit i2c address + +#define I2C_SCL_PIN 11 +#define I2C_SDA_PIN 12 + +#define SI12T_SENSITIVITY1_ADDR 0x02 +#define SI12T_SENSITIVITY2_ADDR 0x03 +#define SI12T_SENSITIVITY3_ADDR 0x04 +#define SI12T_SENSITIVITY4_ADDR 0x05 +#define SI12T_SENSITIVITY5_ADDR 0x06 +#define SI12T_SENSITIVITY6_ADDR 0x07 +#define SI12T_CTRL1_ADDR 0x08 +#define SI12T_CTRL2_ADDR 0x09 +#define SI12T_REF_RST1_ADDR 0x0A +#define SI12T_REF_RST2_ADDR 0x0B +#define SI12T_CH_HOLD1_ADDR 0x0C +#define SI12T_CH_HOLD2_ADDR 0x0D +#define SI12T_CAL_HOLD1_ADDR 0x0E +#define SI12T_CAL_HOLD2_ADDR 0x0F +#define SI12T_OUTPUT1_ADDR 0x10 +#define SI12T_OUTPUT2_ADDR 0x11 +#define SI12T_OUTPUT3_ADDR 0x12 + +enum SI12T_Type { SI12T_Type_Low, SI12T_Type_High }; + +enum SI12T_Sensitivity_Level { + SI12T_Sensitivity_Level_0, + SI12T_Sensitivity_Level_1, + SI12T_Sensitivity_Level_2, + SI12T_Sensitivity_Level_3, + SI12T_Sensitivity_Level_4, + SI12T_Sensitivity_Level_5, + SI12T_Sensitivity_Level_6, + SI12T_Sensitivity_Level_7, + Si12T_Sensitivity_Level_Invalid, +}; + +enum output_e { OUTPUT_NONE, OUTPUT_LOW, OUTPUT_MID, OUTPUT_HIGH }; + +class Si12T : public m5::I2C_Device { +private: + bool SI12T_Writeregister(std::uint8_t regAddr, std::uint8_t value); + bool SI12T_Readregister(std::uint8_t regAddr, std::uint8_t* value); + +public: + Si12T() : m5::I2C_Device(SI12T_GND_ADDRESS, 100000){}; + Si12T(std::uint8_t sens_type, std::uint8_t sens_level, m5::I2C_Class* i2c = &m5::In_I2C); + void begin(); + void enable_channel(void); + int8_t SI12T_Set_Sensitivity(uint8_t sens_type, uint8_t sens_level); + uint8_t set_sens(uint8_t value); + void SI12T_Get_Sensitivity(void); + void set_Ctrl1(void); + void set_Ctrl2(void); + void sleep_enable(void); + void sleep_disable(void); + + void read_touch_result(); + void parse_touch_result(); + + uint8_t sens_type = SI12T_Type_Low; + uint8_t sens_level = SI12T_Sensitivity_Level_0; + byte touch_result; + uint8_t point_type[3]; +}; +#endif \ No newline at end of file diff --git a/src/utils/compat/make_unique.h b/src/utils/compat/make_unique.h new file mode 100644 index 0000000..4f3cca1 --- /dev/null +++ b/src/utils/compat/make_unique.h @@ -0,0 +1,33 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#pragma once +#include +#include +#include +#include + +#if __cplusplus < 201402L +namespace std { + +template +typename std::enable_if::value, std::unique_ptr>::type make_unique(Args&&... args) +{ + return std::unique_ptr(new T(std::forward(args)...)); +} + +template +typename std::enable_if::value && std::extent::value == 0, std::unique_ptr>::type make_unique( + std::size_t n) +{ + using U = typename std::remove_extent::type; + return std::unique_ptr(new U[n]()); +} + +template +typename std::enable_if<(std::extent::value != 0), void>::type make_unique(Args&&...) = delete; + +} // namespace std +#endif diff --git a/src/utils/motion/motion.cpp b/src/utils/motion/motion.cpp new file mode 100644 index 0000000..64354b7 --- /dev/null +++ b/src/utils/motion/motion.cpp @@ -0,0 +1,188 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include "motion.h" +#include +#include + +using namespace uitk_intl; +using namespace stackchan::motion; + +static const char* TAG = "Motion"; + +Motion::~Motion() +{ + _running = false; +} + +void Motion::init(std::unique_ptr yawServo, std::unique_ptr pitchServo) +{ + std::lock_guard lock(_mutex); + + _yaw_servo = std::move(yawServo); + _pitch_servo = std::move(pitchServo); + _yaw_servo->init(); + _pitch_servo->init(); + + if (!_running) { + _running = true; + xTaskCreate(update_task, "motion_task", 4096, this, 10, &_task_handle); + ESP_LOGI(TAG, "Motion task started"); + } +} + +void Motion::update_task(void* param) +{ + auto self = static_cast(param); + while (self->_running) { + vTaskDelay(pdMS_TO_TICKS(20)); + self->update(); + } + self->_task_handle = nullptr; + vTaskDelete(NULL); +} + +void Motion::update() +{ + std::lock_guard lock(_mutex); + if (_yaw_servo) { + _yaw_servo->update(); + } + if (_pitch_servo) { + _pitch_servo->update(); + } +} + +void Motion::moveYaw(int angle, int speed) +{ + std::lock_guard lock(_mutex); + _yaw_servo->moveWithSpeed(angle, speed); +} + +void Motion::movePitch(int angle, int speed) +{ + std::lock_guard lock(_mutex); + _pitch_servo->moveWithSpeed(angle, speed); +} + +void Motion::move(int yawAngle, int pitchAngle, int speed) +{ + std::lock_guard lock(_mutex); + _yaw_servo->moveWithSpeed(yawAngle, speed); + _pitch_servo->moveWithSpeed(pitchAngle, speed); +} + +void Motion::goHome(int speed) +{ + std::lock_guard lock(_mutex); + _yaw_servo->moveWithSpeed(0, speed); + _pitch_servo->moveWithSpeed(0, speed); +} + +void Motion::stop() +{ + std::lock_guard lock(_mutex); + _yaw_servo->move(_yaw_servo->getCurrentAngle()); + _pitch_servo->move(_pitch_servo->getCurrentAngle()); +} + +void Motion::rotateYaw(int velocity) +{ + std::lock_guard lock(_mutex); + _yaw_servo->rotate(velocity); +} + +void Motion::lookAtNormalized(float x, float y, int speed) +{ + std::lock_guard lock(_mutex); + + int yaw_angle = uitk_intl::map_range(x, -1.0f, 1.0f, (float)_yaw_servo->getAngleLimit().x, + (float)_yaw_servo->getAngleLimit().y); + int pitch_angle = uitk_intl::map_range(y, -1.0f, 1.0f, (float)_pitch_servo->getAngleLimit().x, + (float)_pitch_servo->getAngleLimit().y); + + _yaw_servo->moveWithSpeed(yaw_angle, speed); + _pitch_servo->moveWithSpeed(pitch_angle, speed); +} + +void Motion::lookAtPoint(float x, float y, float z, int speed) +{ + // Yaw: 绕 Z 轴旋转。使用 atan2(y, x) + float yaw_rad = std::atan2(y, x); + + // Pitch: 俯仰。使用 atan2(z, sqrt(x*x + y*y)) + float ground_dist = std::sqrt(x * x + y * y); + float pitch_rad = std::atan2(z, ground_dist); + + // 将弧度转换为你的舵机单位 (-1280~1280 等) + int yaw_angle = static_cast(to_degrees(yaw_rad) * 10); + int pitch_angle = static_cast(to_degrees(pitch_rad) * 10); + + std::lock_guard lock(_mutex); + _yaw_servo->moveWithSpeed(yaw_angle, speed); + _pitch_servo->moveWithSpeed(pitch_angle, speed); +} + +bool Motion::isMoving() +{ + std::lock_guard lock(_mutex); + return _yaw_servo->isMoving() || _pitch_servo->isMoving(); +} + +bool Motion::isYawMoving() +{ + return _yaw_servo->isMoving(); +} + +bool Motion::isPitchMoving() +{ + return _pitch_servo->isMoving(); +} + +int Motion::getCurrentYawAngle() +{ + std::lock_guard lock(_mutex); + return _yaw_servo->getCurrentAngle(); +} + +int Motion::getCurrentPitchAngle() +{ + std::lock_guard lock(_mutex); + return _pitch_servo->getCurrentAngle(); +} + +uitk_intl::Vector2i Motion::getCurrentAngles() +{ + std::lock_guard lock(_mutex); + return uitk_intl::Vector2i(_yaw_servo->getCurrentAngle(), _pitch_servo->getCurrentAngle()); +} + +void Motion::setTorqueEnabled(bool enabled) +{ + std::lock_guard lock(_mutex); + _yaw_servo->setTorqueEnabled(enabled); + _pitch_servo->setTorqueEnabled(enabled); +} + +void Motion::setAutoTorqueReleaseEnabled(bool enabled) +{ + std::lock_guard lock(_mutex); + _yaw_servo->setAutoTorqueReleaseEnabled(enabled); + _pitch_servo->setAutoTorqueReleaseEnabled(enabled); +} + +void Motion::setAutoAngleSyncEnabled(bool enabled) +{ + std::lock_guard lock(_mutex); + _yaw_servo->setAutoAngleSyncEnabled(enabled); + _pitch_servo->setAutoAngleSyncEnabled(enabled); +} + +void Motion::setCurrentPostionAsHome() +{ + std::lock_guard lock(_mutex); + _yaw_servo->setCurrentAngleAsZero(); + _pitch_servo->setCurrentAngleAsZero(); +} diff --git a/src/utils/motion/motion.h b/src/utils/motion/motion.h new file mode 100644 index 0000000..079a274 --- /dev/null +++ b/src/utils/motion/motion.h @@ -0,0 +1,203 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#pragma once +#include "servo.h" +#include "../uitk/src/smooth_ui_toolkit.hpp" +#include +#include +#include +#include +#include + +namespace stackchan::motion { + +/** + * @brief + * + */ +class Motion { +public: + ~Motion(); + + /** + * @brief + * + */ + void init(std::unique_ptr yawServo, std::unique_ptr pitchServo); + + /** + * @brief + * + * @param angle + * @param speed (0-1000) + */ + void moveYaw(int angle, int speed = 500); + inline void moveX(int angle, int speed = 500) + { + moveYaw(angle, speed); + } + + /** + * @brief + * + * @param angle + * @param speed (0-1000) + */ + void movePitch(int angle, int speed = 500); + inline void moveY(int angle, int speed = 500) + { + movePitch(angle, speed); + } + + /** + * @brief + * + * @param yawAngle + * @param pitchAngle + * @param speed (0-1000) + */ + void move(int yawAngle, int pitchAngle, int speed = 500); + + /** + * @brief Move head to home position (0,0) + * + * @param speed (0-1000) + */ + void goHome(int speed = 500); + + /** + * @brief Stop head movement + * + */ + void stop(); + + /** + * @brief Rotate yaw servo with given velocity + * + * @param velocity (-1000, 1000) + * Negative values for clockwise (CW), + * positive values for counter-clockwise (CCW). + */ + void rotateYaw(int velocity); + inline void rotateX(int velocity) + { + rotateYaw(velocity); + } + + /** + * @brief Moves the head using normalized coordinates ranging from -1.0 to 1.0. + * + * This method maps a proportional input to the full physical range of the servos. + * It is ideal for visual tracking (e.g., centering a face in a camera frame) + * or joystick-based control. + * + * Mapping Logic: + * - X-axis (Yaw): -1.0 (Max Left) <---> 1.0 (Max Right). 0.0 is center. + * - Y-axis (Pitch): -1.0 (Max Down) <---> 1.0 (Max Up). 0.0 is the midpoint of the pitch range. + * + * @param x Normalized horizontal value [-1.0, 1.0]. + * @param y Normalized vertical value [-1.0, 1.0]. + * @param speed Movement speed from 0 to 1000. + * + * @note The actual angles are calculated based on the servo's `getAngleLimit()`. + * For example, if Pitch range is 0 to 900, y = -1.0 maps to 0 and y = 1.0 maps to 900. + */ + void lookAtNormalized(float x, float y, int speed = 500); + + /** + * @brief Directs the head to look at a target point in 3D Cartesian space. + * + * This method performs Inverse Kinematics (IK) to convert 3D coordinates + * into Yaw and Pitch angles. It assumes the head rotation center is at (0,0,0). + * + * Coordinate System (Right-Handed): + * - X-axis: Forward (positive is in front of the robot). + * - Y-axis: Lateral (positive is to the left, negative is to the right). + * - Z-axis: Vertical (positive is up, negative is down). + * + * @param x The forward distance from the rotation center (usually in mm). + * @param y The lateral distance; positive values move the head left (usually in mm). + * @param z The vertical distance; positive values move the head up (usually in mm). + * @param speed The movement speed, ranging from 0 (slowest) to 1000 (fastest). + * + * @note If the target point is at (0,0,0), the behavior is undefined (mathematical singularity). + */ + void lookAtPoint(float x, float y, float z, int speed = 500); + + bool isMoving(); + bool isYawMoving(); + inline bool isXMoving() + { + return isYawMoving(); + } + bool isPitchMoving(); + inline bool isYMoving() + { + return isPitchMoving(); + } + + uitk_intl::Vector2i getCurrentAngles(); + int getCurrentYawAngle(); + inline int getCurrentXAngle() + { + return getCurrentYawAngle(); + } + int getCurrentPitchAngle(); + inline int getCurrentYAngle() + { + return getCurrentPitchAngle(); + } + + void setTorqueEnabled(bool enabled); + + /** + * @brief Set auto torque release enabled. + * If enabled, torque will auto release on rest. + * + * @param enabled + */ + void setAutoTorqueReleaseEnabled(bool enabled); + + /** + * @brief Enables or disables automatic synchronization of the animation start point + * with the current physical angle. + * + * @param enabled + * - If true: Prevents sudden "jumps" when the servo is moved manually by + * external forces, but may cause stuttering during high-frequency updates + * as it resets the animation's velocity. + * - If false: Maintains animation momentum and velocity for smooth, + * continuous motion, but may cause a "snap" if the actual angle differs + * significantly from the internal state. + */ + void setAutoAngleSyncEnabled(bool enabled); + + /** + * @brief Set the current postion as home position (0,0) + * + */ + void setCurrentPostionAsHome(); + +private: + void update(); + static void update_task(void* param); + + std::unique_ptr _yaw_servo; + std::unique_ptr _pitch_servo; + + std::mutex _mutex; + TaskHandle_t _task_handle{nullptr}; + std::atomic _running{false}; + + static constexpr float _RAD_TO_DEG = 180.0f / M_PI; + + inline float to_degrees(float radians) + { + return radians * _RAD_TO_DEG; + } +}; + +} // namespace stackchan::motion diff --git a/src/utils/motion/servo.cpp b/src/utils/motion/servo.cpp new file mode 100644 index 0000000..7dd34b1 --- /dev/null +++ b/src/utils/motion/servo.cpp @@ -0,0 +1,147 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include "servo.h" +#include + +using namespace uitk_intl; + +namespace stackchan::motion { + +static SpringOptions_t _default_spring_options = { + .stiffness = 170.0, + .damping = 26.0, + + .mass = 1.0, + .velocity = 0.0, + + .restSpeed = 0.1, + .restDelta = 0.1, + + .duration = 0.0, + .bounce = 0.0, + .visualDuration = 0.0, +}; + +void Servo::init() +{ + apply_default_spring_options(); + + _angle_anim.teleport(getCurrentAngle()); + update(); + + setTorqueEnabled(false); +} + +void Servo::update() +{ + // Keep update in at most 50Hz + if (millis() - _last_tick < 20) { + return; + } + _last_tick = millis(); + + // Apply animation + if (!_angle_anim.done()) { + _angle_anim.updateWithDelta(0.02f); // Fixed delta time for consistency + set_angle_impl(static_cast(_angle_anim.directValue())); + } + + // Snap to target angle when animation ends + else if (_snap_to_target_on_rest) { + _snap_to_target_on_rest = false; + set_angle_impl(_angle_anim.end); + } + + // Auto release torque on rest + else if (_auto_torque_release_enabled && !isMoving()) { + if (millis() - _last_torque_check_tick > 200) { + if (getTorqueEnabled()) { + setTorqueEnabled(false); + } + _last_torque_check_tick = millis(); + } + } +} + +void Servo::move(int angle) +{ + apply_default_spring_options(); + update_angle_anim_target(angle); +} + +void Servo::moveWithSpringParams(int angle, float stiffness, float damping) +{ + _angle_anim.springOptions().visualDuration = 0.0f; // Disable timing override + _angle_anim.springOptions().stiffness = stiffness; + _angle_anim.springOptions().damping = damping; + + update_angle_anim_target(angle); +} + +void Servo::moveWithSpeed(int angle, int speed) +{ + auto spring_options = map_speed_to_spring_options(speed); + moveWithSpringParams(angle, spring_options.stiffness, spring_options.damping); +} + +int Servo::getCurrentAngle() +{ + return _angle_anim.directValue(); +} + +bool Servo::isMoving() +{ + return _angle_anim.done() == false || is_moving_impl(); +} + +void Servo::apply_default_spring_options() +{ + auto& options = _angle_anim.springOptions(); + options.visualDuration = 0.0f; // Disable timing override + options.stiffness = _default_spring_options.stiffness; + options.damping = _default_spring_options.damping; +} + +void Servo::update_angle_anim_target(int angle) +{ + angle = uitk_intl::clamp(angle, _angle_limit.x, _angle_limit.y); + + if (_auto_angle_sync_enabled) { + _angle_anim.teleport(getCurrentAngle()); // Use current angle as start + } + _angle_anim = angle; // Apply new target + _snap_to_target_on_rest = true; +} + +uitk_intl::SpringOptions_t Servo::map_speed_to_spring_options(int speed) +{ + speed = uitk_intl::clamp(speed, 0, 1000); + + float k_min = 10.0f; + float k_max = 650.0f; + float normalizedSpeed = speed / 1000.0f; + float stiffness = k_min + (normalizedSpeed * normalizedSpeed) * (k_max - k_min); + + float mass = 1.0f; + float damping = 2.0f * sqrtf(mass * stiffness); + + uitk_intl::SpringOptions_t options = _default_spring_options; + options.stiffness = stiffness; + options.damping = damping; + options.mass = mass; + + if (speed > 800) { + options.restDelta = 0.5f; + options.restSpeed = 0.5f; + } else { + options.restDelta = 0.1f; + options.restSpeed = 0.1f; + } + + return options; +} + +} // namespace stackchan::motion diff --git a/src/utils/motion/servo.h b/src/utils/motion/servo.h new file mode 100644 index 0000000..7a0e09a --- /dev/null +++ b/src/utils/motion/servo.h @@ -0,0 +1,174 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#pragma once +#include "../uitk/src/smooth_ui_toolkit.hpp" +#include + +namespace stackchan::motion { + +/** + * @brief + * + */ +class Servo { +public: + virtual ~Servo() = default; + + /** + * @brief + * + */ + virtual void init(); + + /** + * @brief + * + */ + virtual void update(); + + /** + * @brief Move to angle + * + * @param angle + */ + void move(int angle); + + /** + * @brief Move to angle with custom spring params + * + * @param angle + * @param stiffness + * @param damping + */ + void moveWithSpringParams(int angle, float stiffness = 170.0f, float damping = 26.0f); + + /** + * @brief Move to angle with speed mapping + * + * @param angle + * @param speed (0-1000) + */ + void moveWithSpeed(int angle, int speed); + + /** + * @brief Rotate servo with given velocity + * + * @param velocity (-1000, 1000) + */ + virtual void rotate(int velocity) + { + } + + /** + * @brief Get servo current angle + * + * @return int + */ + virtual int getCurrentAngle(); + + /** + * @brief + * + * @return uitk_intl::Vector2i + */ + virtual uitk_intl::Vector2i getAngleLimit() const + { + return _angle_limit; + } + + /** + * @brief + * + * @return true + * @return false + */ + bool isMoving(); + + /** + * @brief + * + * @param enabled + */ + virtual void setTorqueEnabled(bool enabled) + { + } + virtual bool getTorqueEnabled() + { + return false; + } + + /** + * @brief Auto release torque on rest + * + * @param enabled + */ + void setAutoTorqueReleaseEnabled(bool enabled) + { + _auto_torque_release_enabled = enabled; + } + + /** + * @brief Enables or disables automatic synchronization of the animation start point + * with the current physical angle. + * + * @param enabled + * - If true: Prevents sudden "jumps" when the servo is moved manually by + * external forces, but may cause stuttering during high-frequency updates + * as it resets the animation's velocity. + * - If false: Maintains animation momentum and velocity for smooth, + * continuous motion, but may cause a "snap" if the actual angle differs + * significantly from the internal state. + */ + void setAutoAngleSyncEnabled(bool enabled) + { + _auto_angle_sync_enabled = enabled; + } + + /** + * @brief + * + */ + virtual void setCurrentAngleAsZero() + { + } + +protected: + Servo() + { + } + + void set_angle_limit(uitk_intl::Vector2i angleLimit) + { + _angle_limit = angleLimit; + } + + /** + * @brief Servo set angle implementation + * + * @param angle + */ + virtual void set_angle_impl(int angle) = 0; + virtual bool is_moving_impl() + { + return false; + } + +protected: + uitk_intl::Vector2i _angle_limit; + uitk_intl::AnimateValue _angle_anim; + + uint32_t _last_tick = 0; + uint32_t _last_torque_check_tick = 0; + bool _snap_to_target_on_rest = false; + bool _auto_torque_release_enabled = true; + bool _auto_angle_sync_enabled = true; + + void apply_default_spring_options(); + void update_angle_anim_target(int angle); + uitk_intl::SpringOptions_t map_speed_to_spring_options(int speed); +}; + +} // namespace stackchan::motion diff --git a/src/utils/settings/settings.cpp b/src/utils/settings/settings.cpp new file mode 100644 index 0000000..460b781 --- /dev/null +++ b/src/utils/settings/settings.cpp @@ -0,0 +1,119 @@ +// https://github.com/78/xiaozhi-esp32 +#include "settings.h" + +#include +#include + +#define TAG "Settings" + +Settings::Settings(const std::string& ns, bool read_write) : ns_(ns), read_write_(read_write) +{ + nvs_open(ns.c_str(), read_write_ ? NVS_READWRITE : NVS_READONLY, &nvs_handle_); +} + +Settings::~Settings() +{ + if (nvs_handle_ != 0) { + if (read_write_ && dirty_) { + ESP_ERROR_CHECK(nvs_commit(nvs_handle_)); + } + nvs_close(nvs_handle_); + } +} + +std::string Settings::GetString(const std::string& key, const std::string& default_value) +{ + if (nvs_handle_ == 0) { + return default_value; + } + + size_t length = 0; + if (nvs_get_str(nvs_handle_, key.c_str(), nullptr, &length) != ESP_OK) { + return default_value; + } + + std::string value; + value.resize(length); + ESP_ERROR_CHECK(nvs_get_str(nvs_handle_, key.c_str(), value.data(), &length)); + while (!value.empty() && value.back() == '\0') { + value.pop_back(); + } + return value; +} + +void Settings::SetString(const std::string& key, const std::string& value) +{ + if (read_write_) { + ESP_ERROR_CHECK(nvs_set_str(nvs_handle_, key.c_str(), value.c_str())); + dirty_ = true; + } else { + ESP_LOGW(TAG, "Namespace %s is not open for writing", ns_.c_str()); + } +} + +int32_t Settings::GetInt(const std::string& key, int32_t default_value) +{ + if (nvs_handle_ == 0) { + return default_value; + } + + int32_t value; + if (nvs_get_i32(nvs_handle_, key.c_str(), &value) != ESP_OK) { + return default_value; + } + return value; +} + +void Settings::SetInt(const std::string& key, int32_t value) +{ + if (read_write_) { + ESP_ERROR_CHECK(nvs_set_i32(nvs_handle_, key.c_str(), value)); + dirty_ = true; + } else { + ESP_LOGW(TAG, "Namespace %s is not open for writing", ns_.c_str()); + } +} + +bool Settings::GetBool(const std::string& key, bool default_value) +{ + if (nvs_handle_ == 0) { + return default_value; + } + + uint8_t value; + if (nvs_get_u8(nvs_handle_, key.c_str(), &value) != ESP_OK) { + return default_value; + } + return value != 0; +} + +void Settings::SetBool(const std::string& key, bool value) +{ + if (read_write_) { + ESP_ERROR_CHECK(nvs_set_u8(nvs_handle_, key.c_str(), value ? 1 : 0)); + dirty_ = true; + } else { + ESP_LOGW(TAG, "Namespace %s is not open for writing", ns_.c_str()); + } +} + +void Settings::EraseKey(const std::string& key) +{ + if (read_write_) { + auto ret = nvs_erase_key(nvs_handle_, key.c_str()); + if (ret != ESP_ERR_NVS_NOT_FOUND) { + ESP_ERROR_CHECK(ret); + } + } else { + ESP_LOGW(TAG, "Namespace %s is not open for writing", ns_.c_str()); + } +} + +void Settings::EraseAll() +{ + if (read_write_) { + ESP_ERROR_CHECK(nvs_erase_all(nvs_handle_)); + } else { + ESP_LOGW(TAG, "Namespace %s is not open for writing", ns_.c_str()); + } +} diff --git a/src/utils/settings/settings.h b/src/utils/settings/settings.h new file mode 100644 index 0000000..6ecf154 --- /dev/null +++ b/src/utils/settings/settings.h @@ -0,0 +1,29 @@ +// https://github.com/78/xiaozhi-esp32 +#ifndef SETTINGS_H +#define SETTINGS_H + +#include +#include + +class Settings { +public: + Settings(const std::string& ns, bool read_write = false); + ~Settings(); + + std::string GetString(const std::string& key, const std::string& default_value = ""); + void SetString(const std::string& key, const std::string& value); + int32_t GetInt(const std::string& key, int32_t default_value = 0); + void SetInt(const std::string& key, int32_t value); + bool GetBool(const std::string& key, bool default_value = false); + void SetBool(const std::string& key, bool value); + void EraseKey(const std::string& key); + void EraseAll(); + +private: + std::string ns_; + nvs_handle_t nvs_handle_ = 0; + bool read_write_ = false; + bool dirty_ = false; +}; + +#endif diff --git a/src/utils/touch_sensor/touch_sensor.cpp b/src/utils/touch_sensor/touch_sensor.cpp new file mode 100644 index 0000000..ba24488 --- /dev/null +++ b/src/utils/touch_sensor/touch_sensor.cpp @@ -0,0 +1,119 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include "touch_sensor.h" +#include "../compat/make_unique.h" +#include + +using namespace m5; + +static const char* TAG = "TouchSensor"; + +void TouchSensor_Class::begin() +{ + _touch_sensor = std::make_unique(SI12T_Type_High, SI12T_Sensitivity_Level_4); + _touch_sensor->begin(); + + _in_gesture = false; + _last_touched_time = 0; + for (int i = 0; i < 3; i++) { + _touched_flag[i] = false; + _touch_start_time[i] = 0; + } +} + +void TouchSensor_Class::update() +{ + _touch_sensor->read_touch_result(); + _touch_sensor->parse_touch_result(); + + bool pressed = false; + for (int i = 0; i < 3; i++) { + auto intensity = _touch_sensor->point_type[i]; + _intensities[(3 - 1) - i] = intensity; + if (intensity > 0) { + pressed = true; + } + } + + setRawState(millis(), pressed); + update_gesture(); +} + +void TouchSensor_Class::update_gesture() +{ + _swipe_result = SwipeDir::None; + + auto now = millis(); + bool any_touched = false; + for (int i = 0; i < 3; i++) { + if (_intensities[i] > 0) { + any_touched = true; + _last_touched_time = now; + if (!_touched_flag[i]) { + _touched_flag[i] = true; + _touch_start_time[i] = now; + } + } + } + + if (!any_touched) { + if (_in_gesture) { + _in_gesture = false; + } + // Always clear flags if no touch detected, this prepares for the next gesture + for (int i = 0; i < 3; i++) { + _touched_flag[i] = false; + } + return; + } + + // Check for swipes + if (_touched_flag[0] && _touched_flag[1] && _touched_flag[2]) { + if (_in_gesture) { + // Already reported or in hold + return; + } + + // Calculate time differences + // Front -> Back (0 -> 1 -> 2) + int32_t t1_0 = (int32_t)_touch_start_time[1] - (int32_t)_touch_start_time[0]; + int32_t t2_1 = (int32_t)_touch_start_time[2] - (int32_t)_touch_start_time[1]; + + // Back -> Front (2 -> 1 -> 0) + int32_t t1_2 = (int32_t)_touch_start_time[1] - (int32_t)_touch_start_time[2]; + int32_t t0_1 = (int32_t)_touch_start_time[0] - (int32_t)_touch_start_time[1]; + + const int32_t MAX_SWIPE_INTERVAL = 400; // ms between sensors + const int32_t MIN_SWIPE_INTERVAL = 30; // ms (filter noise) + + if (t1_0 > MIN_SWIPE_INTERVAL && t2_1 > MIN_SWIPE_INTERVAL && t1_0 < MAX_SWIPE_INTERVAL && + t2_1 < MAX_SWIPE_INTERVAL) { + _swipe_result = SwipeDir::Forward; + _in_gesture = true; + // ESP_LOGI(TAG, "Swipe Forward Detected"); + } else if (t1_2 > MIN_SWIPE_INTERVAL && t0_1 > MIN_SWIPE_INTERVAL && t1_2 < MAX_SWIPE_INTERVAL && + t0_1 < MAX_SWIPE_INTERVAL) { + _swipe_result = SwipeDir::Backward; + _in_gesture = true; + // ESP_LOGI(TAG, "Swipe Backward Detected"); + } + } +} + +bool TouchSensor_Class::wasSwiped() +{ + return _swipe_result != SwipeDir::None; +} + +bool TouchSensor_Class::wasSwipedForward() +{ + return _swipe_result == SwipeDir::Forward; +} + +bool TouchSensor_Class::wasSwipedBackward() +{ + return _swipe_result == SwipeDir::Backward; +} diff --git a/src/utils/touch_sensor/touch_sensor.h b/src/utils/touch_sensor/touch_sensor.h new file mode 100644 index 0000000..19b33e0 --- /dev/null +++ b/src/utils/touch_sensor/touch_sensor.h @@ -0,0 +1,69 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#pragma once +#include "../../drivers/Si12T/Si12T.h" +#include +#include + +namespace m5 { + +class TouchSensor_Class : public Button_Class { +public: + void begin(); + void update(); + + /** + * @brief Get the intensitiy values for three channels, + * index 0, 1, and 2 correspond to the Front, Middle, and Back physical zones respectively. + * Each channel's value ranges from 0 to 3, where 0 indicates no touch (idle) and 1–3 represent increasing levels of + * touch intensity. + * + * @return const std::array& + */ + inline const std::array& getIntensities() const + { + return _intensities; + } + + /** + * @brief Was swipe gesture detected. + * + * @return true + * @return false + */ + bool wasSwiped(); + + /** + * @brief Was swipe forward gesture (Front to back) detected. + * + * @return true + * @return false + */ + bool wasSwipedForward(); + + /** + * @brief Was swipe backward gesture (Back to front) detected. + * + * @return true + * @return false + */ + bool wasSwipedBackward(); + +private: + std::unique_ptr _touch_sensor; + std::array _intensities; + + enum class SwipeDir { None, Forward, Backward }; + SwipeDir _swipe_result; + uint32_t _touch_start_time[3]; + bool _touched_flag[3]; + bool _in_gesture; + uint32_t _last_touched_time; + + void update_gesture(); +}; + +} // namespace m5 diff --git a/src/utils/uitk/LICENSE b/src/utils/uitk/LICENSE new file mode 100644 index 0000000..53e54dd --- /dev/null +++ b/src/utils/uitk/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 Forairaaaaa + +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. diff --git a/src/utils/uitk/src/core/animation/animate/animate.cpp b/src/utils/uitk/src/core/animation/animate/animate.cpp new file mode 100644 index 0000000..b408d5d --- /dev/null +++ b/src/utils/uitk/src/core/animation/animate/animate.cpp @@ -0,0 +1,294 @@ +/** + * @file animate.cpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-07 + * + * @copyright Copyright (c) 2025 + * + */ +#include "animate.hpp" +#include "../../../core/hal/hal.hpp" +#include +#include + +using namespace uitk_intl; + +EasingOptions_t& Animate::easingOptions() +{ + if (animationType != AnimationType::Easing) { + animationType = AnimationType::Easing; + _generator_dirty = true; + } + return static_cast(get_key_frame_generator()).easingOptions; +} + +SpringOptions_t& Animate::springOptions() +{ + if (animationType != AnimationType::Spring) { + animationType = AnimationType::Spring; + _generator_dirty = true; + } + return static_cast(get_key_frame_generator()).springOptions; +} + +void Animate::init() +{ + // Setup key frame generator + get_key_frame_generator().start = start; + get_key_frame_generator().end = end; + get_key_frame_generator().init(); +} + +void Animate::play() +{ + if (_playing_state == AnimateState::Playing) { + return; + } + + // If paused, resume + if (_playing_state == AnimateState::Paused) { + _playing_state = _saved_state; + _last_tick = ui_hal::get_tick_s(); + } + // If not, reset repeat count and start time, start animation + else { + _repeat_count = repeat; + _current_duration = 0.0f; + _playing_state = delay > 0 ? AnimateState::Delaying : AnimateState::Playing; + get_key_frame_generator().done = false; + _last_tick = ui_hal::get_tick_s(); + } +} + +void Animate::pause() +{ + if (_playing_state == AnimateState::Playing || _playing_state == AnimateState::Delaying || + _playing_state == AnimateState::RepeatDelaying) { + _saved_state = _playing_state; + _playing_state = AnimateState::Paused; + } +} + +void Animate::complete() +{ + if (_playing_state == AnimateState::Completed) { + return; + } + _playing_state = AnimateState::Completed; + get_key_frame_generator().done = false; + get_key_frame_generator().value = end; +} + +void Animate::cancel() +{ + if (_playing_state == AnimateState::Cancelled) { + return; + } + _playing_state = AnimateState::Cancelled; + get_key_frame_generator().done = false; + get_key_frame_generator().value = start; +} + +void Animate::retarget(float start, float end) +{ + this->start = start; + this->end = end; + get_key_frame_generator().retarget(start, end); + if (_playing_state != AnimateState::Paused) { + _playing_state = AnimateState::Idle; + play(); + } +} + +void Animate::update() +{ + const float currentTime = ui_hal::get_tick_s(); + update(currentTime); +} + +void Animate::update(float currentTime) +{ + float dt = currentTime - _last_tick; + _last_tick = currentTime; + if (dt < 0) { + dt = 0; + } + update_state_machine(dt); +} + +void Animate::updateWithDelta(float dt) +{ + update_state_machine(dt); +} + +void Animate::update_state_machine(float dt) +{ + if (_playing_state == AnimateState::Completed || _playing_state == AnimateState::Cancelled) { + get_key_frame_generator().done = true; + if (_on_update) { + _on_update(value()); + } + return; + } + + if (_playing_state == AnimateState::Idle || _playing_state == AnimateState::Paused) { + return; + } + + _current_duration += dt; + + // Handle delay state + if (_playing_state == AnimateState::Delaying) { + // Invoke callback with current value + if (_on_update) { + _on_update(value()); + } + // Check delay timeout + if (_current_duration >= delay) { + float overflow = _current_duration - delay; + _playing_state = AnimateState::Playing; + _current_duration = overflow; + } else { + return; + } + } + + // Handle playing state + if (_playing_state == AnimateState::Playing) { + if (done()) { + return; + } + + // Update key frame + get_key_frame_generator().next(_current_duration); + if (_on_update) { + _on_update(value()); + } + + // Check if animation is done + if (done()) { + _playing_state = AnimateState::Completed; + if (_on_complete) { + _on_complete(); + } + + // Handle repeat + if (_repeat_count != 0) { + // Decrement repeat count + if (_repeat_count > 0) { + _repeat_count--; + } + + // Check if we need repeat delay + if (repeatDelay > 0) { + _playing_state = AnimateState::RepeatDelaying; + _current_duration = 0; + } else { + // Reset animation immediately + if (repeatType == AnimateRepeatType::Reverse) { + std::swap(start, end); + } + init(); + + if (delay > 0) { + _playing_state = AnimateState::Delaying; + _current_duration = 0; + } else { + _playing_state = AnimateState::Playing; + _current_duration = 0; + } + } + } + } + return; + } + + // Handle repeat delay state + if (_playing_state == AnimateState::RepeatDelaying) { + // Check repeat delay timeout + if (_current_duration >= repeatDelay) { + float overflow = _current_duration - repeatDelay; + // Reset animation + if (repeatType == AnimateRepeatType::Reverse) { + std::swap(start, end); + } + init(); + + if (delay > 0) { + _playing_state = AnimateState::Delaying; + _current_duration = overflow; + } else { + _playing_state = AnimateState::Playing; + _current_duration = overflow; + } + } + return; + } +} + +// Lazy loading, default spring +KeyFrameGenerator& Animate::get_key_frame_generator() +{ + if (_generator_dirty || !_key_frame_generator) { + _key_frame_generator.reset(); + if (animationType == AnimationType::Spring) { + _key_frame_generator = std::make_unique(); + } else if (animationType == AnimationType::Easing) { + _key_frame_generator = std::make_unique(); + } + _generator_dirty = false; + } + return *_key_frame_generator; +} + +Animate::Animate(Animate&& other) noexcept + : start(other.start), + end(other.end), + delay(other.delay), + repeat(other.repeat), + repeatType(other.repeatType), + repeatDelay(other.repeatDelay), + animationType(other.animationType), + _on_update(std::move(other._on_update)), + _on_complete(std::move(other._on_complete)), + _key_frame_generator(std::move(other._key_frame_generator)), + _playing_state(other._playing_state), + _saved_state(other._saved_state), + _last_tick(other._last_tick), + _current_duration(other._current_duration), + _repeat_count(other._repeat_count), + _generator_dirty(other._generator_dirty) +{ + // Reset other object to default state + other._playing_state = AnimateState::Idle; + other._generator_dirty = true; +} + +Animate& Animate::operator=(Animate&& other) noexcept +{ + if (this != &other) { + start = other.start; + end = other.end; + delay = other.delay; + repeat = other.repeat; + repeatType = other.repeatType; + repeatDelay = other.repeatDelay; + animationType = other.animationType; + _on_update = std::move(other._on_update); + _on_complete = std::move(other._on_complete); + _key_frame_generator = std::move(other._key_frame_generator); + _playing_state = other._playing_state; + _saved_state = other._saved_state; + _last_tick = other._last_tick; + _current_duration = other._current_duration; + _repeat_count = other._repeat_count; + _generator_dirty = other._generator_dirty; + + // Reset other object to default state + other._playing_state = AnimateState::Idle; + other._generator_dirty = true; + } + return *this; +} diff --git a/src/utils/uitk/src/core/animation/animate/animate.hpp b/src/utils/uitk/src/core/animation/animate/animate.hpp new file mode 100644 index 0000000..c4e0c27 --- /dev/null +++ b/src/utils/uitk/src/core/animation/animate/animate.hpp @@ -0,0 +1,179 @@ +/** + * @file animate.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-07 + * + * @copyright Copyright (c) 2025 + * + */ +#pragma once +#include "../generators/generators.hpp" +#include "../generators/spring/spring.hpp" +#include "../generators/easing/easing.hpp" +#include +#include + +namespace uitk_intl { + +enum class AnimateRepeatType { + Loop = 0, // 循环播放 + Reverse, // 反向播放 +}; + +enum class AnimateState { + Idle = 0, // 空闲状态 + Delaying, // 等待开始（delay阶段） + Playing, // 正在播放动画 + Paused, // 暂停 + RepeatDelaying, // 重复等待阶段 + Completed, // 完成 + Cancelled // 取消 +}; + +class Animate { +public: + Animate() {} + virtual ~Animate() {} + + // Disable copy constructor and copy assignment operator + Animate(const Animate&) = delete; + Animate& operator=(const Animate&) = delete; + + // Enable move constructor and move assignment operator + Animate(Animate&& other) noexcept; + Animate& operator=(Animate&& other) noexcept; + + // 参数参考：https://motion.dev/docs/animate#options + + // 开始值 + float start = 0.0f; + // 结束值 + float end = 0.0f; + // 动画开始前延迟（秒） + float delay = 0.0f; + // 重复次数，-1 表示无限循环 + int repeat = 0; + // 重复类型 + AnimateRepeatType repeatType = AnimateRepeatType::Loop; + // 重复间隔时间（秒） + float repeatDelay = 0.0f; + // 动画类型 + AnimationType animationType = AnimationType::Spring; + // easing 动画配置，调用此方法，动画类型将自动切换为 easing + EasingOptions_t& easingOptions(); + // spring 动画配置，调用此方法，动画类型将自动切换为 spring + SpringOptions_t& springOptions(); + // 值更新回调 + void onUpdate(std::function callback) + { + _on_update = callback; + } + // 动画完成回调 + void onComplete(std::function callback) + { + _on_complete = callback; + } + + /** + * @brief Init animation + * + */ + void init(); + + /** + * @brief Start playing animation, If an animation is paused, it will resume from its current time, If animation has + * finished, it will restart + * + */ + void play(); + + /** + * @brief Pauses the animation until resumed with play() + * + */ + void pause(); + + /** + * @brief Immediately completes the animation, running it to the end state + * + */ + void complete(); + + /** + * @brief Cancels the animation, reverting it to the initial state + * + */ + void cancel(); + + /** + * @brief Reset start and end value dynamically, spring animation will animate to new value with current velocity + * + * @param start + * @param end + */ + void retarget(float start, float end); + + /** + * @brief Update animation, keep calling this method to update animation, callbacks will be invoked in this method + * + */ + void update(); + + /** + * @brief Update animation with explicit current time, more efficient for batch updates + * + * @param currentTime Current time in seconds + */ + void update(float currentTime); + + /** + * @brief Update animation with delta time + * + * @param dt Delta time in seconds + */ + void updateWithDelta(float dt); + + /** + * @brief Is key frame generator done + * + * @return true + * @return false + */ + inline bool done() + { + return get_key_frame_generator().done; + } + + /** + * @brief Get key frame generator current value + * + * @return float + */ + inline float value() + { + return get_key_frame_generator().value; + } + + inline AnimateState currentPlayingState() + { + return _playing_state; + } + +protected: + std::function _on_update; + std::function _on_complete; + std::unique_ptr _key_frame_generator; + KeyFrameGenerator& get_key_frame_generator(); + AnimateState _playing_state = AnimateState::Idle; + AnimateState _saved_state = AnimateState::Idle; + float _last_tick = 0.0f; + float _current_duration = 0.0f; + int _repeat_count = 0; + bool _generator_dirty = true; + + void update_state_machine(float dt); +}; + +} // namespace smooth_ui_toolkit diff --git a/src/utils/uitk/src/core/animation/animate_value/animate_value.cpp b/src/utils/uitk/src/core/animation/animate_value/animate_value.cpp new file mode 100644 index 0000000..4c33d5d --- /dev/null +++ b/src/utils/uitk/src/core/animation/animate_value/animate_value.cpp @@ -0,0 +1,119 @@ +/** + * @file animate_value.cpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-08 + * + * @copyright Copyright (c) 2025 + * + */ +#include "animate_value.hpp" + +using namespace uitk_intl; + +AnimateValue::AnimateValue() +{ + begin(); +} + +AnimateValue::AnimateValue(float defaultValue) : _default_value(defaultValue) +{ + begin(); +} + +AnimateValue& AnimateValue::operator=(float newValue) +{ + move(newValue); + return *this; +} + +AnimateValue::operator float() +{ + return value(); +} + +void AnimateValue::begin() +{ + if (_is_begin) { + return; + } + _is_begin = true; + start = _default_value; + end = _default_value; + get_key_frame_generator().value = _default_value; + init(); + play(); +} + +void AnimateValue::stop() +{ + if (!_is_begin) { + return; + } + _is_begin = false; + _default_value = value(); +} + +void AnimateValue::teleport(float newValue) +{ + bool is_begin = _is_begin; + stop(); + _default_value = newValue; + if (is_begin) { + begin(); + } +} + +void AnimateValue::move(float newValue) +{ + // If not begin yet, set to default value + if (!_is_begin) { + _default_value = newValue; + return; + } + // If same value, do nothing + if (newValue == get_key_frame_generator().end) { + return; + } + // Else, retarget to new value + retarget(value(), newValue); + return; +} + +float AnimateValue::value() +{ + if (!_is_begin) { + return _default_value; + } + update(); + return get_key_frame_generator().value; +} + +float AnimateValue::directValue() +{ + if (!_is_begin) { + return _default_value; + } + return get_key_frame_generator().value; +} + +AnimateValue::AnimateValue(AnimateValue&& other) noexcept + : Animate(std::move(other)), _is_begin(other._is_begin), _default_value(other._default_value) +{ + other._is_begin = false; + other._default_value = 0.0f; +} + +AnimateValue& AnimateValue::operator=(AnimateValue&& other) noexcept +{ + if (this != &other) { + Animate::operator=(std::move(other)); + _is_begin = other._is_begin; + _default_value = other._default_value; + + other._is_begin = false; + other._default_value = 0.0f; + } + return *this; +} diff --git a/src/utils/uitk/src/core/animation/animate_value/animate_value.hpp b/src/utils/uitk/src/core/animation/animate_value/animate_value.hpp new file mode 100644 index 0000000..bfb5745 --- /dev/null +++ b/src/utils/uitk/src/core/animation/animate_value/animate_value.hpp @@ -0,0 +1,76 @@ +/** + * @file animate_value.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-08 + * + * @copyright Copyright (c) 2025 + * + */ +#pragma once +#include "../animate/animate.hpp" + +namespace uitk_intl { + +class AnimateValue : public Animate { +public: + AnimateValue(); + AnimateValue(float defaultValue); + ~AnimateValue() {} + + // Enable move constructor and move assignment operator + AnimateValue(AnimateValue&& other) noexcept; + AnimateValue& operator=(AnimateValue&& other) noexcept; + + // Override assignment operator + AnimateValue& operator=(float newValue); + + // Override type conversion + operator float(); + + /** + * @brief Begin value animation + * + */ + void begin(); + + /** + * @brief Stop value animation + * + */ + void stop(); + + /** + * @brief Set to a new value immediately + * + */ + void teleport(float newValue); + + /** + * @brief Move to a new value + * + * @param newValue + */ + void move(float newValue); + + /** + * @brief Current value + * + * @return float + */ + float value(); + + /** + * @brief Get current value without trigger auto update + * + * @return float + */ + float directValue(); + +private: + bool _is_begin = false; + float _default_value = 0.0f; +}; + +} // namespace smooth_ui_toolkit diff --git a/src/utils/uitk/src/core/animation/generators/easing/easing.cpp b/src/utils/uitk/src/core/animation/generators/easing/easing.cpp new file mode 100644 index 0000000..3144edc --- /dev/null +++ b/src/utils/uitk/src/core/animation/generators/easing/easing.cpp @@ -0,0 +1,46 @@ +/** + * @file easing.cpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-03-29 + * + * @copyright Copyright (c) 2025 + * + */ +#include "easing.hpp" + +using namespace uitk_intl; + +void Easing::init() +{ + done = false; + value = start; + _range = end - start; // 预计算差值 + _inv_duration = 1.0f / easingOptions.duration; // 预计算持续时间倒数 +} + +void Easing::retarget(float start, float end) +{ + this->start = start; + this->end = end; + _range = end - start; // 重新计算差值 + done = false; +} + +bool Easing::next(float t) +{ + if (done) { + return done; + } + + float progress = t * _inv_duration; + if (progress >= 1.0f) { + value = end; + done = true; + return done; // 早期退出，避免后续计算 + } + + value = start + _range * easingOptions.easingFunction(progress); + return done; +} diff --git a/src/utils/uitk/src/core/animation/generators/easing/easing.hpp b/src/utils/uitk/src/core/animation/generators/easing/easing.hpp new file mode 100644 index 0000000..120d369 --- /dev/null +++ b/src/utils/uitk/src/core/animation/generators/easing/easing.hpp @@ -0,0 +1,55 @@ +/** + * @file easing.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-03-29 + * + * @copyright Copyright (c) 2025 + * + */ +#pragma once +#include "../generators.hpp" +#include "../../../../core/easing/ease.hpp" +#include + +namespace uitk_intl { + +struct EasingOptions_t { + float duration = 1.0f; // 动画持续时间，单位 s + std::function easingFunction = ease::ease_in_out_quad; // 缓动函数 +}; + +class Easing : public KeyFrameGenerator { +public: + Easing() {} + ~Easing() {} + + EasingOptions_t easingOptions; + + inline void setEasingOptions(float duration, std::function easingFunction) + { + easingOptions.duration = duration; + easingOptions.easingFunction = easingFunction; + _inv_duration = 1.0f / duration; // 更新持续时间倒数 + } + inline void setEasingOptions(const EasingOptions_t& options) + { + easingOptions = options; + _inv_duration = 1.0f / options.duration; // 更新持续时间倒数 + } + + virtual void init() override; + virtual void retarget(float start, float end) override; + virtual bool next(float t) override; + virtual AnimationType type() const override + { + return AnimationType::Easing; + } + +private: + float _range = 0.0f; // 缓存 end - start 的计算结果 + float _inv_duration = 1.0f; // 缓存 1.0f / duration 的计算结果 +}; + +} // namespace smooth_ui_toolkit diff --git a/src/utils/uitk/src/core/animation/generators/generators.hpp b/src/utils/uitk/src/core/animation/generators/generators.hpp new file mode 100644 index 0000000..543c5ed --- /dev/null +++ b/src/utils/uitk/src/core/animation/generators/generators.hpp @@ -0,0 +1,42 @@ +/** + * @file generators.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-06 + * + * @copyright Copyright (c) 2025 + * + */ +#pragma once + +namespace uitk_intl { + +enum class AnimationType { + Spring = 0, + Easing, +}; + +class KeyFrameGenerator { +public: + KeyFrameGenerator() {} + virtual ~KeyFrameGenerator() {} + + float start = 0.0f; + float end = 0.0f; + float value = 0.0f; + bool done = false; + + virtual void init() {} + + virtual void retarget(float start, float end) {} + + virtual bool next(float t) + { + return done; + } + + virtual AnimationType type() const = 0; +}; + +} // namespace smooth_ui_toolkit diff --git a/src/utils/uitk/src/core/animation/generators/spring/spring.cpp b/src/utils/uitk/src/core/animation/generators/spring/spring.cpp new file mode 100644 index 0000000..79a9f9a --- /dev/null +++ b/src/utils/uitk/src/core/animation/generators/spring/spring.cpp @@ -0,0 +1,164 @@ +/** + * @file spring.cpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-06 + * + * @copyright Copyright (c) 2025 + * + */ +#include "spring.hpp" +#include +#include + +using namespace uitk_intl; + +void Spring::setSpringOptions(float duration, float bounce, float visualDuration) +{ + // 默认质量 + springOptions.mass = 1.0f; + + // 确保 bounce 在 0.05 到 1 之间 + bounce = std::clamp(bounce, 0.05f, 1.0f); + + // 根据 visualDuration 计算 stiffness 和 damping + if (visualDuration > 0) { + float period_factor = (2 * M_PI) / (visualDuration * 1.2f); // 可视化周期 + springOptions.stiffness = period_factor * period_factor; // 刚度 + + // 预计算sqrt避免重复计算 + float sqrt_stiffness_mass = period_factor; // 因为mass=1，所以sqrt(stiffness*mass) = period_factor + springOptions.damping = 2 * (1.0f - bounce) * sqrt_stiffness_mass; // 阻尼系数 + } else { + // 如果未提供 visualDuration，使用默认的 duration 和 bounce 来推导 + float duration_s = duration / 1000.0f; + springOptions.stiffness = 36.0f / (duration_s * duration_s); // 刚度 + + // 预计算sqrt避免重复计算 + float sqrt_stiffness_mass = 6.0f / duration_s; // sqrt(36/duration²) = 6/duration + springOptions.damping = 2 * (1.0f - bounce) * sqrt_stiffness_mass; // 阻尼系数 + } +} + +void Spring::init() +{ + done = false; + value = start; + + // If or duration or visualDuration is provided, set spring options by duration/bounce-based options + if (springOptions.duration > 0 || springOptions.visualDuration > 0) { + setSpringOptions(springOptions.duration, springOptions.bounce, springOptions.visualDuration); + } + + // 预计算常量 - 优化计算顺序避免重复 + _sqrt_stiffness_mass = std::sqrt(springOptions.stiffness * springOptions.mass); + _undamped_angular_freq = _sqrt_stiffness_mass / springOptions.mass; // sqrt(k/m) = sqrt(k*m)/m + _damping_ratio = springOptions.damping / (2 * _sqrt_stiffness_mass); + _initial_delta = end - start; + + // 根据阻尼比选择不同的公式 + if (_damping_ratio < 1) { + _damping_type = DampingType::Underdamped; + _damped_angular_freq = calc_angular_freq(_undamped_angular_freq, _damping_ratio); + + // 欠阻尼 - 预计算速度公式系数 + _velocity_c1 = + (springOptions.velocity + _damping_ratio * _undamped_angular_freq * _initial_delta) / _damped_angular_freq; + _velocity_c2 = _initial_delta; + } else if (_damping_ratio == 1) { + _damping_type = DampingType::Critical; + // 临界阻尼 - 预计算速度公式系数 + _velocity_c1 = springOptions.velocity + _undamped_angular_freq * _initial_delta; + _velocity_c2 = _initial_delta; + } else { + _damping_type = DampingType::Overdamped; + // 过阻尼 - 预计算速度公式系数 + _damped_angular_freq = _undamped_angular_freq * std::sqrt(_damping_ratio * _damping_ratio - 1); + _velocity_c1 = + (springOptions.velocity + _damping_ratio * _undamped_angular_freq * _initial_delta) / _damped_angular_freq; + _velocity_c2 = _damped_angular_freq * _initial_delta; + } +} + +void Spring::retarget(float start, float end) +{ + springOptions.velocity = -_current_velocity; + this->start = start; + this->end = end; + init(); +} + +bool Spring::next(float t) +{ + if (done) { + return done; + } + + // 使用内联计算替代function对象 + value = calc_position(t); + + // 检查是否接近静止 + calc_velocity_analytical(t); + bool isBelowVelocityThreshold = std::abs(_current_velocity) <= springOptions.restSpeed; + bool isBelowDisplacementThreshold = std::abs(end - value) <= springOptions.restDelta; + done = isBelowVelocityThreshold && isBelowDisplacementThreshold; + + return done; +} + +void Spring::calc_velocity_analytical(float t) +{ + if (_damping_ratio < 1) { + // 欠阻尼解析速度公式 + float envelope = std::exp(-_damping_ratio * _undamped_angular_freq * t); + float sin_term = std::sin(_damped_angular_freq * t); + float cos_term = std::cos(_damped_angular_freq * t); + + _current_velocity = + envelope * (_damping_ratio * _undamped_angular_freq * (_velocity_c1 * sin_term + _velocity_c2 * cos_term) - + _damped_angular_freq * (_velocity_c1 * cos_term - _velocity_c2 * sin_term)); + } else if (_damping_ratio == 1) { + // 临界阻尼解析速度公式 + float envelope = std::exp(-_undamped_angular_freq * t); + _current_velocity = envelope * (_undamped_angular_freq * (_velocity_c2 + _velocity_c1 * t) - _velocity_c1); + } else { + // 过阻尼解析速度公式 + float envelope = std::exp(-_damping_ratio * _undamped_angular_freq * t); + float freqForT = std::min(_damped_angular_freq * t, 300.0f); + float sinh_term = std::sinh(freqForT); + float cosh_term = std::cosh(freqForT); + + _current_velocity = + envelope * + ((_damping_ratio * _undamped_angular_freq * (_velocity_c1 * sinh_term + _velocity_c2 * cosh_term)) / + _damped_angular_freq - + (_velocity_c1 * cosh_term + _velocity_c2 * sinh_term)); + } +} + +float Spring::calc_angular_freq(float undampedFreq, float dampingRatio) +{ + return undampedFreq * std::sqrt(1 - dampingRatio * dampingRatio); +} + +float Spring::calc_position(float t) +{ + switch (_damping_type) { + case DampingType::Underdamped: { + float envelope = std::exp(-_damping_ratio * _undamped_angular_freq * t); + return (end - envelope * (_velocity_c1 * std::sin(_damped_angular_freq * t) + + _velocity_c2 * std::cos(_damped_angular_freq * t))); + } + case DampingType::Critical: { + return end - std::exp(-_undamped_angular_freq * t) * (_velocity_c2 + _velocity_c1 * t); + } + case DampingType::Overdamped: { + float envelope = std::exp(-_damping_ratio * _undamped_angular_freq * t); + float freqForT = std::min(_damped_angular_freq * t, 300.0f); + return (end - (envelope * (_velocity_c1 * std::sinh(freqForT) + _velocity_c2 * std::cosh(freqForT))) / + _damped_angular_freq); + } + } + return end; // fallback +} diff --git a/src/utils/uitk/src/core/animation/generators/spring/spring.hpp b/src/utils/uitk/src/core/animation/generators/spring/spring.hpp new file mode 100644 index 0000000..c5260ef --- /dev/null +++ b/src/utils/uitk/src/core/animation/generators/spring/spring.hpp @@ -0,0 +1,81 @@ +/** + * @file spring.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-06 + * + * @copyright Copyright (c) 2025 + * + */ +#pragma once +#include "../generators.hpp" + +namespace uitk_intl { + +// 更详细参数含义可以参考：https://motion.dev/docs/spring#options +struct SpringOptions_t { + float stiffness = 100.0; // 弹性系数 + float damping = 10.0; // 阻尼系数 + float mass = 1.0; // 质量 + float velocity = 0.0; // 初始速度 + float restSpeed = 0.1; // 静止速度阈值 + float restDelta = 0.1; // 静止位置阈值 + float duration = 0.0; // 动画持续时间 ms + float bounce = 0.3; // 反弹系数 0.05~1.0 + float visualDuration = 0.0; // 可视化时间 s +}; + +class Spring : public KeyFrameGenerator { +public: + Spring() {} + ~Spring() {} + + SpringOptions_t springOptions; + + /** + * @brief Set spring options by duration/bounce-based options + * + * @param duration in ms + * @param bounce 0.05~1.0 + * @param visualDuration in seconds + */ + void setSpringOptions(float duration = 800.0f, float bounce = 0.3f, float visualDuration = 0.3f); + inline void setSpringOptions(const SpringOptions_t& options) + { + springOptions = options; + } + + virtual void init() override; + virtual void retarget(float start, float end) override; + virtual bool next(float t) override; + virtual AnimationType type() const override + { + return AnimationType::Spring; + } + +protected: + float _damping_ratio; // 阻尼比 + float _undamped_angular_freq; // 未阻尼角频率 + float _current_velocity; // 当前速度 + + // 移除function对象，使用内联计算 + enum class DampingType { Underdamped, Critical, Overdamped } _damping_type; + + // 预计算的常量，避免重复计算 + float _sqrt_stiffness_mass; // sqrt(stiffness * mass) + float _damped_angular_freq; // 阻尼角频率 + float _initial_delta; // 初始位置差 + + // 速度计算相关常量 + float _velocity_c1, _velocity_c2; // 速度公式系数 + + void calc_velocity(float t); + void calc_velocity_analytical(float t); + float calc_angular_freq(float undampedFreq, float dampingRatio); + + // 内联位置计算函数 + inline float calc_position(float t); +}; + +} // namespace smooth_ui_toolkit diff --git a/src/utils/uitk/src/core/core.hpp b/src/utils/uitk/src/core/core.hpp new file mode 100644 index 0000000..1bff92a --- /dev/null +++ b/src/utils/uitk/src/core/core.hpp @@ -0,0 +1,16 @@ +/** + * @file core.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-12-19 + * + * @copyright Copyright (c) 2025 + * + */ +#pragma once +#include "animation/animate/animate.hpp" +#include "animation/animate_value/animate_value.hpp" +#include "easing/ease.hpp" +#include "hal/hal.hpp" +#include "math/math.hpp" diff --git a/src/utils/uitk/src/core/easing/ease.cpp b/src/utils/uitk/src/core/easing/ease.cpp new file mode 100644 index 0000000..fcdebdb --- /dev/null +++ b/src/utils/uitk/src/core/easing/ease.cpp @@ -0,0 +1,19 @@ +/** + * @file ease.cpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-06 + * + * @copyright Copyright (c) 2025 + * + */ +#include "ease.hpp" +#include + +using namespace uitk_intl; + +float ease::ease_in_out_quad(float t) +{ + return t < 0.5 ? 2 * t * t : 1 - std::pow(-2 * t + 2, 2) / 2; +}; diff --git a/src/utils/uitk/src/core/easing/ease.hpp b/src/utils/uitk/src/core/easing/ease.hpp new file mode 100644 index 0000000..4b89fb1 --- /dev/null +++ b/src/utils/uitk/src/core/easing/ease.hpp @@ -0,0 +1,20 @@ +/** + * @file ease.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-06 + * + * @copyright Copyright (c) 2025 + * + */ +// Refs: https://easings.net +#pragma once + +namespace uitk_intl { +namespace ease { + +float ease_in_out_quad(float t); + +} // namespace ease +} // namespace smooth_ui_toolkit diff --git a/src/utils/uitk/src/core/hal/hal.cpp b/src/utils/uitk/src/core/hal/hal.cpp new file mode 100644 index 0000000..33f16f0 --- /dev/null +++ b/src/utils/uitk/src/core/hal/hal.cpp @@ -0,0 +1,92 @@ +/** + * @file hal.cpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-07 + * + * @copyright Copyright (c) 2025 + * + */ +#include "hal.hpp" + +#ifndef SMOOTH_UI_TOOLKIT_ENABLE_DEFAULT_HAL +#define SMOOTH_UI_TOOLKIT_ENABLE_DEFAULT_HAL 1 +#endif + +#if SMOOTH_UI_TOOLKIT_ENABLE_DEFAULT_HAL +#include +#include +#endif + +using namespace uitk_intl; + +#if SMOOTH_UI_TOOLKIT_ENABLE_DEFAULT_HAL +static std::chrono::steady_clock::time_point _start_time = std::chrono::steady_clock::now(); +static uint32_t _default_get_tick() +{ + auto now = std::chrono::steady_clock::now(); + return static_cast(std::chrono::duration_cast(now - _start_time).count()); +} +#else +static uint32_t _default_get_tick() +{ + return 0; // Pass function when default HAL is disabled +} +#endif +static std::function _custom_get_tick; + +void ui_hal::on_get_tick(std::function onGetTick) +{ + _custom_get_tick = onGetTick; +} + +uint32_t ui_hal::get_tick() +{ + if (_custom_get_tick) { + return _custom_get_tick(); + } + return _default_get_tick(); +} + +float ui_hal::get_tick_s() +{ + auto ms = get_tick(); + if (ms == 0) { + return 0; + } + return static_cast(ms) / 1000.0f; +} + +#if SMOOTH_UI_TOOLKIT_ENABLE_DEFAULT_HAL +static void _default_delay(uint32_t ms) +{ + std::this_thread::sleep_for(std::chrono::milliseconds(ms)); +} +#else +static void _default_delay(uint32_t ms) +{ + // Pass function when default HAL is disabled + (void)ms; // Suppress unused parameter warning +} +#endif +static std::function _custom_delay; + +void ui_hal::on_delay(std::function onDelay) +{ + _custom_delay = onDelay; +} + +void ui_hal::delay(uint32_t ms) +{ + if (_custom_delay) { + _custom_delay(ms); + return; + } + _default_delay(ms); +} + +void ui_hal::delay_s(float s) +{ + delay(static_cast(s * 1000)); +} diff --git a/src/utils/uitk/src/core/hal/hal.hpp b/src/utils/uitk/src/core/hal/hal.hpp new file mode 100644 index 0000000..514c18f --- /dev/null +++ b/src/utils/uitk/src/core/hal/hal.hpp @@ -0,0 +1,52 @@ +/** + * @file hal.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-01-07 + * + * @copyright Copyright (c) 2025 + * + */ +#pragma once +#include +#include +// https://wiki.libsdl.org/SDL2/SDL_GetTicks +// https://wiki.libsdl.org/SDL2/SDL_Delay + +namespace uitk_intl { +namespace ui_hal { + +void on_get_tick(std::function onGetTick); +void on_delay(std::function onDelay); + +/** + * @brief Get the number of milliseconds since running + * + * @return uint32_t + */ +uint32_t get_tick(); + +/** + * @brief Get the number of seconds since running + * + * @return float + */ +float get_tick_s(); + +/** + * @brief Wait a specified number of milliseconds before returning + * + * @param ms + */ +void delay(uint32_t ms); + +/** + * @brief Wait a specified number of seconds before returning + * + * @param s + */ +void delay_s(float s); + +} // namespace ui_hal +} // namespace smooth_ui_toolkit diff --git a/src/utils/uitk/src/core/math/base.hpp b/src/utils/uitk/src/core/math/base.hpp new file mode 100644 index 0000000..4cfe2e5 --- /dev/null +++ b/src/utils/uitk/src/core/math/base.hpp @@ -0,0 +1,50 @@ +/** + * @file base.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-08-15 + * + * @copyright Copyright (c) 2025 + * + */ +// ref: https://github.com/godotengine/godot/blob/master/core/typedefs.h +#pragma once + +namespace uitk_intl { + +template +constexpr const T sign(const T m_v) +{ + return m_v > 0 ? +1.0f : (m_v < 0 ? -1.0f : 0.0f); +} + +template +constexpr auto min(const T m_a, const T2 m_b) +{ + return m_a < m_b ? m_a : m_b; +} + +template +constexpr auto max(const T m_a, const T2 m_b) +{ + return m_a > m_b ? m_a : m_b; +} + +template +constexpr auto clamp(const T m_a, const T2 m_min, const T3 m_max) +{ + return m_a < m_min ? m_min : (m_a > m_max ? m_max : m_a); +} + +template +constexpr auto map_range(const T m_val, const T2 m_in_min, const T3 m_in_max, const T4 m_out_min, const T5 m_out_max) +{ + if (m_in_max == m_in_min) { + return m_out_min; + } + + return m_out_min + ((m_val - m_in_min) * (m_out_max - m_out_min)) / (m_in_max - m_in_min); +} + +} // namespace smooth_ui_toolkit diff --git a/src/utils/uitk/src/core/math/math.hpp b/src/utils/uitk/src/core/math/math.hpp new file mode 100644 index 0000000..aee9e46 --- /dev/null +++ b/src/utils/uitk/src/core/math/math.hpp @@ -0,0 +1,13 @@ +/** + * @file math.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-08-15 + * + * @copyright Copyright (c) 2025 + * + */ +#pragma once +#include "base.hpp" +#include "vector.hpp" diff --git a/src/utils/uitk/src/core/math/vector.hpp b/src/utils/uitk/src/core/math/vector.hpp new file mode 100644 index 0000000..4242d7b --- /dev/null +++ b/src/utils/uitk/src/core/math/vector.hpp @@ -0,0 +1,161 @@ +/** + * @file vector.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2025-08-15 + * + * @copyright Copyright (c) 2025 + * + */ +// ref: https://github.com/godotengine/godot/blob/master/core/math/vector2.h +#pragma once +#include "base.hpp" +#include + +namespace uitk_intl { + +/** + * @brief + * + */ +template +struct Vector2Base { + union { + struct { + T x; + T y; + }; + + struct { + T width; + T height; + }; + + T coord[2] = {0}; + }; + + Vector2Base() : x(0), y(0) {} + Vector2Base(T x, T y) : x(x), y(y) {} + Vector2Base(const Vector2Base& other) : x(other.x), y(other.y) {} + + Vector2Base& operator=(const Vector2Base& other) + { + if (this != &other) { + x = other.x; + y = other.y; + } + return *this; + } + + void set(const T& new_x, const T& new_y) + { + x = new_x; + y = new_y; + } + + Vector2Base clamp(const Vector2Base& p_min, const Vector2Base& p_max) + { + return Vector2Base(uitk_intl::clamp(x, p_min.x, p_max.x), + uitk_intl::clamp(y, p_min.y, p_max.y)); + } +}; + +/** + * @brief A 2D vector using floating-point coordinates. + * + */ +struct Vector2 : public Vector2Base { + Vector2() : Vector2Base() {} + Vector2(float x, float y) : Vector2Base(x, y) {} + Vector2(const Vector2Base& other) : Vector2Base(other) {} + + // 从整数向量转换 + Vector2(const Vector2Base& other) + : Vector2Base(static_cast(other.x), static_cast(other.y)) + { + } + + // 显式转换操作符 + explicit operator Vector2Base() const + { + return Vector2Base(static_cast(x), static_cast(y)); + } + + // Add + Vector2 operator+(const Vector2& other) const + { + return Vector2(x + other.x, y + other.y); + } + + // Subtract + Vector2 operator-(const Vector2& other) const + { + return Vector2(x - other.x, y - other.y); + } + + // Multiply by scalar + Vector2 operator*(float scalar) const + { + return Vector2(x * scalar, y * scalar); + } + + // += operator + Vector2& operator+=(const Vector2& other) + { + x += other.x; + y += other.y; + return *this; + } + + // -= operator + Vector2& operator-=(const Vector2& other) + { + x -= other.x; + y -= other.y; + return *this; + } + + // Divide by scalar + Vector2 operator/(float scalar) const + { + return Vector2(x / scalar, y / scalar); + } + + float length() const + { + return std::sqrt(x * x + y * y); + } + + Vector2 normalized() const + { + float len = length(); + if (len == 0.0f) { + return Vector2(0.0f, 0.0f); + } + return Vector2(x / len, y / len); + } +}; + +/** + * @brief A 2D vector using integer coordinates. + * + */ +struct Vector2i : public Vector2Base { + Vector2i() : Vector2Base() {} + Vector2i(int x, int y) : Vector2Base(x, y) {} + Vector2i(const Vector2Base& other) : Vector2Base(other) {} + + // 从浮点向量转换 + Vector2i(const Vector2Base& other) : Vector2Base(static_cast(other.x), static_cast(other.y)) + { + } + + // 显式转换操作符 + explicit operator Vector2Base() const + { + return Vector2Base(static_cast(x), static_cast(y)); + } +}; + +} // namespace smooth_ui_toolkit diff --git a/src/utils/uitk/src/smooth_ui_toolkit.hpp b/src/utils/uitk/src/smooth_ui_toolkit.hpp new file mode 100644 index 0000000..9a56e19 --- /dev/null +++ b/src/utils/uitk/src/smooth_ui_toolkit.hpp @@ -0,0 +1,12 @@ +/** + * @file smooth_ui_toolkit.hpp + * @author Forairaaaaa + * @brief + * @version 0.1 + * @date 2023-12-27 + * + * @copyright Copyright (c) 2023 + * + */ +#pragma once +#include "core/core.hpp"