import MotorBridgeI
import time

ServoName1   =  1
ServoName6   =  6
Frequency    =  50
Angle1       =  0
Angle2       =  45
Angle3       =  90
Angle4       =  135
Angle5       =  180
Angles       =  (Angle1, Angle2, Angle3, Angle4, Angle5)

period = 1 / Frequency
setup  = 0.0015

motor = MotorBridgeI.MotorBridgeCape()
motor.ServoInit(ServoName1, Frequency)
motor.ServoInit(ServoName6, Frequency)
motor.ServoMoveAngle(ServoName1, Angle1)
motor.ServoMoveAngle(ServoName6, Angle1)
time.sleep(setup)

while True:
    motor.ServoMoveAngle(ServoName1, Angle2)
    time.sleep(period)

    motor.ServoMoveAngle(ServoName6, Angle4)
    time.sleep(period)
    print('Servo Test!')

    for i in range(0, 135, 45):
        motor.ServoMoveAngle(ServoName1, Angles)