Skip to main content
Raspberry Pi

Return to Question

format code
Source Link
Dirk
Dirk
  • 3.6k
  • 3
  • 18
  • 25

import sys import time import RPi.GPIO as GPIO import smbus import math

mode=GPIO.getmode() print(mode) #GPIO.cleanup() print("Start") Forward1=38 Backward1=37 Forward2=36 Backward2=35 sleeptime=0.1

Power management registers

power_mgmt_1 = 0x6b power_mgmt_2 = 0x6c

bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards address = 0x68 # This is the address value read via the i2cdetect command

#motor pins initialization GPIO.setmode(GPIO.BOARD) GPIO.setup(Forward1, GPIO.OUT) GPIO.setup(Backward1, GPIO.OUT) GPIO.setup(Forward2, GPIO.OUT) GPIO.setup(Backward2, GPIO.OUT)

#motor forward def forward(x): GPIO.output(Forward1, GPIO.HIGH) GPIO.output(Forward2, GPIO.HIGH) print("Moving Forward") time.sleep(x) GPIO.output(Forward1, GPIO.LOW) GPIO.output(Forward2, GPIO.LOW)

#motor reverse def reverse(x): GPIO.output(Backward1, GPIO.HIGH) GPIO.output(Backward2, GPIO.HIGH) print("Moving Backward") time.sleep(x) GPIO.output(Backward1, GPIO.LOW) GPIO.output(Backward2, GPIO.LOW)

mpu6050 read

def read_byte(adr): return bus.read_byte_data(address, adr)

def read_word(adr): high = bus.read_byte_data(address, adr) low = bus.read_byte_data(address, adr+1) val = (high << 8) + low return val

def read_word_2c(adr): val = read_word(adr) if (val >= 0x8000): return -((65535 - val) + 1) else: return val

def dist(a,b): return math.sqrt((aa)+(bb))

def get_y_rotation(x,y,z): radians = math.atan2(x, dist(y,z)) return -math.degrees(radians)

def get_x_rotation(x,y,z): radians = math.atan2(y, dist(x,z)) return math.degrees(radians)

def get_direction():

import sys
import time
import RPi.GPIO as GPIO
import smbus
import math
 
mode=GPIO.getmode()
print(mode)
#GPIO.cleanup()
print("Start")
Forward1=38
Backward1=37
Forward2=36
Backward2=35
sleeptime=0.1
 
# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
 
bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68       # This is the address value read via the i2cdetect command
 
 
#motor pins initialization
GPIO.setmode(GPIO.BOARD)
GPIO.setup(Forward1, GPIO.OUT)
GPIO.setup(Backward1, GPIO.OUT)
GPIO.setup(Forward2, GPIO.OUT)
GPIO.setup(Backward2, GPIO.OUT)
 
#motor forward
def forward(x):
    GPIO.output(Forward1, GPIO.HIGH)
    GPIO.output(Forward2, GPIO.HIGH)
    print("Moving Forward")
    time.sleep(x)
    GPIO.output(Forward1, GPIO.LOW)
    GPIO.output(Forward2, GPIO.LOW)
 
#motor reverse
def reverse(x):
    GPIO.output(Backward1, GPIO.HIGH)
    GPIO.output(Backward2, GPIO.HIGH)
    print("Moving Backward")
    time.sleep(x)
    GPIO.output(Backward1, GPIO.LOW)
    GPIO.output(Backward2, GPIO.LOW)
 
# mpu6050 read 
def read_byte(adr):
    return bus.read_byte_data(address, adr)
 
def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val
 
def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val
 
def dist(a,b):
    return math.sqrt((a*a)+(b*b))
 
def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)
 
def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)
 
