from compLib.PCA9685 import PCA9685

pwm = PCA9685(0x40, debug=True)
pwm.setPWMFreq(50)


class Servo:
    """Control the servo ports on the robot
    """

    @staticmethod
    def set_position(channel: int, angle: int):
        """Set position of servo connected to port

        :param channel: channel between 0 and 7
        :param angle: Angle of servo
        """
        angle = abs(angle)

        if channel == 0:
            pwm.setServoPulse(8 + channel, 2500 - int(angle / 0.09))
        elif channel < 8:
            pwm.setServoPulse(8 + channel, 500 - int(angle / 0.09))

    @staticmethod
    def setup_position():
        """Set position of servos to the position used during the setup process
        """
        pwm.setServoPulse(8, 1500)
        pwm.setServoPulse(9, 1500)