LOTP Two-Wheeled Self-Balancing Robot

from machine import Pin, PWM, I2C, UART
from utime import sleep, ticks_ms
from mpu9250 import MPU9250
from math import atan2, degrees, pi
from stepper import Stepper

class PID():
    def __init__(self, p, i, d, target, minOut, maxOut):
        self.targetBias = 0
        self.target = target
        self.pK = p
        self.iK = i
        self.dK = d
        self.error = 0
        self.sumError = 0
        self.lastError = 0
        self.minOut = minOut
        self.maxOut = maxOut
        
    def update(self, now):
        self.error = (self.target + self.targetBias) - now
        self.sumError += self.error
        out = (self.error * self.pK) + (self.sumError * self.iK) + ((self.error - self.lastError) * self.dK)
        self.lastError = self.error
        
        out = self.minOut if out < self.minOut else out
        out = self.maxOut if out > self.maxOut else out
        return(out)
        

accK =.03
lastTime = ticks_ms()
def readAngle(angles):
    global lastTime
    now = ticks_ms()
    gyro = mpu.gyro
    acc = mpu.acceleration
    #angles[0] += (gyro[0] - gyroBias[0])*100000 /16384
    angles[1] -= (gyro[1] - gyroBias[1])*pi
    angles[2] += (gyro[2] - gyroBias[2])*pi
    
    #angles[0] += (degrees(atan2(acc[1], acc[2])) - angles[0]) * accK
    angles[1] += (degrees(atan2(acc[0], acc[2])) - angles[1]) * accK
    angles[2] += (degrees(atan2(acc[0], acc[1])) - angles[2]) * accK
    
    angles[0] = round(angles[0], 2)
    angles[1] = round(angles[1], 2)
    angles[2] = round(angles[2], 2)
    
    lastTime = now
    return(angles)

def sendCMD(cmd, timeout=2000):
    print("CMD: " + cmd)
    bl.write(cmd)
    waitResp(timeout)
    
def waitResp(timeout=2000):
    prvMills = ticks_ms()
    resp = b""
    while (ticks_ms()-prvMills)<timeout:
        if bl.any():
            resp = b"".join([resp, bl.read(1)])
    print(resp)

led = Pin(2, Pin.OUT)
led.value(0)

bl = UART(1, baudrate=9600, tx=Pin(4), rx=Pin(5))

gyroBias = [-0.0235664, 0.003460463, 0.01118344]
i2c = I2C(id = 1, scl = Pin(15), sda = Pin(14))
mpu = MPU9250(i2c)
 
motorR = Stepper(18, 17, 21, 20, 19)
motorL = Stepper(10, 9, 13, 12, 11)

motorR.start()
motorL.start()

motorR.setMode(5)
motorL.setMode(5)

#sendCMD("AT\r\n")
#sendCMD("AT+VERSION\r\n")
#sendCMD("AT+NAMEHC-06")

directions = {'S' : [0, 0], 'F' : [1, 0], 'B' : [-1, 0],
              'R' : [0, 1], 'L' : [0, -1], 'I' : [1, 1],
              'G' : [1, -1], 'J' : [-1, 1], 'H' : [-1, -1]}
numbers = {"0":0, "1":1, "2":2, "3":3, "4":4, "5":5, "6":6, "7":7, "8":8, "9":9, "q":10}

biasAngle = 0
targetAngle = 89
tagretRPM = 0

speed = 40
speedBias = 0

gyroPID = PID(2, 0.3, 0, targetAngle, -100, 100)


rpmPID = PID(.1, 0.0001, .00, tagretRPM, -12, 12)


motorMax = 10
motorMin = -10

moveDir = [0, 0]
angles = [0, 90, 90]
for i in range(20):
    angles = readAngle(angles)
