mirror of
https://github.com/m5stack/M5Stack.git
synced 2026-05-20 10:06:46 -07:00
157 lines
3.6 KiB
Arduino
157 lines
3.6 KiB
Arduino
/*
|
|
* SPDX-FileCopyrightText: 2025 M5Stack Technology CO LTD
|
|
*
|
|
* SPDX-License-Identifier: MIT
|
|
*/
|
|
/*
|
|
* @Hardwares: M5Core + Module DCMotor
|
|
* @Platform Version: Arduino M5Stack Board Manager v2.1.3
|
|
* @Dependent Library:
|
|
* M5Stack@^0.4.6: https://github.com/m5stack/M5Stack
|
|
*/
|
|
|
|
#include <M5Stack.h>
|
|
#include <Wire.h>
|
|
#include "Free_Fonts.h"
|
|
#include "utility/CommUtil.h"
|
|
|
|
// Description: Use LEGO PLUS Module to drive the LEGO motor to rotate and monitor the encoder value.
|
|
|
|
#define SLAVE_ADDR 0x56
|
|
#define MOTOR_ADDR_BASE 0x00
|
|
#define ENCODER_ADDR_BASE 0x08
|
|
#define STEP_V 51
|
|
#define FRONT 4
|
|
#define X_LOCAL 60
|
|
#define Y_LOCAL 80
|
|
#define XF 30
|
|
#define YF 30
|
|
|
|
int16_t Speed = 0;
|
|
CommUtil Util;
|
|
|
|
/*************************************************
|
|
Function:MotorRun
|
|
Description: Motor forward and reverse API
|
|
Input:
|
|
n:Motor 0 to motor 3
|
|
Speed:Speed value from -255 to +255,when speed=0,The motor stopped.
|
|
|
|
Return: Successful return 1
|
|
Others:
|
|
*************************************************/
|
|
int32_t MotorRun(uint8_t n, int16_t Speed)
|
|
{
|
|
if (n > 3) return 0;
|
|
|
|
if (Speed <= -255) Speed = -255;
|
|
|
|
if (Speed >= 255) Speed = 255;
|
|
|
|
Util.writeBytes(SLAVE_ADDR, MOTOR_ADDR_BASE + n * 2, (uint8_t *)&Speed, 2);
|
|
|
|
return 1;
|
|
}
|
|
|
|
/*************************************************
|
|
Function:ReadEncoder
|
|
Description: Motor rotation code value
|
|
Input:
|
|
n:Motor 0 to motor 3
|
|
|
|
Return: Successful return 1
|
|
Others:
|
|
*************************************************/
|
|
int32_t ReadEncoder(uint8_t n)
|
|
{
|
|
uint8_t dest[4] = {0};
|
|
|
|
if (n > 3) return 0;
|
|
|
|
Util.readBytes(SLAVE_ADDR, ENCODER_ADDR_BASE + n * 4, 4, dest);
|
|
|
|
return *((int32_t *)dest);
|
|
}
|
|
|
|
/*************************************************
|
|
Function:header
|
|
Description:The UI title
|
|
Input:
|
|
Return:
|
|
Others:
|
|
*************************************************/
|
|
void header(const char *string, uint16_t color)
|
|
{
|
|
M5.Lcd.fillScreen(color);
|
|
M5.Lcd.setTextSize(1);
|
|
M5.Lcd.setTextColor(TFT_MAGENTA, TFT_BLUE);
|
|
M5.Lcd.fillRect(0, 0, 320, 30, TFT_BLUE);
|
|
M5.Lcd.setTextDatum(TC_DATUM);
|
|
M5.Lcd.drawString(string, 160, 3, 4);
|
|
}
|
|
|
|
/*************************************************
|
|
Function:motor_demo
|
|
Description:Press the button to control motor rotation,
|
|
and coded values are displayed in real time.
|
|
Input:
|
|
Return:
|
|
Others:A -> Speed-- B -> Speed=0 C -> Speed++
|
|
*************************************************/
|
|
void motor_demo(void)
|
|
{
|
|
uint8_t BtnFlag = 0;
|
|
M5.update();
|
|
|
|
if (M5.BtnA.wasReleased()) {
|
|
Speed -= STEP_V;
|
|
|
|
if (Speed <= -255) Speed = -255;
|
|
|
|
BtnFlag = 1;
|
|
} else if (M5.BtnB.wasReleased()) {
|
|
Speed = 0;
|
|
BtnFlag = 1;
|
|
} else if (M5.BtnC.wasReleased()) {
|
|
Speed += STEP_V;
|
|
|
|
if (Speed >= 255) Speed = 255;
|
|
|
|
BtnFlag = 1;
|
|
}
|
|
|
|
if (BtnFlag == 1) {
|
|
BtnFlag = 0;
|
|
for (int i = 0; i < 4; i++) {
|
|
MotorRun(i, Speed);
|
|
M5.Lcd.setCursor(X_LOCAL, Y_LOCAL + YF * i, FRONT);
|
|
M5.Lcd.printf("S%d:%d \n", i, Speed);
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < 4; i++) {
|
|
M5.Lcd.setCursor(X_LOCAL + XF * 4, Y_LOCAL + YF * i, FRONT);
|
|
M5.Lcd.printf("E%d:%d \n", i, ReadEncoder(i));
|
|
}
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
M5.begin();
|
|
M5.Power.begin();
|
|
Wire.begin();
|
|
Serial.begin(115200);
|
|
M5.Lcd.fillScreen(TFT_BLACK);
|
|
header("L E G O +", TFT_BLACK);
|
|
M5.Lcd.setTextColor(TFT_WHITE, TFT_BLACK);
|
|
for (int i = 0; i < 4; i++) {
|
|
M5.Lcd.setCursor(X_LOCAL, Y_LOCAL + YF * i, FRONT);
|
|
M5.Lcd.printf("S%d:%d \n", i, Speed);
|
|
}
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
motor_demo();
|
|
}
|