else: inputData = csv.reader(f) for line in inputData: servoNum = line.pop(0) pos = 1 for field in line: servoTable[int(servoNum)][int(pos)] = int(field) pos += 1 #default arm position x = 13 y = 9.5 wristangle = -90 #initialize IK model arm = IKArm(x,y,wristangle) shoulderNeutral = 0 shoulderNeutralPWM = 390 shoulderRef = 90 shoulderRefPWM = 575 elbowNeutral = 0 elbowNeutralPWM = 465 elbowRef = 90 elbowRefPWM = 270 wristNeutral = 0 wristNeutralPWM = 455 wristRef = -90 wristRefPWM = 220 # get mid positions and move arms to those locations midPositions = []
# update the servo positions def updatePos(positions): for i in range(0,6): pwm.setPWM(i,0,positions[i]) # Initialise the PWM device using the default address pwm = PWM(0x40) #default arm position x = 13 y = 9.5 wristangle = -90 #initialize IK model arm = IKArm(x,y,wristangle) shoulderNeutral = 0 shoulderNeutralPWM = 390 shoulderRef = 90 shoulderRefPWM = 575 elbowNeutral = 0 elbowNeutralPWM = 465 elbowRef = 90 elbowRefPWM = 270 wristNeutral = 0 wristNeutralPWM = 455 wristRef = -90 wristRefPWM = 220 currentPositions = [0,0,0,0,0,0] #predefined targetPositions = [0,0,0,0,0,0]