errorSumA = 0
errorOldA = 0
errorSumR = 0
errorOldR = 0
filteredAngles = angles.copy()
f = 0.5
oldTick = ticks_ms()
sliderData = 0
while(1):
    angles = readAngle(angles)
    nowTick = ticks_ms()
    if(nowTick - oldTick > 20):
        #print(rpmPID.target)
        oldTick = nowTick
    filteredAngles[0] = (angles[0] * (1 - f)) + filteredAngles[0] * f
    filteredAngles[1] = (angles[1] * (1 - f)) + filteredAngles[1] * f
    filteredAngles[2] = (angles[2] * (1 - f)) + filteredAngles[2] * f
    #print('a:', angles[1], 'f:', filteredAngles[1], 't:', targetAngle, 'e:', filteredAngles[1] - targetAngle)
    
    if(bl.any()):
        data = bl.read()
        #print(data)
        for d in str(data)[2:-1]:
            moveDir[1] = 0
            if(d == "S"):
                rpmPID.target = 0
                moveDir[1] = 0
            if(d == "F" or d == "I" or d == "G"):
                rpmPID.target = speed
            if(d == "B" or d == "J" or d == "H"):
                rpmPID.target = -speed
            if(d == "L" or d == "G" or d == "H"):
                moveDir[1] = -12
            if(d == "R" or d == "I" or d == "J"):
                moveDir[1] = 12
            if(d in numbers):
                sliderData = numbers[d]
                
        
    if(40 > abs(filteredAngles[1] - targetAngle) > 0 and 1):
        gyroPID.targetBias = rpmPID.update(moveDir[0]) + speedBias
        moveDir[0] = -gyroPID.update(filteredAngles[1])
        #print(moveDir[0])
        #print(moveDir)
        motorL.setRPM(moveDir[0] + moveDir[1])
        motorR.setRPM(moveDir[0] - moveDir[1])
    else:
        motorR.stop()
        motorL.stop()
        
    
    sleep(0.01)

from mpu9250 import MPU9250
from machine import Pin, I2C

i2c = I2C(id = 1, scl = Pin(15), sda = Pin(14))
mpu = MPU9250(i2c)

n = 300

rawData = [mpu.gyro for i in range(n)]

calData = [sum([rawData[i][0] for i in range(n)]) / n,
           sum([rawData[i][1] for i in range(n)]) / n,
           sum([rawData[i][2] for i in range(n)]) / n]

print(calData)

# Copyright (c) 2018-2020 Mika Tuupola
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of  this software and associated documentation files (the "Software"), to
# deal in  the Software without restriction, including without limitation the
# rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
# sell copied of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.

# https://github.com/tuupola/micropython-mpu9250

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

__version__ = "0.3.0"

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

_GYRO_CONFIG = const(0x1b)
_ACCEL_CONFIG = const(0x1c)
_ACCEL_CONFIG2 = const(0x1d)
_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 = 62.5
_GYRO_SO_1000DPS = 32.8
_GYRO_SO_2000DPS = 16.4

_TEMP_SO = 333.87
_TEMP_OFFSET = 21

SF_G = 1
SF_M_S2 = 9.80665 # 1 g = 9.80665 m/s2 ie. standard gravity
SF_DEG_S = 1
SF_RAD_S = 0.017453292519943 # 1 deg/s is 0.017453292519943 rad/s

class MPU6500:
    """Class which provides interface to MPU6500 6-axis motion tracking device."""
    def __init__(
        self, i2c, address=0x68,
        accel_fs=ACCEL_FS_SEL_2G, gyro_fs=GYRO_FS_SEL_250DPS,
        accel_sf=SF_M_S2, gyro_sf=SF_RAD_S,
        gyro_offset=(0, 0, 0)
    ):
        self.i2c = i2c
        self.address = address

        # 0x70 = standalone MPU6500, 0x71 = MPU6250 SIP
        if self.whoami not in [0x71, 0x70]:
            raise RuntimeError("MPU6500 not found in I2C bus.")

        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._gyro_offset = gyro_offset

    @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
        ox, oy, oz = self._gyro_offset

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

        xyz[0] -= ox
        xyz[1] -= oy
        xyz[2] -= oz

        return tuple(xyz)

    @property
    def temperature(self):
        """
        Die temperature in celcius as a float.
        """
        temp = self._register_short(_TEMP_OUT_H)
        return ((temp - _TEMP_OFFSET) / _TEMP_SO) + _TEMP_OFFSET

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

    def calibrate(self, count=256, delay=0):
        ox, oy, oz = (0.0, 0.0, 0.0)
        self._gyro_offset = (0.0, 0.0, 0.0)
        n = float(count)

        while count:
            utime.sleep_ms(delay)
            gx, gy, gz = self.gyro
            ox += gx
            oy += gy
            oz += gz
            count -= 1

        self._gyro_offset = (ox / n, oy / n, oz / n)
        return self._gyro_offset

    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

