mirror of
https://github.com/m5stack/M5Stack.git
synced 2026-05-20 10:06:46 -07:00
127 lines
3.3 KiB
Arduino
127 lines
3.3 KiB
Arduino
/*
|
|
* SPDX-FileCopyrightText: 2025 M5Stack Technology CO LTD
|
|
*
|
|
* SPDX-License-Identifier: MIT
|
|
*/
|
|
/*
|
|
* @Hardwares: M5Core + Bala
|
|
* @Platform Version: Arduino M5Stack Board Manager v2.1.3
|
|
* @Dependent Library:
|
|
* M5Stack@^0.4.6: https://github.com/m5stack/M5Stack
|
|
*/
|
|
|
|
#include <M5Stack.h>
|
|
#include <Preferences.h>
|
|
#include <Wire.h>
|
|
#include "M5Bala.h"
|
|
#include "imuCalibration.h"
|
|
|
|
// Description: This code only for mpu6886! first init need press BtnC and reboot to calibrate !
|
|
|
|
#define MAX_LEN 120
|
|
#define X_OFFSET 0
|
|
#define Y_OFFSET 100
|
|
#define X_SCALE 3
|
|
|
|
Preferences preferences;
|
|
|
|
M5Bala m5bala(Wire);
|
|
|
|
// ================ Draw Angle Wavefrom =================
|
|
void draw_waveform()
|
|
{
|
|
static int16_t val_buf[MAX_LEN] = {0};
|
|
static int16_t pt = MAX_LEN - 1;
|
|
|
|
val_buf[pt] = constrain((int16_t)(m5bala.getAngle() * X_SCALE), -80, 80);
|
|
if (--pt < 0) {
|
|
pt = MAX_LEN - 1;
|
|
}
|
|
|
|
for (int i = 1; i < (MAX_LEN); i++) {
|
|
uint16_t now_pt = (pt + i) % (MAX_LEN);
|
|
M5.Lcd.drawLine(i, val_buf[(now_pt + 1) % MAX_LEN] + Y_OFFSET, i + 1,
|
|
val_buf[(now_pt + 2) % MAX_LEN] + Y_OFFSET, TFT_BLACK);
|
|
if (i < MAX_LEN - 1)
|
|
M5.Lcd.drawLine(i, val_buf[now_pt] + Y_OFFSET, i + 1, val_buf[(now_pt + 1) % MAX_LEN] + Y_OFFSET,
|
|
TFT_GREEN);
|
|
}
|
|
}
|
|
|
|
// ================ GYRO offset param ==================
|
|
void auto_tune_gyro_offset()
|
|
{
|
|
M5.Speaker.tone(500, 200);
|
|
delay(300);
|
|
M5.update();
|
|
M5.Lcd.println("Start IMU calculate gyro offsets");
|
|
M5.Lcd.println("DO NOT MOVE A MPU6050...");
|
|
delay(2000);
|
|
|
|
imu_calcGyroOffsets();
|
|
|
|
float gyroXoffset = imu_getOffsetX();
|
|
|
|
M5.Lcd.println("Done!!!");
|
|
M5.Lcd.print("X : ");
|
|
M5.Lcd.println(gyroXoffset);
|
|
M5.Lcd.println("Program will start after 3 seconds");
|
|
M5.Lcd.print("========================================");
|
|
|
|
// Save
|
|
preferences.putFloat("gyroXoffset", gyroXoffset);
|
|
preferences.end();
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
// Power ON Stabilizing...
|
|
M5.begin();
|
|
M5.Power.begin();
|
|
Wire.begin();
|
|
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
|
|
|
|
// Display info
|
|
M5.Lcd.setTextFont(2);
|
|
M5.Lcd.setTextColor(WHITE, BLACK);
|
|
M5.Lcd.println("M5Stack Balance Mode start");
|
|
|
|
// Init M5Bala
|
|
m5bala.begin();
|
|
m5bala.setAngleOffset(-2.2);
|
|
|
|
// Loading the IMU parameters
|
|
if (M5.BtnC.isPressed()) {
|
|
preferences.begin("m5bala-cfg", false);
|
|
auto_tune_gyro_offset();
|
|
} else {
|
|
preferences.begin("m5bala-cfg", true);
|
|
imu_setOffsetX(preferences.getFloat("gyroXoffset"));
|
|
}
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
// LCD display
|
|
static uint32_t print_interval = millis() + 30;
|
|
if (millis() > print_interval) {
|
|
print_interval = millis() + 100;
|
|
M5.Lcd.setCursor(0, 190);
|
|
M5.Lcd.printf("Input Encoer0: %+4d Encoer1: %+4d \r\n", m5bala.getSpeed0(), m5bala.getSpeed1());
|
|
M5.Lcd.printf("Output PWM0: %+4d PWM1: %+4d \r\n", m5bala.getOut0(), m5bala.getOut1());
|
|
M5.Lcd.printf("AngleX: %+05.2f\r\n", m5bala.getAngle());
|
|
}
|
|
|
|
// Draw the waveform
|
|
static uint32_t draw_interval = millis() + 5;
|
|
if (millis() > draw_interval) {
|
|
draw_interval = millis() + 20;
|
|
draw_waveform();
|
|
}
|
|
|
|
// M5Bala balance run
|
|
m5bala.run();
|
|
|
|
// M5 Loop
|
|
M5.update();
|
|
} |