/*
 * SPDX-FileCopyrightText: 2025 M5Stack Technology CO LTD
 *
 * SPDX-License-Identifier: MIT
 */
/*
 * @Hardwares: M5Core + Module13.2 ODrive
 * @Platform Version: Arduino M5Stack Board Manager v2.1.3
 * @Dependent Library:
 * M5Stack@^0.4.6: https://github.com/m5stack/M5Stack
 */

#include "M5Stack.h"
#include "odrive.h"

// Description:
// This case uses the ODrive module to control the high-speed and precise
// rotation of the servo motor Press button C to calibrate (do not touch the
// motor shaft during this period), long press and short button A to control the
// motor rotation. Note: The motor parameter configuration in this case is only
// applicable to the motor model matched with the M5 Odrive kit. When driving
// other types of motors, please configure the parameters according to the motor
// used.

ODrive odrive(Serial1);
TFT_eSprite canvas = TFT_eSprite(&M5.Lcd);

void showStringCenter(const char* str, uint16_t color)
{
    canvas.fillScreen(TFT_BLACK);
    canvas.setTextColor(color);
    canvas.drawString(str, 160, 120, 4);
    canvas.pushSprite(0, 0);
}

void setup()
{
    M5.begin(true, false, true, true);
    canvas.setColorDepth(1);
    canvas.createSprite(320, 240);
    Serial1.begin(115200, SERIAL_8N1, 13, 5);

    canvas.setTextDatum(MC_DATUM);
    showStringCenter("ODrive", TFT_GREEN);
}

void loop()
{
    M5.update();
    if (M5.BtnA.wasReleased()) {
        odrive.setPosition(10);
    }

    if (M5.BtnA.wasReleasefor(800)) {
        odrive.setPosition(0);
    }

    if (M5.BtnB.wasReleased()) {
        showStringCenter("Save default config", TFT_GREEN);
        odrive.setDefaultConfig();
        odrive.reboot();
    }

    if (M5.BtnB.wasReleasefor(800)) {
        showStringCenter("Clear config", TFT_GREEN);
        odrive.eraseConfig();
    }

    if (M5.BtnC.wasReleased()) {
        showStringCenter("Motor CALIBRATION", TFT_WHITE);
        odrive.runState(odrive.AXIS_STATE_MOTOR_CALIBRATION, 10000);
        if (odrive.checkError()) {
            showStringCenter("Motor CALIBRATION", TFT_RED);
            return;
        }

        showStringCenter("Encoder CALIBRATION", TFT_WHITE);
        odrive.runState(odrive.AXIS_STATE_ENCODER_INDEX_SEARCH, 10000);
        if (odrive.checkError()) {
            showStringCenter("Encoder Failed", TFT_RED);
            return;
        }

        showStringCenter("Offset CALIBRATION", TFT_WHITE);
        odrive.runState(odrive.AXIS_STATE_ENCODER_OFFSET_CALIBRATION, 10000);
        if (odrive.checkError()) {
            showStringCenter("Offset Failed", TFT_RED);
            return;
        }

        odrive.setGain(211.0, 0.0225, 0.01125);
        odrive.setControlMode(odrive.CONTROL_MODE_POSITION_CONTROL);
        odrive.runState(odrive.AXIS_STATE_CLOSED_LOOP_CONTROL, 2000);

        showStringCenter("Finish", TFT_GREEN);
    }

    char data_show[100];
    sprintf(data_show, "%f, %d, %d\r\n", odrive.getVbusVoltage(), odrive.getEncoderShadowCount(), odrive.checkError());
    showStringCenter(data_show, TFT_WHITE);
    Serial.printf(data_show);
    delay(20);
}