class Distance(Thread):
def __init__(self):
Thread.__init__(self)
self.wheels = Wheel()
#Ultraschall GPIOs
self.GPIO_TRIGGER_Stat = 12
self.GPIO_ECHO_Stat = 7
#Setup GPIO Ultraschall
GPIO.setup(self.GPIO_TRIGGER_Stat, GPIO.OUT)
GPIO.setup(self.GPIO_ECHO_Stat, GPIO.IN)
#Thread starten
self.daemon = True
self.start()
def run(self):
try:
print("Distance gestartet")
while True:
abstand = AutonomFunctions().entfernung(
self.GPIO_TRIGGER_Stat, self.GPIO_ECHO_Stat)
#print("Gemessene Entfernung = %.1f cm" % abstand)
if (abstand < 10.0):
self.wheels.stopStep()
time.sleep(0.5)
# Beim Abbruch durch STRG+C resetten
except KeyboardInterrupt:
print("Messung vom User gestoppt")
GPIO.cleanup()
def __init__(self, x, y):
Sprite.__init__(self)
self.orig_image = pygame.transform.scale(
pygame.image.load('resources/car.png'), Car.sprite_dim)
self.image = self.orig_image
self.rect = self.image.get_rect()
self.x = x
self.y = y
self.rect = Rect(self.x, self.y,
self.image.get_rect().width,
self.image.get_rect().height)
self._velocity = 0.0
self._rotation = 0.0
self.target_velocity = -1
self.is_acc = False
self.v_input_received = False
self.front_wheel = Wheel()
self.sensor = Sensor((0, 0))
self.update_sensor()
self.weight = Car.default_weight
self.acceleration = Car.default_acceleration
self.deceleration = Car.default_deceleration
self.brake_force = Car.default_brake_force
self.top_speed = Car.default_top_speed
def __init__(self, dataFromClient, socketConnection):
Thread.__init__(self)
self.dataFromClient = dataFromClient
self.komm_handy = socketConnection
self.wheels = Wheel()
self.richtung = 0 #angabe wie gerade gefahren wird. 0=halt 1=vorwärts 2=rückwärts 3=links 4=rechts
self.daemon = True
self.start()
def __init__(self):
Thread.__init__(self)
self.window = curses.initscr()
#Klasse für die drei Schrittmotoren
self.stepperMotor = Stepper()
self.wheels = Wheel()
curses.noecho()
self.window.keypad(True)
self.daemon = True
self.start()
def __init__(self):
Thread.__init__(self)
self.wheels = Wheel()
#Ultraschall GPIOs
self.GPIO_TRIGGER_Stat = 12
self.GPIO_ECHO_Stat = 7
#Setup GPIO Ultraschall
GPIO.setup(self.GPIO_TRIGGER_Stat, GPIO.OUT)
GPIO.setup(self.GPIO_ECHO_Stat, GPIO.IN)
#Thread starten
self.daemon = True
self.start()
def __init__(self, dataFromClient, socketConnection):
Thread.__init__(self)
self.dataFromClient = dataFromClient
self.komm_handy = socketConnection
self.motorCamera = Stepper()
self.wheels = Wheel()
# angabe wie gerade gefahren wird. 0=halt 1=vorwärts 2=rückwärts 3=links 4=rechts
self.richtung = 0
self.verglRichtung1 = 90 # der Motor startet bei 90Grad in der Mitte
self.verglRichtung2 = 90 # der Motor startet bei 90Grad in der Mitte
self.daemon = True
self.start()
def __init__(self):
Thread.__init__(self)
self.wheels = Wheel()
self.functions = AutonomFunctions()
self.listAreas = []
#Ultraschall GPIOs
self.GPIO_TRIGGER_Stat = 12
self.GPIO_ECHO_Stat = 7
GPIO.setup(self.GPIO_TRIGGER_Stat, GPIO.OUT)
GPIO.setup(self.GPIO_ECHO_Stat, GPIO.IN)
self.GPIO_TRIGGER_Dyn = 3
self.GPIO_ECHO_Dyn = 2
GPIO.setup(self.GPIO_TRIGGER_Dyn, GPIO.OUT)
GPIO.setup(self.GPIO_ECHO_Dyn, GPIO.IN)
self.daemon = True
self.start()
class Autonom(Thread):
def __init__(self):
Thread.__init__(self)
self.wheels = Wheel()
self.functions = AutonomFunctions()
self.listAreas = []
#Ultraschall GPIOs
self.GPIO_TRIGGER_Stat = 12
self.GPIO_ECHO_Stat = 7
GPIO.setup(self.GPIO_TRIGGER_Stat, GPIO.OUT)
GPIO.setup(self.GPIO_ECHO_Stat, GPIO.IN)
self.GPIO_TRIGGER_Dyn = 3
self.GPIO_ECHO_Dyn = 2
GPIO.setup(self.GPIO_TRIGGER_Dyn, GPIO.OUT)
GPIO.setup(self.GPIO_ECHO_Dyn, GPIO.IN)
self.daemon = True
self.start()
def run(self):
try:
self.wheels.forwardStep()
while True:
distanz = self.functions.entfernung(self.GPIO_TRIGGER_Stat,
self.GPIO_ECHO_Stat)
print("Distanz = %.1f cm" % distanz)
if (distanz < 20.0):
self.wheels.stopStep()
self.listAreas = self.functions.checkClear(
self.GPIO_TRIGGER_Dyn, self.GPIO_ECHO_Dyn)
#ausgabe aller Bereich für den TEst
print(self.listAreas)
self.wheels.forwardStep()
else:
time.sleep(1.0)
# Programm beenden
except KeyboardInterrupt:
print("Programm abgebrochen")
GPIO.cleanup()
def __init__(self):
GPIO.setmode(GPIO.BCM)
self.areas = 16
self.sonicMotor = Stepper()
self.wheels = Wheel()
self.motorPosition = 0 #gibt an ob der SonicMotor links oder rechts steht 0=links
class AutonomFunctions():
def __init__(self):
GPIO.setmode(GPIO.BCM)
self.areas = 16
self.sonicMotor = Stepper()
self.wheels = Wheel()
self.motorPosition = 0 #gibt an ob der SonicMotor links oder rechts steht 0=links
def entfernung(self, Trigger, Echo):
# Trig High setzen
GPIO.output(Trigger, True)
# Trig Low setzen (nach 0.01ms)
time.sleep(0.00001)
GPIO.output(Trigger, False)
Startzeit = time.time()
Endzeit = time.time()
# Start/Stop Zeit ermitteln
while GPIO.input(Echo) == 0:
Startzeit = time.time()
while GPIO.input(Echo) == 1:
Endzeit = time.time()
# Vergangene Zeit
Zeitdifferenz = Endzeit - Startzeit
# Schallgeschwindigkeit (34300 cm/s) einbeziehen
entfernung = (Zeitdifferenz * 34300) / 2
return entfernung
def checkClear(self, Trigger, Echo):
self.listAreas = []
self.listAreasReturn = []
areaMerker = -1
for i in range(self.areas):
distanz = self.entfernung(Trigger, Echo)
#prüfung ob die Distanz größer als 20cm ist und in einer Reihenfolge zum vorigen Wert liegt,
# wenn ja dann wird der Bereich i in die listAreas eingetragen
if (distanz > 20 and i == areaMerker + 1):
self.listAreas.append(i)
else:
if len(self.listAreas) < 3:
del self.listAreas[:]
elif len(self.listAreas) > len(self.listAreasReturn):
self.listAreasReturn = list(self.listAreas)
del self.listAreas[:]
#Motor zur nächsten Position fahren und erneut die Distanz messen
self.sonicMotor.forwardSonic()
time.sleep(0.2)
areaMerker = i
#erneute Abfrage ob hinten der Wert höher ist. Dies muss geschehen da der letzte Wert angehängt werden kann und dann nicht mehr in die vorige else-Abfrage gegangen wird
if len(self.listAreas) > len(self.listAreasReturn):
self.listAreasReturn = list(self.listAreas)
del self.listAreas[:]
if 0 in self.listAreasReturn:
self.wheels.leftStep()
time.sleep(2.0)
self.wheels.forwardStep()
time.sleep(2.5)
self.wheels.rightStep()
time.sleep(2.0)
self.wheels.stopStep()
elif 15 in self.listAreasReturn:
self.wheels.rightStep()
time.sleep(2.0)
self.wheels.forwardStep()
time.sleep(2.5)
self.wheels.leftStep()
time.sleep(2.0)
self.wheels.stopStep()
else:
self.wheels.backwardStep()
time.sleep(1.0)
self.wheels.stopStep()
self.checkClear(3, 2)
#Motor zurück in die Ausgangsposition bringen
self.sonicMotor.backwardSonic()
return self.listAreasReturn
class TastaturMove(Thread):
def __init__(self):
Thread.__init__(self)
self.window = curses.initscr()
#Klasse für die drei Schrittmotoren
self.stepperMotor = Stepper()
self.wheels = Wheel()
curses.noecho()
self.window.keypad(True)
self.daemon = True
self.start()
def run(self):
try:
while (True):
self.key_char = self.window.get_wch()
print(self.key_char)
if (self.key_char == curses.KEY_LEFT):
print("links")
self.stepperMotor.forwardM1()
Speak().voice("links")
elif (self.key_char == curses.KEY_RIGHT):
print("rechts")
self.stepperMotor.backwardsM1()
Speak().voice("rechts")
elif (self.key_char == curses.KEY_UP):
print("hoch")
self.stepperMotor.backwardsM3()
Speak().voice("hoch")
elif (self.key_char == curses.KEY_DOWN):
print("runter")
self.stepperMotor.forwardM3()
Speak().voice("runter")
elif (self.key_char == curses.KEY_HOME):
print("links fahren")
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.leftStep()
#Speak().voice("links fahren")
elif (self.key_char == curses.KEY_END):
print("rechts fahren")
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.rightStep()
#Speak().voice("rechts fahren")
elif (self.key_char == curses.KEY_PPAGE):
print("vorwärts fahren")
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.forwardStep()
#Speak().voice("vorwärts")
elif (self.key_char == curses.KEY_NPAGE):
print("rückwärts fahren")
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.backwardStep()
#Speak().voice("rückwärts")
elif (self.key_char == curses.KEY_BACKSPACE):
print("Stop Motoren")
self.wheels.stopStep()
#sonic bewegen
elif (self.key_char == "r"):
self.stepperMotor.forwardSonic()
elif (self.key_char == "l"):
self.stepperMotor.backwardSonic()
#Einschalten falls mal die Geschwindigkeitskontrolle funktioniert
elif (self.key_char == "s"):
print("langsam fahren")
self.wheels.slowSpeed()
elif (self.key_char == "m"):
print("mittelscnell fahren")
self.wheels.mediumSpeed()
elif (self.key_char == "f"):
print("schnell fahren")
self.wheels.highSpeed()
else:
curses.beep()
curses.flushinp() # Cache Tasteninhalt löschen
if self.key_char == "q":
curses.flushinp() # Cache Tasteninhalt löschen
break
except Exception as e:
print("es gab einen Fehler: ", e)
self.stepperMotor.setStep0()
distance = pulse_duration * 17150
return distance
#Establishing bluetooth connection
server_socket = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
port = 1
server_socket.bind(("", port))
server_socket.listen(1)
client_socket, address = server_socket.accept()
client_socket.setblocking(0)
print("Accepted connection from ", address)
client_socket.send("Connected")
#Setting up GPIO pins
frontRight = Wheel(36, 32, 0)
frontLeft = Wheel(40, 38, 0)
rearRight = Wheel(35, 37, 0)
rearLeft = Wheel(31, 33, 0)
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(frontRight.forwardPin, GPIO.OUT)
GPIO.setup(frontRight.backwardPin, GPIO.OUT)
GPIO.setup(frontLeft.forwardPin, GPIO.OUT)
GPIO.setup(frontLeft.backwardPin, GPIO.OUT)
GPIO.setup(rearRight.forwardPin, GPIO.OUT)
GPIO.setup(rearRight.backwardPin, GPIO.OUT)
GPIO.setup(rearLeft.forwardPin, GPIO.OUT)
GPIO.setup(rearLeft.backwardPin, GPIO.OUT)
class Car(Sprite):
default_weight = 1800 # kg
default_top_speed = 230 # km/h
sprite_dim = (32, 15)
default_brake_force = 15
default_acceleration = 30
default_deceleration = 0.05
def __init__(self, x, y):
Sprite.__init__(self)
self.orig_image = pygame.transform.scale(
pygame.image.load('resources/car.png'), Car.sprite_dim)
self.image = self.orig_image
self.rect = self.image.get_rect()
self.x = x
self.y = y
self.rect = Rect(self.x, self.y,
self.image.get_rect().width,
self.image.get_rect().height)
self._velocity = 0.0
self._rotation = 0.0
self.target_velocity = -1
self.is_acc = False
self.v_input_received = False
self.front_wheel = Wheel()
self.sensor = Sensor((0, 0))
self.update_sensor()
self.weight = Car.default_weight
self.acceleration = Car.default_acceleration
self.deceleration = Car.default_deceleration
self.brake_force = Car.default_brake_force
self.top_speed = Car.default_top_speed
@property
def position(self) -> Tuple[float, float]:
return self.x, self.y
@property
def rotation(self):
return math.radians(self._rotation)
@property
def forces(self):
rotation = self.rotation
fx = self._velocity * math.cos(rotation)
fy = self._velocity * math.sin(rotation)
return Vector(fx, fy)
@property
def velocity(self):
return self._velocity
@property
def cruise_control_on(self):
return not (self.target_velocity < 0)
@property
def should_acc(self):
return self.cruise_control_on and self._velocity < self.target_velocity and not self.v_input_received
@property
def should_brake(self):
return self.cruise_control_on and self._velocity > self.target_velocity + 3 and not self.v_input_received
def enable_cruise_control(self, target):
self.target_velocity = target
def disable_cruise_control(self):
self.target_velocity = -1
def full_speed(self):
self.v_input_received = True
self._velocity = self.top_speed
def accelerate(self):
self.v_input_received = True
self.is_acc = True
def inverse_acc(self):
return -2.94118 * (1 - math.e**(0.0112642 * self.velocity))
def acc_fun(self, dt):
t = self.inverse_acc() + dt
return 88.777 * math.log(0.34 * (t + 2.94118))
def _acc(self, dt):
new = self.acc_fun(dt)
if self.cruise_control_on and new > self.target_velocity and not self.v_input_received:
self._velocity = self.target_velocity
else:
self._velocity = min(float(self.top_speed), new)
def _dec(self, dt):
delta = dt * max(self.deceleration * ((self._velocity / 10)**2), 1)
if not self.v_input_received and self.cruise_control_on and self._velocity - delta < self.cruise_control_on:
self._velocity = self.target_velocity
else:
self._velocity -= delta
self._velocity = max(self._velocity, 0)
def _brake(self, dt):
delta = dt * self.brake_force
if self.v_input_received or (
self.cruise_control_on
and self._velocity - delta > self.target_velocity):
self._velocity -= dt * self.brake_force
else:
self._velocity = self.target_velocity
self._velocity = max(self._velocity, 0)
def calc_velocity(self, dt):
if self.is_acc or self.should_acc:
self._acc(dt)
else:
self._dec(dt)
if self.cruise_control_on and self.should_brake:
self._brake(dt)
def apply_brake(self, dt):
self.v_input_received = True
self._brake(dt)
def bound_rotation(self):
if self._rotation > 360:
self._rotation -= 360
elif self._rotation < 0:
self._rotation += 360
def rotate(self, dt, v):
self._rotation += v * dt * (self._velocity / self.top_speed * 5)
self.bound_rotation()
self.image = pygame.transform.rotate(self.orig_image,
360.0 - self._rotation)
self.rect = self.image.get_rect()
self.update_rect()
def steer_left(self, dt):
self.front_wheel.steer_left(dt)
def steer_right(self, dt):
self.front_wheel.steer_right(dt)
def steer_target(self, deg, direction):
self.front_wheel.set_target(deg, direction)
def unset_steer_target(self):
self.front_wheel.unset_target()
def update_rect(self):
w = self.rect.width
h = self.rect.height
self.rect = Rect(self.x, self.y, w, h)
def update_sensor(self):
x = self.x + (self.image.get_rect().width / 2)
y = self.y + (self.image.get_rect().height / 2)
self.sensor.center = (x, y)
def move(self, dt):
f = self.forces
dx = f.x * dt
dy = f.y * dt
self.x += dx
self.y += dy
self.update_rect()
self.update_sensor()
def update(self, dt):
self.front_wheel.update(dt)
self.sensor.update(dt)
self.rotate(dt, self.front_wheel.rotation)
self.calc_velocity(dt)
self.is_acc = False
self.v_input_received = False
self.move(dt)
class HandyWheels(Thread):
def __init__(self, dataFromClient, socketConnection):
Thread.__init__(self)
self.dataFromClient = dataFromClient
self.komm_handy = socketConnection
self.wheels = Wheel()
self.richtung = 0 #angabe wie gerade gefahren wird. 0=halt 1=vorwärts 2=rückwärts 3=links 4=rechts
self.daemon = True
self.start()
def readData(self):
self.data = self.dataFromClient.recv(1024).decode("utf-8")
cmdList = self.data.split("\n")
cmdList = cmdList[:-1]
return cmdList
def run(self):
# Daten vom Client empfangen damit Funktionen gestartet werden können
try:
while True:
cmdArray = self.readData()
for oneCmd in cmdArray: # comma, or other
dataSplit=oneCmd.split("#")
#CMD_MOTOR#-1496#-1496#1030#1030
if (len(dataSplit) == 5) and (dataSplit[0] == "CMD_MOTOR"):
print(dataSplit)
if(int(dataSplit[1]) == 0) and (int(dataSplit[3]) == 0):
self.wheels.stopStep()
self.richtung = 0
elif(int(dataSplit[1]) > 0) and (int(dataSplit[3]) > 0):
if (self.richtung != 1):
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.forwardStep()
self.richtung = 1
elif(int(dataSplit[1]) < 0) and (int(dataSplit[3]) < 0):
if (self.richtung != 2):
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.backwardStep()
self.richtung = 2
elif(int(dataSplit[1]) > 0) and (int(dataSplit[3]) < 0):
if (self.richtung != 3):
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.leftStep()
self.richtung = 3
elif(int(dataSplit[1]) < 0) and (int(dataSplit[3]) > 0):
if (self.richtung != 4):
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.rightStep()
self.richtung = 4
except Exception as e:
print(e)
self.komm_handy.kommunikationHandyClose()
print("Programm Handy Steuerung geschlossen")
from sensors import Sensor
from sensors import Sensors
from wheels import Wheel
from wheels import MockWheel
from wheels import Wheels
import RPi.GPIO as GPIO
from runtime import Runtime
from processor import Processor
GPIO.setmode(GPIO.BOARD)
sensor1 = Sensor(11, 13)
sensor3 = Sensor(15, 16)
sensor5 = Sensor(18, 19)
sensor6 = Sensor(21, 22)
sensor8 = Sensor(23, 24)
sensor10 = Sensor(26, 29)
leftMotor = Wheel(5, 3, 7, 7)
rightMotor = Wheel(8, 10, 12, 1)
# rightMotor = MockWheel("rightWheel")
zrobot_runtime = Runtime(
Sensors([sensor1, sensor3, sensor5, sensor6, sensor8, sensor10]),
Processor(), Wheels(leftMotor, rightMotor))
zrobot_runtime.start()
class HandyMove(Thread):
def __init__(self, dataFromClient, socketConnection):
Thread.__init__(self)
self.dataFromClient = dataFromClient
self.komm_handy = socketConnection
self.motorCamera = Stepper()
self.wheels = Wheel()
# angabe wie gerade gefahren wird. 0=halt 1=vorwärts 2=rückwärts 3=links 4=rechts
self.richtung = 0
self.verglRichtung1 = 90 # der Motor startet bei 90Grad in der Mitte
self.verglRichtung2 = 90 # der Motor startet bei 90Grad in der Mitte
self.daemon = True
self.start()
def run(self):
# Daten vom Client empfangen damit Funktionen gestartet werden können
try:
while True:
self.data = self.dataFromClient.recv(1024).decode("utf-8")
#Abfrage ob noch Kommandos kommen, wenn nicht dann wird erneut nach dem Aufbau der Verbindung gefragt. Wird nur hier getan und nicht in handyWheels
if not (self.data):
print("End transmit commands")
self.komm_handy.kommunikationHandyClose()
self.dataFromClient, self.dataFromClient1 = self.komm_handy.kommunikationHandy(
)
cmdList = self.data.split("\n")
cmdList = cmdList[:-1]
for oneCmd in cmdList: # comma, or other
dataSplit = oneCmd.split("#")
if (dataSplit[0] == "CMD_SERVO"):
print(dataSplit)
if ((dataSplit[1] == "0") and
(int(dataSplit[2]) > int(self.verglRichtung1))):
print("motor1, rechts wird übergeben")
self.motorCamera.backwardsM1(
) #Motor1 wird rechts gedreht
Speak().voice("Kamera rechts")
self.verglRichtung1 = dataSplit[2]
elif ((dataSplit[1] == "0") and
(int(dataSplit[2]) < int(self.verglRichtung1))):
print("motor1, links wird übergeben")
self.motorCamera.forwardM1(
) #Motor1 wird links gedreht
Speak().voice("Kamera links")
self.verglRichtung1 = dataSplit[2]
elif ((dataSplit[1] == "1") and
(int(dataSplit[2]) > int(self.verglRichtung2))):
print("motor2, hoch wird übergeben")
self.motorCamera.backwardsM2(
) #Motor2 wird hoch gedreht
Speak().voice("Kamera hoch")
self.verglRichtung2 = dataSplit[2]
elif ((dataSplit[1] == "1") and
(int(dataSplit[2]) < int(self.verglRichtung2))):
print("motor2, runter wird übergeben")
self.motorCamera.forwardM2(
) #Motor2 wird runter gedreht
Speak().voice("Kamera runter")
self.verglRichtung2 = dataSplit[2]
elif (dataSplit[0] == "CMD_MOTOR"):
print(dataSplit)
if (int(dataSplit[1]) == 0) and (int(dataSplit[3])
== 0):
self.wheels.stopStep()
self.richtung = 0
elif (int(dataSplit[1]) > 0) and (int(dataSplit[3]) >
0):
if (self.richtung != 1):
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.forwardStep()
self.richtung = 1
elif (int(dataSplit[1]) < 0) and (int(dataSplit[3]) <
0):
if (self.richtung != 2):
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.backwardStep()
self.richtung = 2
elif (int(dataSplit[1]) > 0) and (int(dataSplit[3]) <
0):
if (self.richtung != 3):
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.leftStep()
self.richtung = 3
elif (int(dataSplit[1]) < 0) and (int(dataSplit[3]) >
0):
if (self.richtung != 4):
self.wheels.stopStep()
time.sleep(0.5)
self.wheels.rightStep()
self.richtung = 4
except Exception as e:
print(e)
self.komm_handy.kommunikationHandyClose()
self.motorCamera.setStep0()
print("Programm Handy Steuerung geschlossen")