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()
Exemple #2
0
        #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)