Advertisement
wa12rior

Python robot double line detector obstacle path algorithm

Nov 11th, 2020
929
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 2.21 KB | None | 0 0
  1. #!/usr/bin/env python3
  2.  
  3. # Import the necessary libraries
  4. import time
  5. import math
  6. from ev3dev2.motor import *
  7. from ev3dev2.sound import Sound
  8. from ev3dev2.sensor import *
  9. from ev3dev2.sensor.lego import *
  10. from ev3dev2.sensor.virtual import *
  11. from time import sleep
  12.  
  13. # Create the sensors and motors objects
  14. motorA = LargeMotor(OUTPUT_A)
  15. motorB = LargeMotor(OUTPUT_B)
  16. left_motor = motorA
  17. right_motor = motorB
  18. tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
  19. steering_drive = MoveSteering(OUTPUT_A, OUTPUT_B)
  20.  
  21. spkr = Sound()
  22.  
  23. color_sensor_in1 = ColorSensor(INPUT_1)
  24. color_sensor_in2 = ColorSensor(INPUT_2)
  25. ultrasonic_sensor_in3 = UltrasonicSensor(INPUT_3)
  26. gyro_sensor_in4 = GyroSensor(INPUT_4)
  27. gps_sensor_in5 = GPSSensor(INPUT_5)
  28.  
  29. motorC = LargeMotor(OUTPUT_C) # Magnet
  30.  
  31. # Here is where your code starts
  32.  
  33. move_value = 20
  34.  
  35. stop_value = 0
  36. angle = 0
  37. time_spent = 0
  38.  
  39. while True:
  40.     c1 = color_sensor_in1.reflected_light_intensity
  41.     c2 = color_sensor_in2.reflected_light_intensity
  42.    
  43.     if ultrasonic_sensor_in3.distance_centimeters_continuous < 25:
  44.         left_value = stop_value
  45.         right_value = stop_value
  46.  
  47.         # obstacle turn left first time
  48.         temp_angle = gyro_sensor_in4.angle
  49.         while gyro_sensor_in4.angle != temp_angle - 90:
  50.             left_motor.on(0)
  51.             right_motor.on(4)
  52.     else:
  53.         if c1 == c2:
  54.             left_value = move_value
  55.             right_value = move_value
  56.            
  57.             # turn right around obstacle
  58.             if c1 == 100 and c2 == 100:
  59.                 time.sleep(0.8)
  60.                 time_spent += 1
  61.                 temp_angle = gyro_sensor_in4.angle
  62.                 if time_spent >= 3:
  63.                     time_spent = 0
  64.                     while gyro_sensor_in4.angle != temp_angle + 90:
  65.                         left_motor.on(3)
  66.                         right_motor.on(0)
  67.                    
  68.         elif c1 > c2:
  69.             left_value = move_value
  70.             right_value = stop_value
  71.         elif c2 > c1:
  72.             left_value = stop_value
  73.             right_value = move_value
  74.        
  75.         left_motor.on(left_value)
  76.         right_motor.on(right_value)
  77.        
  78.         # Copyright Kamil Serafin 11.11.2020
  79.  
  80.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement