Skip to content

Commit 98f6fbc

Browse files
EeeeBinEeeeBin
committed
Add: Bala (mpu6886) example
1 parent 14c5178 commit 98f6fbc

File tree

5 files changed

+470
-0
lines changed

5 files changed

+470
-0
lines changed

‎examples/Modules/Bala/Bala.ino

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
// only for mpu6886 !!!!!
2+
// first init need press BtnC and reboot to calibrate
3+
4+
#include <M5Stack.h>
5+
6+
#include <Wire.h>
7+
#include <Preferences.h>
8+
9+
#include "imuCalibration.h"
10+
#include "M5Bala.h"
11+
12+
Preferences preferences;
13+
14+
M5Bala m5bala(Wire);
15+
16+
// ================ Draw Angle Wavefrom =================
17+
void draw_waveform() {
18+
#define MAX_LEN 120
19+
#define X_OFFSET 0
20+
#define Y_OFFSET 100
21+
#define X_SCALE 3
22+
static int16_t val_buf[MAX_LEN] = {0};
23+
static int16_t pt = MAX_LEN - 1;
24+
25+
val_buf[pt] = constrain((int16_t)(m5bala.getAngle() * X_SCALE), -80, 80);
26+
if (--pt < 0) {
27+
pt = MAX_LEN - 1;
28+
}
29+
30+
for (int i = 1; i < (MAX_LEN); i++) {
31+
uint16_t now_pt = (pt + i) % (MAX_LEN);
32+
M5.Lcd.drawLine(i, val_buf[(now_pt + 1) % MAX_LEN] + Y_OFFSET, i + 1,
33+
val_buf[(now_pt + 2) % MAX_LEN] + Y_OFFSET, TFT_BLACK);
34+
if (i < MAX_LEN - 1)
35+
M5.Lcd.drawLine(i, val_buf[now_pt] + Y_OFFSET, i + 1,
36+
val_buf[(now_pt + 1) % MAX_LEN] + Y_OFFSET, TFT_GREEN);
37+
}
38+
}
39+
40+
// ================ GYRO offset param ==================
41+
void auto_tune_gyro_offset() {
42+
M5.Speaker.tone(500, 200);
43+
delay(300);
44+
M5.update();
45+
M5.Lcd.println("Start IMU calculate gyro offsets");
46+
M5.Lcd.println("DO NOT MOVE A MPU6050...");
47+
delay(2000);
48+
49+
imu_calcGyroOffsets();
50+
51+
float gyroXoffset = imu_getOffsetX();
52+
53+
M5.Lcd.println("Done!!!");
54+
M5.Lcd.print("X : ");M5.Lcd.println(gyroXoffset);
55+
M5.Lcd.println("Program will start after 3 seconds");
56+
M5.Lcd.print("========================================");
57+
58+
// Save
59+
preferences.putFloat("gyroXoffset", gyroXoffset);
60+
preferences.end();
61+
}
62+
63+
64+
void setup() {
65+
// Power ON Stabilizing...
66+
M5.begin();
67+
68+
Wire.begin();
69+
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
70+
71+
// Display info
72+
M5.Lcd.setTextFont(2);
73+
M5.Lcd.setTextColor(WHITE, BLACK);
74+
M5.Lcd.println("M5Stack Balance Mode start");
75+
76+
// Init M5Bala
77+
m5bala.begin();
78+
m5bala.setAngleOffset(-2.2);
79+
80+
81+
// Loading the IMU parameters
82+
if (M5.BtnC.isPressed()) {
83+
preferences.begin("m5bala-cfg", false);
84+
auto_tune_gyro_offset();
85+
} else {
86+
preferences.begin("m5bala-cfg", true);
87+
imu_setOffsetX(preferences.getFloat("gyroXoffset"));
88+
}
89+
}
90+
91+
void loop() {
92+
93+
// LCD display
94+
static uint32_t print_interval = millis() + 30;
95+
if (millis() > print_interval) {
96+
print_interval = millis() + 100;
97+
M5.Lcd.setCursor(0, 190);
98+
M5.Lcd.printf("Input Encoer0: %+4d Encoer1: %+4d \r\n",
99+
m5bala.getSpeed0(), m5bala.getSpeed1());
100+
M5.Lcd.printf("Output PWM0: %+4d PWM1: %+4d \r\n",
101+
m5bala.getOut0(), m5bala.getOut1());
102+
M5.Lcd.printf("AngleX: %+05.2f\r\n", m5bala.getAngle());
103+
}
104+
105+
// Draw the waveform
106+
static uint32_t draw_interval = millis() + 5;
107+
if (millis() > draw_interval) {
108+
draw_interval = millis() + 20;
109+
draw_waveform();
110+
}
111+
112+
// M5Bala balance run
113+
m5bala.run();
114+
115+
// M5 Loop
116+
M5.update();
117+
}

