Author: Ben van Eekelen Language: python
Description: Code voor het hanteren van het lijnvolg principe Timestamp: 2017-02-02 17:34:18 +0000
View raw paste Reply
  1. # Naast het importeren van de time
  2. # importeer ik hier ook mijn custom code
  3. # zoals de 'bengine'
  4. import time, threading
  5. from basecode import lineFollow
  6. from ben import bengine, light
  7.  
  8. # Dit wordt gebruikt om aan te duiden dat het
  9. # pad is teruggevonden en dat de voorwaartse beweging
  10. # kan worden voortgezet
  11. retrievedPath = 0
  12.  
  13.  
  14. # De zoekfunctie
  15. def seekMove(jumpCycle, timeTurn):
  16.  
  17.     global retrievedPath
  18.  
  19.     # De eerste draai
  20.     print("Right")
  21.     bengine.rightF(jumpCycle)
  22.     bengine.leftB(jumpCycle)
  23.  
  24.     for i in range(timeTurn):
  25.         time.sleep(0.1)
  26.         if lineFollow.follow_line() is True:
  27.             print('Right: %d out of %d' % (i, timeTurn))
  28.             bengine.halt()
  29.             retrievedPath = 1
  30.             return
  31.  
  32.     bengine.halt()
  33.  
  34.     # korte pauze
  35.     time.sleep(0.01)
  36.  
  37.  
  38.     # De tweede draai
  39.     print("Left")
  40.     bengine.leftF(jumpCycle)
  41.     bengine.rightB(jumpCycle)
  42.  
  43.     for i in range(timeTurn * 2):
  44.         time.sleep(0.1)
  45.         if lineFollow.follow_line() is True:
  46.             print('Right: %d out of %d' % (i, timeTurn))
  47.             bengine.halt()
  48.             retrievedPath = 1
  49.             return
  50.  
  51.     bengine.halt()
  52.  
  53.     # korte pauze
  54.     time.sleep(0.01)
  55.  
  56.  
  57.     # De derde draai terug naar de originele richting
  58.     print("Right")
  59.     bengine.rightF(jumpCycle)
  60.     bengine.leftB(jumpCycle)
  61.  
  62.     for i in range(timeTurn):
  63.         time.sleep(0.1)
  64.         if lineFollow.follow_line() is True:
  65.             print('Right: %d out of %d' % (i, timeTurn))
  66.             bengine.halt()
  67.             retrievedPath = 1
  68.             return
  69.  
  70.     bengine.halt()
  71.  
  72.  
  73. # Deze functie wordt aangeroepen wanneer de lijn kwijt is
  74. def search():
  75.     global retrievedPath
  76.  
  77.     # De duty-cycle van de pwm
  78.     jumpCycle = 30
  79.     # De minimale tijd die gedraaid wordt
  80.     # gedeeld door 10
  81.     timeTurn = 2
  82.  
  83.     # Aanroepen van de eerste rechts-links-rechts beweging
  84.     # Deze duurt 2x de basistijd
  85.     print('\nseekmove @ 2x')
  86.     seekMove(jumpCycle, timeTurn * 2)
  87.     if retrievedPath == 1:
  88.         retrievedPath = 0
  89.         return
  90.  
  91.     # Aanroepen van de tweede rechts-links-rechts functie
  92.     # Ditmaal duurt deze 4x de basistijd
  93.     print('\nseekmove @ 4x')
  94.     seekMove(jumpCycle, timeTurn * 4)
  95.     if retrievedPath == 1:
  96.         retrievedPath = 0
  97.         return
  98.  
  99.     # Aanroepen van de laatste rechts-links-rechts functie
  100.     # Ditmaal duurt deze 8x de basistijd
  101.     print('\nseekmove @ 8x')
  102.     seekMove(jumpCycle, timeTurn * 8)
  103.     if retrievedPath == 1:
  104.         retrievedPath = 0
  105.         return
  106.  
  107.  
  108. def main():
  109.  
  110.     # Roep de 'init' functie van lineFollow aan, lineFollow is mijn
  111.     # code die verantwoordelijk is voor het volgen van de lijn
  112.     lineFollow.init()
  113.  
  114.     # Roep de 'init' functie in de bengine aan, bengine is mijn code
  115.     # die verantwoordelijk is voor het aansturen van de motoren
  116.     bengine.init()
  117.  
  118.     # Het try-catch block zorgt ervoor dat de alle GPIO pinnen netjes
  119.     # worden uitgezet na het stoppen van de scripts
  120.     try:
  121.         while True:
  122.             # Zolang de robot zich op de lijn bevindt beweegt de robot
  123.             # zich voorwaarts met de 'bengine.forward' functie
  124.             if lineFollow.follow_line() is True:
  125.                 print("Op lijn")
  126.                 bengine.forward(35)
  127.  
  128.             # Zodra de robot van de lijn af is wordt de 'search' functie
  129.             # aangeroepen
  130.             else:
  131.                 print("Lijn kwijt")
  132.                 time.sleep(0.01)
  133.  
  134.                 bengine.halt()
  135.                 search()
  136.  
  137.                 #break
  138.  
  139.             time.sleep(0.2)
  140.  
  141.     # Bij een ctrl-c worden de GPIO pinnen
  142.     # schoongemaakt in de 'linefollow.end' functie
  143.     except KeyboardInterrupt:
  144.         lineFollow.end()
  145.  
  146. # Zorgen dat de 'main' functie wordt gestart
  147. # bij het starten van het script
  148. if __name__ == '__main__':
  149.     main()
  150.  
View raw paste Reply