bala.py

# modified by dpgeorge, works OK, needs a bit of tuning but it could be
# at the limit of the hardware

#
# This file is part of MicroPython MPU9250 driver
# Copyright (c) 2018 Mika Tuupola
# Copyright (c) 2018 0x1abin (added the yaw,picth,roll api and complementary filtering)
#
# Licensed under the MIT license:
#   http://www.opensource.org/licenses/mit-license.php
#
# Project home:
#   https://github.com/tuupola/micropython-mpu9250
#

"""
MicroPython I2C driver for MPU6500 6-axis motion tracking device
"""

__version__ = "0.2.0-dev"

# pylint: disable=import-error
import ustruct
from machine import I2C, Pin
from micropython import const
import math
import utime as time
# pylint: enable=import-error

_SMPLRT_DIV = const(0x19)
_CONFIG = const(0x1a)
_PWR_MGMT_1 = const(0x6b)
_GYRO_CONFIG = const(0x1b)
_ACCEL_CONFIG = const(0x1c)
_ACCEL_CONFIG2 = const(0x1d)
_INT_PIN_CFG = const(0x37)
_ACCEL_XOUT_H = const(0x3b)
_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)
_TEMP_OUT_H = const(0x41)
_TEMP_OUT_L = const(0x42)
_GYRO_XOUT_H = const(0x43)
_GYRO_XOUT_L = const(0x44)
_GYRO_YOUT_H = const(0x45)
_GYRO_YOUT_L = const(0x46)
_GYRO_ZOUT_H = const(0x47)
_GYRO_ZOUT_L = const(0x48)
_WHO_AM_I = const(0x75)

#_ACCEL_FS_MASK = const(0b00011000)
ACCEL_FS_SEL_2G = const(0b00000000)
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

#_GYRO_FS_MASK = const(0b00011000)
GYRO_FS_SEL_250DPS = const(0b00000000)
GYRO_FS_SEL_500DPS = const(0b00001000)
GYRO_FS_SEL_1000DPS = const(0b00010000)
GYRO_FS_SEL_2000DPS = const(0b00011000)

_GYRO_SO_250DPS = 131
_GYRO_SO_500DPS = 65.5
_GYRO_SO_1000DPS = 32.8
_GYRO_SO_2000DPS = 16.4

