def __init__(self): self.targetAngle = [0]*6 self.currentAngle = [0]*6 self.currentSpeed = [0]*6 self.maxSpeed = [0]*6 self.currentScanDirection = [1]*3 self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT self.servos = Ax12() self.angles = StewartPlatformMath() self.currentPosition = PlatformPosition() self.lastPosition = PlatformPosition() self.updateFunction = self.updateLinear ## add angle limits to servo motors for i in range(6): if (i%2==0): self.servos.setAngleLimit((i+1), StewartPlatform.SERVO_MIN_ANGLE_VALUE, StewartPlatform.SERVO_MAX_ANGLE_VALUE) else: self.servos.setAngleLimit((i+1), 1024-StewartPlatform.SERVO_MAX_ANGLE_VALUE, 1024-StewartPlatform.SERVO_MIN_ANGLE_VALUE) sleep(0.1) ## initially set platform to (0,0,0), (0,0,0) self.setTargetAnglesSuccessfully() ## move motors to initial position for (i,targetAngle) in enumerate(self.targetAngle): self.currentAngle[i] = targetAngle servoValue = StewartPlatform.getServoAngleValue(i, self.currentAngle[i]) self.servos.moveSpeedRW((i+1), servoValue, 450) self.servos.action() sleep(1)
def __init__(self): self.targetAngle = [0] * 6 self.currentAngle = [0] * 6 self.currentSpeed = [0] * 6 self.maxSpeed = [0] * 6 self.currentScanDirection = [1] * 3 self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT self.servos = Ax12() self.angles = StewartPlatformMath() self.currentPosition = PlatformPosition() self.lastPosition = PlatformPosition() self.updateFunction = self.updateLinear self.setTargetAnglesSuccessfully() for (i, targetAngle) in enumerate(self.targetAngle): self.currentAngle[i] = targetAngle servoValue = StewartPlatform.getServoAngleValue( i, self.currentAngle[i]) self.servos.moveSpeedRW((i + 1), servoValue, 450) self.servos.action() sleep(1)
def __init__(self): self.targetAngle = [0]*6 self.currentAngle = [0]*6 self.currentSpeed = [0]*6 self.maxSpeed = [0]*6 self.currentScanDirection = [1]*3 self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT self.servos = Ax12() self.angles = StewartPlatformMath() self.currentPosition = PlatformPosition() self.lastPosition = PlatformPosition() self.updateFunction = self.updateLinear self.setTargetAnglesSuccessfully() for (i,targetAngle) in enumerate(self.targetAngle): self.currentAngle[i] = targetAngle servoValue = StewartPlatform.getServoAngleValue(i, self.currentAngle[i]) self.servos.moveSpeedRW((i+1), servoValue, 450) self.servos.action() sleep(1)
class StewartPlatform: SCALE_RADIANS_TO_SERVO_VALUE = 1024.0 / radians(300.0) SERVO_CENTER_ANGLE_VALUE = 512 SERVO_MIN_ANGLE_VALUE = 250 SERVO_MAX_ANGLE_VALUE = 812 SERVO_SPEED_LIMIT = 0.1 SERVO_ACCELERATION = 0.01 MOVE_SHORT_DISTANCE = 16 MOVE_LONG_DISTANCE = 32 MOVE_SHORT_ANGLE = 0.3 MOVE_LONG_ANGLE = 1.0 PERLIN_PHASE = 4 * pi PERLIN_TIME_SCALE = 0.8 PERLIN_POSITION_SCALE = 0.01 # this is roughly 1/MOVE_LONG_DISTANCE PERLIN_SPEED_SCALE = 4.0 PERLIN_ANGLE_SCALE = 0.05 PERLIN_MIN_SPEED = 1.0 PERLIN_MAX_SPEED = 3.0 PERLIN_DISTANCE_LIMIT = 16.0 PERLIN_ANGLE_LIMIT = 0.25 INIT_TIME = time() @staticmethod def getServoAngleValue(servoNumber, angleRadians): angPos = StewartPlatform.SERVO_CENTER_ANGLE_VALUE + int( angleRadians * StewartPlatform.SCALE_RADIANS_TO_SERVO_VALUE) angPos = min(StewartPlatform.SERVO_MAX_ANGLE_VALUE, max(StewartPlatform.SERVO_MIN_ANGLE_VALUE, angPos)) return angPos if servoNumber % 2 == 1 else 1024 - angPos # tries to pick a value that's far from current value # it sometimes flips the current position (x -> -x) # then adds a longValue offset to it, towards the direction with more room @staticmethod def pickLongTargetValue(longValue): def fn(currentPosition): delta = -currentPosition if uniform(0, 1.0) < 0.5 else 0 delta += (-1, 1)[currentPosition < 0] * uniform(0.8, 1.2) * longValue return delta return fn def __init__(self): self.targetAngle = [0] * 6 self.currentAngle = [0] * 6 self.currentSpeed = [0] * 6 self.maxSpeed = [0] * 6 self.currentScanDirection = [1] * 3 self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT self.servos = Ax12() self.angles = StewartPlatformMath() self.currentPosition = PlatformPosition() self.lastPosition = PlatformPosition() self.updateFunction = self.updateLinear self.setTargetAnglesSuccessfully() for (i, targetAngle) in enumerate(self.targetAngle): self.currentAngle[i] = targetAngle servoValue = StewartPlatform.getServoAngleValue( i, self.currentAngle[i]) self.servos.moveSpeedRW((i + 1), servoValue, 450) self.servos.action() sleep(1) # ****Very Important***** # Possible options/parameters: # 'fast' := moves fast (default) # 'slow' := moves slow # 'repeat' := repeats last movement (ignores all other commmands except 'fast'/'slow') # 'long'/'far' := moves large distance (default) # 'short'/'near' := moves small distance # 'translate' := list of which axis to translate # 'rotate' := list of which axis to rotate # Examples: # setNextPosition('fast', 'long', translate='xyz', rotate='zy') # setNextPosition(translate='xyz', rotate='zy') # setNextPosition('slow', rotate='zxy') # setNextPosition('slow', 'repeat') def setNextPositionLinear(self, *args, **kwargs): self.updateFunction = self.updateLinear if ('repeat' in args): if ('slow' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT / 2 elif ('fast' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT self.setTargetAnglesSuccessfully(self.lastPosition.translation, self.lastPosition.rotation) else: if ('slow' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT / 2 else: self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT # pick new valid position translateArg = kwargs.get('translate', '') rotateArg = kwargs.get('rotate', '') deltaDistances = [0] * 3 deltaAngles = [0] * 3 if (('short' in args) or ('near' in args)): # randomly move towards -MOVE_SHORT_ or +MOVE_SHORT_ # makes a list by picking -MOVE_SHORT_ or +MOVE_SHORT_ 3 times deltaDistances = map(lambda x: choice([-1, 1]) * x, [StewartPlatform.MOVE_SHORT_DISTANCE] * 3) deltaAngles = map(lambda x: choice([-1, 1]) * x, [StewartPlatform.MOVE_SHORT_ANGLE] * 3) else: # picks new potential long targets from current position deltaDistances = map( StewartPlatform.pickLongTargetValue( StewartPlatform.MOVE_LONG_DISTANCE), self.currentPosition.getTranslationAsList()) deltaAngles = map( StewartPlatform.pickLongTargetValue( StewartPlatform.MOVE_LONG_ANGLE), self.currentPosition.getRotationAsList()) done = False while not done: translation = Vector3( deltaDistances[0] if 'x' in translateArg else 0, deltaDistances[1] if 'y' in translateArg else 0, deltaDistances[2] if 'z' in translateArg else 0) + self.currentPosition.translation rotation = Vector3( deltaAngles[0] if 'x' in rotateArg else 0, deltaAngles[1] if 'y' in rotateArg else 0, deltaAngles[2] if 'z' in rotateArg else 0) + self.currentPosition.rotation done = self.setTargetAnglesSuccessfully(translation, rotation) deltaDistances = map(lambda x: uniform(0.666, 0.8) * x, deltaDistances) deltaAngles = map(lambda x: uniform(0.666, 0.8) * x, deltaAngles) # ****Very Important***** # Possible options/parameters: # 'fast' := moves fast (default) # 'slow' := moves slow # 'translate' := list of which axis to translate # 'rotate' := list of which axis to rotate # Examples: # setNextPositionPerlin('slow', translate='xyz', rotate='zy') # setNextPositionPerlin(translate='xz', rotate='zy') # setNextPositionPerlin(translate='zy') def setNextPositionPerlin(self, *args, **kwargs): self.updateFunction = self.updatePerlin if ('slow' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT / 2 else: self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT * 2 t = (time() - StewartPlatform.INIT_TIME) * StewartPlatform.PERLIN_TIME_SCALE (x, y, z) = self.currentPosition.getTranslationAsList() # direction u = snoise4( x * StewartPlatform.PERLIN_POSITION_SCALE + 1 * StewartPlatform.PERLIN_PHASE, y * StewartPlatform.PERLIN_POSITION_SCALE + 1 * StewartPlatform.PERLIN_PHASE, z * StewartPlatform.PERLIN_POSITION_SCALE + 1 * StewartPlatform.PERLIN_PHASE, t + 1 * StewartPlatform.PERLIN_PHASE) v = snoise4( x * StewartPlatform.PERLIN_POSITION_SCALE + 2 * StewartPlatform.PERLIN_PHASE, y * StewartPlatform.PERLIN_POSITION_SCALE + 2 * StewartPlatform.PERLIN_PHASE, z * StewartPlatform.PERLIN_POSITION_SCALE + 2 * StewartPlatform.PERLIN_PHASE, t + 2 * StewartPlatform.PERLIN_PHASE) w = snoise4( x * StewartPlatform.PERLIN_POSITION_SCALE + 3 * StewartPlatform.PERLIN_PHASE, y * StewartPlatform.PERLIN_POSITION_SCALE + 3 * StewartPlatform.PERLIN_PHASE, z * StewartPlatform.PERLIN_POSITION_SCALE + 3 * StewartPlatform.PERLIN_PHASE, t + 3 * StewartPlatform.PERLIN_PHASE) #magnitude thisSpeedScale = StewartPlatform.PERLIN_SPEED_SCALE / 3 if ( 'slow' in args) else StewartPlatform.PERLIN_SPEED_SCALE speed = min( StewartPlatform.PERLIN_MAX_SPEED, max(StewartPlatform.PERLIN_MIN_SPEED, thisSpeedScale * (snoise4(u, v, w, t) * 0.5 + 0.5))) # result deltaDistances = (u * speed, v * speed, w * speed) deltaAngles = (snoise2(v, t) * StewartPlatform.PERLIN_ANGLE_SCALE, snoise2(w, t) * StewartPlatform.PERLIN_ANGLE_SCALE, snoise2(u, t) * StewartPlatform.PERLIN_ANGLE_SCALE) # pick new valid position translateArg = kwargs.get('translate', '') rotateArg = kwargs.get('rotate', '') done = False while not done: translation = Vector3( deltaDistances[0] if 'x' in translateArg else 0, deltaDistances[1] if 'y' in translateArg else 0, deltaDistances[2] if 'z' in translateArg else 0) + self.currentPosition.translation translation.constrain(-StewartPlatform.PERLIN_DISTANCE_LIMIT, StewartPlatform.PERLIN_DISTANCE_LIMIT) rotation = Vector3( deltaAngles[0] if 'x' in rotateArg else 0, deltaAngles[1] if 'y' in rotateArg else 0, deltaAngles[2] if 'z' in rotateArg else 0) + self.currentPosition.rotation rotation.constrain(-StewartPlatform.PERLIN_ANGLE_LIMIT, StewartPlatform.PERLIN_ANGLE_LIMIT) done = self.setTargetAnglesSuccessfully(translation, rotation) deltaDistances = map(lambda x: 0.9 * x, deltaDistances) deltaAngles = map(lambda x: 0.9 * x, deltaAngles) # small nudge by rotation # parameters {x,y} should be within [-1, 1] def setNextPositionLook(self, x=0, y=0): self.updateFunction = self.updateLinear self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT / 4 # look x actually means rotate around z-axis # and look y needs to rotate around x-axis # and y shouldn't matter, so we pick a random value deltaAngles = (StewartPlatform.MOVE_SHORT_ANGLE * y, StewartPlatform.MOVE_SHORT_ANGLE * choice([-1, 0, 1, 0]), StewartPlatform.MOVE_SHORT_ANGLE * -x) # move platform slightly closer to its initial height currentTranslation = self.currentPosition.translation translation = Vector3(currentTranslation.x, currentTranslation.y, currentTranslation.z * 0.8) done = False while not done: rotation = Vector3(deltaAngles[0], deltaAngles[1], deltaAngles[2]) + self.currentPosition.rotation done = self.setTargetAnglesSuccessfully(translation, rotation) deltaAngles = map(lambda x: uniform(0.666, 0.8) * x, deltaAngles) # scan by nudging rotation def setNextPositionScan(self, *args): self.updateFunction = self.updateLinear if ('slow' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT / 10 else: self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT / 5 # pan actually means rotate around y-axis # and tilt means rotate around x-axis deltaAngles = (0, StewartPlatform.MOVE_SHORT_ANGLE / 12 * self.currentScanDirection[1], 0) if (abs(self.currentPosition.rotation.y + deltaAngles[1]) > StewartPlatform.MOVE_LONG_ANGLE / 3): self.currentScanDirection[1] *= -1 deltaAngles = (StewartPlatform.MOVE_SHORT_ANGLE / 4 * self.currentScanDirection[0], 0, 0) if (abs(self.currentPosition.rotation.x + deltaAngles[0]) > StewartPlatform.MOVE_LONG_ANGLE / 2): self.currentScanDirection[0] *= -1 deltaAngles = (0, StewartPlatform.MOVE_SHORT_ANGLE / 12 * self.currentScanDirection[1], 0) # move platform slightly closer to its initial height currentTranslation = self.currentPosition.translation translation = Vector3(currentTranslation.x, currentTranslation.y, currentTranslation.z * 0.9) done = False while not done: rotation = Vector3(deltaAngles[0], deltaAngles[1], deltaAngles[2]) + self.currentPosition.rotation done = self.setTargetAnglesSuccessfully(translation, rotation) deltaAngles = map(lambda x: uniform(0.666, 0.8) * x, deltaAngles) def setTargetAnglesSuccessfully(self, translation=Vector3(), rotation=Vector3()): alphaAngles = self.angles.applyTranslationAndRotation( translation, rotation) # check for nans for angle in alphaAngles: if (isnan(angle)): return False # all valid angles for (i, angle) in enumerate(alphaAngles): self.targetAngle[i] = angle self.currentSpeed[i] = StewartPlatform.SERVO_ACCELERATION / 10 self.maxSpeed[i] = 0 # set positions self.lastPosition = self.currentPosition self.currentPosition = PlatformPosition(translation, rotation) return True def isAtTarget(self): for (i, targetAngle) in enumerate(self.targetAngle): if ((abs(targetAngle - self.currentAngle[i]) >= self.currentSpeed[i]) and (self.currentSpeed[i] > 0)): return False return True def stop(self): for (i, targetAngle) in enumerate(self.targetAngle): targetAngle = self.currentAngle[i] self.currentSpeed[i] = 0 def update(self): self.updateFunction() def updateLinear(self): for (i, targetAngle) in enumerate(self.targetAngle): if (abs(targetAngle - self.currentAngle[i]) <= (self.maxSpeed[i]**2) / (2 * StewartPlatform.SERVO_ACCELERATION)): self.currentSpeed[i] = max( self.currentSpeed[i] - StewartPlatform.SERVO_ACCELERATION, 0) else: self.currentSpeed[i] = min( self.currentSpeed[i] + StewartPlatform.SERVO_ACCELERATION, self.currentSpeedLimit) self.maxSpeed[i] = max(self.maxSpeed[i], self.currentSpeed[i]) if (targetAngle > self.currentAngle[i]): self.currentAngle[i] = min( self.currentAngle[i] + self.currentSpeed[i], targetAngle) elif (targetAngle < self.currentAngle[i]): self.currentAngle[i] = max( self.currentAngle[i] - self.currentSpeed[i], targetAngle) servoValue = StewartPlatform.getServoAngleValue( i, self.currentAngle[i]) try: self.servos.moveSpeedRW((i + 1), servoValue, 256) except: try: self.servos.moveSpeedRW((i + 1), servoValue, 256) except: print "3rd time. oooops." self.servos.action() def updatePerlin(self): for (i, targetAngle) in enumerate(self.targetAngle): self.currentSpeed[i] = min(self.currentSpeedLimit, abs(targetAngle - self.currentAngle[i])) self.maxSpeed[i] = max(self.maxSpeed[i], self.currentSpeed[i]) if (targetAngle > self.currentAngle[i]): self.currentAngle[i] = min( self.currentAngle[i] + self.currentSpeed[i], targetAngle) elif (targetAngle < self.currentAngle[i]): self.currentAngle[i] = max( self.currentAngle[i] - self.currentSpeed[i], targetAngle) servoValue = StewartPlatform.getServoAngleValue( i, self.currentAngle[i]) try: self.servos.moveSpeedRW((i + 1), servoValue, 256) except: try: self.servos.moveSpeedRW((i + 1), servoValue, 256) except: print "3rd time. oooops." self.servos.action()
class StewartPlatform: SCALE_RADIANS_TO_SERVO_VALUE = 1024.0/radians(300.0) SERVO_CENTER_ANGLE_VALUE = 512 SERVO_MIN_ANGLE_VALUE = 250 SERVO_MAX_ANGLE_VALUE = 812 SERVO_SPEED_LIMIT = 0.1 SERVO_ACCELERATION = 0.01 MOVE_SHORT_DISTANCE = 16 MOVE_LONG_DISTANCE = 32 MOVE_SHORT_ANGLE = 0.3 MOVE_LONG_ANGLE = 1.0 PERLIN_PHASE = 4*pi PERLIN_TIME_SCALE = 0.8 PERLIN_POSITION_SCALE = 0.01 # this is roughly 1/MOVE_LONG_DISTANCE PERLIN_SPEED_SCALE = 4.0 PERLIN_ANGLE_SCALE = 0.05 PERLIN_MIN_SPEED = 1.0 PERLIN_MAX_SPEED = 3.0 PERLIN_DISTANCE_LIMIT = 16.0 PERLIN_ANGLE_LIMIT = 0.25 INIT_TIME = time() @staticmethod def getServoAngleValue(servoNumber, angleRadians): angPos = StewartPlatform.SERVO_CENTER_ANGLE_VALUE + int(angleRadians*StewartPlatform.SCALE_RADIANS_TO_SERVO_VALUE) angPos = min(StewartPlatform.SERVO_MAX_ANGLE_VALUE, max(StewartPlatform.SERVO_MIN_ANGLE_VALUE, angPos)) return angPos if servoNumber%2==1 else 1024-angPos # tries to pick a value that's far from current value # it sometimes flips the current position (x -> -x) # then adds a longValue offset to it, towards the direction with more room @staticmethod def pickLongTargetValue(longValue): def fn(currentPosition): delta = -currentPosition if uniform(0,1.0)<0.5 else 0 delta += (-1,1)[currentPosition<0]*uniform(0.8,1.2)*longValue return delta return fn def __init__(self): self.targetAngle = [0]*6 self.currentAngle = [0]*6 self.currentSpeed = [0]*6 self.maxSpeed = [0]*6 self.currentScanDirection = [1]*3 self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT self.servos = Ax12() self.angles = StewartPlatformMath() self.currentPosition = PlatformPosition() self.lastPosition = PlatformPosition() self.updateFunction = self.updateLinear self.setTargetAnglesSuccessfully() for (i,targetAngle) in enumerate(self.targetAngle): self.currentAngle[i] = targetAngle servoValue = StewartPlatform.getServoAngleValue(i, self.currentAngle[i]) self.servos.moveSpeedRW((i+1), servoValue, 450) self.servos.action() sleep(1) # ****Very Important***** # Possible options/parameters: # 'fast' := moves fast (default) # 'slow' := moves slow # 'repeat' := repeats last movement (ignores all other commmands except 'fast'/'slow') # 'long'/'far' := moves large distance (default) # 'short'/'near' := moves small distance # 'translate' := list of which axis to translate # 'rotate' := list of which axis to rotate # Examples: # setNextPosition('fast', 'long', translate='xyz', rotate='zy') # setNextPosition(translate='xyz', rotate='zy') # setNextPosition('slow', rotate='zxy') # setNextPosition('slow', 'repeat') def setNextPositionLinear(self, *args, **kwargs): self.updateFunction = self.updateLinear if('repeat' in args): if('slow' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT/2 elif('fast' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT self.setTargetAnglesSuccessfully(self.lastPosition.translation, self.lastPosition.rotation) else: if('slow' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT/2 else: self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT # pick new valid position translateArg = kwargs.get('translate', '') rotateArg = kwargs.get('rotate', '') deltaDistances = [0]*3 deltaAngles = [0]*3 if(('short' in args) or ('near' in args)): # randomly move towards -MOVE_SHORT_ or +MOVE_SHORT_ # makes a list by picking -MOVE_SHORT_ or +MOVE_SHORT_ 3 times deltaDistances = map(lambda x:choice([-1, 1])*x, [StewartPlatform.MOVE_SHORT_DISTANCE]*3) deltaAngles = map(lambda x:choice([-1, 1])*x, [StewartPlatform.MOVE_SHORT_ANGLE]*3) else: # picks new potential long targets from current position deltaDistances = map(StewartPlatform.pickLongTargetValue(StewartPlatform.MOVE_LONG_DISTANCE), self.currentPosition.getTranslationAsList()) deltaAngles = map(StewartPlatform.pickLongTargetValue(StewartPlatform.MOVE_LONG_ANGLE), self.currentPosition.getRotationAsList()) done = False while not done: translation = Vector3( deltaDistances[0] if 'x' in translateArg else 0, deltaDistances[1] if 'y' in translateArg else 0, deltaDistances[2] if 'z' in translateArg else 0) + self.currentPosition.translation rotation = Vector3( deltaAngles[0] if 'x' in rotateArg else 0, deltaAngles[1] if 'y' in rotateArg else 0, deltaAngles[2] if 'z' in rotateArg else 0) + self.currentPosition.rotation done = self.setTargetAnglesSuccessfully(translation, rotation) deltaDistances = map(lambda x:uniform(0.666,0.8)*x, deltaDistances) deltaAngles = map(lambda x:uniform(0.666,0.8)*x, deltaAngles) # ****Very Important***** # Possible options/parameters: # 'fast' := moves fast (default) # 'slow' := moves slow # 'translate' := list of which axis to translate # 'rotate' := list of which axis to rotate # Examples: # setNextPositionPerlin('slow', translate='xyz', rotate='zy') # setNextPositionPerlin(translate='xz', rotate='zy') # setNextPositionPerlin(translate='zy') def setNextPositionPerlin(self, *args, **kwargs): self.updateFunction = self.updatePerlin if('slow' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT/2 else: self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT*2 t = (time()-StewartPlatform.INIT_TIME) * StewartPlatform.PERLIN_TIME_SCALE (x,y,z) = self.currentPosition.getTranslationAsList() # direction u = snoise4(x*StewartPlatform.PERLIN_POSITION_SCALE + 1*StewartPlatform.PERLIN_PHASE, y*StewartPlatform.PERLIN_POSITION_SCALE + 1*StewartPlatform.PERLIN_PHASE, z*StewartPlatform.PERLIN_POSITION_SCALE + 1*StewartPlatform.PERLIN_PHASE, t + 1*StewartPlatform.PERLIN_PHASE) v = snoise4(x*StewartPlatform.PERLIN_POSITION_SCALE + 2*StewartPlatform.PERLIN_PHASE, y*StewartPlatform.PERLIN_POSITION_SCALE + 2*StewartPlatform.PERLIN_PHASE, z*StewartPlatform.PERLIN_POSITION_SCALE + 2*StewartPlatform.PERLIN_PHASE, t + 2*StewartPlatform.PERLIN_PHASE) w = snoise4(x*StewartPlatform.PERLIN_POSITION_SCALE + 3*StewartPlatform.PERLIN_PHASE, y*StewartPlatform.PERLIN_POSITION_SCALE + 3*StewartPlatform.PERLIN_PHASE, z*StewartPlatform.PERLIN_POSITION_SCALE + 3*StewartPlatform.PERLIN_PHASE, t + 3*StewartPlatform.PERLIN_PHASE) #magnitude thisSpeedScale = StewartPlatform.PERLIN_SPEED_SCALE/3 if ('slow' in args) else StewartPlatform.PERLIN_SPEED_SCALE speed = min(StewartPlatform.PERLIN_MAX_SPEED, max(StewartPlatform.PERLIN_MIN_SPEED, thisSpeedScale*(snoise4(u,v,w,t)*0.5+0.5))) # result deltaDistances = ( u*speed, v*speed, w*speed) deltaAngles = ( snoise2(v,t)*StewartPlatform.PERLIN_ANGLE_SCALE, snoise2(w,t)*StewartPlatform.PERLIN_ANGLE_SCALE, snoise2(u,t)*StewartPlatform.PERLIN_ANGLE_SCALE) # pick new valid position translateArg = kwargs.get('translate', '') rotateArg = kwargs.get('rotate', '') done = False while not done: translation = Vector3( deltaDistances[0] if 'x' in translateArg else 0, deltaDistances[1] if 'y' in translateArg else 0, deltaDistances[2] if 'z' in translateArg else 0) + self.currentPosition.translation translation.constrain(-StewartPlatform.PERLIN_DISTANCE_LIMIT, StewartPlatform.PERLIN_DISTANCE_LIMIT) rotation = Vector3( deltaAngles[0] if 'x' in rotateArg else 0, deltaAngles[1] if 'y' in rotateArg else 0, deltaAngles[2] if 'z' in rotateArg else 0) + self.currentPosition.rotation rotation.constrain(-StewartPlatform.PERLIN_ANGLE_LIMIT, StewartPlatform.PERLIN_ANGLE_LIMIT) done = self.setTargetAnglesSuccessfully(translation, rotation) deltaDistances = map(lambda x:0.9*x, deltaDistances) deltaAngles = map(lambda x:0.9*x, deltaAngles) # small nudge by rotation # parameters {x,y} should be within [-1, 1] def setNextPositionLook(self, x=0, y=0): self.updateFunction = self.updateLinear self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT/4 # look x actually means rotate around z-axis # and look y needs to rotate around x-axis # and y shouldn't matter, so we pick a random value deltaAngles = ( StewartPlatform.MOVE_SHORT_ANGLE*y, StewartPlatform.MOVE_SHORT_ANGLE*choice([-1, 0, 1, 0]), StewartPlatform.MOVE_SHORT_ANGLE*-x) # move platform slightly closer to its initial height currentTranslation = self.currentPosition.translation translation = Vector3(currentTranslation.x, currentTranslation.y, currentTranslation.z*0.8) done = False while not done: rotation = Vector3(deltaAngles[0], deltaAngles[1], deltaAngles[2]) + self.currentPosition.rotation done = self.setTargetAnglesSuccessfully(translation, rotation) deltaAngles = map(lambda x:uniform(0.666,0.8)*x, deltaAngles) # scan by nudging rotation def setNextPositionScan(self, *args): self.updateFunction = self.updateLinear if('slow' in args): self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT/10 else: self.currentSpeedLimit = StewartPlatform.SERVO_SPEED_LIMIT/5 # pan actually means rotate around y-axis # and tilt means rotate around x-axis deltaAngles = (0,StewartPlatform.MOVE_SHORT_ANGLE/12*self.currentScanDirection[1],0) if(abs(self.currentPosition.rotation.y + deltaAngles[1]) > StewartPlatform.MOVE_LONG_ANGLE/3): self.currentScanDirection[1] *= -1 deltaAngles = (StewartPlatform.MOVE_SHORT_ANGLE/4*self.currentScanDirection[0],0,0) if(abs(self.currentPosition.rotation.x + deltaAngles[0]) > StewartPlatform.MOVE_LONG_ANGLE/2): self.currentScanDirection[0] *= -1 deltaAngles = (0,StewartPlatform.MOVE_SHORT_ANGLE/12*self.currentScanDirection[1],0) # move platform slightly closer to its initial height currentTranslation = self.currentPosition.translation translation = Vector3(currentTranslation.x, currentTranslation.y, currentTranslation.z*0.9) done = False while not done: rotation = Vector3(deltaAngles[0], deltaAngles[1], deltaAngles[2]) + self.currentPosition.rotation done = self.setTargetAnglesSuccessfully(translation, rotation) deltaAngles = map(lambda x:uniform(0.666,0.8)*x, deltaAngles) def setTargetAnglesSuccessfully(self, translation=Vector3(), rotation=Vector3()): alphaAngles = self.angles.applyTranslationAndRotation(translation, rotation) # check for nans for angle in alphaAngles: if(isnan(angle)): return False # all valid angles for (i,angle) in enumerate(alphaAngles): self.targetAngle[i] = angle self.currentSpeed[i] = StewartPlatform.SERVO_ACCELERATION/10 self.maxSpeed[i] = 0 # set positions self.lastPosition = self.currentPosition self.currentPosition = PlatformPosition(translation, rotation) return True def isAtTarget(self): for (i,targetAngle) in enumerate(self.targetAngle): if((abs(targetAngle-self.currentAngle[i]) >= self.currentSpeed[i]) and (self.currentSpeed[i] > 0)): return False return True def stop(self): for (i,targetAngle) in enumerate(self.targetAngle): targetAngle = self.currentAngle[i] self.currentSpeed[i] = 0 def update(self): self.updateFunction() def updateLinear(self): for (i,targetAngle) in enumerate(self.targetAngle): if(abs(targetAngle - self.currentAngle[i]) <= (self.maxSpeed[i]**2)/(2*StewartPlatform.SERVO_ACCELERATION)): self.currentSpeed[i] = max(self.currentSpeed[i]-StewartPlatform.SERVO_ACCELERATION, 0) else: self.currentSpeed[i] = min(self.currentSpeed[i]+StewartPlatform.SERVO_ACCELERATION, self.currentSpeedLimit) self.maxSpeed[i] = max(self.maxSpeed[i], self.currentSpeed[i]) if(targetAngle > self.currentAngle[i]): self.currentAngle[i] = min(self.currentAngle[i]+self.currentSpeed[i], targetAngle) elif(targetAngle < self.currentAngle[i]): self.currentAngle[i] = max(self.currentAngle[i]-self.currentSpeed[i], targetAngle) servoValue = StewartPlatform.getServoAngleValue(i, self.currentAngle[i]) try: self.servos.moveSpeedRW((i+1), servoValue, 256) except: try: self.servos.moveSpeedRW((i+1), servoValue, 256) except: print "3rd time. oooops." self.servos.action() def updatePerlin(self): for (i,targetAngle) in enumerate(self.targetAngle): self.currentSpeed[i] = min(self.currentSpeedLimit, abs(targetAngle-self.currentAngle[i])) self.maxSpeed[i] = max(self.maxSpeed[i], self.currentSpeed[i]) if(targetAngle > self.currentAngle[i]): self.currentAngle[i] = min(self.currentAngle[i]+self.currentSpeed[i], targetAngle) elif(targetAngle < self.currentAngle[i]): self.currentAngle[i] = max(self.currentAngle[i]-self.currentSpeed[i], targetAngle) servoValue = StewartPlatform.getServoAngleValue(i, self.currentAngle[i]) try: self.servos.moveSpeedRW((i+1), servoValue, 256) except: try: self.servos.moveSpeedRW((i+1), servoValue, 256) except: print "3rd time. oooops." self.servos.action()