class CameraStation(object): def __init__(self, serial_number, yservo, zservo): self.controller = AdvancedServo() self.serial_number = serial_number self.yservo = yservo self.zservo = zservo def initServo(self, index, vel): self.controller.setServoType(index, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) self.controller.setEngaged(index, False) maxAcceleration = self.controller.getAccelerationMax(index) maxVelocity = self.controller.getVelocityMax(index) self.controller.setAcceleration(index, maxAcceleration * 0.5) self.controller.setVelocityLimit(index, maxVelocity * vel / 100) self.controller.setEngaged(index, True) self.controller.setPosition(index, 90) sleep(0.5) print "Servo {} initialized.".format(index) def init(self, yvel, zvel): # Opening servo controller self.controller.openPhidget(self.serial_number) self.controller.waitForAttach(HARDWARE_CONNECTION_TIMEOUT) self.initServo(self.yservo, yvel) self.initServo(self.zservo, zvel) def setPosition(self, yangle, zangle): # Set new position for each servo self.controller.setPosition(self.yservo, yangle) self.controller.setPosition(self.zservo, zangle) def setMoveToPosition(self, yangle, zangle): # Calculate movement time for both servos dy = abs(yangle - self.controller.getPosition(self.yservo)) dz = abs(zangle - self.controller.getPosition(self.zservo)) ty = dy / self.controller.getVelocityLimit(self.yservo) tz = dz / self.controller.getVelocityLimit(self.zservo) # Set new position for each servo self.controller.setPosition(self.yservo, yangle) self.controller.setPosition(self.zservo, zangle) # Wait until motors sets to desire positions sleep(max(ty, tz)) def start(self): self.controller.setEngaged(self.yservo, True) self.controller.setEngaged(self.zservo, True) def stop(self): self.controller.setEngaged(self.yservo, False) self.controller.setEngaged(self.zservo, False) def __del__(self): self.stop() self.controller.closePhidget()
#Pasamos los datos de los acelerometros y el giroscopio por un Filtro Complementario UltimoX = K * (UltimoX + gyroXDelta) + K1 * rotacionXGrados UltimoY = K * (UltimoY + gyroYDelta) + K1 * rotacionYGrados #Utilizamos el angulo calculado para el control PID Ix = Ix + UltimoX * KI PIDx = KP * UltimoX + KD * (UltimoX - UltimoX0) + Ix Iy = Iy + UltimoY * KI PIDy = KP * UltimoY + KD * (UltimoY - UltimoY0) + Iy #Movemos los motores la cantidad que nos indica el PID si este no vale 0.0 if PIDy != 0.0: try: advancedServo.setPosition(1, advancedServo.getPosition(1) + PIDy) except: print( 'Has llegado a la posicion limite en el eje 1: %.2f grados' ) % advancedServo.getPosition(1) print advancedServo.setPosition(1, advancedServo.getPosition(1)) if PIDx != 0.0: try: advancedServo.setPosition( 0, advancedServo.getPosition(0) + PIDx) except: print( 'Has llegado a la posicion limite en el eje 0: %.2f grados' ) % advancedServo.getPosition(0)