def SetIdlePos(kematox, mode):
"""Default value for idle stance is "D"=275, "z"=48"""
if mode == "set":
IK.IK_SixLeg()
kematox.MoveSixLeg(1000, "support")
elif mode == "return":
"""STEP 1 - TRIPOD A lift legs"""
IK.IK_in["POS_Z"] = -20.0
IK.IK_Tripod_A(
"support") # A "swing" az IK_in_for Swing- et használja....
kematox.MoveTripodA("default", "swing", 500)
"""STEP 2 - TRIPOD A lower leg to idle D position (275)"""
IK.IK_in["D"] = 275.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_A("support")
kematox.MoveTripodA("default", "swing", 500)
"""STEP 3 - TRIPOD B lift legs"""
IK.IK_in["D"] = 225.0
IK.IK_in["POS_Z"] = -30.0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 4 - TRIPOD B lower leg to idle D position (275)"""
IK.IK_in["D"] = 275.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 5 - lower body to idle position (z=48) """
IK.IK_in["z"] = 48.0
IK.IK_SixLeg()
kematox.MoveSixLeg(1500, "support")
def SetReadyPos(kematox, mode):
"""Default values for ready stance "D"=225, "z"=110"""
if mode == "set":
"""STEP 1 - Robor lift"""
IK.IK_in["z"] = 110.0
IK.IK_SixLeg()
kematox.MoveSixLeg(1000, "swing")
"""STEP 2 - TRIPOD A lift legs"""
IK.IK_in["POS_Z"] = -50.0
IK.IK_Tripod_A(
"support") # A "swing" az IK_in_for Swing- et használja....
kematox.MoveTripodA("default", "swing", 500)
"""STEP 3 - TRIPOD A lower legs"""
IK.IK_in["D"] = 225.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_A("support")
kematox.MoveTripodA("default", "swing", 500)
"""STEP 4 - TRIPOD B lift legs"""
IK.IK_in["D"] = 275.0
IK.IK_in["POS_Z"] = -50.0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 5 - TRIPOD B lower legs"""
IK.IK_in["D"] = 225.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 6 - TRIPOD A lift legs again to release stress in servos"""
IK.IK_in["POS_Z"] = -25.0
IK.IK_Tripod_A(
"support") # A "swing" az IK_in_for Swing- et használja....
kematox.MoveTripodA("default", "swing", 500)
"""STEP 7 - TRIPOD A lower legs"""
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_A("support")
kematox.MoveTripodA("default", "swing", 500)
"""STEP 8 - TRIPOD B lift legs again to release stress in servos"""
IK.IK_in["POS_Z"] = -25.0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 9 - TRIPOD B release legs"""
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
elif mode == "return":
"""STEP 1 - return to default "z" at no matter what "D" value"""
IK.IK_in["POS_Z"] = 0
IK.IK_SixLeg()
kematox.MoveSixLeg(None, "swing")
"""STEP 2 - TRIPOD A lift legs"""
IK.IK_in["POS_Z"] = -25.0
IK.IK_Tripod_A(
"support") # A "swing" az IK_in_for Swing- et használja....
kematox.MoveTripodA("default", "swing", 500)
"""STEP 3 - TRIPOD A lower legs. "D" is now equals id default value"""
IK.IK_in["D"] = 225.0
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_A("support")
kematox.MoveTripodA("default", "swing", 500)
"""STEP 4 - TRIPOD B lift legs"""
IK.IK_in["POS_Z"] = -25.0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
"""STEP 5 - TRIPOD B lower legs"""
IK.IK_in["POS_Z"] = 0
IK.IK_Tripod_B("support")
kematox.MoveTripodB("default", "swing", 500)
def EventExecute(event, mode_dict, flag_dict, auxval, walkval):
if mode_dict["mode"] == 0: # IDLE
if flag_dict["position_reached"] == False:
if flag_dict["return_to_Idle"] == True:
print "MAIN: Returning to IDLE"
funct.SetIdlePos(kematox, "return")
print "MAIN: IDLE position reached\n"
flag_dict["return_to_Idle"] = False
flag_dict["position_reached"] = True
elif flag_dict["return_to_Idle"] == False:
print "MAIN: MODE set to IDLE" # ide még a fejet idle-be parancsot be kell szúrni
funct.SetIdlePos(kematox, "set")
print "MAIN: IDLE position reached\n"
flag_dict["position_reached"] = True
elif flag_dict["position_reached"] == True:
pass
elif mode_dict["mode"] == 1: # READY
if flag_dict["position_reached"] == False:
if flag_dict["return_to_Ready"] == True:
print "MAIN: Returning to READY"
funct.SetReadyPos(kematox, "return")
funct.CenterHead(kematox)
print "MAIN: READY position reached\n"
flag_dict["return_to_Ready"] = False
flag_dict["position_reached"] = True
elif flag_dict["return_to_Ready"] == False:
print "MAIN: MODE set to READY"
funct.SetReadyPos(kematox, "set")
funct.CenterHead(kematox)
print "MAIN: READY position reached\n"
flag_dict["position_reached"] = True
elif flag_dict["position_reached"] == True:
pass
elif mode_dict["mode"] == 2: # STATIC
if flag_dict["flag_headModeSelected"] == False:
if flag_dict["position_reached"] == False:
IK.IK_SixLeg()
kematox.MoveSixLeg(None, "support")
#IK.IK_Diag(IK.IK_out)
flag_dict["position_reached"] = True
print "DIAG: MOVEMENT READY"
elif flag_dict["position_reached"] == True:
pass
elif flag_dict["flag_headModeSelected"] == True:
if flag_dict["position_reached"] == False:
IK.IK_SixLeg()
kematox.MoveSixLeg(None, "support")
IK.CalcHeadPos(HeadMovInput, HeadCalibrVal, HeadMovOutput)
kematox.MoveHead(HeadMovOutput, 500)
#IK.IK_Diag(IK.IK_out)
flag_dict["position_reached"] = True
print "DIAG: MOVEMENT READY"
elif flag_dict["position_reached"] == True:
pass
elif mode_dict["mode"] == 3: # WALK
WalkVector = IK.CalcWalkVector()
#if ((abs(IK.IK_in["ROT_Z"]) > 0 and IK.IK_in["POS_Z"]) or (IK.IK_in["ROT_Y"] > 0 and IK.IK_in["POS_Z"] > 0) or (WalkVector > 0 and IK.IK_in["POS_Z"] > 0) or (WalkVector > 0 and IK.IK_in["ROT_Y"] > 0 and IK.IK_in["POS_Z"] > 0)):
if ((abs(IK.IK_in["ROT_Z"]) > 0 and IK.IK_in["POS_Z"])
or (WalkVector > 0 and IK.IK_in["POS_Z"] > 0)
or (WalkVector > 0 and IK.IK_in["ROT_Y"] > 0
and IK.IK_in["POS_Z"] > 0)):
print "WALK"
funct.TripodWalk(kematox, walkVal)
elif (IK.IK_in["POS_Z"] > 0 or IK.IK_in["ROT_Y"] > 0 or WalkVector > 0
or (IK.IK_in["ROT_Y"] > 0 and WalkVector > 0)
or (IK.IK_in["POS_Z"] == 0 and IK.IK_in["ROT_Y"] == 0
and WalkVector == 0)):
"""Akkor is STATIC legyen a mód, ha minden 0 -> visszatérjen a robot alaphelyzetbe """
print "STATIC"
if flag_dict["position_reached"] == False:
IK.IK_SixLeg()
kematox.MoveSixLeg(None, "support")
flag_dict["position_reached"] = True
print "DIAG: MOVEMENT READY"
elif flag_dict["position_reached"] == True:
pass