update 6.5

Py Apple Dynamics V6.5/Py Apple Dynamics V6.5（直插版）固件及程序/V6.5 源代码/PA_ATTITUDE.py

@@ -0,0 +1,47 @@
+#Copyright Deng（灯哥） ([email protected])  Py-apple dog project
+#Github:https://github.com/ToanTech/py-apple-quadruped-robot
+#Licensed under the Apache License, Version 2.0 (the "License");
+#you may not use this file except in compliance with the License.
+#You may obtain a copy of the License at:http://www.apache.org/licenses/LICENSE-2.0
+
+from math import sin,cos,pi
+
+def cal_ges(PIT,ROL,l,b,w,x,Hc):
+    YA=0
+    P=PIT*pi/180
+    R=ROL*pi/180
+    Y=YA*pi/180
+    #腿1
+    ABl_x=l/2 - x -(l*cos(P)*cos(Y))/2 + (b*cos(P)*sin(Y))/2
+    ABl_y=w/2 - (b*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)))/2 - (l*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)))/2
+    ABl_z= - Hc - (b*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)))/2 - (l*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)))/2
+    #腿2
+    AB2_x=l/2 - x - (l*cos(P)*cos(Y))/2 - (b*cos(P)*sin(Y))/2
+    AB2_y=(b*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)))/2 - w/2 - (l*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)))/2
+    AB2_z=(b*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)))/2 - Hc - (l*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)))/2
+    #腿3
+    AB3_x=(l*cos(P)*cos(Y))/2 - x - l/2 + (b*cos(P)*sin(Y))/2
+    AB3_y=w/2 - (b*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)))/2 + (l*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)))/2
+    AB3_z=(l*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)))/2 - (b*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)))/2 - Hc
+    #腿4
+    AB4_x=(l*cos(P)*cos(Y))/2 - x - l/2 - (b*cos(P)*sin(Y))/2
+    AB4_y=(b*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)))/2 - w/2 + (l*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)))/2
+    AB4_z=(b*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)))/2 - Hc + (l*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)))/2
+
+    x1=ABl_x
+    y1=ABl_z
+
+    x2=AB2_x
+    y2=AB2_z
+
+    x3=AB4_x
+    y3=AB4_z
+
+    x4=AB3_x
+    y4=AB3_z
+
+    return x1,x2,x3,x4,y1,y2,y3,y4
+
+
+
+

Py Apple Dynamics V6.5/Py Apple Dynamics V6.5（直插版）固件及程序/V6.5 源代码/PA_AVGFILT.py

@@ -0,0 +1,17 @@
+class avg_filiter():
+	def __init__(self,cache_data):
+		self.cache = cache_data
+		self.len = len(cache_data)
+		self.cache[0] = self.len
+		self.sum = 0
+		for item in cache_data[3:]:
+			self.sum += item
+		self.cache[1] = self.sum
+
+	def avg(self,new_data):
+		self.cache[1] = self.cache[1] - self.cache[3]
+		self.cache[1] = self.cache[1] + new_data
+		self.cache[3:-1] = self.cache[4:]
+		self.cache[-1] = new_data
+		return self.cache[1]//(self.len - 3)
+

Py Apple Dynamics V6.5/Py Apple Dynamics V6.5（直插版）固件及程序/V6.5 源代码/PA_IK.py

