From d7b4461c702d44503857638f1447dcb2325f0d64 Mon Sep 17 00:00:00 2001
From: sean <1026025056@qq.com>
Date: Fri, 19 Jan 2024 16:02:19 +0800
Subject: [PATCH] add Module 4EncoderMotor Example

---
 .../Module/4EncoderMotor/4EncoderMotor.ino    | 148 ++++++++++++++++++
 1 file changed, 148 insertions(+)
 create mode 100644 examples/Module/4EncoderMotor/4EncoderMotor.ino

diff --git a/examples/Module/4EncoderMotor/4EncoderMotor.ino b/examples/Module/4EncoderMotor/4EncoderMotor.ino
new file mode 100644
index 00000000..8b1b9502
--- /dev/null
+++ b/examples/Module/4EncoderMotor/4EncoderMotor.ino
@@ -0,0 +1,148 @@
+/**
+ * @file DriverSample.ino
+ * @author SeanKwok (shaoxiang@m5stack.com)
+ * @brief Module 4EncoderMotor Test Demo.
+ * @version 0.1
+ * @date 2024-01-19
+ *
+ *
+ * @Hardwares: M5Core2 + Module 4EncoderMotor
+ * @Platform Version: Arduino M5Stack Board Manager v2.1.0
+ * @Dependent Library:
+ * M5Unified: https://github.com/m5stack/M5Unified
+ * M5GFX: https://github.com/m5stack/M5GFX
+ * M5Module4EncoderMotor: https://github.com/m5stack/M5Module-4EncoderMotor
+ */
+
+#include "M5Unified.h"
+#include "M5GFX.h"
+#include "M5Module4EncoderMotor.h"
+
+M5Module4EncoderMotor driver;
+
+#define MAX_RECORD_SIZE 256
+
+float amp_record[MAX_RECORD_SIZE] = {0};
+uint8_t record_index              = 0;
+float amp_value                   = 0.0f;
+
+uint8_t avg_filter_level = 20;
+
+float avg_filter(float *data, int len) {
+    float sum = 0;
+    float min = data[0];
+    float max = data[0];
+    for (int i = 0; i < len; i++) {
+        if (data[i] < min) {
+            min = data[i];
+        }
+        if (data[i] > max) {
+            max = data[i];
+        }
+        sum += data[i];
+    }
+    sum -= min;
+    sum -= max;
+    return sum / (len - 2);
+}
+
+void setup() {
+    M5.begin();
+    M5.Display.begin();
+
+    M5.Display.setTextColor(WHITE);
+    M5.Display.setTextDatum(top_center);
+    M5.Display.setFont(&fonts::FreeSansBold12pt7b);
+    M5.Display.setTextSize(1);
+
+    while (!driver.begin(&Wire1, MODULE_4ENCODERMOTOR_ADDR, 21, 22)) {
+        Serial.println("Driver Init faild!");
+        M5.Display.drawString("Driver Init faild!", 160, 7);
+        delay(1000);
+    }
+
+    Serial.println("Driver Init success!");
+    M5.Display.clear();
+    M5.Display.fillRect(0, 0, 320, 35, 0x27f);
+    M5.Display.drawString("4Encoder Motor", 160, 7);
+    M5.Display.setTextDatum(top_left);
+
+    // motor channel 0 -3
+    for (uint8_t i = 0; i < 4; i++) {
+        driver.setMode(i, NORMAL_MODE);
+        driver.setMotorSpeed(i, 127);
+    }
+    M5.Display.drawString("NORMAL MODE", 20, 40 + 35 * 5);
+}
+
+bool direction = true;
+int mode       = NORMAL_MODE;
+
+void loop() {
+    M5.update();
+    for (uint8_t i = 0; i < 4; i++) {
+        M5.Display.fillRect(20, 40 + 35 * i, 300, 35, BLACK);
+        int32_t encoder_value = driver.getEncoderValue(i);
+        M5.Display.drawString("CH" + String(i) + ": " + String(encoder_value),
+                              20, 40 + 35 * i);
+    }
+
+    if (avg_filter_level != 0) {
+        amp_record[record_index] = driver.getMotorCurrent();
+        record_index++;
+        if (record_index >= avg_filter_level) {
+            record_index = 0;
+        }
+        amp_value = avg_filter(amp_record, avg_filter_level);
+    }
+
+    float voltage = driver.getAnalogInput(_8bit) / 255.0 * 3.3 / 0.16;
+    float current = amp_value;
+
+    M5.Display.fillRect(20, 40 + 35 * 4, 300, 35, BLACK);
+    M5.Display.drawString(
+        "POWER: " + String(voltage) + "V/" + String(current) + "A", 20,
+        40 + 35 * 4);
+
+    if (M5.BtnA.wasClicked() ||
+        (M5.Touch.getCount() && M5.Touch.getDetail(0).wasClicked())) {
+        mode++;
+        if (mode > SPEED_MODE) {
+            mode = NORMAL_MODE;
+        }
+        M5.Display.fillRect(20, 40 + 35 * 5, 300, 35, BLACK);
+
+        switch (mode) {
+            case NORMAL_MODE: {
+                M5.Display.drawString("NORMAL MODE", 20, 40 + 35 * 5);
+                // motor channel 0 -3 NORMAL_MODE
+                for (uint8_t i = 0; i < 4; i++) {
+                    driver.setMode(i, NORMAL_MODE);
+                    driver.setMotorSpeed(i, 127);
+                }
+                break;
+            }
+            case POSITION_MODE: {
+                M5.Display.drawString("POSITION MODE", 20, 40 + 35 * 5);
+                // motor channel 0 -3 POSITION_MODE
+
+                for (uint8_t i = 0; i < 4; i++) {
+                    driver.setMode(i, POSITION_MODE);
+                    driver.setEncoderValue(i, 0);
+                    driver.setPostionPIDMaxSpeed(i, 127);
+                    driver.setPositionPoint(i, 1000);
+                }
+                break;
+            }
+            case SPEED_MODE: {
+                M5.Display.drawString("SPEED MODE", 20, 40 + 35 * 5);
+                // motor channel 0 -3 SPEED_MODE
+                for (uint8_t i = 0; i < 4; i++) {
+                    driver.setMode(i, SPEED_MODE);
+                    driver.setSpeedPoint(i, 127);
+                }
+                break;
+            }
+        }
+    }
+}