예제 #1
0
def translation(L,N,A):
    date = bytearray(b'\x55\x55\x3B\x03\x12')
    date.extend(Uart.AddTime(65))
    angle = A/180*PI

    x1 = GP.Leg1_Pos[0] - L/N*math.cos(angle)
    y1 = GP.Leg1_Pos[1] - L/N*math.sin(angle)
    z1 = GP.Leg1_Pos[2]
    #IK.InverseKinematics_Leg1(x1,y1,z1,date)

    x2 = GP.Leg2_Pos[0] - L/N*math.cos(angle)
    y2 = GP.Leg2_Pos[1] - L/N*math.sin(angle)
    z2 = GP.Leg2_Pos[2]
    #IK.InverseKinematics_Leg2(x2,y2,z2,date)

    x3 = GP.Leg3_Pos[0] - L/N*math.cos(angle)
    y3 = GP.Leg3_Pos[1] - L/N*math.sin(angle)
    z3 = GP.Leg3_Pos[2]
    #IK.InverseKinematics_Leg3(x3,y3,z3,date)

    x4 = GP.Leg4_Pos[0] - L/N*math.cos(angle)
    y4 = GP.Leg4_Pos[1] - L/N*math.sin(angle)
    z4 = GP.Leg4_Pos[2]
    #IK.InverseKinematics_Leg4(x4,y4,z4,date)

    x5 = GP.Leg5_Pos[0] - L/N*math.cos(angle)
    y5 = GP.Leg5_Pos[1] - L/N*math.sin(angle)
    z5 = GP.Leg5_Pos[2]
    #IK.InverseKinematics_Leg5(x5,y5,z5,date)

    x6 = GP.Leg6_Pos[0] - L/N*math.cos(angle)
    y6 = GP.Leg6_Pos[1] - L/N*math.sin(angle)
    z6 = GP.Leg6_Pos[2]
    gamma_add, beta_add, z_add = ccc.adjust(z1,z2,z3,z4,z5,z6)
    ccc.twist_all(x1,y1,z1+z_add,x2,y2,z2+z_add,x3,y3,z3+z_add,x4,y4,z4+z_add,x5,y5,z5+z_add,x6,y6,z6+z_add, gamma_add, beta_add, z_add,'all')
    #IK.InverseKinematics_Leg6(x6,y6,z6,date)

    #Uart.SendDate(date)

    GP.Leg1_Pos[0] = x1
    GP.Leg1_Pos[1] = y1
    GP.Leg1_Pos[2] = z1
    GP.Leg2_Pos[0] = x2
    GP.Leg2_Pos[1] = y2
    GP.Leg2_Pos[2] = z2
    GP.Leg3_Pos[0] = x3
    GP.Leg3_Pos[1] = y3
    GP.Leg3_Pos[2] = z3
    GP.Leg4_Pos[0] = x4
    GP.Leg4_Pos[1] = y4
    GP.Leg4_Pos[2] = z4
    GP.Leg5_Pos[0] = x5
    GP.Leg5_Pos[1] = y5
    GP.Leg5_Pos[2] = z5
    GP.Leg6_Pos[0] = x6
    GP.Leg6_Pos[1] = y6
    GP.Leg6_Pos[2] = z6
예제 #2
0
def Swing_Leg3(L,A):
    angle = A/180*PI
    
    for i in range(0,23):
        #date = bytearray(b'\x55\x55\x0E\x03\x03')
        #date.extend(Uart.AddTime(65))
        x = GP.Leg3_Pos[0] + L*math.cos(angle)*X_Index[i]
        y = GP.Leg3_Pos[1] + L*math.sin(angle)*Y_Index[i]
        z = GP.Leg3_Pos[2] + H*Z_Index[i]
        gamma_add, beta_add, z_add = ccc.adjust(GP.Leg1_Pos[2],GP.Leg2_Pos[2],z,GP.Leg4_Pos[2],GP.Leg5_Pos[2],GP.Leg6_Pos[2])
        ccc.twist_all(GP.Leg1_Pos[0],GP.Leg1_Pos[1],GP.Leg1_Pos[2],GP.Leg2_Pos[0],GP.Leg2_Pos[1],GP.Leg2_Pos[2],x,y,z+z_add,GP.Leg4_Pos[0],GP.Leg4_Pos[1],GP.Leg4_Pos[2],GP.Leg5_Pos[0],GP.Leg5_Pos[1],GP.Leg5_Pos[2],GP.Leg6_Pos[0],GP.Leg6_Pos[1],GP.Leg6_Pos[2],gamma_add, beta_add, z_add,3)
        #IK.InverseKinematics_Leg3(x,y,z,date)
        #Uart.SendDate(date)
        #time.sleep(0.1)
        GP.Leg3_Pos = [x, y, z]

        if (Read_GPIO(12) and (i>7)):
            #print(GPIO.input(15))
            break