m5/M5Bala
0
0
Fork 0
mirror of https://github.com/m5stack/M5Bala.git synced 2026-05-20 10:23:14 -07:00
Code Issues Packages Projects Releases Wiki Activity

Fix: fix an initialize bug

Browse Source
This commit is contained in:
塩入孔章
2019-03-18 14:00:29 +08:00
parent 54d4167ecd
commit 37d50a1ca9
5 changed files with 30 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+8
View File
@@ -0,0 +1,8 @@
.pio
.pioenvs
.piolibdeps
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/settings.json
.vscode/extensions.json
examples/Default_firmware/Default_firmware.ino
+1 -1
View File
@@ -123,7 +123,7 @@ void loop() {
if (millis() > print_interval) {
print_interval = millis() + 100;
M5.Lcd.setCursor(0, 190);
M5.Lcd.printf("Input Encoer0: %+4d Encoer0: %+4d \r\n",
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());
mpy/m5bala.py
+6 -7
View File
@@ -11,7 +11,7 @@ MOTOR_CTRL_ADDR = const(0x00)
ENCODER_ADDR = const(0x04)
#define constrain(amt,low,high) (amt)<(low)?(low):((amt)>(high)?(high):(amt))
# define constrain(amt,low,high) (amt)<(low)?(low):((amt)>(high)?(high):(amt))
def constrain(amt, low, high):
if amt < low:
return low
@@ -57,15 +57,15 @@ class M5Bala:
self.stop()
def turn(self, speed, duration=5):
if speed > 0: # Turn RIGHT
if speed > 0: # Turn RIGHT
self.left = abs(speed)
self.right = 0
elif speed < 0: # Turn LEFT
elif speed < 0: # Turn LEFT
self.left = 0
self.right = abs(speed)
time.sleep(duration)
self.stop()
def rotate(self, speed, duration=2):
if speed > 0:
self.left = speed
@@ -86,7 +86,7 @@ class M5Bala:
return tuple(ustruct.unpack('<hh', buf))
def balance(self):
if time.ticks_us() >= self.loop_interval: # 10ms
if time.ticks_us() >= self.loop_interval: # 10ms
self.loop_interval = time.ticks_us() + 10000
# Angle X sample
@@ -119,7 +119,7 @@ class M5Bala:
# -- PID
torque = (angle_velocity * self.K1) + (angle * self.K2) + (wheel_velocity * self.K3) + (wheel * self.K4)
torque = constrain(torque, -255, 255)
# -- Wheel offset
speed_diff = (int(self.in_speed0) - int(self.in_speed1))
speed_diff *= self.K5
@@ -127,7 +127,6 @@ class M5Bala:
# -- PWM OUT
self.set_motor(torque + self.left - speed_diff, torque + self.right)
def run(self, blocking=False):
if blocking:
while True:
mpy/mpu6050.py
+14 -13
View File
@@ -14,7 +14,7 @@
MicroPython I2C driver for MPU6500 6-axis motion tracking device
"""
__version__ = "0.2.0-dev"
__version__ = "0.2.0"
# pylint: disable=import-error
import ustruct
@@ -36,7 +36,7 @@ _ACCEL_XOUT_L = const(0x3c)
_ACCEL_YOUT_H = const(0x3d)
_ACCEL_YOUT_L = const(0x3e)
_ACCEL_ZOUT_H = const(0x3f)
_ACCEL_ZOUT_L= const(0x40)
_ACCEL_ZOUT_L = const(0x40)
_TEMP_OUT_H = const(0x41)
_TEMP_OUT_L = const(0x42)
_GYRO_XOUT_H = const(0x43)
@@ -53,10 +53,10 @@ ACCEL_FS_SEL_4G = const(0b00001000)
ACCEL_FS_SEL_8G = const(0b00010000)
ACCEL_FS_SEL_16G = const(0b00011000)
_ACCEL_SO_2G = 16384 # 1 / 16384 ie. 0.061 mg / digit
_ACCEL_SO_4G = 8192 # 1 / 8192 ie. 0.122 mg / digit
_ACCEL_SO_8G = 4096 # 1 / 4096 ie. 0.244 mg / digit
_ACCEL_SO_16G = 2048 # 1 / 2048 ie. 0.488 mg / digit
_ACCEL_SO_2G = 16384 # 1 / 16384 ie. 0.061 mg / digit
_ACCEL_SO_4G = 8192 # 1 / 8192 ie. 0.122 mg / digit
_ACCEL_SO_8G = 4096 # 1 / 4096 ie. 0.244 mg / digit
_ACCEL_SO_16G = 2048 # 1 / 2048 ie. 0.488 mg / digit
#_GYRO_FS_MASK = const(0b00011000)
GYRO_FS_SEL_250DPS = const(0b00000000)
@@ -75,12 +75,14 @@ _I2C_BYPASS_EN = const(0b00000010)
_I2C_BYPASS_DIS = const(0b00000000)
SF_G = 1
SF_M_S2 = 9.80665 # 1 g = 9.80665 m/s2 ie. standard gravity
SF_M_S2 = 9.80665 # 1 g = 9.80665 m/s2 ie. standard gravity
SF_DEG_S = 1
SF_RAD_S = 57.295779513082 # 1 rad/s is 57.295779578552 deg/s
SF_RAD_S = 57.295779513082 # 1 rad/s is 57.295779513082 deg/s
class MPU6050:
"""Class which provides interface to MPU6500 6-axis motion tracking device."""
def __init__(
self, i2c=None, address=0x68,
accel_fs=ACCEL_FS_SEL_2G, gyro_fs=GYRO_FS_SEL_500DPS,
@@ -89,7 +91,6 @@ class MPU6050:
if i2c:
self.i2c = i2c
else:
from machine import I2C
self.i2c = I2C(sda=21, scl=22, speed=400000)
self.address = address
@@ -110,7 +111,7 @@ class MPU6050:
# char &= ~_I2C_BYPASS_MASK # clear I2C bits
# char |= _I2C_BYPASS_EN
# self._register_char(_INT_PIN_CFG, char)
self.preInterval = time.time()
self.preInterval = time.ticks_us()
self.accCoef = 0.02
self.gyroCoef = 0.98
self.angleGyroX = 0
@@ -161,7 +162,7 @@ class MPU6050:
accX, accY, accZ = self.acceleration
angleAccX = math.atan2(accY, accZ + abs(accX)) * SF_RAD_S
angleAccY = math.atan2(accX, accZ + abs(accY)) * (-SF_RAD_S);
angleAccY = math.atan2(accX, accZ + abs(accY)) * (-SF_RAD_S)
gyroX, gyroY, gyroZ = self.gyro
gyroX -= self.gyroXoffset
@@ -175,8 +176,8 @@ class MPU6050:
self.angleGyroY += gyroY * interval
self.angleGyroZ += gyroZ * interval
self.angleX = (self.gyroCoef * (self.angleX + gyroX * interval)) + (self.accCoef * angleAccX);
self.angleY = (self.gyroCoef * (self.angleY + gyroY * interval)) + (self.accCoef * angleAccY);
self.angleX = (self.gyroCoef * (self.angleX + gyroX * interval)) + (self.accCoef * angleAccX)
self.angleY = (self.gyroCoef * (self.angleY + gyroY * interval)) + (self.accCoef * angleAccY)
self.angleZ = self.angleGyroZ
return tuple([self.angleZ, self.angleX, self.angleY])
src/Default_firmware.ino
+1 -1
View File
@@ -128,7 +128,7 @@ void loop() {
if (millis() > print_interval) {
print_interval = millis() + 100;
M5.Lcd.setCursor(0, 190);
M5.Lcd.printf("Input Encoer0: %+4d Encoer0: %+4d \r\n",
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());