‎examples/Modules/Bala/M5Bala.cpp

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
1+
#include "M5Bala.h"
2+
#include "M5Stack.h"
3+
4+
#define MPU9250_ID 0x71
5+
#define MPU6050_ID 0x68
6+
7+
M5Bala::M5Bala() {
8+
wire = &Wire;
9+
}
10+
11+
M5Bala::M5Bala(TwoWire &w) {
12+
wire = &w;
13+
}
14+
15+
void M5Bala::begin() {
16+
17+
// IMU
18+
imu_CalcInit();
19+
20+
// Motor
21+
setMotor(0, 0);
22+
23+
// PID param
24+
K1 = 60;
25+
K2 = 24;
26+
K5 = 0;
27+
K3 = 8.5;
28+
K4 = 5.2;
29+
// K5 = 8;
30+
31+
loop_interval = 0;
32+
left_offset = 0;
33+
right_offset = 0;
34+
forward_offset = 0;
35+
36+
for (int i = 0; i < 128; i++) {
37+
imu_update();
38+
}
39+
pitch = imu_getAngleX();
40+
}
41+
42+
uint8_t M5Bala::i2c_readByte(uint8_t address, uint8_t subAddress) {
43+
uint8_t data;
44+
M5.I2C.readByte(address, subAddress, &data);
45+
return data; // Return data read from slave register
46+
}
47+
48+
void M5Bala::setMotor(int16_t pwm0, int16_t pwm1) {
49+
// Value range
50+
int16_t m0 = constrain(pwm0, -255, 255);
51+
int16_t m1 = constrain(pwm1, -255, 255);
52+
53+
// Dead zone
54+
if (((m0 > 0) && (m0 < DEAD_ZONE)) || ((m0 < 0) && (m0 > -DEAD_ZONE))) m0 = 0;
55+
if (((m1 > 0) && (m1 < DEAD_ZONE)) || ((m1 < 0) && (m1 > -DEAD_ZONE))) m1 = 0;
56+
57+
// Same value
58+
static int16_t pre_m0, pre_m1;
59+
if ((m0 == pre_m0) && (m1 == pre_m1))
60+
return;
61+
pre_m0 = m0;
62+
pre_m1 = m1;
63+
64+
uint8_t i2c_out_buff[4];
65+
i2c_out_buff[0] = m0 & 0xff;
66+
i2c_out_buff[1] = (m0 >> 8) & 0xff;
67+
i2c_out_buff[2] = (m1 >> 0) & 0xff;
68+
i2c_out_buff[3] = (m1 >> 8) & 0xff;
69+
M5.I2C.writeBytes(M5GO_WHEEL_ADDR, MOTOR_CTRL_ADDR, i2c_out_buff, 4);
70+
}
71+
72+
void M5Bala::readEncder() {
73+
static float _speed_input0 = 0, _speed_input1 = 0;
74+
int16_t rx_buf[2];
75+
M5.I2C.readBytes(M5GO_WHEEL_ADDR, ENCODER_ADDR, 4, (uint8_t *)rx_buf);
76+
77+
// filter
78+
_speed_input0 *= 0.9;
79+
_speed_input0 += 0.1 * rx_buf[0];
80+
_speed_input1 *= 0.9;
81+
_speed_input1 += 0.1 * rx_buf[1];
82+
83+
speed_input0 = constrain((int16_t)(-_speed_input0), -255, 255);
84+
speed_input1 = constrain((int16_t)(_speed_input1), -255, 255);
85+
}
86+
87+
void M5Bala::PIDCompute() {
88+
static float last_angle;
89+
static int16_t last_wheel;
90+
float angle, angle_velocity;
91+
int16_t wheel, wheel_velocity;
92+
int16_t torque, speed_diff, speed_diff_adjust;
93+
94+
angle = pitch;
95+
angle_velocity = angle - last_angle;
96+
last_angle = angle;
97+
98+
wheel = (speed_input0 + speed_input1) / 2; /* wheel = read_encoder()-profiler() */
99+
wheel_velocity = wheel - last_wheel;
100+
last_wheel = wheel;
101+
102+
torque = (angle_velocity * K1) + (angle * K2)
103+
+ (wheel_velocity * K3) + (wheel * K4);
104+
105+
speed_diff = (speed_input0 - speed_input1);
106+
speed_diff_adjust = (K5 * speed_diff);
107+
108+
pwm_out0 = torque - speed_diff_adjust;
109+
pwm_out1 = torque;
110+
pwm_out0 = constrain(pwm_out0, -255, 255);
111+
pwm_out1 = constrain(pwm_out1, -255, 255);
112+
}
113+
114+
void M5Bala::run() {
115+
if (micros() >= loop_interval) {
116+
loop_interval = micros() + 10000;
117+
118+
// Attitude sample
119+
imu_update();
120+
pitch = imu_getAngleX() + angle_offset;
121+
122+
// Car down
123+
if (abs(pitch) > 45) {
124+
setMotor(0, 0);
125+
return;
126+
}
127+
128+
// Encoder sample
129+
readEncder();
130+
131+
// PID Compute
132+
PIDCompute();
133+
134+
// Motor out
135+
setMotor(pwm_out0 + forward_offset + left_offset,
136+
pwm_out1 + forward_offset + right_offset);
137+
}
138+
}
139+
140+
void M5Bala::stop() {
141+
left_offset = 0;
142+
right_offset = 0;
143+
}
144+
145+
void M5Bala::move(int16_t speed, uint16_t duration) {
146+
forward_offset = -speed;
147+
if (duration != 0) {
148+
delay(duration);
149+
stop();
150+
}
151+
}
152+
153+
void M5Bala::turn(int16_t speed, uint16_t duration) {
154+
if (speed > 0) {
155+
left_offset = speed;
156+
right_offset = 0;
157+
158+
} else if (speed < 0) {
159+
left_offset = 0;
160+
right_offset = -speed;
161+
}
162+
163+
if (duration != 0) {
164+
delay(duration);
165+
stop();
166+
}
167+
}
168+
169+
void M5Bala::rotate(int16_t speed, uint16_t duration) {
170+
left_offset = speed;
171+
right_offset = -speed;
172+
173+
if (duration != 0) {
174+
delay(duration);
175+
stop();
176+
}
177+
}