# Used for enablind and disabling the i2c bypass access
_I2C_BYPASS_MASK = const(0b00000010)
_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_DEG_S = 1
SF_RAD_S = 57.295779513082 # 1 rad/s is 57.295779578552 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,
        accel_sf=SF_G, gyro_sf=SF_DEG_S
    ):
        if i2c:
            self.i2c = i2c
        else:
            from machine import I2C
            self.i2c = I2C(sda=21, scl=22, speed=400000)
        self.address = address

        # if 0x71 != self.whoami:
        #     raise RuntimeError("MPU6500 not found in I2C bus.")

        # Init
        self._register_char(_SMPLRT_DIV, 0x00)
        self._register_char(_CONFIG, 0x00)
        self._accel_so = self._accel_fs(accel_fs)
        self._gyro_so = self._gyro_fs(gyro_fs)
        self._accel_sf = accel_sf
        self._gyro_sf = gyro_sf
        self._register_char(_PWR_MGMT_1, 0x01)

        # Enable I2C bypass to access for MPU9250 magnetometer access.
        # char = self._register_char(_INT_PIN_CFG)
        # char &= ~_I2C_BYPASS_MASK # clear I2C bits
        # char |= _I2C_BYPASS_EN
        # self._register_char(_INT_PIN_CFG, char)
        self.preInterval = time.time()
        self.accCoef = 0.02
        self.gyroCoef = 0.98
        self.angleGyroX = 0
        self.angleGyroY = 0
        self.angleGyroZ = 0
        self.angleX = 0
        self.angleZ = 0
        self.angleY = 0
        self.gyroXoffset = 0
        self.gyroYoffset = 0
        self.gyroZoffset = 0

    def setGyroOffsets(self, x, y, z):
        self.gyroXoffset = x
        self.gyroYoffset = y
        self.gyroZoffset = z

    @property
    def acceleration(self):
        """
        Acceleration measured by the sensor. By default will return a
        3-tuple of X, Y, Z axis acceleration values in m/s^2 as floats. Will
        return values in g if constructor was provided `accel_sf=SF_M_S2`
        parameter.
        """
        so = self._accel_so
        sf = self._accel_sf

        xyz = self._register_three_shorts(_ACCEL_XOUT_H)
        return tuple([value / so * sf for value in xyz])

    @property
    def gyro(self):
        """
        X, Y, Z radians per second as floats.
        """
        so = self._gyro_so
        sf = self._gyro_sf

        xyz = self._register_three_shorts(_GYRO_XOUT_H)
        return tuple([value / so * sf for value in xyz])

    @property
    def ypr(self):
        """
        yaw, pitch, roll as floats.
        """
        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);

        gyroX, gyroY, gyroZ = self.gyro
        gyroX -= self.gyroXoffset
        gyroY -= self.gyroYoffset
        gyroZ -= self.gyroZoffset

        interval = (time.ticks_us() - self.preInterval) / 1000000
        self.preInterval = time.ticks_us()

        self.angleGyroX += gyroX * interval
        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.angleZ = self.angleGyroZ

        return tuple([self.angleZ, self.angleX, self.angleY])

    @property
    def whoami(self):
        """ Value of the whoami register. """
        return self._register_char(_WHO_AM_I)

    def _register_short(self, register, value=None, buf=bytearray(2)):
        if value is None:
            self.i2c.readfrom_mem_into(self.address, register, buf)
            return ustruct.unpack(">h", buf)[0]

        ustruct.pack_into(">h", buf, 0, value)
        return self.i2c.writeto_mem(self.address, register, buf)

    def _register_three_shorts(self, register, buf=bytearray(6)):
        self.i2c.readfrom_mem_into(self.address, register, buf)
        return ustruct.unpack(">hhh", buf)

    def _register_char(self, register, value=None, buf=bytearray(1)):
        if value is None:
            self.i2c.readfrom_mem_into(self.address, register, buf)
            return buf[0]

        ustruct.pack_into("<b", buf, 0, value)
        return self.i2c.writeto_mem(self.address, register, buf)

    def _accel_fs(self, value):
        self._register_char(_ACCEL_CONFIG, value)

        # Return the sensitivity divider
        if ACCEL_FS_SEL_2G == value:
            return _ACCEL_SO_2G
        elif ACCEL_FS_SEL_4G == value:
            return _ACCEL_SO_4G
        elif ACCEL_FS_SEL_8G == value:
            return _ACCEL_SO_8G
        elif ACCEL_FS_SEL_16G == value:
            return _ACCEL_SO_16G

    def _gyro_fs(self, value):
        self._register_char(_GYRO_CONFIG, value)

        # Return the sensitivity divider
        if GYRO_FS_SEL_250DPS == value:
            return _GYRO_SO_250DPS
        elif GYRO_FS_SEL_500DPS == value:
            return _GYRO_SO_500DPS
        elif GYRO_FS_SEL_1000DPS == value:
            return _GYRO_SO_1000DPS
        elif GYRO_FS_SEL_2000DPS == value:
            return _GYRO_SO_2000DPS

    def __enter__(self):
        return self

    def __exit__(self, exception_type, exception_value, traceback):
        pass


# ==============================================================

from micropython import const
import uos as os
import utime as time
import machine
import ustruct


M5GO_WHEEL_ADDR = const(0x56)
MOTOR_CTRL_ADDR = const(0x00)
ENCODER_ADDR = const(0x04)


#define constrain(amt,low,high) (amt)<(low)?(low):((amt)>(high)?(high):(amt))
def constrain(amt, low, high):
    if amt < low:
        return low
    if amt > high:
        return high
    return amt


