def volglijn(tijdsdatalijst):
global last_error
MINIMUM = CALIBRATEDMINIMUM #nog in te vullen
MAXIMUM = CALIBRATEDMAXIMUM #nog in te vullen
KP = 80 #nog in te vullen
KD = 10 #nog in te vullen
SETPOINTPOSITIE = 3500 # 3*1000*sensor3 + 4*1000*sensor4 /(sensor3 + sensor 4)
LINKSBASISSPEED = 100
RECHTSBASISSPEED = 100
positie = readpositie(tijdsdatalijst, MINIMUM,
MAXIMUM) #de gekalibreerde positiewaarde
print("positie:", positie)
error = (positie - SETPOINTPOSITIE) / 1000.
print("error", error)
#print last_error
correctiespeedlinks = LINKSBASISSPEED + KP * error + KD * (error -
last_error)
correctiespeedrechts = RECHTSBASISSPEED - KP * error + KD * (error -
last_error)
if correctiespeedlinks > 100:
correctiespeedlinks = 100
if correctiespeedrechts < 0:
correctiespeedrechts = 0
if correctiespeedlinks < 0:
correctiespeedlinks = 0
if correctiespeedrechts > 100:
correctiespeedrechts = 100
leftmotorspeed(correctiespeedlinks)
rightmotorspeed(correctiespeedrechts)
last_error = error
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):
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)