def get_direction():
    
    # Now wake the 6050 up as it starts in sleep mode
    bus.write_byte_data(address, power_mgmt_1, 0)
 
    print("gyro data")
    print("---------")
 
    gyro_xout = read_word_2c(0x43)
    gyro_yout = read_word_2c(0x45)
    gyro_zout = read_word_2c(0x47)
 
    print("gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131))
    print("gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131))
    print("gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131))
 
    print("")
    print("accelerometer data")
    print("------------------")
 
    accel_xout = read_word_2c(0x3b)
    accel_yout = read_word_2c(0x3d)
    accel_zout = read_word_2c(0x3f)
 
    accel_xout_scaled = accel_xout / 16384.0
    accel_yout_scaled = accel_yout / 16384.0
    accel_zout_scaled = accel_zout / 16384.0
 
    #print("accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled)
    #print("accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled)
    #print("accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled)
 
    print("x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
    print("y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
    return get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
 
# while loop begins
margin = 7
try:
    while (1):
        dir = get_direction()
        if(dir < -margin):
            print("move forward")
            forward(sleeptime)
        elif(dir > margin):
            print("move reverse")
            reverse(sleeptime)
 
except:
    print("Some error")
 
GPIO.cleanup()

while loop begins

margin = 7 try: while (1): dir = get_direction() if(dir < -margin): print("move forward") forward(sleeptime) elif(dir > margin): print("move reverse") reverse(sleeptime)

except: print("Some error")

GPIO.cleanup()

import sys import time import RPi.GPIO as GPIO import smbus import math

mode=GPIO.getmode() print(mode) #GPIO.cleanup() print("Start") Forward1=38 Backward1=37 Forward2=36 Backward2=35 sleeptime=0.1

Power management registers

power_mgmt_1 = 0x6b power_mgmt_2 = 0x6c

bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards address = 0x68 # This is the address value read via the i2cdetect command

#motor pins initialization GPIO.setmode(GPIO.BOARD) GPIO.setup(Forward1, GPIO.OUT) GPIO.setup(Backward1, GPIO.OUT) GPIO.setup(Forward2, GPIO.OUT) GPIO.setup(Backward2, GPIO.OUT)

#motor forward def forward(x): GPIO.output(Forward1, GPIO.HIGH) GPIO.output(Forward2, GPIO.HIGH) print("Moving Forward") time.sleep(x) GPIO.output(Forward1, GPIO.LOW) GPIO.output(Forward2, GPIO.LOW)

#motor reverse def reverse(x): GPIO.output(Backward1, GPIO.HIGH) GPIO.output(Backward2, GPIO.HIGH) print("Moving Backward") time.sleep(x) GPIO.output(Backward1, GPIO.LOW) GPIO.output(Backward2, GPIO.LOW)

mpu6050 read

def read_byte(adr): return bus.read_byte_data(address, adr)

def read_word(adr): high = bus.read_byte_data(address, adr) low = bus.read_byte_data(address, adr+1) val = (high << 8) + low return val

def read_word_2c(adr): val = read_word(adr) if (val >= 0x8000): return -((65535 - val) + 1) else: return val

def dist(a,b): return math.sqrt((aa)+(bb))

def get_y_rotation(x,y,z): radians = math.atan2(x, dist(y,z)) return -math.degrees(radians)

def get_x_rotation(x,y,z): radians = math.atan2(y, dist(x,z)) return math.degrees(radians)

def get_direction():

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)

print("gyro data")
print("---------")

gyro_xout = read_word_2c(0x43)
gyro_yout = read_word_2c(0x45)
gyro_zout = read_word_2c(0x47)

print("gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131))
print("gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131))
print("gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131))

print("")
print("accelerometer data")
print("------------------")

accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)

accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0

#print("accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled)
#print("accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled)
#print("accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled)

print("x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
print("y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
return get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)

while loop begins

margin = 7 try: while (1): dir = get_direction() if(dir < -margin): print("move forward") forward(sleeptime) elif(dir > margin): print("move reverse") reverse(sleeptime)

except: print("Some error")

GPIO.cleanup()

import sys
import time
import RPi.GPIO as GPIO
import smbus
import math
 
mode=GPIO.getmode()
print(mode)
#GPIO.cleanup()
print("Start")
Forward1=38
Backward1=37
Forward2=36
Backward2=35
sleeptime=0.1
 
# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
 
bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68       # This is the address value read via the i2cdetect command
 
 
#motor pins initialization
GPIO.setmode(GPIO.BOARD)
GPIO.setup(Forward1, GPIO.OUT)
GPIO.setup(Backward1, GPIO.OUT)
GPIO.setup(Forward2, GPIO.OUT)
GPIO.setup(Backward2, GPIO.OUT)
 
#motor forward
def forward(x):
    GPIO.output(Forward1, GPIO.HIGH)
    GPIO.output(Forward2, GPIO.HIGH)
    print("Moving Forward")
    time.sleep(x)
    GPIO.output(Forward1, GPIO.LOW)
    GPIO.output(Forward2, GPIO.LOW)
 
#motor reverse
def reverse(x):
    GPIO.output(Backward1, GPIO.HIGH)
    GPIO.output(Backward2, GPIO.HIGH)
    print("Moving Backward")
    time.sleep(x)
    GPIO.output(Backward1, GPIO.LOW)
    GPIO.output(Backward2, GPIO.LOW)
 
# mpu6050 read 
def read_byte(adr):
    return bus.read_byte_data(address, adr)
 
def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val
 
def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val
 
def dist(a,b):
    return math.sqrt((a*a)+(b*b))
 
def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)
 
def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)
 
def get_direction():
    
    # Now wake the 6050 up as it starts in sleep mode
    bus.write_byte_data(address, power_mgmt_1, 0)
 
    print("gyro data")
    print("---------")
 
    gyro_xout = read_word_2c(0x43)
    gyro_yout = read_word_2c(0x45)
    gyro_zout = read_word_2c(0x47)
 
    print("gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131))
    print("gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131))
    print("gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131))
 
    print("")
    print("accelerometer data")
    print("------------------")
 
    accel_xout = read_word_2c(0x3b)
    accel_yout = read_word_2c(0x3d)
    accel_zout = read_word_2c(0x3f)
 
    accel_xout_scaled = accel_xout / 16384.0
    accel_yout_scaled = accel_yout / 16384.0
    accel_zout_scaled = accel_zout / 16384.0
 
    #print("accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled)
    #print("accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled)
    #print("accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled)
 
    print("x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
    print("y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
    return get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
 
# while loop begins
margin = 7
try:
    while (1):
        dir = get_direction()
        if(dir < -margin):
            print("move forward")
            forward(sleeptime)
        elif(dir > margin):
            print("move reverse")
            reverse(sleeptime)
 
except:
    print("Some error")
 
GPIO.cleanup()
Source Link
ece12345
ece12345
  • 11
  • 2

Self-balancing robot using Raspberry 3b+

I have built a self balancing robot using raspberry pi 3b+. I am not able to reduce the response time. Hence, the robot doesn't balance properly. I am using MPU 6050, 150rpm 12V DC motor. I checked mpu 6050 and the motors individually and they seem to be working fine. The code also works. But, since the response time is more, the robot falls to the ground.
Please tell me what is wrong with my code.

CODE:

import sys import time import RPi.GPIO as GPIO import smbus import math

mode=GPIO.getmode() print(mode) #GPIO.cleanup() print("Start") Forward1=38 Backward1=37 Forward2=36 Backward2=35 sleeptime=0.1

Power management registers

power_mgmt_1 = 0x6b power_mgmt_2 = 0x6c

bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards address = 0x68 # This is the address value read via the i2cdetect command

#motor pins initialization GPIO.setmode(GPIO.BOARD) GPIO.setup(Forward1, GPIO.OUT) GPIO.setup(Backward1, GPIO.OUT) GPIO.setup(Forward2, GPIO.OUT) GPIO.setup(Backward2, GPIO.OUT)

#motor forward def forward(x): GPIO.output(Forward1, GPIO.HIGH) GPIO.output(Forward2, GPIO.HIGH) print("Moving Forward") time.sleep(x) GPIO.output(Forward1, GPIO.LOW) GPIO.output(Forward2, GPIO.LOW)

#motor reverse def reverse(x): GPIO.output(Backward1, GPIO.HIGH) GPIO.output(Backward2, GPIO.HIGH) print("Moving Backward") time.sleep(x) GPIO.output(Backward1, GPIO.LOW) GPIO.output(Backward2, GPIO.LOW)

mpu6050 read

def read_byte(adr): return bus.read_byte_data(address, adr)

def read_word(adr): high = bus.read_byte_data(address, adr) low = bus.read_byte_data(address, adr+1) val = (high << 8) + low return val

def read_word_2c(adr): val = read_word(adr) if (val >= 0x8000): return -((65535 - val) + 1) else: return val

def dist(a,b): return math.sqrt((aa)+(bb))

def get_y_rotation(x,y,z): radians = math.atan2(x, dist(y,z)) return -math.degrees(radians)

def get_x_rotation(x,y,z): radians = math.atan2(y, dist(x,z)) return math.degrees(radians)

def get_direction():

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)

print("gyro data")
print("---------")

gyro_xout = read_word_2c(0x43)
gyro_yout = read_word_2c(0x45)
gyro_zout = read_word_2c(0x47)

print("gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131))
print("gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131))
print("gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131))

print("")
print("accelerometer data")
print("------------------")

accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)

accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0

#print("accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled)
#print("accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled)
#print("accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled)

print("x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
print("y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
return get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)

while loop begins

margin = 7 try: while (1): dir = get_direction() if(dir < -margin): print("move forward") forward(sleeptime) elif(dir > margin): print("move reverse") reverse(sleeptime)

except: print("Some error")

GPIO.cleanup()