Aufgeräumt
This commit is contained in:
@@ -0,0 +1,534 @@
|
||||
#!/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())
|
||||
@@ -0,0 +1,11 @@
|
||||
from picamera import PiCamera
|
||||
from time import sleep
|
||||
'''
|
||||
This script helps to test the functionality of the Raspberry Pi camera.
|
||||
The script must be run dicrectly on the Raspberry Pi (not in the Docker container)
|
||||
'''
|
||||
camera = PiCamera()
|
||||
|
||||
camera.start_preview()
|
||||
sleep(5)
|
||||
camera.stop_preview()
|
||||
@@ -0,0 +1,94 @@
|
||||
import Adafruit_PCA9685
|
||||
import yaml
|
||||
import time
|
||||
import os
|
||||
|
||||
config_filename = '../config/exomy.yaml'
|
||||
WHEELS = ('fl', 'fr', 'cl', 'cr', 'rl', 'rr')
|
||||
|
||||
|
||||
def get_driving_pins():
|
||||
with open(config_filename, 'r') as file:
|
||||
param_dict = yaml.load(file)
|
||||
|
||||
return [param_dict['pin_drive_' + wheel] for wheel in WHEELS]
|
||||
|
||||
def get_drive_pwm_neutral_values():
|
||||
with open(config_filename, 'r') as file:
|
||||
param_dict = yaml.load(file)
|
||||
|
||||
values = {}
|
||||
for wheel in WHEELS:
|
||||
key = 'drive_pwm_neutral_' + wheel
|
||||
if key not in param_dict:
|
||||
print('The parameter ' + key + ' could not be found in the exomy.yaml \n')
|
||||
print('It was set to the default value: 300\n')
|
||||
values[wheel] = 300
|
||||
else:
|
||||
values[wheel] = param_dict[key]
|
||||
return values
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(
|
||||
'''
|
||||
$$$$$$$$\ $$\ $$\
|
||||
$$ _____| $$$\ $$$ |
|
||||
$$ | $$\ $$\ $$$$$$\ $$$$\ $$$$ |$$\ $$\
|
||||
$$$$$\ \$$\ $$ |$$ __$$\ $$\$$\$$ $$ |$$ | $$ |
|
||||
$$ __| \$$$$ / $$ / $$ |$$ \$$$ $$ |$$ | $$ |
|
||||
$$ | $$ $$< $$ | $$ |$$ |\$ /$$ |$$ | $$ |
|
||||
$$$$$$$$\ $$ /\$$\ \$$$$$$ |$$ | \_/ $$ |\$$$$$$$ |
|
||||
\________|\__/ \__| \______/ \__| \__| \____$$ |
|
||||
$$\ $$ |
|
||||
\$$$$$$ |
|
||||
\______/
|
||||
'''
|
||||
)
|
||||
print(
|
||||
'''
|
||||
This script helps you to set the neutral values of PWM of the driving motors correctly.
|
||||
It will send the intended signal for "not moving" to all the motors.
|
||||
On each motor you have to turn the correction screw until the motor really stands still.
|
||||
'''
|
||||
)
|
||||
|
||||
if not os.path.exists(config_filename):
|
||||
print("exomy.yaml does not exist. Finish config_motor_pins.py to generate it.")
|
||||
exit()
|
||||
|
||||
|
||||
pwm = Adafruit_PCA9685.PCA9685()
|
||||
|
||||
'''
|
||||
The drive_pwm_neutral values are determined from the exomy.yaml file.
|
||||
But it can be also calculated from the values of the PWM board and motors,
|
||||
like shown in the following calculation:
|
||||
|
||||
# For most motors a pwm frequency of 50Hz is normal
|
||||
pwm_frequency = 50.0 # Hz
|
||||
pwm.set_pwm_freq(pwm_frequency)
|
||||
|
||||
# The cycle is the inverted frequency converted to milliseconds
|
||||
cycle = 1.0/pwm_frequency * 1000.0 # 20 ms
|
||||
|
||||
# The time the pwm signal is set to on during the duty cycle
|
||||
on_time = 1.5 # ms
|
||||
|
||||
# Duty cycle is the percentage of a cycle the signal is on
|
||||
duty_cycle = on_time/cycle # 0.075
|
||||
|
||||
# The PCA 9685 board requests a 12 bit number for the duty_cycle
|
||||
value = int(duty_cycle*4096.0) # 307
|
||||
'''
|
||||
|
||||
value_dict = get_drive_pwm_neutral_values()
|
||||
pin_list = get_driving_pins()
|
||||
|
||||
for index, pin in enumerate(pin_list):
|
||||
pwm.set_pwm(pin, 0, value_dict[WHEELS[index]])
|
||||
time.sleep(0.1)
|
||||
|
||||
raw_input('Press any button if you are done to complete configuration\n')
|
||||
|
||||
for pin in pin_list:
|
||||
pwm.set_pwm(pin, 0, 0)
|
||||
@@ -0,0 +1,220 @@
|
||||
import Adafruit_PCA9685
|
||||
import time
|
||||
from shutil import copyfile
|
||||
import os
|
||||
|
||||
DRIVE_MOTOR, STEER_MOTOR = [0, 1]
|
||||
|
||||
pos_names = {
|
||||
1: 'fl',
|
||||
2: 'fr',
|
||||
3: 'cl',
|
||||
4: 'cr',
|
||||
5: 'rl',
|
||||
6: 'rr',
|
||||
}
|
||||
|
||||
pin_dict = {
|
||||
|
||||
}
|
||||
|
||||
pwm = Adafruit_PCA9685.PCA9685()
|
||||
# For most motors a pwm frequency of 50Hz is normal
|
||||
pwm_frequency = 50.0 # Hz
|
||||
pwm.set_pwm_freq(pwm_frequency)
|
||||
# The cycle is the inverted frequency converted to milliseconds
|
||||
cycle = 1.0/pwm_frequency * 1000.0 # ms
|
||||
|
||||
# The time the pwm signal is set to on during the duty cycle
|
||||
on_time_1 = 2.4 # ms
|
||||
on_time_2 = 1.5 # ms
|
||||
|
||||
# Duty cycle is the percentage of a cycle the signal is on
|
||||
duty_cycle_1 = on_time_1/cycle
|
||||
duty_cycle_2 = on_time_2/cycle
|
||||
|
||||
# The PCA 9685 board requests a 12 bit number for the duty_cycle
|
||||
value_1 = 200 #int(duty_cycle_1*4096.0)
|
||||
value_2 = 400#int(duty_cycle_2*4096.0)
|
||||
|
||||
class Motor():
|
||||
def __init__(self, pin):
|
||||
|
||||
self.pin_name = 'pin_'
|
||||
self.pin_number = pin
|
||||
|
||||
def wiggle_motor(self):
|
||||
|
||||
# Set the motor to the second value
|
||||
pwm.set_pwm(self.pin_number, 0, value_2)
|
||||
# Wait for 1 seconds
|
||||
time.sleep(1.0)
|
||||
# Set the motor to the first value
|
||||
pwm.set_pwm(self.pin_number, 0, value_1)
|
||||
# Wait for 1 seconds
|
||||
time.sleep(1.0)
|
||||
# Set the motor to neutral
|
||||
pwm.set_pwm(self.pin_number, 0, 307)
|
||||
# Wait for half seconds
|
||||
time.sleep(0.5)
|
||||
# Stop the motor
|
||||
pwm.set_pwm(self.pin_number, 0, 0)
|
||||
|
||||
def stop_motor(self):
|
||||
# Turn the motor off
|
||||
pwm.set_pwm(self.pin_number, 0, 0)
|
||||
|
||||
|
||||
def print_exomy_layout():
|
||||
print(
|
||||
'''
|
||||
1 fl-||-fr 2
|
||||
||
|
||||
3 cl-||-cr 4
|
||||
5 rl====rr 6
|
||||
'''
|
||||
)
|
||||
|
||||
|
||||
def update_config_file():
|
||||
file_name = '../config/exomy.yaml'
|
||||
template_file_name = file_name+'.template'
|
||||
|
||||
if not os.path.exists(file_name):
|
||||
copyfile(template_file_name, file_name)
|
||||
print("exomy.yaml.template was copied to exomy.yaml")
|
||||
|
||||
output = ''
|
||||
with open(file_name, 'rt') as file:
|
||||
for line in file:
|
||||
for key, value in pin_dict.items():
|
||||
if(key in line):
|
||||
line = line.replace(line.split(': ', 1)[
|
||||
1], str(value) + '\n')
|
||||
|
||||
break
|
||||
output += line
|
||||
|
||||
with open(file_name, 'w') as file:
|
||||
file.write(output)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(
|
||||
'''
|
||||
$$$$$$$$\ $$\ $$\
|
||||
$$ _____| $$$\ $$$ |
|
||||
$$ | $$\ $$\ $$$$$$\ $$$$\ $$$$ |$$\ $$\
|
||||
$$$$$\ \$$\ $$ |$$ __$$\ $$\$$\$$ $$ |$$ | $$ |
|
||||
$$ __| \$$$$ / $$ / $$ |$$ \$$$ $$ |$$ | $$ |
|
||||
$$ | $$ $$< $$ | $$ |$$ |\$ /$$ |$$ | $$ |
|
||||
$$$$$$$$\ $$ /\$$\ \$$$$$$ |$$ | \_/ $$ |\$$$$$$$ |
|
||||
\________|\__/ \__| \______/ \__| \__| \____$$ |
|
||||
$$\ $$ |
|
||||
\$$$$$$ |
|
||||
\______/
|
||||
'''
|
||||
)
|
||||
print(
|
||||
'''
|
||||
###############
|
||||
Motor Configuration
|
||||
|
||||
This scripts leads you through the configuration of the motors.
|
||||
First we have to find out, to which pin of the PWM board a motor is connected.
|
||||
Look closely which motor moves and type in the answer.
|
||||
|
||||
Ensure to run the script until the end, otherwise your changes will not be saved!
|
||||
This script can always be stopped with ctrl+c and restarted.
|
||||
All other controls will be explained in the process.
|
||||
###############
|
||||
'''
|
||||
)
|
||||
|
||||
for pin_number in range(16):
|
||||
motor = Motor(pin_number)
|
||||
motor.stop_motor()
|
||||
|
||||
for pin_number in range(16):
|
||||
motor = Motor(pin_number)
|
||||
motor.wiggle_motor()
|
||||
type_selection = ''
|
||||
while(1):
|
||||
print("Pin #{}".format(pin_number))
|
||||
print(
|
||||
'Was it a steering or driving motor that moved, or should I repeat the movement? ')
|
||||
type_selection = raw_input('(d)rive (s)teer (r)epeat - (n)one (f)inish_configuration\n')
|
||||
if(type_selection == 'd'):
|
||||
motor.pin_name += 'drive_'
|
||||
print('Good job\n')
|
||||
break
|
||||
elif(type_selection == 's'):
|
||||
motor.pin_name += 'steer_'
|
||||
print('Good job\n')
|
||||
break
|
||||
elif(type_selection == 'r'):
|
||||
print('Look closely\n')
|
||||
motor.wiggle_motor()
|
||||
elif(type_selection == 'n'):
|
||||
print('Skipping pin')
|
||||
break
|
||||
elif(type_selection == 'f'):
|
||||
print('Finishing calibration at pin {}.'.format(pin_number))
|
||||
break
|
||||
else:
|
||||
print('Input must be d, s, r, n or f\n')
|
||||
|
||||
if (type_selection == 'd' or type_selection == 's'):
|
||||
while(1):
|
||||
print_exomy_layout()
|
||||
pos_selection = raw_input(
|
||||
'Type the position of the motor that moved.[1-6] or (r)epeat\n')
|
||||
if(pos_selection == 'r'):
|
||||
print('Look closely\n')
|
||||
else:
|
||||
try:
|
||||
pos = int(pos_selection)
|
||||
if(pos >= 1 and pos <= 6):
|
||||
motor.pin_name += pos_names[pos]
|
||||
break
|
||||
else:
|
||||
print('The input was not a number between 1 and 6\n')
|
||||
except ValueError:
|
||||
print('The input was not a number between 1 and 6\n')
|
||||
|
||||
pin_dict[motor.pin_name] = motor.pin_number
|
||||
print('Motor set!\n')
|
||||
print('########################################################\n')
|
||||
elif (type_selection == 'f'):
|
||||
break
|
||||
|
||||
|
||||
print('Now we will step through all the motors and check whether they have been assigned correctly.\n')
|
||||
print('Press ctrl+c if something is wrong and start the script again. \n')
|
||||
|
||||
for pin_name in pin_dict:
|
||||
print('moving {}'.format(pin_name))
|
||||
print_exomy_layout()
|
||||
|
||||
pin = pin_dict[pin_name]
|
||||
motor = Motor(pin)
|
||||
motor.wiggle_motor()
|
||||
raw_input('Press button to continue')
|
||||
|
||||
print("You assigned {}/12 motors.".format(len(pin_dict.keys())))
|
||||
|
||||
print('Write to config file.\n')
|
||||
update_config_file()
|
||||
print(
|
||||
'''
|
||||
$$$$$$$$\ $$\ $$\ $$\ $$\
|
||||
$$ _____|\__| \__| $$ | $$ |
|
||||
$$ | $$\ $$$$$$$\ $$\ $$$$$$$\ $$$$$$$\ $$$$$$\ $$$$$$$ |
|
||||
$$$$$\ $$ |$$ __$$\ $$ |$$ _____|$$ __$$\ $$ __$$\ $$ __$$ |
|
||||
$$ __| $$ |$$ | $$ |$$ |\$$$$$$\ $$ | $$ |$$$$$$$$ |$$ / $$ |
|
||||
$$ | $$ |$$ | $$ |$$ | \____$$\ $$ | $$ |$$ ____|$$ | $$ |
|
||||
$$ | $$ |$$ | $$ |$$ |$$$$$$$ |$$ | $$ |\$$$$$$$\ \$$$$$$$ |
|
||||
\__| \__|\__| \__|\__|\_______/ \__| \__| \_______| \_______|
|
||||
|
||||
''')
|
||||
|
||||
@@ -0,0 +1,147 @@
|
||||
import Adafruit_PCA9685
|
||||
import yaml
|
||||
import time
|
||||
import os
|
||||
|
||||
config_filename = '../config/exomy.yaml'
|
||||
|
||||
|
||||
def get_steering_motor_pins():
|
||||
steering_motor_pins = {}
|
||||
with open(config_filename, 'r') as file:
|
||||
param_dict = yaml.load(file)
|
||||
|
||||
for param_key, param_value in param_dict.items():
|
||||
if('pin_steer_' in str(param_key)):
|
||||
steering_motor_pins[param_key] = param_value
|
||||
return steering_motor_pins
|
||||
|
||||
def get_steering_pwm_neutral_values():
|
||||
steering_pwm_neutral_values = {}
|
||||
with open(config_filename, 'r') as file:
|
||||
param_dict = yaml.load(file)
|
||||
|
||||
for param_key, param_value in param_dict.items():
|
||||
if('steer_pwm_neutral_' in str(param_key)):
|
||||
steering_pwm_neutral_values[param_key] = param_value
|
||||
return steering_pwm_neutral_values
|
||||
|
||||
|
||||
def get_position_name(name):
|
||||
position_name = ''
|
||||
if('_fl' in name):
|
||||
position_name = 'Front Left'
|
||||
elif('_fr' in name):
|
||||
position_name = 'Front Right'
|
||||
elif('_cl' in name):
|
||||
position_name = 'Center Left'
|
||||
elif('_cr' in name):
|
||||
position_name = 'Center Right'
|
||||
elif('_rl' in name):
|
||||
position_name = 'Rear Left'
|
||||
elif('_rr' in name):
|
||||
position_name = 'Rear Right'
|
||||
|
||||
return position_name
|
||||
|
||||
|
||||
def update_config_file(steering_pwm_neutral_dict):
|
||||
output = ''
|
||||
with open(config_filename, 'rt') as file:
|
||||
for line in file:
|
||||
for key, value in steering_pwm_neutral_dict.items():
|
||||
if(key in line):
|
||||
line = line.replace(line.split(': ', 1)[
|
||||
1], str(value) + '\n')
|
||||
|
||||
break
|
||||
output += line
|
||||
|
||||
with open(config_filename, 'w') as file:
|
||||
file.write(output)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(
|
||||
'''
|
||||
$$$$$$$$\ $$\ $$\
|
||||
$$ _____| $$$\ $$$ |
|
||||
$$ | $$\ $$\ $$$$$$\ $$$$\ $$$$ |$$\ $$\
|
||||
$$$$$\ \$$\ $$ |$$ __$$\ $$\$$\$$ $$ |$$ | $$ |
|
||||
$$ __| \$$$$ / $$ / $$ |$$ \$$$ $$ |$$ | $$ |
|
||||
$$ | $$ $$< $$ | $$ |$$ |\$ /$$ |$$ | $$ |
|
||||
$$$$$$$$\ $$ /\$$\ \$$$$$$ |$$ | \_/ $$ |\$$$$$$$ |
|
||||
\________|\__/ \__| \______/ \__| \__| \____$$ |
|
||||
$$\ $$ |
|
||||
\$$$$$$ |
|
||||
\______/
|
||||
'''
|
||||
)
|
||||
print(
|
||||
'''
|
||||
This script helps you to set the neutral pwm values for the steering motors.
|
||||
You will iterate over all steering motors and set them to a neutral position.
|
||||
The determined value is written to the config file.
|
||||
|
||||
Commands:
|
||||
a - Decrease value for current pin
|
||||
d - Increase value for current pin
|
||||
q - Finish setting value for current pin
|
||||
|
||||
[Every of these commands must be confirmed with the enter key]
|
||||
|
||||
ctrl+c - Exit script
|
||||
------------------------------------------------------------------------------
|
||||
'''
|
||||
)
|
||||
|
||||
if not os.path.exists(config_filename):
|
||||
print("exomy.yaml does not exist. Finish config_motor_pins.py to generate it.")
|
||||
exit()
|
||||
|
||||
pwm = Adafruit_PCA9685.PCA9685()
|
||||
# For most motors a pwm frequency of 50Hz is normal
|
||||
pwm_frequency = 50.0 # Hz
|
||||
pwm.set_pwm_freq(pwm_frequency)
|
||||
|
||||
# The cycle is the inverted frequency converted to milliseconds
|
||||
cycle = 1.0/pwm_frequency * 1000.0 # ms
|
||||
|
||||
# The time the pwm signal is set to on during the duty cycle
|
||||
on_time = 1.5 # ms
|
||||
|
||||
# Duty cycle is the percentage of a cycle the signal is on
|
||||
duty_cycle = on_time/cycle
|
||||
|
||||
# The PCA 9685 board requests a 12 bit number for the duty_cycle
|
||||
initial_value = int(duty_cycle*4096.0)
|
||||
|
||||
# Get all steering pins
|
||||
steering_motor_pins = get_steering_motor_pins()
|
||||
pwm_neutral_dict = get_steering_pwm_neutral_values()
|
||||
# Iterating over all motors and fine tune the zero value
|
||||
for pin_name, pin_value in steering_motor_pins.items():
|
||||
pwm_neutral_name = pin_name.replace('pin_steer_', 'steer_pwm_neutral_')
|
||||
pwm_neutral_value = pwm_neutral_dict[pwm_neutral_name]
|
||||
|
||||
print('Set ' + get_position_name(pin_name) + ' steering motor: \n')
|
||||
while(1):
|
||||
# Set motor
|
||||
pwm.set_pwm(pin_value, 0, pwm_neutral_value)
|
||||
time.sleep(0.1)
|
||||
print('Current value: ' + str(pwm_neutral_value) + '\n')
|
||||
input = raw_input(
|
||||
'q-set / a-decrease pwm neutral value/ d-increase pwm neutral value\n')
|
||||
if(input is 'q'):
|
||||
print('PWM neutral value for ' + get_position_name(pin_name) +
|
||||
' has been set.\n')
|
||||
break
|
||||
elif(input is 'a'):
|
||||
print('Decreased pwm neutral value')
|
||||
pwm_neutral_value-= 5
|
||||
elif(input is 'd'):
|
||||
print('Increased pwm neutral value')
|
||||
pwm_neutral_value += 5
|
||||
pwm_neutral_dict[pwm_neutral_name] = pwm_neutral_value
|
||||
update_config_file(pwm_neutral_dict)
|
||||
print("Finished configuration!!!")
|
||||
@@ -0,0 +1,13 @@
|
||||
[Unit]
|
||||
Description=ExoMy Admin API
|
||||
After=network-online.target
|
||||
Wants=network-online.target
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
ExecStart=/usr/bin/python3 /home/pi/ExoMy_Software/scripts/exomy_admin_api.py
|
||||
Restart=always
|
||||
RestartSec=2
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
@@ -0,0 +1,14 @@
|
||||
[Unit]
|
||||
Description=ExoMy Video Delay Proxy
|
||||
After=network.target exomy-camera-stream.service
|
||||
Requires=exomy-camera-stream.service
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=pi
|
||||
ExecStart=/usr/bin/python3 /home/pi/ExoMy_Software/scripts/video_delay_proxy.py
|
||||
Restart=always
|
||||
RestartSec=3
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
@@ -0,0 +1,13 @@
|
||||
[Unit]
|
||||
Description=ExoMy WLAN Fallback Manager
|
||||
After=NetworkManager.service
|
||||
Wants=NetworkManager.service
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
ExecStart=/usr/local/bin/exomy-wifi-fallback-manager.sh
|
||||
Restart=always
|
||||
RestartSec=5
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,170 @@
|
||||
#!/usr/bin/env python3
|
||||
import http.server
|
||||
import json
|
||||
import os
|
||||
import urllib.parse
|
||||
import socketserver
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
|
||||
BOUNDARY = b'--frame'
|
||||
LATEST_JPEG = None
|
||||
LATEST_LOCK = threading.Lock()
|
||||
|
||||
PORT = 8081
|
||||
RUNTIME_CONFIG_FILE = '/tmp/exomy_camera_settings.json'
|
||||
DEFAULT_CAMERA_SETTINGS = {
|
||||
'width': 1296,
|
||||
'height': 972,
|
||||
'fps': 10,
|
||||
}
|
||||
|
||||
|
||||
def load_camera_settings():
|
||||
settings = dict(DEFAULT_CAMERA_SETTINGS)
|
||||
try:
|
||||
with open(RUNTIME_CONFIG_FILE, 'r', encoding='utf-8') as handle:
|
||||
raw_settings = json.load(handle)
|
||||
except (FileNotFoundError, json.JSONDecodeError, OSError, ValueError, TypeError):
|
||||
return settings
|
||||
|
||||
try:
|
||||
width = int(raw_settings.get('width', settings['width']))
|
||||
height = int(raw_settings.get('height', settings['height']))
|
||||
fps = int(raw_settings.get('fps', settings['fps']))
|
||||
except (TypeError, ValueError):
|
||||
return settings
|
||||
|
||||
settings['width'] = max(320, min(2592, width))
|
||||
settings['height'] = max(240, min(1944, height))
|
||||
settings['fps'] = max(1, min(30, fps))
|
||||
return settings
|
||||
|
||||
|
||||
CAMERA_SETTINGS = load_camera_settings()
|
||||
WIDTH = CAMERA_SETTINGS['width']
|
||||
HEIGHT = CAMERA_SETTINGS['height']
|
||||
FPS = CAMERA_SETTINGS['fps']
|
||||
|
||||
|
||||
def camera_reader():
|
||||
global LATEST_JPEG
|
||||
cmd = [
|
||||
'rpicam-vid',
|
||||
'--timeout', '0',
|
||||
'--nopreview',
|
||||
'--width', str(WIDTH),
|
||||
'--height', str(HEIGHT),
|
||||
'--framerate', str(FPS),
|
||||
'--codec', 'mjpeg',
|
||||
'-o', '-',
|
||||
]
|
||||
|
||||
while True:
|
||||
proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL, bufsize=0)
|
||||
buffer = bytearray()
|
||||
try:
|
||||
while True:
|
||||
chunk = proc.stdout.read(4096)
|
||||
if not chunk:
|
||||
break
|
||||
buffer.extend(chunk)
|
||||
|
||||
while True:
|
||||
start = buffer.find(b'\xff\xd8')
|
||||
if start == -1:
|
||||
if len(buffer) > 1024 * 1024:
|
||||
del buffer[:-2]
|
||||
break
|
||||
end = buffer.find(b'\xff\xd9', start + 2)
|
||||
if end == -1:
|
||||
if start > 0:
|
||||
del buffer[:start]
|
||||
break
|
||||
|
||||
jpeg = bytes(buffer[start:end + 2])
|
||||
del buffer[:end + 2]
|
||||
with LATEST_LOCK:
|
||||
LATEST_JPEG = jpeg
|
||||
finally:
|
||||
proc.kill()
|
||||
proc.wait()
|
||||
time.sleep(1)
|
||||
|
||||
|
||||
class ReusableThreadingTCPServer(socketserver.ThreadingTCPServer):
|
||||
allow_reuse_address = True
|
||||
|
||||
|
||||
class Handler(http.server.BaseHTTPRequestHandler):
|
||||
def normalized_path(self):
|
||||
return urllib.parse.urlsplit(self.path).path
|
||||
|
||||
def do_HEAD(self):
|
||||
path = self.normalized_path()
|
||||
if path in ('/', '/index.html'):
|
||||
self.send_response(200)
|
||||
self.send_header('Content-Type', 'text/html; charset=utf-8')
|
||||
self.end_headers()
|
||||
return
|
||||
|
||||
if path == '/stream.mjpg':
|
||||
self.send_response(200)
|
||||
self.send_header('Age', '0')
|
||||
self.send_header('Cache-Control', 'no-cache, private')
|
||||
self.send_header('Pragma', 'no-cache')
|
||||
self.send_header('Content-Type', 'multipart/x-mixed-replace; boundary=frame')
|
||||
self.end_headers()
|
||||
return
|
||||
|
||||
self.send_error(404)
|
||||
|
||||
def do_GET(self):
|
||||
path = self.normalized_path()
|
||||
if path in ('/', '/index.html'):
|
||||
body = b'<html><body><img src="/stream.mjpg" /></body></html>'
|
||||
self.send_response(200)
|
||||
self.send_header('Content-Type', 'text/html; charset=utf-8')
|
||||
self.send_header('Content-Length', str(len(body)))
|
||||
self.end_headers()
|
||||
self.wfile.write(body)
|
||||
return
|
||||
|
||||
if path != '/stream.mjpg':
|
||||
self.send_error(404)
|
||||
return
|
||||
|
||||
self.send_response(200)
|
||||
self.send_header('Age', '0')
|
||||
self.send_header('Cache-Control', 'no-cache, private')
|
||||
self.send_header('Pragma', 'no-cache')
|
||||
self.send_header('Content-Type', 'multipart/x-mixed-replace; boundary=frame')
|
||||
self.end_headers()
|
||||
|
||||
while True:
|
||||
with LATEST_LOCK:
|
||||
frame = LATEST_JPEG
|
||||
if frame is None:
|
||||
time.sleep(0.1)
|
||||
continue
|
||||
try:
|
||||
self.wfile.write(BOUNDARY + b'\r\n')
|
||||
self.wfile.write(b'Content-Type: image/jpeg\r\n')
|
||||
self.wfile.write(f'Content-Length: {len(frame)}\r\n\r\n'.encode('ascii'))
|
||||
self.wfile.write(frame)
|
||||
self.wfile.write(b'\r\n')
|
||||
self.wfile.flush()
|
||||
time.sleep(1.0 / FPS)
|
||||
except (BrokenPipeError, ConnectionResetError):
|
||||
break
|
||||
|
||||
def log_message(self, fmt, *args):
|
||||
return
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
threading.Thread(target=camera_reader, daemon=True).start()
|
||||
with ReusableThreadingTCPServer(('0.0.0.0', PORT), Handler) as server:
|
||||
server.daemon_threads = True
|
||||
server.serve_forever()
|
||||
@@ -0,0 +1,94 @@
|
||||
import Adafruit_PCA9685
|
||||
import time
|
||||
import sys
|
||||
'''
|
||||
This script helps to test pwm motors with the Adafruit PCA9685 board
|
||||
Example usage:
|
||||
python motor_test.py 3
|
||||
|
||||
Performs a motor test for the motor connected to pin 3 of the PWM board
|
||||
'''
|
||||
# Check if the pin number is given as an argument
|
||||
if len(sys.argv) < 2:
|
||||
print('You must give the pin number of the motor to be tested as argument.')
|
||||
print('E.g: python motor_test.py 3')
|
||||
print('Tests the motor connected to pin 3.')
|
||||
exit()
|
||||
|
||||
# Set the pin of the motor
|
||||
pin = int(sys.argv[1])
|
||||
print('Pin: '+str(pin))
|
||||
|
||||
|
||||
pwm = Adafruit_PCA9685.PCA9685()
|
||||
# For most motors a pwm frequency of 50Hz is normal
|
||||
pwm_frequency = 50.0 #Hz
|
||||
pwm.set_pwm_freq(pwm_frequency)
|
||||
|
||||
# The cycle is the inverted frequency converted to milliseconds
|
||||
cycle = 1.0/pwm_frequency * 1000.0 #ms
|
||||
|
||||
# The time the pwm signal is set to on during the duty cycle
|
||||
on_time = 2.0 #ms
|
||||
|
||||
selection = ''
|
||||
|
||||
while(selection != '0'):
|
||||
|
||||
print("What do you want to test?")
|
||||
print("1. Min to Max oscilation")
|
||||
print('2. Incremental positioning')
|
||||
print('0. Abort')
|
||||
selection = raw_input()
|
||||
|
||||
if (int(selection) == 1):
|
||||
|
||||
min_t = 0.5 # ms
|
||||
max_t = 2.5 # ms
|
||||
mid_t = min_t+max_t/2
|
||||
|
||||
print("pulsewidth_min = {:.2f}, pulsewidth_max = {:.2f}".format(min_t, max_t))
|
||||
|
||||
# *_dc is the percentage of a cycle the signal is on
|
||||
min_dc = min_t/cycle
|
||||
max_dc = max_t/cycle
|
||||
mid_dc = mid_t/cycle
|
||||
|
||||
dc_list = [min_dc, mid_dc, max_dc, mid_dc]
|
||||
for dc in dc_list:
|
||||
pwm.set_pwm(pin, 0, int(dc*4096.0))
|
||||
time.sleep(2.0)
|
||||
|
||||
if (int(selection) == 2):
|
||||
|
||||
curr_t = 1.5 # ms
|
||||
curr_dc = curr_t/cycle
|
||||
|
||||
step_size = 0.1 # ms
|
||||
|
||||
step_size_scaling = 0.2
|
||||
|
||||
dc_selection = ''
|
||||
|
||||
while (dc_selection != '0'):
|
||||
dc_selection = raw_input('a-d: change pulsewidth | w-s: change step size | 0: back to menu\n')
|
||||
if dc_selection == 'a':
|
||||
curr_t = curr_t - step_size
|
||||
elif dc_selection == 'd':
|
||||
curr_t = curr_t + step_size
|
||||
elif dc_selection == 's':
|
||||
step_size = step_size*(1-step_size_scaling)
|
||||
elif dc_selection == 'w':
|
||||
step_size = step_size*(1+step_size_scaling)
|
||||
|
||||
curr_dc = curr_t/cycle
|
||||
|
||||
curr_pwm = int(curr_dc*4096.0)
|
||||
print("t_current:\t{0:.4f} [ms]\nstep_size:\t{1:.4f} [ms]\ncurr_pwm: {2:.2f}".format(curr_t, step_size, curr_pwm))
|
||||
|
||||
pwm.set_pwm(pin, 0, curr_pwm)
|
||||
|
||||
|
||||
|
||||
# The PCA 9685 board requests a 12 bit number for the duty_cycle
|
||||
pwm.set_pwm(pin, 0, 0)
|
||||
@@ -0,0 +1,14 @@
|
||||
import Adafruit_PCA9685
|
||||
import time
|
||||
import sys
|
||||
|
||||
'''
|
||||
This script simply stops all the motors, in case they were left in a running state.
|
||||
'''
|
||||
pwm = Adafruit_PCA9685.PCA9685()
|
||||
# For most motors a pwm frequency of 50Hz is normal
|
||||
pwm_frequency = 50.0 # Hz
|
||||
pwm.set_pwm_freq(pwm_frequency)
|
||||
|
||||
for pin_number in range(16):
|
||||
pwm.set_pwm(pin_number, 0, 0)
|
||||
@@ -0,0 +1,161 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
MJPEG Video Delay Proxy
|
||||
Puffert Frames vom Kamerastream (Port 8081) und liefert sie
|
||||
mit einstellbarer Verzögerung auf Port 8083.
|
||||
Delay wird aus /tmp/exomy_delay.txt gelesen (Sekunden als float).
|
||||
"""
|
||||
import collections
|
||||
import http.server
|
||||
import os
|
||||
import socketserver
|
||||
import threading
|
||||
import time
|
||||
import urllib.parse
|
||||
import urllib.request
|
||||
|
||||
DELAY_FILE = '/tmp/exomy_delay.txt'
|
||||
SOURCE_URL = 'http://localhost:8081/stream.mjpg'
|
||||
PORT = 8083
|
||||
BOUNDARY = b'--frame'
|
||||
|
||||
# Deque: (timestamp_float, jpeg_bytes)
|
||||
frame_buffer = collections.deque()
|
||||
buffer_lock = threading.Lock()
|
||||
latest_frame = None
|
||||
latest_lock = threading.Lock()
|
||||
|
||||
|
||||
def read_delay():
|
||||
try:
|
||||
with open(DELAY_FILE) as f:
|
||||
return max(0.0, float(f.read().strip()))
|
||||
except Exception:
|
||||
return 0.0
|
||||
|
||||
|
||||
def camera_reader():
|
||||
global latest_frame
|
||||
while True:
|
||||
try:
|
||||
req = urllib.request.urlopen(SOURCE_URL, timeout=5)
|
||||
buf = bytearray()
|
||||
while True:
|
||||
chunk = req.read(4096)
|
||||
if not chunk:
|
||||
break
|
||||
buf.extend(chunk)
|
||||
while True:
|
||||
start = buf.find(b'\xff\xd8')
|
||||
if start == -1:
|
||||
if len(buf) > 1024 * 1024:
|
||||
del buf[:-2]
|
||||
break
|
||||
end = buf.find(b'\xff\xd9', start + 2)
|
||||
if end == -1:
|
||||
if start > 0:
|
||||
del buf[:start]
|
||||
break
|
||||
jpeg = bytes(buf[start:end + 2])
|
||||
del buf[:end + 2]
|
||||
ts = time.monotonic()
|
||||
with buffer_lock:
|
||||
frame_buffer.append((ts, jpeg))
|
||||
# Puffer auf 12 Sekunden begrenzen
|
||||
cutoff = ts - 12.0
|
||||
while frame_buffer and frame_buffer[0][0] < cutoff:
|
||||
frame_buffer.popleft()
|
||||
with latest_lock:
|
||||
latest_frame = jpeg
|
||||
except Exception:
|
||||
time.sleep(1)
|
||||
|
||||
|
||||
def get_delayed_frame():
|
||||
delay = read_delay()
|
||||
if delay <= 0:
|
||||
with latest_lock:
|
||||
return latest_frame
|
||||
target_ts = time.monotonic() - delay
|
||||
with buffer_lock:
|
||||
if not frame_buffer:
|
||||
return None
|
||||
best = frame_buffer[0][1]
|
||||
for ts, jpeg in frame_buffer:
|
||||
if ts <= target_ts:
|
||||
best = jpeg
|
||||
else:
|
||||
break
|
||||
return best
|
||||
|
||||
|
||||
class ReusableTCPServer(socketserver.ThreadingTCPServer):
|
||||
allow_reuse_address = True
|
||||
|
||||
|
||||
class Handler(http.server.BaseHTTPRequestHandler):
|
||||
def normalized_path(self):
|
||||
return urllib.parse.urlsplit(self.path).path
|
||||
|
||||
def do_GET(self):
|
||||
path = self.normalized_path()
|
||||
if path == '/snapshot.jpg':
|
||||
frame = get_delayed_frame()
|
||||
if frame is None:
|
||||
self.send_error(503)
|
||||
return
|
||||
self.send_response(200)
|
||||
self.send_header('Cache-Control', 'no-cache, private')
|
||||
self.send_header('Pragma', 'no-cache')
|
||||
self.send_header('Access-Control-Allow-Origin', '*')
|
||||
self.send_header('Content-Type', 'image/jpeg')
|
||||
self.send_header('Content-Length', str(len(frame)))
|
||||
self.end_headers()
|
||||
self.wfile.write(frame)
|
||||
return
|
||||
|
||||
if path != '/stream.mjpg':
|
||||
self.send_error(404)
|
||||
return
|
||||
self.send_response(200)
|
||||
self.send_header('Age', '0')
|
||||
self.send_header('Cache-Control', 'no-cache, private')
|
||||
self.send_header('Pragma', 'no-cache')
|
||||
self.send_header('Access-Control-Allow-Origin', '*')
|
||||
self.send_header('Content-Type', 'multipart/x-mixed-replace; boundary=frame')
|
||||
self.end_headers()
|
||||
fps = 10
|
||||
while True:
|
||||
frame = get_delayed_frame()
|
||||
if frame is None:
|
||||
time.sleep(0.1)
|
||||
continue
|
||||
try:
|
||||
self.wfile.write(BOUNDARY + b'\r\n')
|
||||
self.wfile.write(b'Content-Type: image/jpeg\r\n')
|
||||
self.wfile.write(f'Content-Length: {len(frame)}\r\n\r\n'.encode())
|
||||
self.wfile.write(frame)
|
||||
self.wfile.write(b'\r\n')
|
||||
self.wfile.flush()
|
||||
time.sleep(1.0 / fps)
|
||||
except (BrokenPipeError, ConnectionResetError):
|
||||
break
|
||||
|
||||
def log_message(self, fmt, *args):
|
||||
return
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Delay-Datei initialisieren (world-writable damit Admin-API schreiben kann)
|
||||
try:
|
||||
fd = os.open(DELAY_FILE, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o666)
|
||||
os.write(fd, b'0.0')
|
||||
os.close(fd)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
threading.Thread(target=camera_reader, daemon=True).start()
|
||||
with ReusableTCPServer(('0.0.0.0', PORT), Handler) as server:
|
||||
server.daemon_threads = True
|
||||
print(f'Video-Delay-Proxy läuft auf Port {PORT}')
|
||||
server.serve_forever()
|
||||
@@ -0,0 +1,49 @@
|
||||
#!/bin/bash
|
||||
set -euo pipefail
|
||||
|
||||
WIFI_IFACE="wlan0"
|
||||
PRIMARY_CONN="netplan-wlan0-eskimue.de"
|
||||
SECONDARY_CONN="4pi"
|
||||
FALLBACK_AP_CONN="CUNO-AP"
|
||||
CHECK_INTERVAL=20
|
||||
|
||||
get_active_connection() {
|
||||
nmcli -t -f GENERAL.CONNECTION device show "$WIFI_IFACE" 2>/dev/null | head -n 1 | cut -d: -f2-
|
||||
}
|
||||
|
||||
activate_connection() {
|
||||
local connection_name="$1"
|
||||
nmcli --wait 15 connection up "$connection_name" ifname "$WIFI_IFACE" >/dev/null 2>&1
|
||||
}
|
||||
|
||||
ensure_best_connection() {
|
||||
local active_connection
|
||||
active_connection="$(get_active_connection)"
|
||||
|
||||
if [[ "$active_connection" == "$PRIMARY_CONN" ]]; then
|
||||
return
|
||||
fi
|
||||
|
||||
if activate_connection "$PRIMARY_CONN"; then
|
||||
return
|
||||
fi
|
||||
|
||||
active_connection="$(get_active_connection)"
|
||||
if [[ "$active_connection" == "$SECONDARY_CONN" ]]; then
|
||||
return
|
||||
fi
|
||||
|
||||
if activate_connection "$SECONDARY_CONN"; then
|
||||
return
|
||||
fi
|
||||
|
||||
active_connection="$(get_active_connection)"
|
||||
if [[ "$active_connection" != "$FALLBACK_AP_CONN" ]]; then
|
||||
activate_connection "$FALLBACK_AP_CONN" || true
|
||||
fi
|
||||
}
|
||||
|
||||
while true; do
|
||||
ensure_best_connection
|
||||
sleep "$CHECK_INTERVAL"
|
||||
done
|
||||
Reference in New Issue
Block a user