351 lines
9.5 KiB
Python
351 lines
9.5 KiB
Python
import logging
|
||
import os
|
||
import random
|
||
import time
|
||
from datetime import datetime
|
||
from logging import info, fatal, error
|
||
from random import randint
|
||
from time import sleep
|
||
from typing import Any
|
||
|
||
import controller.controller_reifined as c
|
||
|
||
|
||
def setup_logs() -> None:
|
||
os.makedirs("logs", exist_ok=True)
|
||
logging.basicConfig(
|
||
level=logging.INFO,
|
||
format="%(asctime)s [%(levelname)s] %(message)s",
|
||
handlers=[
|
||
logging.FileHandler(f'./logs/{datetime.now().strftime("%Y%m%d-%H%M%S")}.log'),
|
||
logging.StreamHandler()
|
||
]
|
||
)
|
||
|
||
|
||
def testFunction(func, n: int, controller: c.ControllerInterface, delay=0) -> list[int | Any]:
|
||
passed = 0
|
||
for i in range(1, n + 1):
|
||
info(f'Test {i}/{n} of {func.__name__}...')
|
||
res = func(controller)
|
||
if res:
|
||
passed += 1
|
||
info(f'Test {i}/{n} of {func.__name__}: {"successful" if res else "failed"}\n\n')
|
||
|
||
if delay != 0:
|
||
time.sleep(delay)
|
||
|
||
if passed == n:
|
||
info(f'Tests of {func.__name__} passed\n\n')
|
||
else:
|
||
error(f'Tests of {func.__name__} failed, passed {passed}/{n}\n\n')
|
||
|
||
return [passed, n]
|
||
|
||
|
||
def batch_test(functions: list[tuple[Any, int, int]], test_delay: int, controller: c.ControllerInterface) -> list[
|
||
int | Any]:
|
||
res = [[0, 0] for i in range(len(functions))]
|
||
for index, (func, n, delay) in enumerate(functions):
|
||
res[index] = testFunction(func, n, controller, delay)
|
||
|
||
passed_tests = 0
|
||
|
||
info('Test results!')
|
||
for i, (passed, total) in enumerate(res):
|
||
if passed == total:
|
||
passed_tests += 1
|
||
|
||
info(f'Tests of {functions[i][0].__name__}: passed {passed}/{total}')
|
||
sleep(test_delay)
|
||
|
||
info(f'Passed {passed_tests}/{len(functions)} tests')
|
||
|
||
return [passed_tests, len(functions)]
|
||
|
||
|
||
def start() -> None:
|
||
setup_logs()
|
||
|
||
port = '/dev/ttyUSB0'
|
||
controller = c.ControllerInterface(c.ControllerRefined(0xFFFFFFFFFFFF, 0x112233445542))
|
||
|
||
controller.connect(port)
|
||
info(f'Controller connected to {port}')
|
||
batch_test(
|
||
[
|
||
(test_power_write, 30, 2),
|
||
|
||
(test_power_read, 100, 0),
|
||
|
||
(test_mode_read, 10, 0),
|
||
|
||
(test_dev_info_read, 100, 0),
|
||
(test_dev_info_write, 30, 1),
|
||
|
||
(test_address_read, 100, 0),
|
||
(test_address_write, 30, 1),
|
||
|
||
(test_current_read, 100, 0),
|
||
(test_current_write, 30, 1)
|
||
],
|
||
2,
|
||
controller
|
||
)
|
||
|
||
|
||
## TESTS ##
|
||
def test_current_read(controller: c.ControllerInterface) -> bool:
|
||
info('Reading currents...')
|
||
currents = controller.readCurrents()
|
||
if currents:
|
||
info(f'Read currents: {currents}')
|
||
return True
|
||
else:
|
||
error('Failed to read currents')
|
||
return False
|
||
|
||
|
||
def test_current_write(controller: c.ControllerInterface) -> bool:
|
||
current_min = 40
|
||
current_max = 67
|
||
currents = [
|
||
random.randint(current_min, current_max),
|
||
random.randint(current_min, current_max),
|
||
random.randint(current_min, current_max),
|
||
random.randint(current_min, current_max),
|
||
]
|
||
|
||
info(f'Writing currents {currents}...')
|
||
res = controller.writeCurrents(currents)
|
||
if res:
|
||
info(f'Written currents: {res}')
|
||
else:
|
||
error('Failed to write currents')
|
||
return False
|
||
|
||
time.sleep(1)
|
||
real = controller.readCurrents()
|
||
if real:
|
||
info(f'Read real currents: {real}')
|
||
if currents == real:
|
||
info(f'Check successful')
|
||
return True
|
||
else:
|
||
error('Check failed: currents do not match')
|
||
else:
|
||
error('Failed to read currents')
|
||
|
||
return False
|
||
|
||
|
||
def test_address_read(controller: c.ControllerInterface) -> bool:
|
||
info('Reading address...')
|
||
address = controller.readAddress()
|
||
if address != -1:
|
||
info(f'Read address: {address}')
|
||
return True
|
||
else:
|
||
error('Failed to read address')
|
||
return False
|
||
|
||
|
||
def test_address_write(controller: c.ControllerInterface) -> bool:
|
||
address_min = 1
|
||
address_max = 63
|
||
address = random.randint(address_min, address_max)
|
||
|
||
info(f'Writing address {address}...')
|
||
res = controller.writeAddress(address)
|
||
if res:
|
||
info(f'Written address: {res}')
|
||
else:
|
||
error('Failed to write address')
|
||
return False
|
||
|
||
time.sleep(1)
|
||
real = controller.readAddress()
|
||
if real:
|
||
info(f'Read real address: {real}')
|
||
if address == real:
|
||
info(f'Check successful')
|
||
return True
|
||
else:
|
||
error('Check failed: addresses do not match')
|
||
else:
|
||
error('Failed to read address')
|
||
|
||
return False
|
||
|
||
|
||
def test_mode_read(controller: c.ControllerInterface) -> bool:
|
||
info('Reading mode...')
|
||
modes = controller.readModes()
|
||
if modes:
|
||
info(f'Read modes: {modes}')
|
||
return True
|
||
else:
|
||
error('Failed to read modes')
|
||
return False
|
||
|
||
|
||
def test_dev_info_read(controller: c.ControllerInterface) -> bool:
|
||
info('Reading dev info...')
|
||
dev_info = controller.readDeviceInfo()
|
||
if dev_info:
|
||
info(f'Read dev info: {dev_info}')
|
||
return True
|
||
else:
|
||
error('Failed to read dev_info')
|
||
return False
|
||
|
||
|
||
def test_dev_info_write(controller: c.ControllerInterface) -> bool:
|
||
info('Reading dev info...')
|
||
dev_info = controller.readDeviceInfo()
|
||
if dev_info:
|
||
info(f'Read dev info: {dev_info}')
|
||
else:
|
||
error('Failed to read dev_info')
|
||
return False
|
||
|
||
mode_min = 1
|
||
mode_max = dev_info[2] - 1
|
||
mode = random.randint(mode_min, mode_max)
|
||
|
||
info(f'Writing mode {mode}...')
|
||
res = controller.writeMode(mode)
|
||
if res:
|
||
info(f'Written mode: {mode}')
|
||
else:
|
||
error('Failed to write mode')
|
||
return False
|
||
|
||
time.sleep(1)
|
||
dev_info = controller.readDeviceInfo()
|
||
if dev_info:
|
||
info(f'Read dev info: {dev_info}')
|
||
else:
|
||
error('Failed to read dev_info')
|
||
return False
|
||
|
||
real = controller.readMode(mode)
|
||
if real:
|
||
info(f'Read real mode: {real}')
|
||
if mode == dev_info[1]:
|
||
info(f'Check successful')
|
||
return True
|
||
else:
|
||
error('Check failed: mode indexes do not match')
|
||
else:
|
||
error('Failed to read mode')
|
||
|
||
return False
|
||
|
||
|
||
def test_power_read(controller: c.ControllerInterface) -> bool:
|
||
info('Reading powers...')
|
||
powers = controller.readPowers()
|
||
if powers:
|
||
info(f'Read powers: {powers}')
|
||
return True
|
||
else:
|
||
error('Failed to read powers')
|
||
return False
|
||
|
||
|
||
def test_power_write(controller: c.ControllerInterface) -> bool:
|
||
# Ia Ib Ic Id Ua Ub Uc Ud Pposs Kn_a Kn_b Kn_c Kn_d Am_H Am_L Pwm_H Pwm_L Pm
|
||
# 43, 41, 48, 57, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 117, 48, 30
|
||
|
||
info('Reading powers...')
|
||
powers = controller.readPowers()
|
||
if powers:
|
||
info(f'Read powers: {powers}')
|
||
else:
|
||
error('Failed to read powers')
|
||
return False
|
||
|
||
p_max = powers[17]
|
||
p_poss = powers[8]
|
||
pwm_max = int.from_bytes([powers[15], powers[16]], byteorder='big')
|
||
|
||
current_min = 400
|
||
current_max = 670
|
||
currents = [
|
||
random.randint(current_min, current_max) // 10 * 10,
|
||
random.randint(current_min, current_max) // 10 * 10,
|
||
random.randint(current_min, current_max) // 10 * 10,
|
||
random.randint(current_min, current_max) // 10 * 10,
|
||
]
|
||
|
||
voltage_min = 2
|
||
voltage_max = 60
|
||
voltages = [
|
||
random.randint(voltage_min, voltage_max),
|
||
random.randint(voltage_min, voltage_max),
|
||
random.randint(voltage_min, voltage_max),
|
||
random.randint(voltage_min, voltage_max),
|
||
]
|
||
|
||
|
||
currents_ma = [currents[i] * 0.001 for i in range(4)]
|
||
|
||
currents_controller = [int(currents[i] * 0.1) for i in range(4)]
|
||
voltages_controller = [int(voltages[i] / 0.235) for i in range(4)]
|
||
|
||
powers = [currents_ma[i] * voltages[i] for i in range(4)]
|
||
p_sum = sum(powers)
|
||
ka = [powers[i] / p_sum for i in range(4)]
|
||
km = min(p_poss / p_sum, 1)
|
||
Amax = int(km * pwm_max)
|
||
kn = [min(int(ka[i] * 256), 255) for i in range(4)] # TODO check if 255
|
||
|
||
info(f'Currents: {currents}')
|
||
info(f'Currents controller: {currents_controller}')
|
||
info(f'Currents ma: {currents_ma}')
|
||
info(f'Voltages: {voltages}')
|
||
info(f'Voltages controller: {voltages_controller}')
|
||
info(f'Powers: {powers}')
|
||
info(f'Power sum: {p_sum}')
|
||
info(f'Ka: {ka}')
|
||
info(f'Km: {km}')
|
||
info(f'Pmax: {p_max}')
|
||
info(f'Pext: {p_poss}')
|
||
info(f'PwmMax: {pwm_max}')
|
||
info(f'Amax: {Amax}')
|
||
info(f'Kn: {kn}')
|
||
|
||
data = (currents_controller + voltages_controller + [p_poss] + kn
|
||
# + [Amax.to_bytes(2, byteorder='big')[0], Amax.to_bytes(2, byteorder='big')[1]] # TODO баг в прошивке
|
||
+ [kn[2], kn[3]]
|
||
)
|
||
|
||
info(f'Writing powers: {data}...')
|
||
res = controller.writePowers(currents_controller, voltages_controller, p_poss, kn, Amax)
|
||
if res:
|
||
info(f'Written powers: {data}')
|
||
else:
|
||
error('Failed to write powers')
|
||
return False
|
||
|
||
time.sleep(1)
|
||
|
||
real = controller.readPowers()
|
||
if real:
|
||
data = data + [pwm_max.to_bytes(2, byteorder='big')[0], pwm_max.to_bytes(2, byteorder='big')[1]] + [p_max]
|
||
info(f'Added const data to written data')
|
||
info(f'Modded powers: {data}')
|
||
info(f'Read rl powers: {real}')
|
||
|
||
if real == data:
|
||
info(f'Check successful')
|
||
return True
|
||
else:
|
||
error('Check failed: powers do not match')
|
||
else:
|
||
error('Failed to read powers')
|
||
|
||
return False
|
||
|