‎examples/Modules/Bala/M5Bala.h

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
// The MIT License (MIT)
2+
//
3+
// Copyright (c) 2018 M5Stack
4+
// Author: [email protected] (0x1abin)
5+
//
6+
// Permission is hereby granted, free of charge, to any person obtaining a copy
7+
// of this software and associated documentation files (the "Software"), to deal
8+
// in the Software without restriction, including without limitation the rights
9+
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10+
// copies of the Software, and to permit persons to whom the Software is
11+
// furnished to do so, subject to the following conditions:
12+
//
13+
// The above copyright notice and this permission notice shall be included in
14+
// all copies or substantial portions of the Software.
15+
//
16+
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17+
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18+
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19+
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20+
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21+
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
22+
// THE SOFTWARE.
23+
24+
#ifndef _M5BALA_H
25+
#define _M5BALA_H
26+
27+
#include "Arduino.h"
28+
#include "Wire.h"
29+
#include "imuCalibration.h"
30+
31+
#define MOTOR_RPM 150
32+
#define MAX_PWM 255
33+
#define DEAD_ZONE 20
34+
35+
#define M5GO_WHEEL_ADDR 0x56
36+
#define MOTOR_CTRL_ADDR 0x00
37+
#define ENCODER_ADDR 0x04
38+
39+
40+
class M5Bala {
41+
public:
42+
M5Bala();
43+
M5Bala(TwoWire &w);
44+
45+
void begin();
46+
void run();
47+
void PIDCompute();
48+
void setMotor(int16_t pwm0, int16_t pwm1);
49+
void readEncder();
50+
uint8_t i2c_readByte(uint8_t address, uint8_t subAddress);
51+
52+
void setAngleOffset(float offset) { angle_offset = offset; };
53+
float getAngle() { return pitch; };
54+
int16_t getSpeed0() { return speed_input0; };
55+
int16_t getSpeed1() { return speed_input1; };
56+
int16_t getOut0() { return pwm_out0; };
57+
int16_t getOut1() { return pwm_out1; };
58+
59+
int16_t left_offset, right_offset;
60+
int16_t forward_offset;
61+
62+
// control
63+
void stop();
64+
void move(int16_t speed, uint16_t duration = 0);
65+
void turn(int16_t speed, uint16_t duration = 0);
66+
void rotate(int16_t speed, uint16_t duration = 0);
67+
68+
private:
69+
TwoWire *wire;
70+
float yaw, pitch, roll;
71+
int16_t speed_input0, speed_input1;
72+
int16_t pwm_out0, pwm_out1;
73+
int8_t angle_offset;
74+
uint32_t loop_interval;
75+
float K1, K2, K3, K4, K5;
76+
uint8_t imu_id;
77+
};
78+
79+
#endif

0 commit comments

Comments
 (0)