ich habe mal wieder ein kleineres Problem. Aktuell versuche ich mich an einem Self-Balancing-Robot. Geschrieben ist die Software in Python. Für die Motoren verwende ich einen DRV 8825 Schrittmotortreiber. Als Gyroscop benutze ich das MPU6050. Motoren sind die Nema 17.
Die ansteuerung der Treiber erfolgt über einen while loop, welcher als Thread im Hintergrund läuft. Die Frequenz in der Schleife, also die verzögerung zwischen dem Einschalten und Ausschalten des PIN für den Schritt, erfolgt über die sleep funktion. Soweit so gut. Das funktioniert auch alles bis zu dem Punkt wo dann die I2C Abfrage für den Gyrosensor in einem weiteren Thread kommt. Denn dann fängt der Motor immer an zu stotern und läuft nicht mehr sauber.
Lasse ich jeden Thread einzelt ohne den anderen laufen funktioniert das alles.
Zum Auslesen wird das smbus modul benutzt.
Das ist der Code der in der Libary für das Auslesen des MPU Sensor verwendet wird.
Code: Select all
def read_i2c_word(self, register):
"""Read two i2c registers and combine them.
register -- the first register to read from.
Returns the combined read results.
"""
# Read the data from the registers
high = self.bus.read_byte_data(self.address, register)
low = self.bus.read_byte_data(self.address, register + 1)
value = (high << 8) + low
if (value >= 0x8000):
return -(65535 - value) + 1
else:
return value
def get_accel_data(self, g=True, get_raw_data = False):
"""Gets and returns the X, Y and Z values from the accelerometer.
If g is True, it will return the data in g
If g is False, it will return the data in m/s^2
Returns a dictionary with the measurement results.
"""
x = self.read_i2c_word(self.ACCEL_XOUT0) - self.acc_x_offset
y = self.read_i2c_word(self.ACCEL_YOUT0) - self.acc_y_offset
z = self.read_i2c_word(self.ACCEL_ZOUT0) - self.acc_z_offset
Hier ist mein Programm Code um den es sich handelt.
Code: Select all
import mpu6050,time,math, _thread, threading
import RPi.GPIO as gpio
import pid_regler
import mpu6050cal
import simple_pid
class KalmanAngle:
def __init__(self):
self.QAngle = 0.001
self.QBias = 0.003
self.RMeasure = 0.03
self.angle = 0.0
self.bias = 0.0
self.rate = 0.0
self.P=[[0.0,0.0],[0.0,0.0]]
'''def kalman():
QAngle = 0.001
QBias = 0.003
RMeasure = 0.03
angle = 0.0
bias = 0.0
P[0][0] = 0.0
P[0][1] = 0.0
P[1][0] = 0.0
P[1][1] = 0.0'''
def getAngle(self,newAngle, newRate,dt):
#step 1:
self.rate = newRate - self.bias; #new_rate is the latest Gyro measurement
self.angle += dt * self.rate;
#Step 2:
self.P[0][0] += dt * (dt*self.P[1][1] -self.P[0][1] - self.P[1][0] + self.QAngle)
self.P[0][1] -= dt * self.P[1][1]
self.P[1][0] -= dt * self.P[1][1]
self.P[1][1] += self.QBias * dt
#Step 3: Innovation
y = newAngle - self.angle
#Step 4: Innovation covariance
s = self.P[0][0] + self.RMeasure
#Step 5: Kalman Gain
K=[0.0,0.0]
K[0] = self.P[0][0]/s
K[1] = self.P[1][0]/s
#Step 6: Update the Angle
self.angle += K[0] * y
self.bias += K[1] * y
#Step 7: Calculate estimation error covariance - Update the error covariance
P00Temp = self.P[0][0]
P01Temp = self.P[0][1]
self.P[0][0] -= K[0] * P00Temp;
self.P[0][1] -= K[0] * P01Temp;
self.P[1][0] -= K[1] * P00Temp;
self.P[1][1] -= K[1] * P01Temp;
return self.angle
def setAngle(self,angle):
self.angle = angle
def setQAngle(self,QAngle):
self.QAngle = QAngle
def setQBias(self,QBias):
self.QBias = QBias
def setRMeasure(self,RMeasure):
self.RMeasure = RMeasure
def getRate(self):
return self.rate
def getQAngle(self):
return self.QAngle
def getQBias(self):
return self.QBias
def getRMeasure(self):
return self.RMeasure
class euler_angle(threading.Thread):
#Address
address = 0x68
#Offset
angle_y_offset = 0
#Angle
offset_comAngleY = 0
comAngleY = 0
angle = 0
pitch = 0
kalangle = 0
#Timer
start_time = time.time()
stop_time = time.time()
#Variabel für Stepper um Störungen zwischen dem Bus und dem Step pin zu verhindern
steps_running = False
def __init__(self, angle_y_offset=0):
threading.Thread.__init__(self)
self.running = True
self.angle_y_offset = angle_y_offset
def run(self):
lock = _thread.allocate_lock()
while self.running == True:
#Timing
if not self.steps_running:
self.stop_time = time.time()
self.dt = self.stop_time - self.start_time
self.start_time = time.time()
# get accel_data with an Threading lock
lock.acquire()
accel_data = mpu.get_accel_data(get_raw_data = True)
self.accel_data_x = accel_data["x"]
self.accel_data_z = accel_data["z"]
self.accel_data_y = accel_data["y"]
# get gyro data
self.gyro_data_y = mpu.get_gyro_data()["y"]
lock.release()
# Calculate Pitch
self.pitch = math.atan2(self.accel_data_z, self.accel_data_x) * 57.2958
# Using Complenmentair Filter
self.comAngleY = 0.98 * (self.comAngleY + self.gyro_data_y * self.dt) + 0.02 * self.pitch
#Angle offset
self.offset_comAngleY = self.comAngleY - self.angle_y_offset
self.kalangle = kal.getAngle(self.offset_comAngleY, self.gyro_data_y, self.dt)
time.sleep(0.05)
def stop(self):
self.running = False
class stepper(threading.Thread):
step_pin = 23
dir_pin = 24
m_1_pin = 7
m_2_pin = 8
m_2_pin = 9
m_3_pin = 10
#Stepmodes
steps = {"Full step": [0, 0, 0],
"Half step": [1, 0, 0],
"1/4 step": [0, 1, 0],
"1/8 step": [1, 1, 0],
"1/16 step": [0, 0, 1],
"1/32 step": [1, 0, 1]}
current_stepmode = ""
#Running Variabels
delay = 0.0008
running = True
def __init__(self, step_pin=16, dir_pin=21, m_1_pin=26, m_2_pin=19, m_3_pin=13, stepmode="Full step"):
threading.Thread.__init__(self)
output_channel = []
self.step_pin = step_pin
output_channel.append(step_pin)
self.dir_pin = dir_pin
output_channel.append(dir_pin)
self.m_1_pin = m_1_pin
output_channel.append(m_1_pin)
self.m_2_pin = m_2_pin
output_channel.append(m_2_pin)
self.m_3_pin = m_3_pin
output_channel.append(m_3_pin)
# Gpio initialisieren
gpio.setmode(gpio.BCM)
gpio.setup(output_channel, gpio.OUT)
#stepmode configurate
gpio.output(m_1_pin,self.steps[stepmode][0])
gpio.output(m_2_pin, self.steps[stepmode][1])
gpio.output(m_2_pin, self.steps[stepmode][2])
self.current_stepmode = stepmode
def run(self):
while self.running:
gpio.output(self.step_pin, 1)
time.sleep(self.delay)
gpio.output(self.step_pin, 0)
print("Stepper Output Disable")
def change_stepmode(self,stepmode):
gpio.output(self.m_1_pin, self.steps[stepmode][0])
gpio.output(self.m_2_pin, self.steps[stepmode][1])
gpio.output(self.m_2_pin, self.steps[stepmode][2])
self.current_stepmode = stepmode
def change_direction(self, direction):
gpio.output(self.dir_pin, direction)
#Address from MPU6050
address = 0x68
#Calibrating?
calibrating = False
if calibrating:
mpucal = mpu6050cal
mpucal.calibrate()
else:
print("Loading Offsets from file.")
#timings
start_time = time.time()
stop_time = time.time()
#Creating Objects
kal = KalmanAngle()
mpu = mpu6050.mpu6050(address, calibration_offsets=True)
pid = simple_pid.PID(1, 1, 0.05, setpoint=90)
#Configurate PID
pid.output_limits = (-100, 100)
#Starting Threads
angle = euler_angle()
angle.start()
stepper_engine = stepper(23, 24)
stepper_engine.start()
try:
while True:
#Timings
stop_time = time.time()
dt = stop_time - start_time
start_time = time.time()
pid.sample_time = dt
#Calculating
new_angle = round(angle.kalangle)
raw_value_pid = round(pid(new_angle))
#Controling
#Maximum Delay = 0.0008 minimum Delay = 0.005
if new_angle > 90:
stepper_engine.change_direction(1)
stepper_engine.delay = 0.005 - (-1*raw_value_pid * 0.00004)
else:
stepper_engine.change_direction(0)
stepper_engine.delay = 0.005 - (raw_value_pid * 0.00004)
time.sleep(0.001)
except KeyboardInterrupt:
angle.stop()
stepper_engine.running = False
gpio.cleanup()
Kann das sein, dass es eine Signalüberschneidung gibt wenn der BUS abgefragt wird und über den Step_Pin ein Signal an den Motor geschickt wird?
Mit freundlichen Grüßen