@@ -0,0 +1,108 @@
+#Copyright Deng（灯哥） ([email protected])  Py-apple dog project
+#Github:https://github.com/ToanTech/py-apple-quadruped-robot
+#Licensed under the Apache License, Version 2.0 (the "License");
+#you may not use this file except in compliance with the License.
+#You may obtain a copy of the License at:http://www.apache.org/licenses/LICENSE-2.0
+
+from math import asin,acos,atan,pi,sqrt
+
+def ik(case,l1,l2,x1,x2,x3,x4,y1,y2,y3,y4):
+    if case==0:
+      #=============串联腿=============
+      #腿1
+      x1=-x1
+      shank1=pi-acos((x1*x1+y1*y1-l1*l1-l2*l2)/(-2*l1*l2))
+      fai1=acos((l1*l1+x1*x1+y1*y1-l2*l2)/(2*l1*sqrt(x1*x1+y1*y1)))
+      if x1>0:
+          ham1=abs(atan(y1/x1))-fai1
+      elif x1<0:
+          ham1=pi-abs(atan(y1/x1))-fai1
+      else:
+          ham1=pi-1.5707-fai1
+      shank1=180*shank1/pi
+      ham1=180*ham1/pi
+
+      #腿2
+      x2=-x2
+      shank2=pi-acos((x2*x2+y2*y2-l1*l1-l2*l2)/(-2*l1*l2))
+
+      fai2=acos((l1*l1+x2*x2+y2*y2-l2*l2)/(2*l1*sqrt(x2*x2+y2*y2)))
+      if x2>0:
+          ham2=abs(atan(y2/x2))-fai2
+      elif x2<0:
+          ham2=pi-abs(atan(y2/x2))-fai2
+      else:
+          ham2=pi-1.5707-fai2
+      shank2=180*shank2/pi
+      ham2=180*ham2/pi
+
+      #腿3
+      x3=-x3
+      shank3=pi-acos((x3*x3+y3*y3-l1*l1-l2*l2)/(-2*l1*l2))
+      fail3=acos((l1*l1+x3*x3+y3*y3-l2*l2)/(2*l1*sqrt(x3*x3+y3*y3)))
+      if x3>0:
+          ham3=abs(atan(y3/x3))-fail3
+      elif x3<0:
+          ham3=pi-abs(atan(y3/x3))-fail3
+      else:
+          ham3=pi-1.5707-fail3
+      shank3=180*shank3/pi
+      ham3=180*ham3/pi
+
+      #腿4
+      x4=-x4
+      shank4=pi-acos((x4*x4+y4*y4-l1*l1-l2*l2)/(-2*l1*l2))
+      fai4=acos((l1*l1+x4*x4+y4*y4-l2*l2)/(2*l1*sqrt(x4*x4+y4*y4)))
+      if x4>0:
+          ham4=abs(atan(y4/x4))-fai4
+      elif x4<0:
+          ham4=pi-abs(atan(y4/x4))-fai4
+      else:
+          ham4=pi-1.5707-fai4
+
+      shank4=180*shank4/pi
+      ham4=180*ham4/pi
+
+      return ham1,ham2,ham3,ham4,shank1,shank2,shank3,shank4
+    #=============并连腿=============
+    elif case==1:
+      #腿1
+      y1=-y1
+      L1=sqrt(x1*x1+y1*y1)
+      psai1=asin(x1/L1)
+      fai1=acos((L1*L1+l1*l1-l2*l2)/(2*l1*L1))
+      sita1_1=180*(fai1-psai1)/pi
+      sita2_1=180*(fai1+psai1)/pi
+      #腿2
+      #x2=0
+      #y2=71
+      y2=-y2
+      L2=sqrt(x2*x2+y2*y2)
+      psai2=asin(x2/L2)
+      fai2=acos((L2*L2+l1*l1-l2*l2)/(2*l1*L2))
+      sita1_2=180*(fai2-psai2)/pi
+      sita2_2=180*(fai2+psai2)/pi
+      #腿3
+      #x3=0
+      #y3=71
+      y3=-y3
+      L3=sqrt(x3*x3+y3*y3)
+      psai3=asin(x3/L3)
+      fai3=acos((L3*L3+l1*l1-l2*l2)/(2*l1*L3))
+      sita1_3=180*(fai3-psai3)/pi
+      sita2_3=180*(fai3+psai3)/pi
+      #腿4
+      y4=-y4
+      L4=sqrt(x4*x4+y4*y4)
+      psai4=asin(x4/L4)
+      fai4=acos((L4*L4+l1*l1-l2*l2)/(2*l1*L4))
+      sita1_4=180*(fai4-psai4)/pi
+      sita2_4=180*(fai4+psai4)/pi
+
+      return sita1_1,sita1_2,sita1_3,sita1_4,sita2_1,sita2_2,sita2_3,sita2_4
+
+
+
+
+
+

Py Apple Dynamics V6.5/Py Apple Dynamics V6.5（直插版）固件及程序/V6.5 源代码/PA_IMU.py

