from compLib.Motor import Motor
from compLib.Encoder import Encoder
from compLib.IRSensor import IRSensor
from compLib.CompLibClient import CompLibClient

import time
import math

CompLibClient.use_unix_socket()

sign = 1

for pwr in (18, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 35, 40, 45, 50, 60, 70, 80 , 90, 100):

    # Motor.multiple_pulse_width((0, pwr * sign), (3, -pwr * sign))
    Motor.multiple_power((0, pwr * sign), (3, -pwr * sign))

    start_time = time.time()
    while time.time() - start_time < 3:
        Encoder.read_all_velocities()
        time.sleep(0.01)

    avg = 0.0
    for i in range(0, 20):
        vels = Encoder.read_all_velocities()
        avg += ((abs(vels[0]) + abs(vels[3])) / 2.0)
        time.sleep(0.01)
    
    avg = avg / 10

    vel = str(avg).replace(".", ",")
    print(f"{pwr} {vel}")

    sign *= -1