Advertisement
Nawor3565

Final Robot Code

May 12th, 2022
752
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env pybricks-micropython
  2. from pybricks.hubs import EV3Brick
  3. from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
  4.                                  InfraredSensor, UltrasonicSensor, GyroSensor)
  5. from pybricks.nxtdevices import (LightSensor)
  6. from pybricks.nxtdevices import ColorSensor as NXTColorSensor
  7. #from pybricks import nxtdevices.ColorSensor
  8. from pybricks.parameters import Port, Stop, Direction, Button, Color
  9. from pybricks.tools import wait, StopWatch, DataLog
  10. from pybricks.robotics import DriveBase
  11. from pybricks.media.ev3dev import SoundFile, ImageFile
  12. from _thread import start_new_thread
  13.  
  14.  
  15. # This program requires LEGO EV3 MicroPython v2.0 or higher.
  16. # Click "Open user guide" on the EV3 extension tab for more information.
  17.  
  18.  
  19. # Create your objects here.
  20. ev3 = EV3Brick()
  21. #speaker_lock = allocate_lock()
  22. right_motor = Motor(Port.B)
  23. left_motor = Motor(Port.C)
  24. lift_motor = Motor(Port.A)
  25. claw = Motor(Port.D)
  26. light_sensor = LightSensor(Port.S1)
  27. color_sensor = ColorSensor(Port.S2) # Faces fish
  28. color_sensor_two = NXTColorSensor(Port.S3) # Faces down
  29. wheel_diameter = 43.3
  30. axle_track = 130
  31.  
  32. POND_PROPORTIONAL_GAIN = 4#3.4
  33. LINE_PROPORTIONAL_GAIN = 1
  34. DRIVE_SPEED = 110#75
  35. #RAD_TO_DEG = 57.29578
  36. HIGH_READING = 53
  37. LOW_READING = 42
  38. line_midpoint = (HIGH_READING + LOW_READING)/2
  39. pond_midpoint = 43#50
  40.  
  41. driver = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
  42. driver.settings(800, 200, 60, 1200)
  43. fish_collected = 0
  44. amount_per_pond = 4
  45. line_counter = 0
  46. previous_state = False
  47. # Write your program here.
  48. ev3.speaker.beep()
  49. wait(300)
  50.  
  51. #def locked_sound(filename):
  52. #    with speaker_lock:
  53. #        ev3.speaker.play_file(filename)
  54. #def background_sound(filename):
  55. #    start_new_thread(locked_sound, (filename))
  56.  
  57. def print_on_screen(text):
  58.     ev3.screen.clear()
  59.     ev3.screen.print(str(text))
  60.  
  61. def down_color_percent(color_to_read, return_proportional):
  62.     # Reads the RBG value from the downwards-facing color sensor
  63.     color_reading = color_sensor_two.rgb()
  64.    
  65.     # Separates the tuple into three variables
  66.     read_red_percent = color_reading[0]
  67.     read_green_percent = color_reading[1]
  68.     read_blue_percent = color_reading[2]
  69.  
  70.     # Calculates each color's proportion of the total light returned
  71.     total_color = read_red_percent + read_green_percent + read_blue_percent
  72.     if total_color == 0:
  73.         red_of_total = 0
  74.         green_of_total = 0
  75.         blue_of_total = 0
  76.     else:
  77.         red_of_total = read_red_percent/total_color
  78.         green_of_total = read_green_percent/total_color
  79.         blue_of_total = read_blue_percent/total_color
  80.    
  81.     # Returns the requested value
  82.     if return_proportional:
  83.         if color_to_read == "red":
  84.             return 100*red_of_total
  85.         if color_to_read == "green":
  86.             return 100*green_of_total
  87.         if color_to_read == "blue":
  88.             return 100*blue_of_total
  89.     else:
  90.         if color_to_read == "red":
  91.             return read_red_percent
  92.         if color_to_read == "green":
  93.             return read_green_percent
  94.         if color_to_read == "blue":
  95.             return read_blue_percent
  96.  
  97. def line_counter_check(tape_limit):
  98.     global line_counter
  99.     global previous_state
  100.     # Fucky thing to only set previous_state to False the first time this runs
  101.     #try:
  102.     #    previous_state
  103.     #except NameError:
  104.     #    previous_state = False
  105.     #    line_counter = 0
  106.  
  107.     # Prints the line counter to the screen
  108.     print_on_screen(line_counter)
  109.  
  110.     # Reads value from light sensor, returns integer on scale from 1-100
  111.     line_reading = light_sensor.reflection()
  112.    
  113.     # If the measurement is lower than 35, it must be over a line of tape
  114.     if line_reading < 40:
  115.         over_line = True
  116.     else:
  117.         over_line = False
  118.    
  119.     # If the current state is  different from the last state, so it's only run when something actually changes
  120.     if previous_state != over_line:
  121.         # Only increment the line counter when the over_line variable changes from False to True
  122.         if over_line:
  123.             line_counter = line_counter + 1
  124.        
  125.         # This is used to keep track of what the last reading's result was
  126.         previous_state = over_line
  127.     if line_counter < tape_limit:
  128.         return True
  129.     else:
  130.         line_counter = 0
  131.         return False
  132.  
  133. # function for collecting Color.GREEN fish
  134. def green_fish(backup_dist, forward_dist, turn_degrees):
  135.     # Stops any motion that's in progress
  136.     driver.stop()
  137.  
  138.     # Drive back so axis of rotation lines up with fish, then rotate 90 degrees and drive forward a bit
  139.     driver.straight(0-backup_dist)
  140.     driver.turn(turn_degrees)
  141.     driver.straight(forward_dist)
  142.  
  143.     # Reset angles
  144.     claw.reset_angle(0)
  145.     lift_motor.reset_angle(0)
  146.  
  147.     # Open claw
  148.     #claw.track_target(95)
  149.     wait(200)
  150.     claw.hold()
  151.     #wait(1000)
  152.  
  153.     # Lower tray
  154.     lift_motor.run_angle(100, -145, then=Stop.HOLD, wait=True)
  155.     #wait(1000)
  156.  
  157.     # Close claw
  158.     claw.track_target(-110)
  159.     wait(400)
  160.     claw.hold()
  161.     #wait(1000)
  162.  
  163.     # Lift tray
  164.     #ev3.speaker.play_file("Lift load.wav")
  165.     lift_motor.run_angle(100, 145, then=Stop.COAST, wait=True)
  166.     #wait(200)
  167.     claw.track_target(0)
  168.     #wait(200)
  169.     #lift_motor.run_angle(300, 40, then=Stop.HOLD, wait=True)
  170.    
  171.     #background_sound("new_audio_loud.wav")
  172.     ev3.speaker.play_file("new_audio_loud.wav")
  173.  
  174.     # Drive back and rotate to original heading
  175.     driver.straight(0-forward_dist)
  176.     driver.turn(0-turn_degrees)
  177.     driver.straight(backup_dist)
  178.  
  179. def circle_pond(radius, maxlines, pond_size):
  180.     global line_counter
  181.     # Loops while the line counter is under a set amount
  182.     if pond_size == "small":
  183.         driver.stop()
  184.         driver.turn(-65) # rotate left X degrees once locating the small pond
  185.     if pond_size == "large":
  186.         driver.stop()
  187.         driver.turn(-40)
  188.     while line_counter_check(maxlines):
  189.         # calculates a constant turning rate for a given velocity and radius
  190.         #constant_turn_rate = DRIVE_SPEED/(radius + (0.5 * axle_track))
  191.         #const_turn_rate_degrees = RAD_TO_DEG * (constant_turn_rate)
  192.        
  193.         #print_on_screen(driver.angle())
  194.  
  195.         blue = down_color_percent("blue", True)
  196.         if blue < 30:
  197.             blue = 30
  198.         #green = down_color_percent("green", False)
  199.  
  200.         # Checks for green fish
  201.         if color_sensor.color() == Color.GREEN:
  202.             if pond_size == "large":
  203.                 green_fish(40, 30, 96)
  204.             elif pond_size == "medium":
  205.                 if line_counter == 0 or line_counter == 5:
  206.                     green_fish(42.5, 35, 95) #normally 44.5, 35, 95
  207.                 else:
  208.                     green_fish(44.5, 35, 90)
  209.             elif pond_size == "small":
  210.                 green_fish(48, 30, 85)
  211.             print_on_screen("saw fish!")
  212.            
  213.         #line_count= line_counter_check(maxlines)
  214.         #if line_counter_check(maxlines) == False
  215.            # driver.stop()
  216.        
  217.         else:# (blue - green >= 1 ):
  218.                
  219.            
  220.             #turn_rate = (((8 / radius)*(blue - green))*400) -40
  221.            
  222.             #if turn_rate > 90:
  223.              #   turn_rate = 90
  224.            
  225.        
  226.            
  227.             # Calculate the deviation from the midpoint.
  228.             deviation = pond_midpoint - blue
  229.             # Calculate the turn rate.
  230.             turn_rate = (POND_PROPORTIONAL_GAIN * deviation ) #+ const_turn_rate_degrees
  231.  
  232.         # Set the drive base speed and turn rate.
  233.             driver.drive(DRIVE_SPEED, turn_rate)
  234.             #driver.curve(320, 360)
  235.  
  236.         # You can wait for a short time or do other things in this loop.
  237.         wait(30)
  238.    
  239.     # When the line counter reaches that amount, stop all motion
  240.     driver.stop()
  241.  
  242. def drive_to_next_pond(two_large, is_small):
  243.     #print_on_screen("to next pond...")
  244.     # catches the loop until the color sensor is off the initial pond
  245.     #down_red = down_color_percent("red", True)
  246.     #while down_red < 12:
  247.      #   driver.drive(DRIVE_SPEED, 0)
  248.       #  down_red = down_color_percent("red", True)
  249.        # wait(3000)
  250.     driver.drive(DRIVE_SPEED, 0)
  251.     wait(1000)
  252.     if two_large:
  253.        
  254.         driver.turn(25)
  255.         driver.drive(DRIVE_SPEED, 0)
  256.         wait(1000)
  257.         #driver.drive(DRIVE_SPEED, 0)
  258.  
  259.     # Drives forward until blue portion is higher than 55%
  260.     down_blue = down_color_percent("blue", True)
  261.     while down_blue < 45:  
  262.         driver.drive(DRIVE_SPEED, 0)
  263.         print_on_screen(down_blue)
  264.  
  265.         down_blue = down_color_percent("blue", True)
  266.         wait(10)
  267.     #while color_sensor_two.color() != Color.BLUE:
  268.         #driver.drive(DRIVE_SPEED, 0)
  269.    
  270.     # When a pond is reached, stop all motion
  271.     if is_small:
  272.         driver.drive(DRIVE_SPEED, 0)
  273.         wait(300)
  274.     driver.stop()
  275.     #driver.drive(DRIVE_SPEED, 0)
  276.     #driver.straight(100, then=Stop.HOLD, wait=True)
  277.  
  278. def follow_line_straight():
  279.      # Calculate the deviation from the threshold.
  280.     deviation = light_sensor.reflection() - line_midpoint
  281.  
  282.  
  283.  
  284.     # Calculate the turn rate.
  285.     turn_rate = LINE_PROPORTIONAL_GAIN * deviation
  286.  
  287.     print_on_screen(deviation)
  288.  
  289.     # Set the drive base speed and turn rate.
  290.     driver.drive(0-(DRIVE_SPEED), turn_rate)
  291.  
  292.     # You can wait for a short time or do other things in this loop.
  293.     wait(10)
  294.    
  295. def return_dock():
  296.     driver.reset()
  297.     print_on_screen("to dock...")
  298.     driver.drive(-(DRIVE_SPEED*2),-2) #-305
  299.     wait(15000)
  300.    # driver.turn(25)
  301.     #driver.straight(-275)
  302.     #driver.turn(-30)
  303.     #while driver.distance() >= -200000000:
  304.     #    follow_line_straight()
  305.    
  306.     driver.stop()
  307.    
  308.    
  309. drive_to_next_pond(False, True)
  310. circle_pond(203.2, 11, "medium")
  311. drive_to_next_pond(False, False)
  312. circle_pond(152.4, 9, "small")
  313. drive_to_next_pond(True, False)
  314. circle_pond(254, 11, "large")
  315. return_dock()
  316.  
  317. #background_sound("smb_world_clear.wav")
  318. ev3.speaker.play_file("smb_world_clear.wav")
Advertisement
Advertisement
Advertisement
RAW Paste Data Copied
Advertisement