def setAngles(a0, a1, a2, a3, a4):
tfa0 = constants.transform2ServoAngles(a0, 0)
tfa1 = constants.transform2ServoAngles(a1, 1)
tfa2 = constants.transform2ServoAngles(a2, 2)
tfa3 = constants.transform2ServoAngles(a3, 3)
tfa4 = constants.transform2ServoAngles(a4, 4)
setAngle(constants.servoIDArm[4], tfa4)
setAngle(constants.servoIDArm[3], tfa3)
setAngle(constants.servoIDArm[2], tfa2)
setAngle(constants.servoIDArm[1], tfa1)
setAngle(constants.servoIDArm[0], tfa0)
print("Servo Angles Written Are :", tfa0, tfa1, tfa2, tfa3, tfa4)
def setTransformedAngle(index, Angle): #(_,0-360)
return setAngle(constants.servoIDArm[index],
constants.transform2ServoAngles(Angle, index))
super.setSpeed(12, SERVO_SPEED)
super.setSpeed(15, int(SERVO_SPEED))
super.setSpeed(17, SERVO_SPEED)
super.setSpeed(16, SERVO_SPEED)
x = float(raw_input("Enter Target x:"))
y = float(raw_input("Enter Target y:"))
z1 = float(raw_input("Enter Initial z:"))
z2 = float(raw_input("Enter Final z:s"))
a0, a1, a2, a3, a4, isPossible = ki.process(x, y, z1)
if isPossible == True:
# print("Output Angles are:",a0,a1,a2,a3,a4)
tfa0 = constants.transform2ServoAngles(a0, 0)
tfa1 = constants.transform2ServoAngles(a1, 1)
tfa2 = constants.transform2ServoAngles(a2, 2)
tfa3 = constants.transform2ServoAngles(a3, 3)
tfa4 = constants.transform2ServoAngles(a4, 4)
a = raw_input("Enter Any Key to Move to Initial Position ")
super.setAngle(8, tfa0)
super.setAngle(12, tfa2)
super.setAngle(15, tfa3)
super.setAngle(17, tfa4)
super.setAngle(9, tfa1)
#beign stepping down
raw_input(":Press Any Key to Begin Step Down:")