Update Ver 7.3

Py Apple Dynamics V7.3 SRC/PA-Dynamics V7.3/PA_ATTITUDE.py

@@ -0,0 +1,42 @@
+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 V7.3 SRC/PA-Dynamics V7.3/PA_GAIT.py

@@ -0,0 +1,88 @@
+import padog
+import PA_STABLIZE
+from math import pi,tan
+
+x1=0;x2=0;x3=0;x4=0;y1=0;y2=0;y3=0;y4=0
+
+def pit_cause_cg_adjust(sita,h,Kp):
+  result=round((h*tan((sita)*Kp)),2)
+  return result
+
+def foward_cg_stab(r1,r4,r2,r3,gait_need,enable):
+  if enable==True and (abs(r1)+abs(r4)+abs(r2)+abs(r3))!=0:  # When Key_stap set True in padog, self stab can be use here.
+    PA_STABLIZE.get_imu_value()
+    if PA_STABLIZE.gyro_cal_sta==1:   #等待gyro校准完成
+      gyro_p=PA_STABLIZE.filter_data_p
+      padog.X_goal=padog.in_y+gait_need+pit_cause_cg_adjust((gyro_p-PA_STABLIZE.p_origin)*pi/180,110,1.1)+PA_STABLIZE.gyro_x_fitted*5
+
+def trot(t,x_target,z_target,r1,r4,r2,r3):
+  global x1,x2,x3,x4,y1,y2,y3,y4
+  Tf=0.5
+  #陀螺仪引入
+  foward_cg_stab(r1,r4,r2,r3,0,padog.key_stab)
+  if t<Tf:
+    phase_1_swing=padog.swing_curve_generate(t,Tf,x_target,z_target,0,0,0)
+    phase_1_support=padog.support_curve_generate(0.5+t,Tf,x_target,0.5,0)
+    #TROT
+    x1=phase_1_swing[0]*r1;x2=phase_1_support[0]*r2;x3=phase_1_swing[0]*r3;x4=phase_1_support[0]*r4
+    y1=phase_1_swing[1];y2=phase_1_support[1];y3=phase_1_swing[1];y4=phase_1_support[1]
+
+  if t>=Tf:
+    phase_2_swing=padog.swing_curve_generate(t-0.5,Tf,x_target,z_target,0,0,0)
+    phase_2_support=padog.support_curve_generate(t,Tf,x_target,0.5,0)
+    #TROT
+    x1=phase_2_support[0]*r1;x2=phase_2_swing[0]*r2;x3=phase_2_support[0]*r3;x4=phase_2_swing[0]*r4
+    y1=phase_2_support[1];y2=phase_2_swing[1];y3=phase_2_support[1];y4=phase_2_swing[1]
+  return x1,x2,x3,x4,y1,y2,y3,y4
+
+
+
+def walk(t,x_target,z_target,r1,r4,r2,r3):
+  global x1,x2,x3,x4,y1,y2,y3,y4
+  Tf=0.5
+  #陀螺仪引入
+  if t<Tf:
+    foward_cg_stab(r1,r4,r2,r3,-30,True)
+    if abs(padog.X_S-padog.X_goal)<1:
+      padog.t=padog.t+padog.speed/5
+      phase_w_swing=padog.swing_curve_generate(t,Tf,x_target,z_target,0,0,0)
+      x1=phase_w_swing[0];x2=0;x3=0;x4=0
+      y1=phase_w_swing[1];y2=0;y3=0;y4=0
+
+  if t>=Tf and t<2*Tf:
+    foward_cg_stab(r1,r4,r2,r3,-30,True)
+    if abs(padog.X_S-padog.X_goal)<1:
+      padog.t=padog.t+padog.speed/5
+      phase_w_swing=padog.swing_curve_generate(t-0.5,Tf,x_target,z_target,0,0,0)
+      x1=x_target;x2=phase_w_swing[0];x3=0;x4=0
+      y1=0;y2=phase_w_swing[1];y3=0;y4=0
+
+  if t>=2*Tf and t<3*Tf:
+    foward_cg_stab(r1,r4,r2,r3,40,True)
+    if abs(padog.X_S-padog.X_goal)<1:
+      padog.t=padog.t+padog.speed/5
+      phase_w_swing=padog.swing_curve_generate(t-1,Tf,x_target,z_target,0,0,0)
+
+      x1=x_target;x2=x_target;x3=phase_w_swing[0];x4=0
+      y1=0;y2=0;y3=phase_w_swing[1];y4=0
+
+  if t>=3*Tf and t<4*Tf:
+    foward_cg_stab(r1,r4,r2,r3,40,True)
+    if abs(padog.X_S-padog.X_goal)<1:
+      padog.t=padog.t+padog.speed/5
+      phase_w_swing=padog.swing_curve_generate(t-1.5,Tf,x_target,z_target,0,0,0)
+
+      x1=x_target;x2=x_target;x3=x_target;x4=phase_w_swing[0]
+      y1=0;y2=0;y3=0;y4=phase_w_swing[1]
+
+  if t>=4*Tf:
+    foward_cg_stab(r1,r4,r2,r3,-30,True)
+    padog.t=padog.t+padog.speed/5
+    phase_w_support=padog.support_curve_generate(t-1.5,Tf,x_target,0.5,0)
+
+    x1=phase_w_support[0];x2=phase_w_support[0];x3=phase_w_support[0];x4=phase_w_support[0]
+    y1=phase_w_support[1];y2=phase_w_support[1];y3=phase_w_support[1];y4=phase_w_support[1]
+
+  return x1,x2,x3,x4,y1,y2,y3,y4
+
+

