Ano-sys
167 lines
3.7 KiB
Python
167 lines
3.7 KiB
Python
import RPi.GPIO as GPIO
|
||
import threading
|
||
import time
|
||
import subprocess
|
||
import os, signal, sys
|
||
from pid_line_follow1 import *
|
||
from AlphaBot2 import AlphaBot2
|
||
from TRSensors import TRSensor
|
||
|
||
|
||
Ab = AlphaBot2()
|
||
|
||
IR = 17
|
||
PWM = 50
|
||
|
||
'''def move_and_stop(func, duration=0.5):
|
||
func()
|
||
time.sleep(duration)
|
||
Ab.stop()'''
|
||
def setup_gpio():
|
||
"""Initialisiert die GPIO-Pins."""
|
||
GPIO.setmode(GPIO.BCM)
|
||
GPIO.setwarnings(False)
|
||
GPIO.setup(IR, GPIO.IN)
|
||
|
||
def getkey():
|
||
if GPIO.input(IR) == 0:
|
||
count = 0
|
||
while GPIO.input(IR) == 0 and count < 200: #9ms
|
||
count += 1
|
||
time.sleep(0.00006)
|
||
|
||
if(count < 10):
|
||
#print("None returned")
|
||
return None
|
||
count = 0
|
||
while GPIO.input(IR) == 1 and count < 80: #4.5ms
|
||
count += 1
|
||
time.sleep(0.00006)
|
||
#print("High-Burst-Length:", count)
|
||
if (count < 20):
|
||
#print("repeat returned")
|
||
return "repeat"
|
||
|
||
idx = 0
|
||
cnt = 0
|
||
data = [0,0,0,0]
|
||
for i in range(0,32):
|
||
count = 0
|
||
while GPIO.input(IR) == 0 and count < 15: #0.56ms
|
||
count += 1
|
||
time.sleep(0.00006)
|
||
|
||
count = 0
|
||
while GPIO.input(IR) == 1 and count < 40: #0: 0.56mx
|
||
count += 1 #1: 1.69ms
|
||
time.sleep(0.00006)
|
||
|
||
if count > 7:
|
||
data[idx] |= 1<<cnt
|
||
if cnt == 7:
|
||
cnt = 0
|
||
idx += 1
|
||
else:
|
||
cnt += 1
|
||
# print data
|
||
if data[0]+data[1] == 0xFF and data[2]+data[3] == 0xFF: #check
|
||
print("OK")
|
||
return data[2]
|
||
return None
|
||
|
||
def stop_listener():
|
||
"""
|
||
Deine Remote‑API muss hier so lange blocken, bis
|
||
ein Key‑Event eintrifft und dir den Code 0x43 liefert.
|
||
"""
|
||
while True:
|
||
code = getkey()
|
||
if code == 0x43:
|
||
os.kill(os.getpid(), signal.SIGINT)
|
||
return
|
||
|
||
setup_gpio()
|
||
print('IRremote Test Start ...')
|
||
Ab.stop()
|
||
last_key = 0
|
||
last_key_press_time = 0
|
||
current_action = None
|
||
try:
|
||
while True:
|
||
key = getkey()
|
||
# 0x43 == line_follow
|
||
|
||
if key == "repeat":
|
||
key = last_key
|
||
last_key_press_time = time.time()
|
||
elif key is not None:
|
||
print("getkey:")
|
||
print(hex(key))
|
||
last_key = key
|
||
last_key_press_time = time.time()
|
||
|
||
if key is not None:
|
||
last_key_press_time = time.time()
|
||
|
||
if key == 0x18 and current_action != "forward":
|
||
Ab.forward()
|
||
print("forward")
|
||
current_action = "forward"
|
||
elif key == 0x08 and current_action != "left":
|
||
Ab.left()
|
||
print("left")
|
||
current_action = "left"
|
||
elif key == 0x1c and current_action != "stop":
|
||
Ab.stop()
|
||
print("stop")
|
||
current_action = "stop"
|
||
elif key == 0x5a and current_action != "right":
|
||
Ab.right()
|
||
print("right")
|
||
current_action = "right"
|
||
elif key == 0x52 and current_action != "backward":
|
||
Ab.backward()
|
||
print("backward")
|
||
current_action = "backward"
|
||
elif key == 0x15:
|
||
if(PWM + 10 < 101):
|
||
PWM += 10
|
||
Ab.setPWMA(PWM)
|
||
Ab.setPWMB(PWM)
|
||
print("PWM:", PWM)
|
||
elif key == 0x07:
|
||
if(PWM - 10 > -1):
|
||
PWM -= 10
|
||
Ab.setPWMA(PWM)
|
||
Ab.setPWMB(PWM)
|
||
print("PWM:", PWM)
|
||
elif key == 0x43:
|
||
tr = TRSensor()
|
||
running = threading.Event()
|
||
process = subprocess.Popen(
|
||
["python", "pid_line_follow1.py"],
|
||
)
|
||
# follow_thread = threading.Thread(target=follow, args=(tr, Ab, running), daemon=True)
|
||
# follow_thread.start()
|
||
key = getkey()
|
||
while(key != 0x43):
|
||
key = getkey()
|
||
|
||
# running.set()
|
||
process.terminate()
|
||
|
||
Ab.setPWMA(PWM)
|
||
Ab.setPWMB(PWM)
|
||
|
||
time.sleep(0.05)
|
||
|
||
else:
|
||
if time.time() - last_key_press_time > 0.1 and current_action != "stop":
|
||
Ab.stop()
|
||
print("stopping bc no key is being pressed")
|
||
current_action = "stop"
|
||
|
||
except KeyboardInterrupt:
|
||
GPIO.cleanup()
|
||
|