# Copyright (c) 2018-2020 Mika Tuupola
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of  this software and associated documentation files (the "Software"), to
# deal in  the Software without restriction, including without limitation the
# rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
# sell copied of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.

# https://github.com/tuupola/micropython-mpu9250

"""
MicroPython I2C driver for MPU9250 9-axis motion tracking device
"""

# pylint: disable=import-error
from micropython import const
from mpu6500 import MPU6500
from ak8963 import AK8963
# pylint: enable=import-error

__version__ = "0.3.0"

# Used for enabling and disabling the I2C bypass access
_INT_PIN_CFG = const(0x37)
_I2C_BYPASS_MASK = const(0b00000010)
_I2C_BYPASS_EN = const(0b00000010)
_I2C_BYPASS_DIS = const(0b00000000)

class MPU9250:
    """Class which provides interface to MPU9250 9-axis motion tracking device."""
    def __init__(self, i2c, mpu6500 = None, ak8963 = None):
        if mpu6500 is None:
            self.mpu6500 = MPU6500(i2c)
        else:
            self.mpu6500 = mpu6500

        # Enable I2C bypass to access AK8963 directly.
        char = self.mpu6500._register_char(_INT_PIN_CFG)
        char &= ~_I2C_BYPASS_MASK # clear I2C bits
        char |= _I2C_BYPASS_EN
        self.mpu6500._register_char(_INT_PIN_CFG, char)

        if ak8963 is None:
            self.ak8963 = AK8963(i2c)
        else:
            self.ak8963 = ak8963

    @property
    def acceleration(self):
        """
        Acceleration measured by the sensor. By default will return a
        3-tuple of X, Y, Z axis values in m/s^2 as floats. To get values in g
        pass `accel_fs=SF_G` parameter to the MPU6500 constructor.
        """
        return self.mpu6500.acceleration

    @property
    def gyro(self):
        """
        Gyro measured by the sensor. By default will return a 3-tuple of
        X, Y, Z axis values in rad/s as floats. To get values in deg/s pass
        `gyro_sf=SF_DEG_S` parameter to the MPU6500 constructor.
        """
        return self.mpu6500.gyro

    @property
    def temperature(self):
        """
        Die temperature in celcius as a float.
        """
        return self.mpu6500.temperature

    @property
    def magnetic(self):
        """
        X, Y, Z axis micro-Tesla (uT) as floats.
        """
        return self.ak8963.magnetic

    @property
    def whoami(self):
        return self.mpu6500.whoami

    def __enter__(self):
        return self

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

from machine import Pin, PWM


class Stepper():
    def __init__(self, stepP, dirP, m1 = None, m2 = None, m3 = None, enP = None):
        self.modeInitState = m1 != None
        self.enInitState = enP != None
        self.stepP = PWM(Pin(stepP))
        self.dirP = Pin(dirP, Pin.OUT)
        if(self.modeInitState): self.modePs = [Pin(m3, Pin.OUT), Pin(m2, Pin.OUT), Pin(m1, Pin.OUT)]
        if(self.enInitState): self.enP = Pin(enP, Pin.OUT)
        
        self.stepP.duty_u16(0)
        self.dirP.value(0)
        if(self.enInitState): self.enP.value(0)
        if(self.modeInitState):
            for mode in self.modePs:
                mode.value(0)
                
        self.currentMode = 0
            
    def lock(self): self.enP.value(0)
    def unlock(self): self.enP.value(1)
            
    def start(self): self.stepP.duty_u16(2**15)
    def stop(self): self.stepP.duty_u16(0)
        
    def setStep(self, step):
        self.dirP.value(step > 0)
        step = abs(int(step))
        if(step > 15):
            self.stepP.freq(step)
            self.start()
            
    def setRPM(self, rpm):
        self.setStep(rpm * 200)
        
    def setMode(self, mode):
        self.currentMode = mode
        mode = bin(min(max(mode, 0), 7))[2:]
        mode = '0' * (3 - len(mode)) + mode
        for m, p in zip(mode, self.modePs):
            p.value(int(m))