def linksInslaan(kanaal=0, tijd=3.1):
motor.rightmotorspeed(90)
motor.leftmotorspeed(82)
time.sleep(1)
while adc.getAfstand(kanaal) <= 12:
motor.stopMotor()
time.sleep(1)
time.sleep(tijd)
motor.stopMotor()
motor.turnLeftNinety()
motor.stopMotor()
while not zoeklijn():
while adc.getAfstand(kanaal) <= 12:
motor.stopMotor()
time.sleep(1)
motor.forward()
time.sleep(1)
motor.rightmotorspeed(93)
motor.leftmotorspeed(60)
time.sleep(0.2)
motor.rightmotorspeed(80)
motor.leftmotorspeed(90)
time.sleep(0.2)
def rechtsInslaan(kanaal=0, tijd=1):
motor.rightmotorspeed(90)
motor.leftmotorspeed(82)
time.sleep(2)
while adc.getAfstand(kanaal) <= 12:
motor.stopMotor()
time.sleep(1)
time.sleep(tijd)
motor.stopMotor()
motor.turnRightNinety()
motor.stopMotor()
while not zoeklijn():
while adc.getAfstand(kanaal) <= 12:
motor.stopMotor()
time.sleep(1)
motor.rightmotorspeed(93)
motor.leftmotorspeed(30)
time.sleep(0.2)
motor.rightmotorspeed(75)
motor.leftmotorspeed(90)
time.sleep(0.2)
def oversteken(kanaal=0):
print('vooruit over stopstreep')
motor.rightmotorspeed(90)
motor.leftmotorspeed(82)
time.sleep(3)
print('uit zoeklijn')
while not zoeklijn():
while adc.getAfstand(kanaal) <= 12:
print('adc')
print(adc.getAfstand(kanaal))
motor.stopMotor()
time.sleep(1)
motor.rightmotorspeed(90)
motor.leftmotorspeed(82)
def rechtsInslaan(kanaal=0, tijd=1):
global override
override = False
print('rechts inslaan')
rightmotorspeed(90)
leftmotorspeed(82)
time.sleep(2)
time.sleep(tijd)
stopMotor()
turnRightNinety()
stopMotor()
while not zoeklijn() and override == False:
ber5 = server.listen(timeout=0.250)
ber5 = str(ber5)
if ber5.find('start') >= 0:
override = True
break
if adc.getAfstand(kanaal) <= 12:
stopMotor()
time.sleep(1)
else:
rightmotorspeed(93)
leftmotorspeed(30)
time.sleep(0.2)
rightmotorspeed(75)
leftmotorspeed(90)
time.sleep(0.2)
def oversteken(kanaal=0):
while zoeklijn():
motor.forward(30)
while not zoeklijn():
while adc.getAfstand(kanaal) <= 12:
motor.stopMotor()
time.sleep(1)
motor.forward(30)
def linksInslaan(kanaal=0, tijd=3):
while zoeklijn():
motor.forward(30)
motor.forward(30)
while adc.getAfstand(kanaal) <= 12:
motor.stopMotor()
time.sleep(1)
time.sleep(tijd)
motor.stopMotor()
motor.turnLeftNinety()
motor.stopMotor()
while not zoeklijn():
while adc.getAfstand(kanaal) <= 12:
motor.stopMotor()
time.sleep(1)
motor.forward(30)
def oversteken(kanaal=0):
global override
override = False
print('vooruit over stopstreep')
for i in range(6):
rightmotorspeed(90)
leftmotorspeed(80)
time.sleep(1)
ber3 = server.listen(timeout=0.250)
ber3 = str(ber3)
if ber3.find('start') >= 0:
override = True
break
print('op kruispunt')
while not zoeklijn() and override == False:
ber4 = server.listen(timeout=0.250)
ber4 = str(ber4)
if ber4.find('start') >= 0:
override = True
break
if adc.getAfstand(kanaal) <= 12:
print('adc')
print(adc.getAfstand(kanaal))
stopMotor()
time.sleep(2)
else:
rightmotorspeed(95)
leftmotorspeed(30)
time.sleep(0.2)
rightmotorspeed(75)
leftmotorspeed(90)
time.sleep(0.2)
def main():
# override is en blijft False totdat er een bericht 'start' binnenkomt uit de manuele override
override = False
kruispuntnr = 1
einde = False # True waarde nog te implementeren
GPIO.setmode(GPIO.BOARD)
# SPI-object maken voor afstandssensor
CE = 0 # Eerste kanaal op ADC selecteren
spi = spidev.SpiDev(0, CE)
spi.max_speed_hz = 1000000
kanaal = 0 # Eerste analoge signaal
# Kleurensensor initialiseren
kleurensensor = Adafruit_TCS34725.TCS34725()
# Server initialiseren
server = Server()
last_error = 0 # Nodig voor de eerst keer volglijn uit te voeren
while not einde:
bericht = server.listen(timeout=0.250)
print(bericht)
mesg = str(bericht)
if mesg.find('start') >= 0:
kruispunt_reserve = kruispuntnr
override = True
# Manuele override
while override:
msg = str(server.listen(timeout=0.2))
print(msg)
if msg.find('vooruit') >= 0:
forward()
server.send('Vooruit.')
elif msg.find('achteruit') >= 0:
backwards()
server.send('Achteruit.')
elif msg.find('links') >= 0:
turnLeft()
server.send('Links.')
elif msg.find('rechts') >= 0:
turnRight()
server.send('Rechts.')
elif msg.find('stop') >= 0:
kruispunt_manueel = 0
print(msg[-3])
print(msg[-2])
try:
cijfer = int(msg[-3])
kruispunt_manueel += cijfer * 10
except:
pass
try:
cijfer = int(msg[-2])
kruispunt_manueel += cijfer
except:
pass
if kruispunt_manueel != 0:
kruispuntnr = kruispunt_manueel
else:
kruispuntnr = kruispunt_reserve
finaalBericht = str(kruispuntnr)
server.send('Kruispunt = ' + finaalBericht)
override = False
else:
server.send(str(kruispuntnr))
stopMotor()
# Volg de lijn 20 keer
for i in range(9):
#print("lijnvolg")
returnwaarde = lijninterpretatie()
if returnwaarde == "stopstreep":
print("kruispunt")
stopMotor()
time.sleep(0.5)
print(kruispuntnr)
kruispunt(kruispuntnr)
kruispuntnr += 1
server.send(str(kruispuntnr))
if kruispuntnr >= 26:
einde = True
break
last_error = 0 # Herinitialisatie van lijnvolgalgoritme
else:
volglijn(returnwaarde)
# Na 20 maal lijn te volgen, check de sensoren
if adc.getAfstand(kanaal) < 15:
print("afstandssensor")
stopMotor()
while adc.getAfstand(kanaal) < 20:
print('nog steeds')
pass
raise Exception("Fout in de code.")