@@ -0,0 +1,138 @@
+import math
+import machine
+from time import sleep,sleep_ms
+Kp=0.8 #比例增益支配率收敛到加速度计/磁强计
+Ki=0.001 #积分增益支配率的陀螺仪偏见的衔接
+halfT=0.004 #采样周期的一半
+q0=1
+q1=0
+q2=0
+q3=0; #四元数的元素 ,代表估计方向
+exInt=0
+eyInt=0
+ezInt=0 #按比例缩小积分误差
+
+def IMUupdate(gx,gy,gz,ax,ay,az):
+    K=0.7
+    a=[0,0,0,0,0,0,0,0]
+    global Kp,Ki,halfT,q0,q1,q2,q3,exInt,eyInt,ezInt
+    if ax!=0 or ay!=0 or az!=0: 
+        norm=math.sqrt(ax*ax+ay*ay+az*az);
+    ax=ax/norm; #单位化
+    ay=ay/norm;
+    az=az/norm;
+    #估计方向的重力
+    vx=2* (q1*q3-q0*q2 );
+    vy=2* (q0*q1+q2*q3 );
+    vz=q0*q0-q1*q1-q2*q2+q3*q3;
+    #错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
+    ex= (ay*vz-az*vy );
+    ey= (az*vx-ax*vz );
+    ez= (ax*vy-ay*vx );
+    #积分误差比例积分增益
+    exInt=exInt+ex*Ki;
+    eyInt=eyInt+ey*Ki;
+    ezInt=ezInt+ez*Ki;
+    #调整后的陀螺仪测量
+    gx=gx+Kp*ex+exInt;
+    gy=gy+Kp*ey+eyInt;
+    gz=gz+Kp*ez+ezInt;
+    #整合四元数率和正常化
+    q0=q0+ (-q1*gx-q2*gy-q3*gz )*halfT;
+    q1=q1+ (q0*gx+q2*gz-q3*gy )*halfT;
+    q2=q2+ (q0*gy-q1*gz+q3*gx )*halfT;
+    q3=q3+ (q0*gz+q1*gy-q2*gx )*halfT;
+    #正常化四元
+    norm=math.sqrt(q0*q0+q1*q1+q2*q2+q3*q3 );
+    q0=q0/norm;
+    q1=q1/norm;
+    q2=q2/norm;
+    q3=q3/norm;
+    Pitch=math.asin (-2*q1*q3+2*q0*q2 )*57.3; #pitch ,转换为度数
+    if -2*q1*q1-2*q2*q2+1!=0:
+        Roll=math.atan ((2*q2*q3+2*q0*q1)/(-2*q1*q1-2*q2*q2+1) )*57.3; #rollv
+    a[0]=Pitch
+    a[1]=Roll
+    if ay*ay+az*az!=0:
+        a[2]=-math.atan(ax/math.sqrt(ay*ay+az*az))*57.2957795
+    if ax*ax+az*az!=0:
+        a[3]=math.atan(ay/math.sqrt(ax*ax+az*az))*57.2957795
+    a[4]=gx
+    a[5]=gy
+    a[6]=gz
+    a[0]=-K*Pitch-(1-K)*a[2]
+    a[1]=K*Roll+(1-K)*a[3]
+    return a
+
+class accel():
+    error=[0,0,0]
+    def __init__(self, i2c, addr=0x68):
+        self.iic = i2c
+        self.addr = addr
+        self.iic.start()
+        sleep_ms(1)
+        self.iic.writeto(self.addr, bytearray([107, 0]))
+        sleep_ms(1)
+        self.iic.writeto_mem(self.addr,0x19,b'\x07') #gyro 125hz
+        sleep_ms(1)
+        self.iic.writeto_mem(self.addr,0x1a,b'\x04')  #low filter 21hz
+        sleep_ms(1)
+        self.iic.writeto_mem(self.addr,0x1b,b'\x08') #gryo 500/s 65.5lsb/g
+        sleep_ms(1)
+        self.iic.writeto_mem(self.addr,0x1c,b'\x08') #acceler 4g ,8192lsb.g
+        sleep_ms(1)
+        self.iic.stop()
+
+    def get_raw_values(self):
+        self.iic.start()
+        a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
+        self.iic.stop()
+        return a
+
+    def get_ints(self):
+        b = self.get_raw_values()
+        c = []
+        for i in b:
+            c.append(i)
+        return c
+
+    def bytes_toint(self, firstbyte, secondbyte):
+        if not firstbyte & 0x80:
+            return firstbyte << 8 | secondbyte
+        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)
+
+    def error_gy(self):
+        sleep(3)
+        global error
+        error=[0,0,0]
+        vals = {}
+        for i in range(0,10):
+	            raw_ints = self.get_raw_values()
+	            vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])
+	            vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])
+	            vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])
+	            error[0]= error[0]+vals["GyX"]
+	            error[1]= error[1]+vals["GyY"]
+	            error[2]= error[2]+vals["GyZ"]
+	            sleep_ms(8)
+        error[1]=error[1]/10.0
+        error[2]=error[2]/10.0
+        error[0]=error[0]/10.0
+
+    def get_values(self):
+        global error
+        vals = {}
+        raw_ints = self.get_raw_values()
+        vals["AcX"] = self.bytes_toint(raw_ints[0], raw_ints[1])
+        vals["AcY"] = self.bytes_toint(raw_ints[2], raw_ints[3])
+        vals["AcZ"] = self.bytes_toint(raw_ints[4], raw_ints[5])
+        vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
+        vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])-error[0]
+        vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])-error[1]
+        vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])-error[2]
+        #vals["GyZ1"] = self.bytes_toint(raw_ints[12], raw_ints[13])
+        return vals  # returned in range of Int16
+        # -32768 to 32767
+
+
+