535 lines
16 KiB
Python
535 lines
16 KiB
Python
#!/usr/bin/env python3
|
|
import json
|
|
import math
|
|
import os
|
|
import subprocess
|
|
import sys
|
|
import time
|
|
|
|
|
|
WHEELS = ('fl', 'fr', 'cl', 'cr', 'rl', 'rr')
|
|
|
|
WHEEL_LABELS = {
|
|
'fl': 'Vorne links',
|
|
'fr': 'Vorne rechts',
|
|
'cl': 'Mitte links',
|
|
'cr': 'Mitte rechts',
|
|
'rl': 'Hinten links',
|
|
'rr': 'Hinten rechts',
|
|
}
|
|
|
|
WHEEL_DIRECTIONS = {
|
|
'fl': -1,
|
|
'fr': 1,
|
|
'cl': -1,
|
|
'cr': 1,
|
|
'rl': -1,
|
|
'rr': 1,
|
|
}
|
|
|
|
STEER_PIN_KEYS = {
|
|
'fl': 'pin_steer_fl',
|
|
'fr': 'pin_steer_fr',
|
|
'cl': 'pin_steer_cl',
|
|
'cr': 'pin_steer_cr',
|
|
'rl': 'pin_steer_rl',
|
|
'rr': 'pin_steer_rr',
|
|
}
|
|
|
|
STEER_NEUTRAL_KEYS = {
|
|
'fl': 'steer_pwm_neutral_fl',
|
|
'fr': 'steer_pwm_neutral_fr',
|
|
'cl': 'steer_pwm_neutral_cl',
|
|
'cr': 'steer_pwm_neutral_cr',
|
|
'rl': 'steer_pwm_neutral_rl',
|
|
'rr': 'steer_pwm_neutral_rr',
|
|
}
|
|
|
|
DRIVE_PIN_KEYS = {
|
|
'fl': 'pin_drive_fl',
|
|
'fr': 'pin_drive_fr',
|
|
'cl': 'pin_drive_cl',
|
|
'cr': 'pin_drive_cr',
|
|
'rl': 'pin_drive_rl',
|
|
'rr': 'pin_drive_rr',
|
|
}
|
|
|
|
DRIVE_NEUTRAL_KEYS = {
|
|
'fl': 'drive_pwm_neutral_fl',
|
|
'fr': 'drive_pwm_neutral_fr',
|
|
'cl': 'drive_pwm_neutral_cl',
|
|
'cr': 'drive_pwm_neutral_cr',
|
|
'rl': 'drive_pwm_neutral_rl',
|
|
'rr': 'drive_pwm_neutral_rr',
|
|
}
|
|
|
|
ACTION_SETTINGS = {
|
|
'steer_left': {
|
|
'kind': 'steer',
|
|
'angle': -35,
|
|
'duration': 0.6,
|
|
'label': 'Lenkung links',
|
|
},
|
|
'steer_right': {
|
|
'kind': 'steer',
|
|
'angle': 35,
|
|
'duration': 0.6,
|
|
'label': 'Lenkung rechts',
|
|
},
|
|
'drive_forward': {
|
|
'kind': 'drive',
|
|
'speed': 35,
|
|
'duration': 0.7,
|
|
'label': 'Vorwärts',
|
|
},
|
|
'drive_backward': {
|
|
'kind': 'drive',
|
|
'speed': -35,
|
|
'duration': 0.7,
|
|
'label': 'Rückwärts',
|
|
},
|
|
}
|
|
|
|
CONTAINER_NAME = 'exomy_autostart'
|
|
CONTAINER_SCRIPT_PATH = '/root/exomy_ws/src/exomy/scripts/admin_motor_test.py'
|
|
|
|
|
|
class PCA9685Direct:
|
|
MODE1 = 0x00
|
|
PRESCALE = 0xFE
|
|
LED0_ON_L = 0x06
|
|
|
|
def __init__(self, busnum=1, address=0x40):
|
|
try:
|
|
import smbus
|
|
except ImportError as exc:
|
|
try:
|
|
import smbus2 as smbus
|
|
except ImportError as inner_exc:
|
|
raise RuntimeError('smbus ist nicht installiert.') from inner_exc
|
|
|
|
self.address = address
|
|
self.bus = smbus.SMBus(busnum)
|
|
self.write8(self.MODE1, 0x00)
|
|
time.sleep(0.005)
|
|
|
|
def write8(self, reg, value):
|
|
self.bus.write_byte_data(self.address, reg, value & 0xFF)
|
|
|
|
def read8(self, reg):
|
|
return self.bus.read_byte_data(self.address, reg)
|
|
|
|
def set_pwm_freq(self, freq_hz):
|
|
prescaleval = 25000000.0
|
|
prescaleval /= 4096.0
|
|
prescaleval /= float(freq_hz)
|
|
prescaleval -= 1.0
|
|
prescale = int(math.floor(prescaleval + 0.5))
|
|
|
|
oldmode = self.read8(self.MODE1)
|
|
sleepmode = (oldmode & 0x7F) | 0x10
|
|
self.write8(self.MODE1, sleepmode)
|
|
self.write8(self.PRESCALE, prescale)
|
|
self.write8(self.MODE1, oldmode)
|
|
time.sleep(0.005)
|
|
self.write8(self.MODE1, oldmode | 0xA1)
|
|
|
|
def set_pwm(self, channel, on, off):
|
|
base = self.LED0_ON_L + 4 * int(channel)
|
|
self.write8(base, on & 0xFF)
|
|
self.write8(base + 1, (on >> 8) & 0xFF)
|
|
self.write8(base + 2, off & 0xFF)
|
|
self.write8(base + 3, (off >> 8) & 0xFF)
|
|
|
|
|
|
def _load_config(config_path):
|
|
config = {}
|
|
with open(config_path, 'r', encoding='utf-8') as handle:
|
|
for raw_line in handle:
|
|
line = raw_line.strip()
|
|
if not line or line.startswith('#') or ':' not in line:
|
|
continue
|
|
key, value = line.split(':', 1)
|
|
config[key.strip()] = value.strip()
|
|
return config
|
|
|
|
|
|
def _update_config_values(config_path, updates):
|
|
output_lines = []
|
|
with open(config_path, 'r', encoding='utf-8') as handle:
|
|
for raw_line in handle:
|
|
line = raw_line
|
|
stripped = raw_line.strip()
|
|
if stripped and not stripped.startswith('#') and ':' in raw_line:
|
|
key = raw_line.split(':', 1)[0].strip()
|
|
if key in updates:
|
|
line = f'{key}: {updates[key]}\n'
|
|
output_lines.append(line)
|
|
|
|
with open(config_path, 'w', encoding='utf-8') as handle:
|
|
handle.writelines(output_lines)
|
|
|
|
|
|
class MotorTester:
|
|
def __init__(self, config_path=None):
|
|
try:
|
|
import Adafruit_PCA9685
|
|
self.pwm = Adafruit_PCA9685.PCA9685()
|
|
except ImportError:
|
|
self.pwm = PCA9685Direct(busnum=1)
|
|
|
|
if config_path is None:
|
|
config_path = os.path.join(os.path.dirname(__file__), '..', 'config', 'exomy.yaml')
|
|
|
|
if not os.path.exists(config_path):
|
|
raise FileNotFoundError(f'Konfigurationsdatei nicht gefunden: {config_path}')
|
|
|
|
self.config_path = os.path.abspath(config_path)
|
|
self.config = _load_config(self.config_path)
|
|
self.pwm.set_pwm_freq(50)
|
|
self.steer_pwm_range = int(self.config['steer_pwm_range'])
|
|
self.drive_pwm_neutral = {
|
|
wheel: int(self.config[DRIVE_NEUTRAL_KEYS[wheel]])
|
|
for wheel in WHEELS
|
|
}
|
|
self.drive_pwm_range = int(self.config['drive_pwm_range'])
|
|
|
|
def _steer_pin(self, wheel):
|
|
return int(self.config[STEER_PIN_KEYS[wheel]])
|
|
|
|
def _steer_neutral(self, wheel):
|
|
return int(self.config[STEER_NEUTRAL_KEYS[wheel]])
|
|
|
|
def _drive_pin(self, wheel):
|
|
return int(self.config[DRIVE_PIN_KEYS[wheel]])
|
|
|
|
def _drive_neutral(self, wheel):
|
|
return int(self.drive_pwm_neutral[wheel])
|
|
|
|
def set_drive_neutral_pwm(self, values):
|
|
for wheel in WHEELS:
|
|
self.pwm.set_pwm(self._drive_pin(wheel), 0, int(values[wheel]))
|
|
time.sleep(0.03)
|
|
|
|
def center_all_steering(self):
|
|
for wheel in WHEELS:
|
|
self.pwm.set_pwm(self._steer_pin(wheel), 0, self._steer_neutral(wheel))
|
|
time.sleep(0.03)
|
|
|
|
def stop_all_driving(self):
|
|
for wheel in WHEELS:
|
|
self.pwm.set_pwm(self._drive_pin(wheel), 0, self._drive_neutral(wheel))
|
|
time.sleep(0.03)
|
|
|
|
def set_steering_angle(self, wheel, angle):
|
|
duty_cycle = int(self._steer_neutral(wheel) + angle / 90.0 * self.steer_pwm_range)
|
|
self.pwm.set_pwm(self._steer_pin(wheel), 0, duty_cycle)
|
|
|
|
def set_steering_neutral(self, wheel):
|
|
self.pwm.set_pwm(self._steer_pin(wheel), 0, self._steer_neutral(wheel))
|
|
|
|
def set_steering_neutral_pwm(self, wheel, pwm_value):
|
|
self.pwm.set_pwm(self._steer_pin(wheel), 0, int(pwm_value))
|
|
|
|
def set_steering_percent(self, wheel, signed_percent):
|
|
percent = max(-100.0, min(100.0, float(signed_percent)))
|
|
duty_cycle = int(self._steer_neutral(wheel) + percent / 100.0 * self.steer_pwm_range)
|
|
self.pwm.set_pwm(self._steer_pin(wheel), 0, duty_cycle)
|
|
|
|
def set_all_steering_angle(self, angle):
|
|
for wheel in WHEELS:
|
|
self.set_steering_angle(wheel, angle)
|
|
time.sleep(0.03)
|
|
|
|
def set_all_steering_neutral(self):
|
|
for wheel in WHEELS:
|
|
self.set_steering_neutral(wheel)
|
|
time.sleep(0.03)
|
|
|
|
def test_steering(self, wheel, angle, duration):
|
|
self.set_steering_angle(wheel, angle)
|
|
time.sleep(duration)
|
|
self.set_steering_neutral(wheel)
|
|
|
|
def test_driving(self, wheel, speed, duration):
|
|
self.pwm.set_pwm(self._steer_pin(wheel), 0, self._steer_neutral(wheel))
|
|
duty_cycle = int(
|
|
self._drive_neutral(wheel) +
|
|
speed / 100.0 * self.drive_pwm_range * WHEEL_DIRECTIONS[wheel]
|
|
)
|
|
self.pwm.set_pwm(self._drive_pin(wheel), 0, duty_cycle)
|
|
time.sleep(duration)
|
|
self.pwm.set_pwm(self._drive_pin(wheel), 0, self._drive_neutral(wheel))
|
|
|
|
def execute(self, wheel, action):
|
|
if wheel not in WHEELS:
|
|
raise ValueError('Unbekanntes Rad.')
|
|
|
|
if action not in ACTION_SETTINGS:
|
|
raise ValueError('Unbekannte Testaktion.')
|
|
|
|
settings = ACTION_SETTINGS[action]
|
|
|
|
self.stop_all_driving()
|
|
|
|
if settings['kind'] == 'steer':
|
|
self.test_steering(wheel, settings['angle'], settings['duration'])
|
|
else:
|
|
self.test_driving(wheel, settings['speed'], settings['duration'])
|
|
|
|
return {
|
|
'wheel': wheel,
|
|
'wheel_label': WHEEL_LABELS[wheel],
|
|
'action': action,
|
|
'action_label': settings['label'],
|
|
}
|
|
|
|
def hold_steering(self, wheel, angle):
|
|
if wheel not in WHEELS:
|
|
raise ValueError('Unbekanntes Rad.')
|
|
|
|
self.stop_all_driving()
|
|
self.set_steering_angle(wheel, angle)
|
|
return {
|
|
'scope': 'single',
|
|
'wheel': wheel,
|
|
'wheel_label': WHEEL_LABELS[wheel],
|
|
'angle': angle,
|
|
}
|
|
|
|
def hold_steering_percent(self, wheel, percent, direction):
|
|
if wheel not in WHEELS:
|
|
raise ValueError('Unbekanntes Rad.')
|
|
|
|
if direction not in ('left', 'right'):
|
|
raise ValueError('Unbekannte Richtung.')
|
|
|
|
clamped_percent = max(0.0, min(100.0, float(percent)))
|
|
signed_percent = -clamped_percent if direction == 'left' else clamped_percent
|
|
|
|
self.stop_all_driving()
|
|
self.set_steering_percent(wheel, signed_percent)
|
|
return {
|
|
'scope': 'single',
|
|
'wheel': wheel,
|
|
'wheel_label': WHEEL_LABELS[wheel],
|
|
'percent': clamped_percent,
|
|
'direction': direction,
|
|
'signed_percent': signed_percent,
|
|
}
|
|
|
|
def hold_all_steering(self, angle):
|
|
self.stop_all_driving()
|
|
self.set_all_steering_angle(angle)
|
|
return {
|
|
'scope': 'all',
|
|
'wheel': 'all',
|
|
'wheel_label': 'Alle Räder',
|
|
'angle': angle,
|
|
}
|
|
|
|
def neutral_single_steering(self, wheel):
|
|
if wheel not in WHEELS:
|
|
raise ValueError('Unbekanntes Rad.')
|
|
|
|
self.stop_all_driving()
|
|
self.set_steering_neutral(wheel)
|
|
return {
|
|
'scope': 'single',
|
|
'wheel': wheel,
|
|
'wheel_label': WHEEL_LABELS[wheel],
|
|
'angle': 0,
|
|
}
|
|
|
|
def neutral_all_steering(self):
|
|
self.stop_all_driving()
|
|
self.set_all_steering_neutral()
|
|
return {
|
|
'scope': 'all',
|
|
'wheel': 'all',
|
|
'wheel_label': 'Alle Räder',
|
|
'angle': 0,
|
|
}
|
|
|
|
|
|
def run_motor_test(wheel, action, config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
return tester.execute(wheel.lower(), action)
|
|
|
|
|
|
def hold_single_steering(wheel, angle, config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
return tester.hold_steering(wheel.lower(), angle)
|
|
|
|
|
|
def hold_all_steering(angle, config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
return tester.hold_all_steering(angle)
|
|
|
|
|
|
def hold_single_steering_percent(wheel, percent, direction, config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
return tester.hold_steering_percent(wheel.lower(), percent, direction)
|
|
|
|
|
|
def neutral_single_steering(wheel, config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
return tester.neutral_single_steering(wheel.lower())
|
|
|
|
|
|
def neutral_all_steering(config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
return tester.neutral_all_steering()
|
|
|
|
|
|
def get_steering_neutral_values(config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
values = {}
|
|
for wheel in WHEELS:
|
|
key = STEER_NEUTRAL_KEYS[wheel]
|
|
values[wheel] = {
|
|
'wheel': wheel,
|
|
'wheel_label': WHEEL_LABELS[wheel],
|
|
'key': key,
|
|
'value': int(tester.config[key]),
|
|
'pin': tester._steer_pin(wheel),
|
|
}
|
|
return {
|
|
'config_path': tester.config_path,
|
|
'values': values,
|
|
}
|
|
|
|
|
|
def preview_steering_neutral_value(wheel, pwm_value, config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
if wheel not in WHEELS:
|
|
raise ValueError('Unbekanntes Rad.')
|
|
|
|
tester.stop_all_driving()
|
|
tester.set_steering_neutral_pwm(wheel, int(pwm_value))
|
|
return {
|
|
'wheel': wheel,
|
|
'wheel_label': WHEEL_LABELS[wheel],
|
|
'value': int(pwm_value),
|
|
'pin': tester._steer_pin(wheel),
|
|
}
|
|
|
|
|
|
def save_steering_neutral_values(values, config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
updates = {}
|
|
|
|
for wheel in WHEELS:
|
|
key = STEER_NEUTRAL_KEYS[wheel]
|
|
if key not in values:
|
|
raise ValueError(f'Neutralwert fehlt: {key}')
|
|
updates[key] = int(values[key])
|
|
|
|
_update_config_values(tester.config_path, updates)
|
|
tester.config = _load_config(tester.config_path)
|
|
tester.stop_all_driving()
|
|
tester.center_all_steering()
|
|
|
|
return get_steering_neutral_values(config_path=tester.config_path)
|
|
|
|
|
|
def get_drive_neutral_value(config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
values = {}
|
|
for wheel in WHEELS:
|
|
key = DRIVE_NEUTRAL_KEYS[wheel]
|
|
values[wheel] = {
|
|
'wheel': wheel,
|
|
'wheel_label': WHEEL_LABELS[wheel],
|
|
'key': key,
|
|
'value': int(tester.config[key]),
|
|
'pin': tester._drive_pin(wheel),
|
|
}
|
|
return {
|
|
'config_path': tester.config_path,
|
|
'values': values,
|
|
}
|
|
|
|
|
|
def preview_drive_neutral_value(values, config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
preview_values = {}
|
|
for wheel in WHEELS:
|
|
if wheel not in values:
|
|
raise ValueError(f'Fahr-Neutralwert fehlt: {DRIVE_NEUTRAL_KEYS[wheel]}')
|
|
preview_values[wheel] = int(values[wheel])
|
|
|
|
tester.center_all_steering()
|
|
tester.set_drive_neutral_pwm(preview_values)
|
|
return {
|
|
'values': {
|
|
wheel: {
|
|
'wheel': wheel,
|
|
'wheel_label': WHEEL_LABELS[wheel],
|
|
'key': DRIVE_NEUTRAL_KEYS[wheel],
|
|
'value': preview_values[wheel],
|
|
'pin': tester._drive_pin(wheel),
|
|
}
|
|
for wheel in WHEELS
|
|
},
|
|
}
|
|
|
|
|
|
def save_drive_neutral_value(values, config_path=None):
|
|
tester = MotorTester(config_path=config_path)
|
|
updates = {}
|
|
for wheel in WHEELS:
|
|
key = DRIVE_NEUTRAL_KEYS[wheel]
|
|
if key not in values:
|
|
raise ValueError(f'Fahr-Neutralwert fehlt: {key}')
|
|
updates[key] = int(values[key])
|
|
|
|
_update_config_values(tester.config_path, updates)
|
|
tester.config = _load_config(tester.config_path)
|
|
tester.drive_pwm_neutral = {
|
|
wheel: int(tester.config[DRIVE_NEUTRAL_KEYS[wheel]])
|
|
for wheel in WHEELS
|
|
}
|
|
tester.center_all_steering()
|
|
tester.set_drive_neutral_pwm(tester.drive_pwm_neutral)
|
|
return get_drive_neutral_value(config_path=tester.config_path)
|
|
|
|
|
|
def run_motor_test_in_container(wheel, action):
|
|
command = [
|
|
'docker',
|
|
'exec',
|
|
CONTAINER_NAME,
|
|
'python',
|
|
CONTAINER_SCRIPT_PATH,
|
|
wheel.lower(),
|
|
action,
|
|
]
|
|
result = subprocess.run(command, capture_output=True, text=True, check=False)
|
|
if result.returncode != 0:
|
|
stderr = result.stderr.strip()
|
|
stdout = result.stdout.strip()
|
|
message = stderr or stdout or 'Motortest im Container fehlgeschlagen'
|
|
raise RuntimeError(message)
|
|
|
|
try:
|
|
return json.loads(result.stdout)
|
|
except json.JSONDecodeError as exc:
|
|
raise RuntimeError('Antwort des Container-Motortests war ungültig') from exc
|
|
|
|
|
|
def main():
|
|
if len(sys.argv) != 3:
|
|
print('Aufruf: admin_motor_test.py <fl|fr|cl|cr|rl|rr> <steer_left|steer_right|drive_forward|drive_backward>')
|
|
return 1
|
|
|
|
wheel = sys.argv[1].lower()
|
|
action = sys.argv[2]
|
|
result = run_motor_test(wheel, action, config_path=None)
|
|
print(json.dumps(result))
|
|
return 0
|
|
|
|
|
|
if __name__ == '__main__':
|
|
raise SystemExit(main())
|