Py Apple Dynamics V7.3 SRC/PA-Dynamics V7.3/PA_IMU.py

@@ -0,0 +1,141 @@
+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'\x04') #gyro 400hz
+        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
+
+
+
+
+
+

Py Apple Dynamics V7.3 SRC/PA-Dynamics V7.3/PA_SERVO.py

@@ -0,0 +1,119 @@
+from machine import SoftI2C
+from machine import Pin
+import ustruct
+import time
+
+class PCA9685:
+    def __init__(self, i2c, address=0x40):
+        self.i2c = i2c
+        self.address = address
+        self.reset()
+
+    def _write(self, address, value):
+        self.i2c.writeto_mem(self.address, address, bytearray([value]))
+
+    def _read(self, address):
+        return self.i2c.readfrom_mem(self.address, address, 1)[0]
+
+    def reset(self):
+        self._write(0x00, 0x00) # Mode1
+
+    def freq(self, freq=None):
+
+        if freq is None:
+            return int(25000000.0 / 4096 / (self._read(0xfe) - 0.5))
+        prescale = int(25000000.0 / 4096.0 / freq + 0.5)
+        old_mode = self._read(0x00) # Mode 1
+        self._write(0x00, (old_mode & 0x7F) | 0x10) # Mode 1, sleep
+        self._write(0xfe, prescale) # Prescale
+        self._write(0x00, old_mode) # Mode 1
+        time.sleep_us(5)
+        self._write(0x00, old_mode | 0xa1) # Mode 1, autoincrement on
+
+    def pwm(self, index, on=None, off=None):
+        if on is None or off is None:
+            data = self.i2c.readfrom_mem(self.address, 0x06 + 4 * index, 4)
+            return ustruct.unpack('<HH', data)
+        data = ustruct.pack('<HH', on, off)
+        self.i2c.writeto_mem(self.address, 0x06 + 4 * index,  data)
+
+    def duty(self, index, value=None, invert=False):
+        if value is None:
+            pwm = self.pwm(index)
+            if pwm == (0, 4096):
+                value = 0
+            elif pwm == (4096, 0):
+                value = 4095
+            value = pwm[1]
+            if invert:
+                value = 4095 - value
+            return value
+        if not 0 <= value <= 4095:
+            raise ValueError("Out of range")
+        if invert:
+            value = 4095 - value
+        if value == 0:
+            self.pwm(index, 0, 4096)
+        elif value == 4095:
+            self.pwm(index, 4096, 0)
+        else:
+            self.pwm(index, 0, value)
+
+
+
+
+class Servos:
+    def __init__(self, i2c, address=0x40, freq=50, min_us=500, max_us=2500,  #根据舵机参数自行设置
+                 degrees=180):
+        self.period = 1000000 / freq
+        self.min_duty = self._us2duty(min_us)
+        self.max_duty = self._us2duty(max_us)
+        self.degrees = degrees
+        self.freq = freq
+        self.pca9685 = PCA9685(i2c, address)
+        self.pca9685.freq(freq)
+
+    def _us2duty(self, value):
+        return int(4095 * value / self.period)
+
+    def position(self, index, degrees=None, radians=None, us=None, duty=None):
+        span = self.max_duty - self.min_duty
+        if degrees is not None:
+            duty = self.min_duty + span * degrees / self.degrees
+        elif radians is not None:
+            duty = self.min_duty + span * radians / math.radians(self.degrees)
+        elif us is not None:
+            duty = self._us2duty(us)
+        elif duty is not None:
+            pass
+        else:
+            return self.pca9685.duty(index)
+        duty = min(self.max_duty, max(self.min_duty, int(duty)))
+        self.pca9685.duty(index, duty)
+
+    def release(self, index):
+        self.pca9685.duty(index, 0)
+
+    def position_duty(self, index, degrees=None, radians=None, us=None, duty=None):
+        int_dutu=int(duty)
+        self.pca9685.duty(index, int_dutu)
+
+servos = Servos(SoftI2C(scl=Pin(19), sda=Pin(18), freq=100000), address=0x40) #舵机控制板(集成)
+
+def angle(pin_num,degrees):            #设置舵机角度
+    servos.position(pin_num, degrees)
+
+def release():
+    for i in range(0,15):
+        servos.release(i)
+
+
+
+
+
+
+
+
+
+
+