def InverseKinematics_Leg4(x,y,z,date):
L1 = 60.01
L2 = 75.81
L3 = 143.69
X0 = 65.505
Y0 = -120.505
Z0 = 0
q0 = 0.7853981633974483
q41 = 500+int((-q0 + math.atan(-(y-Y0)/(x-X0)))*750/math.pi)
X1 = X0 + L1*math.cos(math.atan(-(y-Y0)/(x-X0)))
Y1 = Y0 - L1*math.sin(math.atan(-(y-Y0)/(x-X0)))
Z1 = 0
La = math.sqrt((x-X1)**2+(y-Y1)**2+(z-Z1)**2)
Lb = math.sqrt((x-X0)**2+(y-Y0)**2+(z-Z0)**2)
q42 = 500+int(((math.pi - math.acos((La**2+L2**2-L3**2)/(2*L2*La)) - math.acos((L1**2+La**2-Lb**2)/(2*L1*La)))*750/math.pi))
q43 = 500-int((140/180*math.pi - math.acos((L2**2+L3**2-La**2)/(2*L2*L3)))*750/math.pi)
date.extend(Uart.Cmd_Servo_Move(10,q41))
date.extend(Uart.Cmd_Servo_Move(11,q42))
date.extend(Uart.Cmd_Servo_Move(12,q43))
def InverseKinematics_Leg3(x,y,z,date):
L1 = 60.01
L2 = 75.81
L3 = 143.69
X0 = -65.505
Y0 = -120.505
Z0 = 0
q0 = 0.7853981633974483
q31 = 500+int((q0 - math.atan((y-Y0)/(x-X0)))*750/math.pi)
X1 = X0 - L1*math.cos(math.atan((y-Y0)/(x-X0)))
Y1 = Y0 - L1*math.sin(math.atan((y-Y0)/(x-X0)))
Z1 = 0
La = math.sqrt((x-X1)**2+(y-Y1)**2+(z-Z1)**2)
Lb = math.sqrt((x-X0)**2+(y-Y0)**2+(z-Z0)**2)
q32 = 500+int((math.pi - math.acos((La**2+L2**2-L3**2)/(2*L2*La)) - math.acos((L1**2+La**2-Lb**2)/(2*L1*La)))*750/math.pi)
q33 = 500-int((140/180*math.pi - math.acos((L2**2+L3**2-La**2)/(2*L2*L3)))*750/math.pi)
date.extend(Uart.Cmd_Servo_Move(7,q31))
date.extend(Uart.Cmd_Servo_Move(8,q32))
date.extend(Uart.Cmd_Servo_Move(9,q33))
def Robot_Init():
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(GP.Leg1_Pos[0],GP.Leg1_Pos[1],GP.Leg1_Pos[2],date)
IK.InverseKinematics_Leg2(GP.Leg2_Pos[0],GP.Leg2_Pos[1],GP.Leg2_Pos[2],date)
IK.InverseKinematics_Leg3(GP.Leg3_Pos[0],GP.Leg3_Pos[1],GP.Leg3_Pos[2],date)
IK.InverseKinematics_Leg4(GP.Leg4_Pos[0],GP.Leg4_Pos[1],GP.Leg4_Pos[2],date)
IK.InverseKinematics_Leg5(GP.Leg5_Pos[0],GP.Leg5_Pos[1],GP.Leg5_Pos[2],date)
IK.InverseKinematics_Leg6(GP.Leg6_Pos[0],GP.Leg6_Pos[1],GP.Leg6_Pos[2],date)
Uart.SendDate(date)
time.sleep(0.5)
def Robot_Init():
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(180, 240, -95, date)
IK.InverseKinematics_Leg2(230, 0, -95, date)
IK.InverseKinematics_Leg3(180, -240, -95, date)
IK.InverseKinematics_Leg4(-180, 240, -95, date)
IK.InverseKinematics_Leg5(-230, 0, -95, date)
IK.InverseKinematics_Leg6(-180, -240, -95, date)
Uart.SendDate(date)
time.sleep(0.5)
def straightLeg_h(Length, dir, time):
step = 8
for i in range(0, 17):
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
straight_h_Leg1(Length, dir, i, step, date)
straight_h_Leg2(Length, dir, i, step, date)
straight_h_Leg3(Length, dir, i, step, date)
straight_h_Leg4(Length, dir, i, step, date)
straight_h_Leg5(Length, dir, i, step, date)
straight_h_Leg6(Length, dir, i, step, date)
Uart.SendDate(date)
def shrinkageLeg(Length,time):
step = 8
for i in range(0,17):
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
shrinkage_Leg1(Length,0,i,step,date)
shrinkage_Leg2(Length,0,i,step,date)
shrinkage_Leg3(Length,0,i,step,date)
shrinkage_Leg4(Length,0,i,step,date)
shrinkage_Leg5(Length,0,i,step,date)
shrinkage_Leg6(Length,0,i,step,date)
Uart.SendDate(date)
def CurveLeg(Radius, angle, time):
step = 8
for i in range(0, 17):
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
Curve_Leg1(Radius, angle, i, step, date)
Curve_Leg2(Radius, angle, i, step, date)
Curve_Leg3(Radius, angle, i, step, date)
Curve_Leg4(Radius, angle, i, step, date)
Curve_Leg5(Radius, angle, i, step, date)
Curve_Leg6(Radius, angle, i, step, date)
Uart.SendDate(date)
def test():
Xa = 130
Ya = 130
Za = -90
Xb = 250
Yb = 0
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(Xa, Ya, Za, date)
IK.InverseKinematics_Leg2(Xb, Yb, Za, date)
IK.InverseKinematics_Leg3(Xa, -Ya, Za, date)
IK.InverseKinematics_Leg4(-Xa, Ya, Za, date)
IK.InverseKinematics_Leg5(-Xb, Yb, Za, date)
IK.InverseKinematics_Leg6(-Xa, -Ya, Za, date)
Uart.SendDate(date)
def straightLeg_Link(Length, dir, time):
step = 15
tx1 = ForwardKinematics.Tempxyz()[0]
ty1 = ForwardKinematics.Tempxyz()[1]
tz1 = ForwardKinematics.Tempxyz()[2]
tx2 = ForwardKinematics.Tempxyz()[3]
ty2 = ForwardKinematics.Tempxyz()[4]
tz2 = ForwardKinematics.Tempxyz()[5]
tx3 = ForwardKinematics.Tempxyz()[6]
ty3 = ForwardKinematics.Tempxyz()[7]
tz3 = ForwardKinematics.Tempxyz()[8]
tx4 = ForwardKinematics.Tempxyz()[9]
ty4 = ForwardKinematics.Tempxyz()[10]
tz4 = ForwardKinematics.Tempxyz()[11]
tx5 = ForwardKinematics.Tempxyz()[12]
ty5 = ForwardKinematics.Tempxyz()[13]
tz5 = ForwardKinematics.Tempxyz()[14]
tx6 = ForwardKinematics.Tempxyz()[15]
ty6 = ForwardKinematics.Tempxyz()[16]
tz6 = ForwardKinematics.Tempxyz()[17]
for i in range(0, 31):
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
straight_Leg1_Link(Length, dir, tx1, ty1, tz1, i, step, date)
straight_Leg2_Link(Length, dir, tx2, ty2, tz2, i, step, date)
straight_Leg3_Link(Length, dir, tx3, ty3, tz3, i, step, date)
straight_Leg4_Link(Length, dir, tx4, ty4, tz4, i, step, date)
straight_Leg5_Link(Length, dir, tx5, ty5, tz5, i, step, date)
straight_Leg6_Link(Length, dir, tx6, ty6, tz6, i, step, date)
ser.write(date)
def Robot_Init():
x1 = 200
y1 = 240
z1 = -130
x2 = 250
y2 = 0
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(x1, y1, z1, date)
IK.InverseKinematics_Leg2(x2, y2, z1, date)
IK.InverseKinematics_Leg3(x1, -y1, z1, date)
IK.InverseKinematics_Leg4(-x1, y1, z1, date)
IK.InverseKinematics_Leg5(-x2, y2, z1, date)
IK.InverseKinematics_Leg6(-x1, -y1, z1, date)
Uart.SendDate(date)
time.sleep(0.5)
def InverseKinematics_Leg6(x, y, z, date):
PI = 3.14
L1 = 55
L2 = 108
L3 = 183
X0 = -50
Y0 = -105
Z0 = 0
Bias_A1 = 45
Bias_A2 = 180
Bias_A3 = 90
Q61 = 500 + (int)((math.atan(
(y - Y0) / (x - X0)) - Bias_A1 / 180 * PI) * 750 / PI)
X1 = X0 - L1 * math.cos(math.atan((y - Y0) / (x - X0)))
Y1 = Y0 - L1 * math.sin(math.atan((y - Y0) / (x - X0)))
Z1 = Z0
La = math.sqrt((x - X1) * (x - X1) + (y - Y1) * (y - Y1) + (z - Z1) *
(z - Z1))
Lb = math.sqrt((x - X0) * (x - X0) + (y - Y0) * (y - Y0) + (z - Z0) *
(z - Z0))
Q63 = 500 - (int)(
(math.acos((L2 * L2 + L3 * L3 - La * La) /
(2 * L2 * L3)) - Bias_A3 / 180 * PI) * 750 / PI)
if (z < Z0):
Q62 = 500 + (int)((-Bias_A2 / 180 * PI + 2 * PI - math.acos(
(La * La + L2 * L2 - L3 * L3) / (2 * L2 * La)) - math.acos(
(L1 * L1 + La * La - Lb * Lb) / (2 * L1 * La))) * 750 / PI)
elif (z == Z0):
Q62 = 500 + (int)((-Bias_A2 / 180 * PI + PI - (math.acos(
(La * La + L2 * L2 - L3 * L3) / (2 * L2 * La)))) * 750 / PI)
else:
Q62 = 500 + (int)((-Bias_A2 / 180 * PI + math.acos(
(L1 * L1 + La * La - Lb * Lb) / (2 * L1 * La)) - math.acos(
(La * La + L2 * L2 - L3 * L3) / (2 * L2 * La))) * 750 / PI)
#print('Leg6:')
#print(Q61,Q62,Q63,'\n')
date.extend(Uart.Cmd_Servo_Move(16, Q61))
date.extend(Uart.Cmd_Servo_Move(17, Q62))
date.extend(Uart.Cmd_Servo_Move(18, Q63))
def InverseKinematics_Leg1(x, y, z, date):
PI = 3.14
L1 = 55
L2 = 108
L3 = 183
X0 = 50
Y0 = 105
Z0 = 0
Bias_A1 = 45
Bias_A2 = 132.63
Bias_A3 = 72.93
Q11 = 500 + (int)((math.atan(
(y - Y0) / (x - X0)) - Bias_A1 / 180 * PI) * 750 / PI)
X1 = X0 + L1 * math.cos(math.atan((y - Y0) / (x - X0)))
Y1 = Y0 + L1 * math.sin(math.atan((y - Y0) / (x - X0)))
Z1 = Z0
La = math.sqrt((x - X1) * (x - X1) + (y - Y1) * (y - Y1) + (z - Z1) *
(z - Z1))
Lb = math.sqrt((x - X0) * (x - X0) + (y - Y0) * (y - Y0) + (z - Z0) *
(z - Z0))
Q13 = 500 - (int)(
(math.acos((L2 * L2 + L3 * L3 - La * La) /
(2 * L2 * L3)) - Bias_A3 / 180 * PI) * 750 / PI)
if (z < Z0):
Q12 = 500 + (int)((-Bias_A2 / 180 * PI + 2 * PI - math.acos(
(La * La + L2 * L2 - L3 * L3) / (2 * L2 * La)) - math.acos(
(L1 * L1 + La * La - Lb * Lb) / (2 * L1 * La))) * 750 / PI)
elif (z == 0):
Q12 = 500 + (int)((-Bias_A2 / 180 * PI + PI - (math.acos(
(La * La + L2 * L2 - L3 * L3) / (2 * L2 * La)))) * 750 / PI)
else:
Q12 = 500 + (int)((-Bias_A2 / 180 * PI + math.acos(
(L1 * L1 + La * La - Lb * Lb) / (2 * L1 * La)) - math.acos(
(La * La + L2 * L2 - L3 * L3) / (2 * L2 * La))) * 750 / PI)
#print('Leg1:')
#print(Q11,Q12,Q13,'\n')
date.extend(Uart.Cmd_Servo_Move(1, Q11))
date.extend(Uart.Cmd_Servo_Move(2, Q12))
date.extend(Uart.Cmd_Servo_Move(3, Q13))
def InverseKinematics_Leg2(x, y, z, date):
PI = 3.14
L1 = 55
L2 = 108
L3 = 183
X0 = 95
Y0 = 0
Z0 = 0
Bias_A1 = 0
Bias_A2 = 180
Bias_A3 = 90
Q21 = 500 + (int)((math.atan(
(y - Y0) / (x - X0)) - Bias_A1 / 180 * PI) * 750 / PI)
X1 = X0 + L1 * math.cos(math.atan((y - Y0) / (x - X0)))
Y1 = Y0 + L1 * math.sin(math.atan((y - Y0) / (x - X0)))
Z1 = Z0
La = math.sqrt((x - X1) * (x - X1) + (y - Y1) * (y - Y1) + (z - Z1) *
(z - Z1))
Lb = math.sqrt((x - X0) * (x - X0) + (y - Y0) * (y - Y0) + (z - Z0) *
(z - Z0))
Q23 = 500 - (int)(
(math.acos((L2 * L2 + L3 * L3 - La * La) /
(2 * L2 * L3)) - Bias_A3 / 180 * PI) * 750 / PI)
if (z < Z0):
Q22 = 500 + (int)((-Bias_A2 / 180 * PI + 2 * PI - math.acos(
(La * La + L2 * L2 - L3 * L3) / (2 * L2 * La)) - math.acos(
(L1 * L1 + La * La - Lb * Lb) / (2 * L1 * La))) * 750 / PI)
elif (z == Z0):
Q22 = 500 + (int)((-Bias_A2 / 180 * PI + PI - (math.acos(
(La * La + L2 * L2 - L3 * L3) / (2 * L2 * La)))) * 750 / PI)
else:
Q22 = 500 + (int)((-Bias_A2 / 180 * PI + math.acos(
(L1 * L1 + La * La - Lb * Lb) / (2 * L1 * La)) - math.acos(
(La * La + L2 * L2 - L3 * L3) / (2 * L2 * La))) * 750 / PI)
#print('Leg2:')
#print(Q21,Q22,Q23,'\n')
date.extend(Uart.Cmd_Servo_Move(4, Q21))
date.extend(Uart.Cmd_Servo_Move(5, Q22))
date.extend(Uart.Cmd_Servo_Move(6, Q23))
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 robot_stand():
date = bytearray(b'\x55\x55\x20\x03\x09')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(145, 155, -90, date)
IK.InverseKinematics_Leg3(145, -155, -90, date)
IK.InverseKinematics_Leg5(-185, 0, -90, date)
Uart.SendDate(date)
time.sleep(0.1)
date = bytearray(b'\x55\x55\x20\x03\x09')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(145, 155, -95, date)
IK.InverseKinematics_Leg3(145, -155, -95, date)
IK.InverseKinematics_Leg5(-185, 0, -95, date)
Uart.SendDate(date)
time.sleep(0.1)
date = bytearray(b'\x55\x55\x20\x03\x09')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg2(185, 0, -90, date)
IK.InverseKinematics_Leg4(-145, 155, -90, date)
IK.InverseKinematics_Leg6(-145, -155, -90, date)
Uart.SendDate(date)
time.sleep(0.1)
date = bytearray(b'\x55\x55\x20\x03\x09')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg2(185, 0, -95, date)
IK.InverseKinematics_Leg4(-145, 155, -95, date)
IK.InverseKinematics_Leg6(-145, -155, -95, date)
Uart.SendDate(date)
time.sleep(0.1)
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1( 145, 155,-255, date)
IK.InverseKinematics_Leg2( 185, 0, -255, date)
IK.InverseKinematics_Leg3( 145,-155,-255, date)
IK.InverseKinematics_Leg4(-145, 155,-255, date)
IK.InverseKinematics_Leg5(-185, 0, -255, date)
IK.InverseKinematics_Leg6(-145,-155,-255, date)
Uart.SendDate(date)
time.sleep(0.5)
def InverseKinematics_Leg5(x,y,z,date):
PI = 3.14
L1 = 65
L2 = 76
L3 = 140
X0 = -115
Y0 = 0
Z0 = 0
Bias_A1 = 0
Bias_A2 = 180
Bias_A3 = 140
Q51 = 500 - (int)((math.atan((y-Y0)/(x-X0)) + Bias_A1/180 * PI)*750/PI)
X1 = X0 - L1*math.cos(math.atan((y-Y0)/(x-X0)))
Y1 = Y0 - L1*math.sin(math.atan((y-Y0)/(x-X0)))
Z1 = Z0
La = math.sqrt((x-X1)*(x-X1)+(y-Y1)*(y-Y1)+(z-Z1)*(z-Z1))
Lb = math.sqrt((x-X0)*(x-X0)+(y-Y0)*(y-Y0)+(z-Z0)*(z-Z0))
Q53 = 500 + (int)((math.acos((L2*L2+L3*L3-La*La)/(2*L2*L3)) - Bias_A3/180*PI)*750/PI)
if (z < Z0):
Q52 = 500 + (int)((-Bias_A2/180*PI + 2*PI - math.acos((La*La+L2*L2-L3*L3)/(2*L2*La)) - math.acos((L1*L1+La*La-Lb*Lb)/(2*L1*La)))*750/PI)
elif (z == Z0):
Q52 = 500 + (int)((-Bias_A2/180*PI + PI - (math.acos((La*La+L2*L2-L3*L3)/(2*L2*La))))*750/PI)
else:
Q52 = 500 + (int)((-Bias_A2/180*PI + math.acos((L1*L1+La*La-Lb*Lb)/(2*L1*La)) - math.acos((La*La+L2*L2-L3*L3)/(2*L2*La)))*750/PI)
#print('Leg5:')
#print(Q51,Q52,Q53,'\n')
date.extend(Uart.Cmd_Servo_Move(13,Q51))
date.extend(Uart.Cmd_Servo_Move(14,Q52))
date.extend(Uart.Cmd_Servo_Move(15,Q53))
def InverseKinematics_Leg3(x,y,z,date):
PI = 3.14
L1 = 65
L2 = 76
L3 = 140
X0 = 66
Y0 = -120
Z0 = 0
Bias_A1 = 45
Bias_A2 = 180
Bias_A3 = 140
Q31 = 500 - (int)((math.atan((y-Y0)/(x-X0)) + Bias_A1/180 * PI)*750/PI)
X1 = X0 + L1*math.cos(math.atan((y-Y0)/(x-X0)))
Y1 = Y0 + L1*math.sin(math.atan((y-Y0)/(x-X0)))
Z1 = Z0
La = math.sqrt((x-X1)*(x-X1)+(y-Y1)*(y-Y1)+(z-Z1)*(z-Z1))
Lb = math.sqrt((x-X0)*(x-X0)+(y-Y0)*(y-Y0)+(z-Z0)*(z-Z0))
Q33 = 500 + (int)((math.acos((L2*L2+L3*L3-La*La)/(2*L2*L3)) - Bias_A3/180*PI)*750/PI)
if (z < Z0):
Q32 = 500 + (int)((-Bias_A2/180*PI + 2*PI - math.acos((La*La+L2*L2-L3*L3)/(2*L2*La)) - math.acos((L1*L1+La*La-Lb*Lb)/(2*L1*La)))*750/PI)
elif (z == Z0):
Q32 = 500 + (int)((-Bias_A2/180*PI + PI - (math.acos((La*La+L2*L2-L3*L3)/(2*L2*La))))*750/PI)
else:
Q32 = 500 + (int)((-Bias_A2/180*PI + math.acos((L1*L1+La*La-Lb*Lb)/(2*L1*La)) - math.acos((La*La+L2*L2-L3*L3)/(2*L2*La)))*750/PI)
#print('Leg3:')
#print(Q31,Q32,Q33,'\n')
date.extend(Uart.Cmd_Servo_Move(7,Q31))
date.extend(Uart.Cmd_Servo_Move(8,Q32))
date.extend(Uart.Cmd_Servo_Move(9,Q33))
def InverseKinematics_Leg2(x,y,z,date):
L1 = 60.01
L2 = 75.81
L3 = 143.69
X0 = -116
Y0 = 0
Z0 = 0
q21 = 500+int(math.atan((y-Y0)/(X0-x))*750/math.pi)
X1 = X0 - L1*math.cos(math.atan((y-Y0)/(X0-x)))
Y1 = Y0 + L1*math.sin(math.atan((y-Y0)/(X0-x)))
Z1 = 0
La = math.sqrt((x-X1)**2+(y-Y1)**2+(z-Z1)**2)
Lb = math.sqrt((x-X0)**2+(y-Y0)**2+(z-Z0)**2)
q22 = 500+int((math.pi - math.acos((La**2+L2**2-L3**2)/(2*L2*La)) - math.acos((L1**2+La**2-Lb**2)/(2*L1*La)))*750/math.pi)
q23 = 500-int((140/180*math.pi - math.acos((L2**2+L3**2-La**2)/(2*L2*L3)))*750/math.pi)
date.extend(Uart.Cmd_Servo_Move(4,q21))
date.extend(Uart.Cmd_Servo_Move(5,q22))
date.extend(Uart.Cmd_Servo_Move(6,q23))
def twist_all(x1, y1, z1, x2, y2, z2, x3, y3, z3, x4, y4, z4, x5, y5, z5, x6,
y6, z6, gamma, beta, zc, leg_id):
Rz = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
Ry = np.array([[m.cos(beta), 0, m.sin(beta)], [0, 1, 0],
[-m.sin(beta), 0, m.cos(beta)]])
Rx = np.array([[1, 0, 0], [0, m.cos(gamma), -m.sin(gamma)],
[0, m.sin(gamma), m.cos(gamma)]])
R1 = np.dot(
Rz,
Ry,
)
R = np.dot(R1, Rx)
Rp = np.array([[R[0][0], R[0][1], R[0][2], 0],
[R[1][0], R[1][1], R[1][2], 0],
[R[2][0], R[2][1], R[2][2], zc], [0, 0, 0, 1]])
# one = np.array([[-114.94], [261.0668], [-140], [1]])
# two = np.array([[-276], [0], [-140], [1]])
# three = np.array([[-114.94], [-261.0668], [-140], [1]])
# four = np.array([[114.94], [-261.06680], [-140], [1]])
# five = np.array([[276], [0], [-140], [1]])
# six = np.array([[114.94], [261.0668], [-140], [1]])
one = np.array([[x1], [y1], [z1], [1]])
two = np.array([[x2], [y2], [z2], [1]])
three = np.array([[x3], [y3], [z3], [1]])
four = np.array([[x4], [y4], [z4], [1]])
five = np.array([[x5], [y5], [z5], [1]])
six = np.array([[x6], [y6], [z6], [1]])
P_one = np.dot(Rp, one)
P_two = np.dot(Rp, two)
P_three = np.dot(Rp, three)
P_four = np.dot(Rp, four)
P_five = np.dot(Rp, five)
P_six = np.dot(Rp, six)
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
#print(P_one[0],P_one[1],z1)
#stepdrive.InverseKinematics_Leg1(P_one[0],P_one[1],P_one[2],date)
#stepdrive.InverseKinematics_Leg2(P_two[0],P_two[1],P_two[2],date)
#stepdrive.InverseKinematics_Leg3(P_three[0],P_three[1],P_three[2],date)
#stepdrive.InverseKinematics_Leg4(P_four[0],P_four[1],P_four[2],date)
#stepdrive.InverseKinematics_Leg5(P_five[0],P_five[1],P_five[2],date)
#stepdrive.InverseKinematics_Leg6(P_six[0],P_six[1],P_six[2],date)
if leg_id == 1:
stepdrive.InverseKinematics_Leg1(P_one[0], P_one[1], z1, date)
else:
stepdrive.InverseKinematics_Leg1(P_one[0], P_one[1], P_one[2], date)
if leg_id == 2:
stepdrive.InverseKinematics_Leg2(P_two[0], P_two[1], z2, date)
else:
stepdrive.InverseKinematics_Leg2(P_two[0], P_two[1], P_two[2], date)
if leg_id == 3:
stepdrive.InverseKinematics_Leg3(P_three[0], P_three[1], z3, date)
else:
stepdrive.InverseKinematics_Leg3(P_three[0], P_three[1], P_three[2],
date)
if leg_id == 4:
stepdrive.InverseKinematics_Leg4(P_four[0], P_four[1], z4, date)
else:
stepdrive.InverseKinematics_Leg4(P_four[0], P_four[1], P_four[2], date)
if leg_id == 5:
stepdrive.InverseKinematics_Leg5(P_five[0], P_five[1], z5, date)
else:
stepdrive.InverseKinematics_Leg5(P_five[0], P_five[1], P_five[2], date)
if leg_id == 6:
stepdrive.InverseKinematics_Leg6(P_six[0], P_six[1], z6, date)
else:
stepdrive.InverseKinematics_Leg6(P_six[0], P_six[1], P_six[2], date)
#print(P_two)
Uart.SendDate(date)
def __init__(self, root):
"""Do GUI stuff and attach to ObserverPattern"""
self.root = root
self.root.title ("Wirbelstrom-Control") #Titel de Fensters
self.root.geometry("1100x700+0+0")
self.menu = Menu(root)
self.filemenu = Menu(self.menu)
self.filemenu.add_command(label="Sichern", command = self.store_setting)
self.filemenu.add_command(label="Laden", command = self.load_setting)
self.menu.add_cascade(label="Datei",menu = self.filemenu)
self.root.config(menu=self.menu)
self.button_connect = Button(root, text="Verbinde COM", width = 16, command=self.connect)
self.button_disconnect = Button(root, text="COM trennen", width = 16, command=self.disconnect, state=DISABLED)
self.entry_t_time = Entry(root)
self.entry_t_lever = Entry(root)
self.entry_part = Entry(root)
self.entry_t_time.insert(0,"1")
self.entry_t_lever.insert(0,"1")
self.entry_part.insert(0,"1")
self.label_t_plus_time = Label(root, text="Kanten Ausblendung")
self.label_lever_long_time = Label(root, text="Auswerfer EIN_Zeit")
self.label_part = Label(root, text="Anz. Bauteile")
self.label_eject = Label(root, text="Sicheres Auswerfen")
self.eject_state = IntVar()
self.radiobutton_eject = Checkbutton(root, text = "EIN", variable = self.eject_state)
self.label_com = Label(root, text="SCHNITTSTELLEN Status")
self.label_dev_ok = Label(root, text="ELOTEST Bereit")
self.label_dev_ready = Label(root, text="ELOTEST Fehlerfrei")
self.label_config_ok = Label(root, text="WS-100 CONFIG erhalten")
self.label_picfault_ok = Label(root, text="WS-100 Fehlerfrei")
self.label_com_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_dev_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_dev_fault_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_config_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_picfault_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_shift = Label(root, text="Bauteil - FIFO")
self.label_count = Label(root, text="Anzahl getestet")
self.button_send = Button(root, text="Sende Konfig", fg="blue",command=lambda: self.get_gui_command(0,0), width = 41)
self.label_counts = Label(root, text="0", font=("Calibri",20), relief = GROOVE, width = 10)
self.label_input = Label(root, text="Keine Daten bis jetzt", relief = GROOVE, width = 41)
self.label_input_shift = Label(root, text="--------", font=("Calibri",20), relief = GROOVE, width = 10)
self.button_connect.place (x= 100, y=50)
self.button_disconnect.place (x= 270, y=50)
self.label_t_plus_time.place (x= 100, y = 100)
self.label_lever_long_time.place (x= 100, y = 150)
self.label_part.place (x= 100, y = 200)
self.label_eject.place (x= 100, y = 250)
self.label_com.place (x= 650, y = 100)
self.label_dev_ok.place (x= 650, y = 150)
self.label_dev_ready.place (x= 650, y = 200)
self.label_config_ok.place (x= 650, y = 250)
self.label_picfault_ok.place (x= 650, y = 300)
self.label_com_status.place (x= 850, y = 100)
self.label_dev_status.place (x= 850, y = 150)
self.label_dev_fault_status.place (x= 850, y = 200)
self.label_config_status.place (x= 850, y = 250)
self.label_picfault_status.place (x= 850, y = 300)
self.entry_t_time.place (x= 270, y = 100)
self.entry_t_lever.place (x= 270, y = 150)
self.entry_part.place (x= 270, y = 200)
self.radiobutton_eject.place (x= 270, y = 250)
self.button_send.place (x = 100, y = 300)
self.label_count.place (x = 650, y = 400)
self.label_counts.place (x = 850, y = 400)
self.label_shift.place (x = 650, y = 450)
self.label_input_shift.place (x = 850, y = 450)
self.label_input.place (x = 100, y = 350)
self.old_counts = 0
self.show_counts = 0
self.uart = Uart()
self.uart.attach(Uart.EVT_CONNECTION_STATUS, self.connection_status_changed)
self.uart.attach(Uart.EVT_DATA, self.data_received)
self.uart.attach(Uart.EVT_CONNECTION_INTERRUPTED, self.connection_interrupted)
self.root.bind("<<CONNECTION_INTERRUPTED>>", self.show_connection_interrupted)
class Gui():
def __init__(self, root):
"""Do GUI stuff and attach to ObserverPattern"""
self.root = root
self.root.title ("Wirbelstrom-Control") #Titel de Fensters
self.root.geometry("1100x700+0+0")
self.menu = Menu(root)
self.filemenu = Menu(self.menu)
self.filemenu.add_command(label="Sichern", command = self.store_setting)
self.filemenu.add_command(label="Laden", command = self.load_setting)
self.menu.add_cascade(label="Datei",menu = self.filemenu)
self.root.config(menu=self.menu)
self.button_connect = Button(root, text="Verbinde COM", width = 16, command=self.connect)
self.button_disconnect = Button(root, text="COM trennen", width = 16, command=self.disconnect, state=DISABLED)
self.entry_t_time = Entry(root)
self.entry_t_lever = Entry(root)
self.entry_part = Entry(root)
self.entry_t_time.insert(0,"1")
self.entry_t_lever.insert(0,"1")
self.entry_part.insert(0,"1")
self.label_t_plus_time = Label(root, text="Kanten Ausblendung")
self.label_lever_long_time = Label(root, text="Auswerfer EIN_Zeit")
self.label_part = Label(root, text="Anz. Bauteile")
self.label_eject = Label(root, text="Sicheres Auswerfen")
self.eject_state = IntVar()
self.radiobutton_eject = Checkbutton(root, text = "EIN", variable = self.eject_state)
self.label_com = Label(root, text="SCHNITTSTELLEN Status")
self.label_dev_ok = Label(root, text="ELOTEST Bereit")
self.label_dev_ready = Label(root, text="ELOTEST Fehlerfrei")
self.label_config_ok = Label(root, text="WS-100 CONFIG erhalten")
self.label_picfault_ok = Label(root, text="WS-100 Fehlerfrei")
self.label_com_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_dev_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_dev_fault_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_config_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_picfault_status = Label(root, text="False", relief = GROOVE, fg = "red", width = 6)
self.label_shift = Label(root, text="Bauteil - FIFO")
self.label_count = Label(root, text="Anzahl getestet")
self.button_send = Button(root, text="Sende Konfig", fg="blue",command=lambda: self.get_gui_command(0,0), width = 41)
self.label_counts = Label(root, text="0", font=("Calibri",20), relief = GROOVE, width = 10)
self.label_input = Label(root, text="Keine Daten bis jetzt", relief = GROOVE, width = 41)
self.label_input_shift = Label(root, text="--------", font=("Calibri",20), relief = GROOVE, width = 10)
self.button_connect.place (x= 100, y=50)
self.button_disconnect.place (x= 270, y=50)
self.label_t_plus_time.place (x= 100, y = 100)
self.label_lever_long_time.place (x= 100, y = 150)
self.label_part.place (x= 100, y = 200)
self.label_eject.place (x= 100, y = 250)
self.label_com.place (x= 650, y = 100)
self.label_dev_ok.place (x= 650, y = 150)
self.label_dev_ready.place (x= 650, y = 200)
self.label_config_ok.place (x= 650, y = 250)
self.label_picfault_ok.place (x= 650, y = 300)
self.label_com_status.place (x= 850, y = 100)
self.label_dev_status.place (x= 850, y = 150)
self.label_dev_fault_status.place (x= 850, y = 200)
self.label_config_status.place (x= 850, y = 250)
self.label_picfault_status.place (x= 850, y = 300)
self.entry_t_time.place (x= 270, y = 100)
self.entry_t_lever.place (x= 270, y = 150)
self.entry_part.place (x= 270, y = 200)
self.radiobutton_eject.place (x= 270, y = 250)
self.button_send.place (x = 100, y = 300)
self.label_count.place (x = 650, y = 400)
self.label_counts.place (x = 850, y = 400)
self.label_shift.place (x = 650, y = 450)
self.label_input_shift.place (x = 850, y = 450)
self.label_input.place (x = 100, y = 350)
self.old_counts = 0
self.show_counts = 0
self.uart = Uart()
self.uart.attach(Uart.EVT_CONNECTION_STATUS, self.connection_status_changed)
self.uart.attach(Uart.EVT_DATA, self.data_received)
self.uart.attach(Uart.EVT_CONNECTION_INTERRUPTED, self.connection_interrupted)
self.root.bind("<<CONNECTION_INTERRUPTED>>", self.show_connection_interrupted)
def cleanup(self):
"""Close UART and Close GUI"""
print("Clean-Up")
self.uart.close()
self.root.destroy()
def store_setting(self):
"""Get Str vom Entry's and Pickle to file"""
self.stored_file = filedialog.asksaveasfile(filetypes = (("txt files","*.txt"),("all files","*.*")), defaultextension = ".txt")
if self.stored_file:
print("Write data...")
data = [self.entry_t_time.get(),
self.entry_t_lever.get(),
self.entry_part.get()]
pickle.dump(data, self.stored_file)
self.stored_file.close()
def load_setting(self):
"""Get data from file and update Entry's"""
self.loaded_file = filedialog.askopenfile(filetypes = (("txt files","*.txt"),("all files","*.*")))
if self.loaded_file:
print("Loading...")
try:
data = pickle.load(self.loaded_file)
self.entry_t_time.delete(0, END)
self.entry_t_time.insert(0, data[0])
self.entry_t_lever.delete(0, END)
self.entry_t_lever.insert(0,data[1])
self.entry_part.delete(0, END)
self.entry_part.insert(0, data[2])
except:
print("Fehler beim Laden der Daten")
def connect(self):
"""Connect to COM Port"""
self.uart.open_uart("COM1",9600)
def disconnect(self):
self.uart.close()
def connection_status_changed(self, status):
"""Set Connect/Disconect Button State"""
if status:
self.button_connect.configure(state = DISABLED)
self.button_disconnect.configure(state = NORMAL)
self.label_com_status.configure(text = str(status), fg = "darkgreen")
else:
self.button_connect.configure(state = NORMAL)
self.button_disconnect.configure(state = DISABLED)
self.label_com_status.configure(text = str(status), fg = "red")
def data_received(self, data):
"""Read data from uP
data[0] -> Startbit
data[1] -> t_plus_time KantenAusblendung
data[2] -> lever_long_time Anzugszeit Auswerfer
data[3] -> lever_delay_time NOT USED
data[4] -> count Anzahlbauteile getestet
data[5] -> part FIFO byte für Auswerfer
data[6] -> part_bit FIFO bit_ für Auswerfer
data[7] -> schift_data FIFO erstes Byte
data[8] -> error_status
bit[0] -> Elotest bereit
bit[1] -> Elotest fehlerfrei
bit[2] -> uP hat CONFIG erhalten
bit[3] -> uP fehler UART
bit[4] -> uP fehler Timer1 overflow
data[9] -> save_eject Sicheres Auswerfen 1 == Activ 0 == OFF"""
if len(data) > 8:
self.label_input.configure(text = data[1:-1]) #Statustext Serial input
if int(data[8]) & 0b00000001: #Status Dev_ready
self.label_dev_status.configure( text = "False", fg= "red")
else:
self.label_dev_status.configure( text = "True", fg= "darkgreen")
if int(data[8]) & 0b00000010: #Status Dev_fault
self.label_dev_fault_status.configure( text = "False", fg= "red")
else:
self.label_dev_fault_status.configure( text = "True", fg= "darkgreen")
if int(data[8]) & 0b00000100: #Status Config_done
self.label_config_status.configure( text = "False", fg= "red")
else:
self.label_config_status.configure( text = "True", fg= "darkgreen")
if int(data[8]) & 0b00011000: #Status uP fail
self.label_picfault_status.configure( text = "False", fg= "red")
else:
self.label_picfault_status.configure( text = "True", fg= "darkgreen")
#data[9] is for save_eject
actual_counts = int(data[4]) #Prevent Count from Overflow
if actual_counts < self.old_counts: #Update Count label
self.show_counts = self.show_counts + 256 + actual_counts
self.old_counts = actual_counts
self.label_counts.configure(text = str(self.show_counts))
else:
self.old_counts = actual_counts
self.label_counts.configure(text = str(self.show_counts + actual_counts))
fifo_text = '{0:08b}'.format(int(data[7])) #Update FIFO (reversed L-R)
fifo_reversed = ''.join(reversed(fifo_text))
self.label_input_shift.configure(text = fifo_reversed)
def connection_interrupted(self, _):
self.root.event_generate("<<CONNECTION_INTERRUPTED>>", when='tail')
def show_connection_interrupted(self, event = None):
tkMessageBox.showerror(title="Error",message="Verbindung unterbrochen. No Data > 5sec",parent=self.root)
def get_part(self, text):
"""Read Str from Parts-Entry and generate Byte+Bit for uP / Part maximal 100"""
if int(text) > 100:
tkMessageBox.showerror(title="Error",message="Parts nur 1-100",parent=self.root)
return(str(254), str(254))
else:
part_byte = int(text) / 8
part_bit = int(text) % 8
return(str(part_byte), str(part_bit))
def get_gui_command(self,x,y):
"""Read Str vom Entry's and send data to COM"""
data = ""
data = self.text_to_char(255) #Set commands[arr] in PIC to zero / Startbyte
text = self.entry_t_time.get() #t_plus_time Kantenausblendung
text = self.check_user_input(text)
data = data + self.text_to_char(text)
text = self.entry_t_lever.get() #lever_long_time Auswerfer ON Time
text = self.check_user_input(text)
data = data + self.text_to_char(text)
text = "1" #Dummy reserved for future
text = self.check_user_input(text)
data = data + self.text_to_char(text)
text = "1"
text = self.check_user_input(text) #Dummy count reserved for future
data = data + self.text_to_char(text)
text = self.entry_part.get() #Part anzahl in byte und bit
text = self.check_user_input(text)
part_byte, part_bit = self.get_part(text)
data = data + self.text_to_char(part_byte)
data = data + self.text_to_char(part_bit)
data = data + self.text_to_char("0") #Send error_status Null -> all OK
status_eject = str(self.eject_state.get()) #Send Save_eject state 1->ON 0->OFF
data = data + self.text_to_char(status_eject)
if chr(254) not in data: #Send when all inputs "okay"
print("Data-Out: " +str(data)) #Send data to COM
self.uart.send_commands(data)
self.show_counts = 0 #Counter to Zero
self.old_counts = 0 #Old Counter to Zero
else:
print("Fehler in User_Inputs")
def text_to_char(self, data):
"""Convert a str[Number] to char"""
char_to_send = chr(int(data))
return(char_to_send)
def check_user_input(self, text):
"Checks text for range = 1-254 shows MessageBox if Not"""
if text.isdigit() == False:
text = "254"
tkMessageBox.showerror(title="Error",message="Keine gültige Zahl",parent=self.root)
if int(text) < 0 or int(text) > 254:
text = "254"
tkMessageBox.showerror(title="Error",message="Zahl nur 1 - 254 ",parent=self.root)
return (text)