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
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
