From 37d50a1ca9e2857c70522da52f60e974f8cf91d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=A1=A9=E5=85=A5=E5=AD=94=E7=AB=A0?= Date: Mon, 18 Mar 2019 14:00:29 +0800 Subject: [PATCH] Fix: fix an initialize bug --- .gitignore | 8 ++++++ .../Default_firmware/Default_firmware.ino | 2 +- mpy/m5bala.py | 13 +++++---- mpy/mpu6050.py | 27 ++++++++++--------- src/Default_firmware.ino | 2 +- 5 files changed, 30 insertions(+), 22 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..6915e71 --- /dev/null +++ b/.gitignore @@ -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 \ No newline at end of file diff --git a/examples/Default_firmware/Default_firmware.ino b/examples/Default_firmware/Default_firmware.ino index cfb9d08..fc668d5 100644 --- a/examples/Default_firmware/Default_firmware.ino +++ b/examples/Default_firmware/Default_firmware.ino @@ -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()); diff --git a/mpy/m5bala.py b/mpy/m5bala.py index f6aa28e..d6f8171 100644 --- a/mpy/m5bala.py +++ b/mpy/m5bala.py @@ -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('= 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: diff --git a/mpy/mpu6050.py b/mpy/mpu6050.py index f25c63f..4f4d9c2 100644 --- a/mpy/mpu6050.py +++ b/mpy/mpu6050.py @@ -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]) diff --git a/src/Default_firmware.ino b/src/Default_firmware.ino index bfa213d..32f0ffd 100644 --- a/src/Default_firmware.ino +++ b/src/Default_firmware.ino @@ -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());