【试用测评】pinpong板——MPU6050控制物理沙盒

from pyphysicssandbox import *
 # -*- coding: utf-8 -*-

 #效果：打印检测到的加速度与角速度
#接线：使用windows电脑连接一块arduino主控板

import time
from math import *
from pinpong.board import Board
from pinpong.libs.dfrobot_mpu6050 import MPU6050

K1 =0.05   # 对加速度计取值的权重
dt=20*0.001  #注意：dt的取值为滤波器采样时间
angle1=0.0
Board("PinPong Board").begin()#初始化，选择板型和端口号，不输入端口号则进行自动识别

accelgyro = MPU6050()

accelgyro.init()

if accelgyro.connection():
     print("MPU6050 connection successful")
else: 
     print("MPU6050 connection failed")


window("Python物理沙盒测试程序", 1024, 600)
gravity(0.0, 500.0)                     #  设定重力参数


floor       = static_rounded_box((0,570), 1024, 25, 3) # 最下面的地板，静止的
floor.color = Color('brown')
floor.friction=1.0 #摩擦力
floor.elasticity=0.5 #弹性

box2 = static_rounded_box((974,320), 50, 250, 5 ) # 右下品红色的圆角正方形
box2.color = Color('magenta')
box3 = static_rounded_box((0,320), 50, 250, 5 ) # 左下品红色的圆角正方形
box3.color = Color('magenta')

wheel1                 = ball((500, 130), 30)        
wheel1.color           = Color('green')
wheel1.draw_radius_line=True
wheel1.friction        = 1 #摩擦力
wheel1.elasticity=0.5 #弹性
wheel1.wrap  = False

wheel2                 = ball((600, 130), 30)        
wheel2.color           = Color('green')
wheel2.draw_radius_line=True
wheel2.friction        = 1 #摩擦力
wheel2.elasticity=0.5 #弹性
wheel2.wrap  = False


chassis=box((470,0),160,100)
chassis.color=Color("blue")


pin((500,130),wheel1,(470,0),chassis)
pin((500,130),wheel1,(470,100),chassis)
pin((600,130),wheel2,(630,0),chassis)
pin((600,130),wheel2,(630,100),chassis)

def my_observer(keys):
     global K1 ,dt,angle1 
     buf = accelgyro.get_motion6()
     ax=buf[0]
     az=buf[2]
     gy=buf[4]
     angleAx=atan2(ax,az)*180/3.1415926 #加速度计算角度
     gyroGy=-gy/131.00 #陀螺仪角速度，注意正负号与放置有关
    #一阶互补滤波
     angle1 = K1 * angleAx +(1-K1) * (angle1 +gyroGy * dt)
     #print("ax:{} ay:{} az:{} gx:{} gy:{} gz:{}".format(buf[0], buf[1], buf[2],buf[3],buf[4],buf[5]))
     print(angle1)
    
     motor(wheel1,angle1)
     motor(wheel2,angle1)
     time.sleep(20*0.001)
add_observer(my_observer)
run()

print("测试完成")
复制代码