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)