105 lines
3.3 KiB
Python
105 lines
3.3 KiB
Python
from pynput import keyboard
|
|
import time
|
|
|
|
|
|
TICKS_PER_SEC = 30
|
|
|
|
|
|
class PIDController:
|
|
def __init__(self, kp, ki, kd, current_time):
|
|
self.kp = kp
|
|
self.ki = ki
|
|
self.kd = kd
|
|
self.previous_error = 0
|
|
self.integral = 0
|
|
self.last_time = current_time
|
|
|
|
def control(self, error, current_time):
|
|
current_time = current_time
|
|
delta_time = current_time - self.last_time
|
|
if delta_time == 0:
|
|
delta_time = 1 / TICKS_PER_SEC
|
|
derivative = (error - self.previous_error) / delta_time
|
|
self.integral += error * delta_time
|
|
|
|
output = self.kp * error + self.ki * self.integral + self.kd * derivative
|
|
|
|
self.previous_error = error
|
|
self.last_time = current_time
|
|
|
|
return output
|
|
|
|
|
|
class ManualControl:
|
|
def __init__(self, uav):
|
|
self.uav = uav
|
|
self.listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release)
|
|
|
|
def start(self):
|
|
self.listener.start()
|
|
|
|
def on_press(self, key):
|
|
try:
|
|
if key == keyboard.Key.up:
|
|
self.uav.for_back_velocity = 50
|
|
elif key == keyboard.Key.down:
|
|
self.uav.for_back_velocity = -50
|
|
elif key == keyboard.Key.left:
|
|
self.uav.left_right_velocity = -50
|
|
elif key == keyboard.Key.right:
|
|
self.uav.left_right_velocity = 50
|
|
elif key.char == 'w':
|
|
self.uav.up_down_velocity = 50
|
|
elif key.char == 's':
|
|
self.uav.up_down_velocity = -50
|
|
elif key.char == 'q':
|
|
self.uav.yaw_velocity = -50
|
|
elif key.char == 'e':
|
|
self.uav.yaw_velocity = 50
|
|
# print(f'left_right_velocity: {self.uav.left_right_velocity}, for_back_velocity: {self.uav.for_back_velocity}, up_down_velocity: {self.uav.up_down_velocity}, yaw_velocity: {self.uav.yaw_velocity}')
|
|
except AttributeError:
|
|
pass
|
|
|
|
def on_release(self, key):
|
|
try:
|
|
if key == keyboard.Key.up or key == keyboard.Key.down:
|
|
self.uav.for_back_velocity = 0
|
|
elif key == keyboard.Key.left or key == keyboard.Key.right:
|
|
self.uav.left_right_velocity = 0
|
|
elif key.char == 'w' or key.char == 's':
|
|
self.uav.up_down_velocity = 0
|
|
elif key.char == 'q' or key.char == 'e':
|
|
self.uav.yaw_velocity = 0
|
|
except AttributeError:
|
|
pass
|
|
|
|
|
|
class AutoControl:
|
|
def __init__(self, uav):
|
|
self.uav = uav
|
|
self.target = [0, 0, 0, 0]
|
|
self.pid = PIDController(1, 0, 0, time.time())
|
|
|
|
def detect(self):
|
|
pass
|
|
|
|
class FakeUAV:
|
|
def __init__(self) -> None:
|
|
self.left_right_velocity = 0
|
|
self.for_back_velocity = 0
|
|
self.up_down_velocity = 0
|
|
self.yaw_velocity = 0
|
|
|
|
def send_rc_control(self, left_right_velocity, for_back_velocity, up_down_velocity, yaw_velocity):
|
|
print(f'left_right_velocity: {left_right_velocity}, for_back_velocity: {for_back_velocity}, up_down_velocity: {up_down_velocity}, yaw_velocity: {yaw_velocity}')
|
|
|
|
if __name__ == '__main__':
|
|
# uav = FakeUAV()
|
|
# manual_control=ManualControl(uav)
|
|
# manual_control.start()
|
|
# while True:
|
|
# time.sleep(0.05)
|
|
|
|
import logging
|
|
logging.info('aaa')
|