class M5Bala:
    def __init__(self, i2c=None):
        if i2c is None:
            self.i2c = machine.I2C(sda=machine.Pin(21), scl=machine.Pin(22), freq=400000)
        else:
            self.i2c = i2c
        self.imu = MPU6050(self.i2c)
        # self.set_motor(0, 0)
        self.imu.setGyroOffsets(-2.71, -0.01, -0.04)
        self.loop_interval = time.ticks_us()
        self.dt = time.ticks_us()
        self.angleX = 0
        self.angleX_offset = 0
        self.last_angle = 0.0
        self.last_wheel = 0.0
        self.in_speed0 = 0
        self.in_speed1 = 0
        self.left = 0
        self.right = 0
        self.K1 = 40
        self.K2 = 40
        self.K3 = 6.5
        self.K4 = 5.5
        self.K5 = 0
        self.enc_filter = 0.90

    def stop(self):
        self.left = 0
        self.right = 0

    def move(self, speed, duration=5):
        self.left = -speed
        self.right = -speed
        time.sleep(duration)
        self.stop()

    def turn(self, speed, duration=5):
        if speed > 0: # Turn RIGHT
            self.left = abs(speed)
            self.right = 0
        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
            self.right = -speed
        elif speed < 0:
            self.left = speed
            self.right = -speed
        time.sleep(duration)
        self.stop()

    def set_motor(self, m0_pwm, m1_pwm):
        buf = ustruct.pack('<hh', int(m0_pwm), int(m1_pwm))
        self.i2c.writeto_mem(M5GO_WHEEL_ADDR, MOTOR_CTRL_ADDR, buf)

    def read_encoder(self):
        buf = bytearray(4)
        self.i2c.readfrom_mem_into(M5GO_WHEEL_ADDR, ENCODER_ADDR, buf)
        return tuple(ustruct.unpack('<hh', buf))

    def balance(self):
        #print(time.ticks_us(), self.loop_interval, time.ticks_diff(time.ticks_us(), self.loop_interval))
        if time.ticks_diff(time.ticks_us(), self.loop_interval) >= 0:
            self.loop_interval = time.ticks_add(time.ticks_us(), 10000)

            # Angle X sample
            pitch = self.imu.ypr[1]
            self.angleX = (pitch + self.angleX_offset)

            #print('go', self.angleX)
            # Car Down
            if self.angleX > 45 or self.angleX < -45:
                self.set_motor(0, 0)
                return

            # Encoder sample
            new_speed0, new_speed1 = self.read_encoder()
            self.in_speed0 *= self.enc_filter
            self.in_speed0 += -new_speed0 * (1 - self.enc_filter)
            self.in_speed1 *= self.enc_filter
            self.in_speed1 += new_speed1 * (1 - self.enc_filter)

            # ========== PID computing ==========
            # -- Angle
            angle = self.angleX
            angle_velocity = angle - self.last_angle
            self.last_angle = angle

            # -- Postiton
            wheel = int(self.in_speed0 + self.in_speed1) // 2
            wheel_velocity = wheel - self.last_wheel
            self.last_wheel = wheel

            # -- 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

            # -- PWM OUT
            self.set_motor(torque + self.left - speed_diff, torque + self.right)



    def run(self, blocking=False):
        if blocking:
            while True:
                self.balance()
        else:
            self.balance()

    def start(self, thread=True):
        if thread:
            import _thread
            _thread.start_new_thread(self.run, (True, ))

# ==============================================================

import machine

but_a = machine.Pin(39, machine.Pin.IN)
but_b = machine.Pin(38, machine.Pin.IN)
but_c = machine.Pin(37, machine.Pin.IN)

# Init the I2C bus
i2c = machine.I2C(sda=machine.Pin(21), scl=machine.Pin(22), freq=400000)
print(i2c)
print(i2c.scan())

# Balance START
m5bala = M5Bala(i2c)
m5bala.angleX_offset = -8
while but_a():
    t0 = time.ticks_us()
    m5bala.balance()
    ret = time.ticks_diff(m5bala.loop_interval, time.ticks_us())
    dt = time.ticks_diff(time.ticks_us(), t0)
    print(dt / (dt + ret))
    time.sleep_us(ret)
print('finish')
m5bala.set_motor(0, 0)