Я пытаюсь использовать показания акселерометра IMU MPU9250 для управления сервоприводами с помощью RPi4. Для этого я создал (с помощью ресурсов онлайн) скрипт (IMUmodule), который читает IMU и выдает ускорение X = AccXangle и Y = AccYangle.
Если я запускаю скрипт IMUmodule, я могу напечатать X и У нет проблем. X и Y обновляются каждые 0,1 секунды.
Но если я создаю другой скрипт, он импортирует модуль IMU и пытается вывести значения AccXangle и AccYangle либо зависает, либо говорит, что IMUmodlue не имеет атрибута с именем AccXangle. (что я уверен, что он имеет)
Здесь код, который выплевывает AccXangle и AccYangle, иначе известный как IMUmodule.py, обратите внимание, в конце, функции печати вызывают AccXangle и AccYangle, и он печатает очень хорошо.
from mpu9250_jmdev.registers import *
from mpu9250_jmdev.mpu_9250 import MPU9250
import sys
import time
import math
# import IMU
import datetime
import os
global ejex
global ejey
mpu = MPU9250(
address_ak=AK8963_ADDRESS,
address_mpu_master=MPU9050_ADDRESS_68, # In 0x68 Address
address_mpu_slave=None,
bus=1,
gfs=GFS_2000,
afs=AFS_8G,
mfs=AK8963_BIT_16,
mode=AK8963_MODE_C100HZ)
mpu.calibrate()
mpu.configure()
# If the IMU is upside down (Skull logo facing up), change this value to 1
IMU_UPSIDE_DOWN = 0
RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846
G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly
AA = 0.40 # Complementary filter constant
MAG_LPF_FACTOR = 0.4 # Low pass filter constant magnetometer
ACC_LPF_FACTOR = 0.4 # Low pass filter constant for accelerometer
ACC_MEDIANTABLESIZE = 9 # Median filter table size for accelerometer. Higher = smoother but a longer delay
MAG_MEDIANTABLESIZE = 9 # Median filter table size for magnetometer. Higher = smoother but a longer delay
################# Compass Calibration values ############
# Use calibrateBerryIMU.py to get calibration values
# Calibrating the compass isnt mandatory, however a calibrated
# compass will result in a more accurate heading value.
magXmin = 0
magYmin = 0
magZmin = 0
magXmax = 0
magYmax = 0
magZmax = 0
'''
Here is an example:
magXmin = -1748
magYmin = -1025
magZmin = -1876
magXmax = 959
magYmax = 1651
magZmax = 708
Dont use the above values, these are just an example.
'''
#Kalman filter variables
Q_angle = 0.02
Q_gyro = 0.0015
R_angle = 0.005
y_bias = 0.0
x_bias = 0.0
XP_00 = 0.0
XP_01 = 0.0
XP_10 = 0.0
XP_11 = 0.0
YP_00 = 0.0
YP_01 = 0.0
YP_10 = 0.0
YP_11 = 0.0
KFangleX = 0.0
KFangleY = 0.0
def kalmanFilterY ( accAngle, gyroRate, DT):
y=0.0
S=0.0
global KFangleY
global Q_angle
global Q_gyro
global y_bias
global YP_00
global YP_01
global YP_10
global YP_11
KFangleY = KFangleY + DT * (gyroRate - y_bias)
YP_00 = YP_00 + ( - DT * (YP_10 + YP_01) + Q_angle * DT )
YP_01 = YP_01 + ( - DT * YP_11 )
YP_10 = YP_10 + ( - DT * YP_11 )
YP_11 = YP_11 + ( + Q_gyro * DT )
y = accAngle - KFangleY
S = YP_00 + R_angle
K_0 = YP_00 / S
K_1 = YP_10 / S
KFangleY = KFangleY + ( K_0 * y )
y_bias = y_bias + ( K_1 * y )
YP_00 = YP_00 - ( K_0 * YP_00 )
YP_01 = YP_01 - ( K_0 * YP_01 )
YP_10 = YP_10 - ( K_1 * YP_00 )
YP_11 = YP_11 - ( K_1 * YP_01 )
return KFangleY
def kalmanFilterX ( accAngle, gyroRate, DT):
x=0.0
S=0.0
global KFangleX
global Q_angle
global Q_gyro
global x_bias
global XP_00
global XP_01
global XP_10
global XP_11
KFangleX = KFangleX + DT * (gyroRate - x_bias)
XP_00 = XP_00 + ( - DT * (XP_10 + XP_01) + Q_angle * DT )
XP_01 = XP_01 + ( - DT * XP_11 )
XP_10 = XP_10 + ( - DT * XP_11 )
XP_11 = XP_11 + ( + Q_gyro * DT )
x = accAngle - KFangleX
S = XP_00 + R_angle
K_0 = XP_00 / S
K_1 = XP_10 / S
KFangleX = KFangleX + ( K_0 * x )
x_bias = x_bias + ( K_1 * x )
XP_00 = XP_00 - ( K_0 * XP_00 )
XP_01 = XP_01 - ( K_0 * XP_01 )
XP_10 = XP_10 - ( K_1 * XP_00 )
XP_11 = XP_11 - ( K_1 * XP_01 )
return KFangleX
gyroXangle = 0.0
gyroYangle = 0.0
gyroZangle = 0.0
CFangleX = 0.0
CFangleY = 0.0
CFangleXFiltered = 0.0
CFangleYFiltered = 0.0
kalmanX = 0.0
kalmanY = 0.0
oldXMagRawValue = 0
oldYMagRawValue = 0
oldZMagRawValue = 0
oldXAccRawValue = 0
oldYAccRawValue = 0
oldZAccRawValue = 0
a = datetime.datetime.now()
#Setup the tables for the mdeian filter. Fill them all with '1' so we dont get devide by zero error
acc_medianTable1X = [1] * ACC_MEDIANTABLESIZE
acc_medianTable1Y = [1] * ACC_MEDIANTABLESIZE
acc_medianTable1Z = [1] * ACC_MEDIANTABLESIZE
acc_medianTable2X = [1] * ACC_MEDIANTABLESIZE
acc_medianTable2Y = [1] * ACC_MEDIANTABLESIZE
acc_medianTable2Z = [1] * ACC_MEDIANTABLESIZE
mag_medianTable1X = [1] * MAG_MEDIANTABLESIZE
mag_medianTable1Y = [1] * MAG_MEDIANTABLESIZE
mag_medianTable1Z = [1] * MAG_MEDIANTABLESIZE
mag_medianTable2X = [1] * MAG_MEDIANTABLESIZE
mag_medianTable2Y = [1] * MAG_MEDIANTABLESIZE
mag_medianTable2Z = [1] * MAG_MEDIANTABLESIZE
# IMU.detectIMU() #Detect if BerryIMUv1 or BerryIMUv2 is connected.
# IMU.initIMU() #Initialise the accelerometer, gyroscope and compass
while True:
acceleration = mpu.readAccelerometerMaster()
accelerationX = acceleration[0]
accelerationY = acceleration[1]
accelerationZ = acceleration[2]
rotation = mpu.readGyroscopeMaster()
rotationX = rotation[0]
rotationY = rotation[1]
rotationZ = rotation[2]
magnetic = mpu.readMagnetometerMaster()
magneticX = magnetic[0]
magneticY = magnetic[1]
magneticZ = magnetic[2]
#Read the accelerometer,gyroscope and magnetometer values
ACCx = acceleration[0]#IMU.readACCx()
ACCy = acceleration[1]#IMU.readACCy()
ACCz = acceleration[2]#IMU.readACCz()
GYRx = rotation[0]#IMU.readGYRx()
GYRy = rotation[1]#IMU.readGYRy()
GYRz = rotation[2]#IMU.readGYRz()
MAGx = magnetic[0]#IMU.readMAGx()
MAGy = magnetic[1]#IMU.readMAGy()
MAGz = magnetic[2]#IMU.readMAGz()
#Apply compass calibration
MAGx -= (magXmin + magXmax) /2
MAGy -= (magYmin + magYmax) /2
MAGz -= (magZmin + magZmax) /2
##Calculate loop Period(LP). How long between Gyro Reads
b = datetime.datetime.now() - a
a = datetime.datetime.now()
LP = b.microseconds/(1000000*1.0)
outputString = "Loop Time %5.2f " % ( LP )
###############################################
#### Apply low pass filter ####
###############################################
MAGx = MAGx * MAG_LPF_FACTOR + oldXMagRawValue*(1 - MAG_LPF_FACTOR);
MAGy = MAGy * MAG_LPF_FACTOR + oldYMagRawValue*(1 - MAG_LPF_FACTOR);
MAGz = MAGz * MAG_LPF_FACTOR + oldZMagRawValue*(1 - MAG_LPF_FACTOR);
ACCx = ACCx * ACC_LPF_FACTOR + oldXAccRawValue*(1 - ACC_LPF_FACTOR);
ACCy = ACCy * ACC_LPF_FACTOR + oldYAccRawValue*(1 - ACC_LPF_FACTOR);
ACCz = ACCz * ACC_LPF_FACTOR + oldZAccRawValue*(1 - ACC_LPF_FACTOR);
oldXMagRawValue = MAGx
oldYMagRawValue = MAGy
oldZMagRawValue = MAGz
oldXAccRawValue = ACCx
oldYAccRawValue = ACCy
oldZAccRawValue = ACCz
#########################################
#### Median filter for accelerometer ####
#########################################
# cycle the table
for x in range (ACC_MEDIANTABLESIZE-1,0,-1 ):
acc_medianTable1X[x] = acc_medianTable1X[x-1]
acc_medianTable1Y[x] = acc_medianTable1Y[x-1]
acc_medianTable1Z[x] = acc_medianTable1Z[x-1]
# Insert the lates values
acc_medianTable1X[0] = ACCx
acc_medianTable1Y[0] = ACCy
acc_medianTable1Z[0] = ACCz
# Copy the tables
acc_medianTable2X = acc_medianTable1X[:]
acc_medianTable2Y = acc_medianTable1Y[:]
acc_medianTable2Z = acc_medianTable1Z[:]
# Sort table 2
acc_medianTable2X.sort()
acc_medianTable2Y.sort()
acc_medianTable2Z.sort()
# The middle value is the value we are interested in
ACCx = acc_medianTable2X[int(ACC_MEDIANTABLESIZE/2)];
ACCy = acc_medianTable2Y[int(ACC_MEDIANTABLESIZE/2)];
ACCz = acc_medianTable2Z[int(ACC_MEDIANTABLESIZE/2)];
#########################################
#### Median filter for magnetometer ####
#########################################
# cycle the table
for x in range (MAG_MEDIANTABLESIZE-1,0,-1 ):
mag_medianTable1X[x] = mag_medianTable1X[x-1]
mag_medianTable1Y[x] = mag_medianTable1Y[x-1]
mag_medianTable1Z[x] = mag_medianTable1Z[x-1]
# Insert the latest values
mag_medianTable1X[0] = MAGx
mag_medianTable1Y[0] = MAGy
mag_medianTable1Z[0] = MAGz
# Copy the tables
mag_medianTable2X = mag_medianTable1X[:]
mag_medianTable2Y = mag_medianTable1Y[:]
mag_medianTable2Z = mag_medianTable1Z[:]
# Sort table 2
mag_medianTable2X.sort()
mag_medianTable2Y.sort()
mag_medianTable2Z.sort()
# The middle value is the value we are interested in
MAGx = mag_medianTable2X[int(MAG_MEDIANTABLESIZE/2)];
MAGy = mag_medianTable2Y[int(MAG_MEDIANTABLESIZE/2)];
MAGz = mag_medianTable2Z[int(MAG_MEDIANTABLESIZE/2)];
#Convert Gyro raw to degrees per second
rate_gyr_x = GYRx * G_GAIN
rate_gyr_y = GYRy * G_GAIN
rate_gyr_z = GYRz * G_GAIN
#Calculate the angles from the gyro.
gyroXangle+=rate_gyr_x*LP
gyroYangle+=rate_gyr_y*LP
gyroZangle+=rate_gyr_z*LP
#Convert Accelerometer values to degrees
# if not IMU_UPSIDE_DOWN:
# # If the IMU is up the correct way (Skull logo facing down), use these calculations
# AccXangle = (math.atan2(ACCy,ACCz)*RAD_TO_DEG)
# AccYangle = (math.atan2(ACCz,ACCx)+M_PI)*RAD_TO_DEG
# else:
# #Us these four lines when the IMU is upside down. Skull logo is facing up
# AccXangle = (math.atan2(-ACCy,-ACCz)*RAD_TO_DEG)
# AccYangle = (math.atan2(-ACCz,-ACCx)+M_PI)*RAD_TO_DEG
# if not IMU_UPSIDE_DOWN:
# # If the IMU is up the correct way (Skull logo facing down), use these calculations
AccXangle = (math.atan2(ACCy,ACCz)*RAD_TO_DEG)
AccYangle = (math.atan2(ACCz,ACCx)+M_PI)*RAD_TO_DEG
# else:
# #Us these four lines when the IMU is upside down. Skull logo is facing up
# AccXangle = (math.atan2(-ACCy,-ACCz)*RAD_TO_DEG)
# AccYangle = (math.atan2(-ACCz,-ACCx)+M_PI)*RAD_TO_DEG
#Change the rotation value of the accelerometer to -/+ 180 and
#move the Y axis '0' point to up. This makes it easier to read.
if AccYangle > 90:
AccYangle -= 270.0
else:
AccYangle += 90.0
#Complementary filter used to combine the accelerometer and gyro values.
CFangleX=AA*(CFangleX+rate_gyr_x*LP) +(1 - AA) * AccXangle
CFangleY=AA*(CFangleY+rate_gyr_y*LP) +(1 - AA) * AccYangle
#Kalman filter used to combine the accelerometer and gyro values.
kalmanY = kalmanFilterY(AccYangle, rate_gyr_y,LP)
kalmanX = kalmanFilterX(AccXangle, rate_gyr_x,LP)
# if IMU_UPSIDE_DOWN:
# MAGy = -MAGy #If IMU is upside down, this is needed to get correct heading.
#Calculate heading
heading = 180 * math.atan2(MAGy,MAGx)/M_PI
#Only have our heading between 0 and 360
if heading < 0:
heading += 360
####################################################################
###################Tilt compensated heading#########################
####################################################################
#Normalize accelerometer raw values.
# if not IMU_UPSIDE_DOWN:
# #Use these two lines when the IMU is up the right way. Skull logo is facing down
# accXnorm = ACCx/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
# accYnorm = ACCy/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
# else:
# #Us these four lines when the IMU is upside down. Skull logo is facing up
# accXnorm = -ACCx/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
# accYnorm = ACCy/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
# if not IMU_UPSIDE_DOWN:
# #Use these two lines when the IMU is up the right way. Skull logo is facing down
accXnorm = ACCx/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
accYnorm = ACCy/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
# else:
# #Us these four lines when the IMU is upside down. Skull logo is facing up
# accXnorm = -ACCx/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
# accYnorm = ACCy/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
#Calculate pitch and roll
pitch = math.asin(accXnorm)
roll = -math.asin(accYnorm/math.cos(pitch))
#Calculate the new tilt compensated values
magXcomp = MAGx*math.cos(pitch)+MAGz*math.sin(pitch)
#The compass and accelerometer are orientated differently on the LSM9DS0 and LSM9DS1 and the Z axis on the compass
#is also reversed. This needs to be taken into consideration when performing the calculations
# if(IMU.LSM9DS0):
# magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)-MAGz*math.sin(roll)*math.cos(pitch) #LSM9DS0
# else:
# magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)+MAGz*math.sin(roll)*math.cos(pitch) #LSM9DS1
magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)-MAGz*math.sin(roll)*math.cos(pitch) #LSM9DS0
#Calculate tilt compensated heading
tiltCompensatedHeading = 180 * math.atan2(magYcomp,magXcomp)/M_PI
if tiltCompensatedHeading < 0:
tiltCompensatedHeading += 360
##to help test things i created this function, please ignore indentation im new to this editor here:
Accel=[AccXangle, AccYangle]
def getXY():
global ejex
global ejey
ejex = Accel[0]
ejey = Accel[1]
print('EXITO!', ejex, ejey)
return
############################ END ##################################
print("IMU TEST START", '\n', "Accel in X= ", AccXangle, '\n', "Accel in Y= ", AccYangle, '\n', 'GyroX = ', gyroXangle, '\n', "GyroY = ", gyroYangle, '\n', "GyroZ = ", gyroZangle, '\n', "Heading = ", heading, '\n', 'Compensated Heading = ', '\n', tiltCompensatedHeading, '\n')
# if 1: #Change to '0' to stop showing the angles from the accelerometer
# outputString += "t# ACCX Angle %5.2f ACCY Angle %5.2f # " % (AccXangle, AccYangle)
#
#
# if 1: #Change to '0' to stop showing the angles from the gyro
# outputString +="\t# GRYX Angle %5.2f GYRY Angle %5.2f GYRZ Angle %5.2f # " % (gyroXangle,gyroYangle,gyroZangle)
#
#
# if 1: #Change to '0' to stop showing the angles from the complementary filter
# outputString +="\t# CFangleX Angle %5.2f CFangleY Angle %5.2f #" % (CFangleX,CFangleY)
#
#
# if 1: #Change to '0' to stop showing the heading
# outputString +="\t# HEADING %5.2f tiltCompensatedHeading %5.2f #" % (heading,tiltCompensatedHeading)
#
#
# if 0: #Change to '0' to stop showing the angles from the Kalman filter
# outputString +="# kalmanX %5.2f kalmanY %5.2f #" % (kalmanX,kalmanY)
#
# print(outputString)
#slow program down a bit, makes the output more readable
time.sleep(0.1)
В качестве теста я создал тестовый код, чтобы убедиться, что могу импортировать x и y:
import sys
import IMUmodule
print(IMUmodule.getXY())
Я попытался вызвать переменную, безрезультатно. Я пытался вызвать только функцию, без эффекта. Я попытался создать функцию, которая переписывает глобальную (последнюю версию, которую вы можете увидеть выше), безрезультатно. IMUmodlue работает нормально, и функции, которые я создал для тестирования, работают корректно, когда я выполняю IMUmodule.py. Если я импортирую IMUmodule в другой скрипт, скрипт работает отлично и печатает все данные, которые IMUmodule должен распечатать.
Это ошибка, которую я получаю при последней попытке написать глобальные переменные ejex и ejey: Traceback (последний вызов был последним): файл «IntegratedImuServo.py», строка 4, в print (imumodule.ejex) AttributeError: module 'imumodule' не имеет атрибута 'getXY'
Все файлы находятся в одной папке. Я новичок в Python и во всех RPI, поэтому, пожалуйста, прости мое невежество.
Спасибо всем.
Привет OZZMAKER за создание в основном всего кода для фильтрации данных IMU.
--- EDIT ---
Я добавляю сюда код, который я сделал для перемещения сервоприводов вручную. Я хочу использовать измерения из IMUmodule (AccXangle и AccYangle), чтобы создать условия для перемещения сервоприводов, если кому-то понадобится их изучить.
import RPi.GPIO as GPIO
import time
import curses
import os
from adafruit_servokit import ServoKit
kit = ServoKit(channels=16)
# global angle0
# global angle1
# global angle2
# global angle3
angle0 = 90
angle1 = 90
angle2 = 90
angle3 = 90
increment_factor = 2
screen = curses.initscr()
curses.noecho()
curses.cbreak()
screen.keypad(True)
#Set the GPIO port to BCM encoding mode
GPIO.setmode(GPIO.BCM)
#Ignore warning information
GPIO.setwarnings(False)
#Servo initialization operation
def Servo_init():
# global angleleft
# global angleright
#servo1
kit.servo[0].angle = 90
kit.servo[0].actuation_range = 180
#servo2
kit.servo[1].angle = 90
kit.servo[1].actuation_range = 180
#servo3
kit.servo[2].angle = 90
kit.servo[2].actuation_range = 180
#servo4
kit.servo[3].angle = 90
kit.servo[3].actuation_range = 180
kit.servo[0].set_pulse_width_range(475, 2550)
kit.servo[1].set_pulse_width_range(475, 2550)
kit.servo[2].set_pulse_width_range(475, 2550)
kit.servo[3].set_pulse_width_range(475, 2550)
def center():
global angle0
global angle1
global angle2
global angle3
kit.servo[0].angle = 90
kit.servo[1].angle = 90
kit.servo[2].angle = 90
kit.servo[3].angle = 90
angle0 = 90
angle1 = 90
angle2 = 90
angle3 = 90
print("Centered")
return
def rise():#we add degs to raise the platform
global angle0
global angle1
global angle2
global angle3
if 0 <= angle0 < 180:
angle0 += increment_factor
kit.servo[0].angle = angle0
if 0 <= angle1 < 180:
angle1 += increment_factor
kit.servo[1].angle = 180 - angle1
#
if 0 <= angle2 < 180:
angle2 += increment_factor
kit.servo[2].angle = angle2
#
if 0 <= angle3 < 180:
angle3 += increment_factor
kit.servo[3].angle = 180 - angle3
print(angle0, angle1, angle2, angle3)
return
def fall():#we substract degrees to lower the platform
global angle0
global angle1
global angle2
global angle3
if 0 < angle0 <= 180:
angle0 -= increment_factor
kit.servo[0].angle = angle0
if 0 < angle1 <= 180:
angle1 -= increment_factor
kit.servo[1].angle = 180 - angle1
#
if 0 < angle2 <= 180:
angle2 -= increment_factor
kit.servo[2].angle = angle2
#
if 0 < angle3 <= 180:
angle3 -= increment_factor
kit.servo[3].angle = 180 - angle3
print(angle0, angle1, angle2, angle3)
return
def tilt_left():
global angle0
global angle1
global angle2
global angle3
if 0 < angle0 <=180: #we lift the left track
angle0 -= increment_factor
kit.servo[0].angle = angle0
if 0 < angle2 <=180:
angle2 -= increment_factor
kit.servo[2].angle = angle2
if 0 <= angle1 < 180: #we lower the right track
angle1 += increment_factor
kit.servo[1].angle = 180 - angle1
if 0 <= angle3 < 180: #we lower the right track
angle3 += increment_factor
kit.servo[3].angle = 180 - angle3
print(angle0, angle1, angle2, angle3)
return
def tilt_right():
global angle0
global angle1
global angle2
global angle3
if 0 < angle1 <=180: #we lift the left track
angle1 -= increment_factor
kit.servo[1].angle = 180 - angle1
if 0 < angle3 <=180:
angle3 -= increment_factor
kit.servo[3].angle = 180 - angle3
if 0 <= angle0 < 180: #we lower the right track
angle0 += increment_factor
kit.servo[0].angle = angle0
if 0 <= angle2 < 180: #we lower the right track
angle2 += increment_factor
kit.servo[2].angle = angle2
print(angle0, angle1, angle2, angle3)
return
#def tilt_forward():
try:
Servo_init()
while True:
char = screen.getch()
if char == ord('o'):
rise()
elif char == ord('l'):
fall()
elif char == ord('k'):
tilt_left()
elif char == ord(';'):
tilt_right()
elif char == ord(' '):
# if 180 > angleleft >= 0 and 180 > angleright >= 0:
# angleleft -= 2
# angleright += 2
angleleft = angleright
kit.servo[0].angle = angleleft
kit.servo[1].angle = 180 - angleright
kit.servo[2].angle = angleleft
kit.servo[3].angle = 180 - angleright
print(angleleft, angleright)
elif char == ord('w'):
#if 180 > angleleft >= 0 and 180 > angleright >= 0:
# angleleft -= 2
# angleright += 2
kit.servo[0].angle = 0
kit.servo[1].angle = 180
kit.servo[2].angle = 0
kit.servo[3].angle = 180
elif char == ord('s'):
#if 180 > angleleft >= 0 and 180 > angleright >= 0:
# angleleft -= 2
# angleright += 2
kit.servo[0].angle = 180
kit.servo[1].angle = 0
kit.servo[2].angle = 180
kit.servo[3].angle = 0
angleleft = 180
angleright = 180
print(angleleft, angleright)
elif char == ord('r'):
center()
# global angle0
# global angle1
# global angle2
# global angle3
# #if 180 > angleleft >= 0 and 180 > angleright >= 0:
# # angleleft -= 2
# # angleright += 2
# kit.servo[0].angle = 180 - angle0
# kit.servo[1].angle = 180 - angle1
# kit.servo[2].angle = 180 - angle2
# kit.servo[3].angle = 180 - angle3
finally:
curses.nocbreak(); screen.keypad(0); curses.echo()
curses.endwin()
GPIO.cleanup()
kit.servo[0].angle = 90
kit.servo[1].angle = 90
kit.servo[2].angle = 90
kit.servo[3].angle = 90
kit.servo[4].angle = 90