class MotionInterface(object): def __init__(self, mode=1): self._Height = 75 self._Length = 65 self._DefaultPulse = 375 self._Multiplier = 1 self._currentModeInt = mode pulses = self.calculatePulse(self._Height, self._Length) self._Legs = [ Leg([0x41, 0x41, 0x41], [0, 1, 2], [375, pulses[0], pulses[1]], [0, 0, 0]), #leg 1 Leg([0x40, 0x40, 0x40], [0, 1, 2], [375, pulses[0], pulses[1]], True, [0, 0, 0]), #leg 2 Leg([0x41, 0x41, 0x41], [4, 5, 6], [375, pulses[0], pulses[1]], [0, 0, 0]), #leg 3 Leg([0x40, 0x40, 0x40], [4, 5, 6], [375, pulses[0], pulses[1]], True, [0, 0, 0]), #leg 4 Leg([0x41, 0x41, 0x41], [8, 9, 10], [375, pulses[0], pulses[1]], [0, 0, 0]), #leg 5 Leg([0x40, 0x40, 0x40], [8, 9, 10], [375, pulses[0], pulses[1]], True, [0, 0, 0]), #leg 6 ] self._CurrentCommand = 10 self._Exit = False self._Semaphore = threading.Semaphore(1) self._ExitSemaphore = threading.Semaphore(1) self.SleepTime = 0.0005 if mode == 1: self._CurrentMode = Move(self) if mode == 2: self._CurrentMode = FingerControl(self) if mode == 3: self._CurrentMode = SpiderGap(self) if mode == 4: self._CurrentMode = Dance(self) if mode == 5: self._CurrentMode = BalloonRace(self) if mode == 6: self._CurrentMode = Move(self) if mode == 7: self._CurrentMode = Move(self) if mode == 8: self._CurrentMode = Dance2(self) if mode == 9: self._CurrentMode = SpiderGap2(self) def get_CurrentCommand(self): self._Semaphore.acquire() try: if self._CurrentCommand is None: self._Semaphore.release() return 10 else: temp = self._CurrentCommand self._Semaphore.release() return temp except: return 10 def set_CurrentCommand(self, _CurrentCommand): self._Semaphore.acquire() self._CurrentCommand = _CurrentCommand self._Semaphore.release() return "Command " + str(_CurrentCommand) + " has been received" def runThread(self): self._ExitSemaphore.acquire() while self.isExited() == False: self._CurrentMode.executeCommand(self.get_CurrentCommand()) self._ExitSemaphore.release() time.sleep(0.001) def exit(self): self._ExitSemaphore.acquire() self._Exit = True self._CurrentMode.exit() self._ExitSemaphore.release() def getSemaphore(self): return self._Semaphore def getExitSemaphore(self): return self._ExitSemaphore def isExited(self): return self._Exit def setHeight(self, height): try: height = int(height) except: height = self._Height if height < 40: self._Height = 40 elif height > 150: self._Height = 150 else: self._Height = height self.set_CurrentCommand(9) def getHeight(self): return self._Height def setLength(self, length): try: length = int(length) except: length = self._Length if length < 50: self._Length = 50 elif length > 200: self._Length = 200 else: self._Length = length self.set_CurrentCommand(9) def getLength(self): return self._Length def setSpeed(self, Speed): try: Speed = int(Speed) except: Speed = self.SleepTime if Speed > 0.01: self.SleepTime = 0.01 elif Speed < 0.001: self.SleepTime = 0.001 else: self.SleepTime = Speed def getSpeed(self): return self.SleepTime def convertDegreesToPulse(self, pulse): return int(pulse * 2.8125) def convertPulseToDegrees(self, pulse): return int(pulse / 2.8125) def setDefaultPulse(self, pulse): self._DefaultPulse = int(pulse) #Inverse kinematics! def calculatePulse(self, height, length): a = float(height) b = float(length) c = math.sqrt((a * a) + (b * b)) d = float(80) e = float(145) hoekB = math.degrees(math.atan(b/a)) #Calculate servo angles hoekC = float(math.degrees((math.acos(((d * d) + (e * e) - (c * c)) / (2 * d * e))))) hoekE = float(math.degrees((math.acos(((c * c) + (d * d) - (e * e)) / (2 * c * d))))) HoekE2 = 90 - hoekB HoekE1 = hoekE - HoekE2 HoekC1 = 90 - HoekE1 HoekC2 = hoekC - HoekC1 HoekD = 90 - HoekC1 #Calculate difference in degrees if hoekC <= HoekC1: differenceInDegreesC = (90 - hoekC - HoekD) else: differenceInDegreesC = ((HoekC2 - 90)* -1) if hoekE <= HoekE2: differenceInDegreesE = (HoekE2 * -1) else: differenceInDegreesE = (HoekE1) diffrencePulseE = self.convertDegreesToPulse(differenceInDegreesE) diffrencePulseC = self.convertDegreesToPulse(differenceInDegreesC) Ankle = int(self._DefaultPulse - diffrencePulseC) Knee = int(self._DefaultPulse - diffrencePulseE) return [Knee, Ankle] def calculateLengthLeg(self, widthLeg, forwardDegrees): hoekA = float(forwardDegrees) b = float(widthLeg) a = float((math.tan(math.radians(hoekA))) * b) c = float(math.sqrt(a * a + b * b)) return int(c) def calculateHipPulse(self, degrees): pulse = self.convertDegreesToPulse(degrees) return int(self._DefaultPulse + pulse) def calculateVerticalPulse(self, startingPulse, EndPulse, Step, numberOfSteps=160): if Step > numberOfSteps or Step < 1: print "Step out of range - " + str(Step) raise Exception("") pulsedifference = 150 # wordt deze gebruikt? if EndPulse < startingPulse: pulsedifference = startingPulse - EndPulse pulse = startingPulse - int((float(pulsedifference) / float(numberOfSteps)) * Step) return pulse else: pulsedifference = EndPulse - startingPulse pulse = startingPulse + int((float(pulsedifference) / float(numberOfSteps)) * Step) return pulse def getDefaultPulse(self): return self._DefaultPulse def Calibreren(self, number, pulse): for Leg in self._Legs: if number == 1: Leg.moveHip(pulse) if number == 2: Leg.moveAnkle(pulse) if number == 3: Leg.moveKnee(pulse) def raiseLegs(self, Legs, offset=[0, 95]): steps = 30 pulses = self.calculatePulse(self._Height, self._Length) startpulseAnkle = Legs[0].getAnkle() startpulseKnee = Legs[0].getKnee() for step in range(1, steps, 1 * self._Multiplier): #raise leg for Leg in Legs: Leg.moveAnkle(self.calculateVerticalPulse(startpulseAnkle, pulses[1] - offset[0], step, steps)) Leg.moveKnee(self.calculateVerticalPulse(startpulseKnee, pulses[0] - offset[1], step, steps)) time.sleep(self.SleepTime) def LowerLegs(self, Legs): steps = 30 startpulseAnkle = Legs[0].getAnkle() #klopt die array? twee keer 0 voor ankle en knee. startpulseKnee = Legs[0].getKnee() pulses = self.calculatePulse(self._Height, self._Length) for step in range(1, steps, 1 * self._Multiplier): #lower leg for Leg in Legs: Leg.moveAnkle(self.calculateVerticalPulse(startpulseAnkle, pulses[1], step, steps)) Leg.moveKnee(self.calculateVerticalPulse(startpulseKnee, pulses[0], step, steps)) #startpulseKnee in plaats van Ankle time.sleep(self.SleepTime) def MoveLegsForward(self, Legs, Raised, Turn, offset=[0,95]): if Turn[0] > Turn[1]: self.MoveLegsBackward(Legs, Raised, Turn) return startpoint = Turn[0] steps = Turn[1] pulses = self.calculatePulse(self._Height, self._Length) if Raised: pulses[0] -= offset[0] pulses[1] -= offset[1] for step in range(startpoint, steps, 1 * self._Multiplier): #move leg forward #pulses = self._MInterface.calculatePulse(self._MInterface._Height, self._MInterface.calculateLengthLeg(self._MInterface._Length, step)) #if Raised: #pulses[0] -= 90 #pulses[1] -= 90 for Leg in Legs: Leg.moveHip(self.calculateHipPulse(step)) time.sleep(self.SleepTime) def MoveLegsBackward(self, Legs, Raised, Turn, offset=[90,90]): if Turn[0] < Turn[1]: self.MoveLegsForward(Legs, Raised, Turn) return startpoint = Turn[0] steps = Turn[1] pulses = self.calculatePulse(self._Height, self._Length) if Raised: pulses[0] -= offset[0] pulses[1] -= offset[1] for step in range(startpoint, steps, -1 * self._Multiplier): for Leg in Legs: Leg.moveHip(self.calculateHipPulse(step)) time.sleep(self.SleepTime) def setDegrees(self, degrees): if int(degrees) < 35 and int(degrees) > 8 and (self._currentModeInt is 1 or self._currentModeInt is 5 or self._currentModeInt is 6 or self._currentModeInt is 7): try: self._CurrentMode._MaxAngle = int(degrees) return "Degrees has been set to " + degrees except: return "invalid input. Current mode = " + str(self._currentModeInt) + " and degrees is: " + str(degrees) + "!" else: return "invalid input. Current mode = " + str(self._currentModeInt) + " and degrees is: " + str(degrees) def getDegrees(self): if self._currentModeInt == 1 or self._currentModeInt == 5 or self._currentModeInt == 6 or self._currentModeInt == 7: return self._CurrentMode._MaxAngle def setMultiplier(self, multiplier): if int(multiplier) >= 1 and int(multiplier) <= 5: self._Multiplier = int(multiplier) return "multiplier has been set to: " + str(multiplier) else: return "invalid input" def getMultiplier(self): return self._Multiplier def turbo(self): self._Multiplier = 5