|
| 1 | +/** |
| 2 | + * @file DriverSample.ino |
| 3 | + * @author SeanKwok (shaoxiang@m5stack.com) |
| 4 | + * @brief Module 4EncoderMotor Test Demo. |
| 5 | + * @version 0.1 |
| 6 | + * @date 2024年01月19日 |
| 7 | + * |
| 8 | + * |
| 9 | + * @Hardwares: M5Core + Module 4EncoderMotor |
| 10 | + * @Platform Version: Arduino M5Stack Board Manager v2.1.0 |
| 11 | + * @Dependent Library: |
| 12 | + * M5Unified: https://github.com/m5stack/M5Unified |
| 13 | + * M5GFX: https://github.com/m5stack/M5GFX |
| 14 | + * M5Module4EncoderMotor: https://github.com/m5stack/M5Module-4EncoderMotor |
| 15 | + */ |
| 16 | + |
| 17 | +#include "M5Unified.h" |
| 18 | +#include "M5GFX.h" |
| 19 | +#include "M5Module4EncoderMotor.h" |
| 20 | + |
| 21 | +M5Module4EncoderMotor driver; |
| 22 | + |
| 23 | +#define MAX_RECORD_SIZE 256 |
| 24 | + |
| 25 | +float amp_record[MAX_RECORD_SIZE] = {0}; |
| 26 | +uint8_t record_index = 0; |
| 27 | +float amp_value = 0.0f; |
| 28 | + |
| 29 | +uint8_t avg_filter_level = 20; |
| 30 | + |
| 31 | +float avg_filter(float *data, int len) { |
| 32 | + float sum = 0; |
| 33 | + float min = data[0]; |
| 34 | + float max = data[0]; |
| 35 | + for (int i = 0; i < len; i++) { |
| 36 | + if (data[i] < min) { |
| 37 | + min = data[i]; |
| 38 | + } |
| 39 | + if (data[i] > max) { |
| 40 | + max = data[i]; |
| 41 | + } |
| 42 | + sum += data[i]; |
| 43 | + } |
| 44 | + sum -= min; |
| 45 | + sum -= max; |
| 46 | + return sum / (len - 2); |
| 47 | +} |
| 48 | + |
| 49 | +void setup() { |
| 50 | + M5.begin(); |
| 51 | + M5.Display.begin(); |
| 52 | + |
| 53 | + M5.Display.setTextColor(WHITE); |
| 54 | + M5.Display.setTextDatum(top_center); |
| 55 | + M5.Display.setFont(&fonts::FreeSansBold12pt7b); |
| 56 | + M5.Display.setTextSize(1); |
| 57 | + |
| 58 | + while (!driver.begin(&Wire, MODULE_4ENCODERMOTOR_ADDR, 21, 22)) { |
| 59 | + Serial.println("Driver Init faild!"); |
| 60 | + M5.Display.drawString("Driver Init faild!", 160, 7); |
| 61 | + delay(1000); |
| 62 | + } |
| 63 | + |
| 64 | + Serial.println("Driver Init success!"); |
| 65 | + M5.Display.clear(); |
| 66 | + M5.Display.fillRect(0, 0, 320, 35, 0x27f); |
| 67 | + M5.Display.drawString("4Encoder Motor", 160, 7); |
| 68 | + M5.Display.setTextDatum(top_left); |
| 69 | + |
| 70 | + // motor channel 0 -3 |
| 71 | + for (uint8_t i = 0; i < 4; i++) { |
| 72 | + driver.setMode(i, NORMAL_MODE); |
| 73 | + driver.setMotorSpeed(i, 127); |
| 74 | + } |
| 75 | + M5.Display.drawString("NORMAL MODE", 20, 40 + 35 * 5); |
| 76 | +} |
| 77 | + |
| 78 | +bool direction = true; |
| 79 | +int mode = NORMAL_MODE; |
| 80 | + |
| 81 | +void loop() { |
| 82 | + M5.update(); |
| 83 | + for (uint8_t i = 0; i < 4; i++) { |
| 84 | + M5.Display.fillRect(20, 40 + 35 * i, 300, 35, BLACK); |
| 85 | + int32_t encoder_value = driver.getEncoderValue(i); |
| 86 | + M5.Display.drawString("CH" + String(i) + ": " + String(encoder_value), |
| 87 | + 20, 40 + 35 * i); |
| 88 | + } |
| 89 | + |
| 90 | + if (avg_filter_level != 0) { |
| 91 | + amp_record[record_index] = driver.getMotorCurrent(); |
| 92 | + record_index++; |
| 93 | + if (record_index >= avg_filter_level) { |
| 94 | + record_index = 0; |
| 95 | + } |
| 96 | + amp_value = avg_filter(amp_record, avg_filter_level); |
| 97 | + } |
| 98 | + |
| 99 | + float voltage = driver.getAnalogInput(_8bit) / 255.0 * 3.3 / 0.16; |
| 100 | + float current = amp_value; |
| 101 | + |
| 102 | + M5.Display.fillRect(20, 40 + 35 * 4, 300, 35, BLACK); |
| 103 | + M5.Display.drawString( |
| 104 | + "POWER: " + String(voltage) + "V/" + String(current) + "A", 20, |
| 105 | + 40 + 35 * 4); |
| 106 | + |
| 107 | + if (M5.BtnA.wasClicked() || |
| 108 | + (M5.Touch.getCount() && M5.Touch.getDetail(0).wasClicked())) { |
| 109 | + mode++; |
| 110 | + if (mode > SPEED_MODE) { |
| 111 | + mode = NORMAL_MODE; |
| 112 | + } |
| 113 | + M5.Display.fillRect(20, 40 + 35 * 5, 300, 35, BLACK); |
| 114 | + |
| 115 | + switch (mode) { |
| 116 | + case NORMAL_MODE: { |
| 117 | + M5.Display.drawString("NORMAL MODE", 20, 40 + 35 * 5); |
| 118 | + // motor channel 0 -3 NORMAL_MODE |
| 119 | + for (uint8_t i = 0; i < 4; i++) { |
| 120 | + driver.setMode(i, NORMAL_MODE); |
| 121 | + driver.setMotorSpeed(i, 127); |
| 122 | + } |
| 123 | + break; |
| 124 | + } |
| 125 | + case POSITION_MODE: { |
| 126 | + M5.Display.drawString("POSITION MODE", 20, 40 + 35 * 5); |
| 127 | + // motor channel 0 -3 POSITION_MODE |
| 128 | + |
| 129 | + for (uint8_t i = 0; i < 4; i++) { |
| 130 | + driver.setMode(i, POSITION_MODE); |
| 131 | + driver.setEncoderValue(i, 0); |
| 132 | + driver.setPostionPIDMaxSpeed(i, 127); |
| 133 | + driver.setPositionPoint(i, 1000); |
| 134 | + } |
| 135 | + break; |
| 136 | + } |
| 137 | + case SPEED_MODE: { |
| 138 | + M5.Display.drawString("SPEED MODE", 20, 40 + 35 * 5); |
| 139 | + // motor channel 0 -3 SPEED_MODE |
| 140 | + for (uint8_t i = 0; i < 4; i++) { |
| 141 | + driver.setMode(i, SPEED_MODE); |
| 142 | + driver.setSpeedPoint(i, 127); |
| 143 | + } |
| 144 | + break; |
| 145 | + } |
| 146 | + } |
| 147 | + } |
| 148 | +} |
0 commit comments