diff --git a/Color_Sorting/Program/Python_Obstical_sorter.py b/Color_Sorting/Program/Python_Obstical_sorter.py index 1d7a8f6..d6ad722 100644 --- a/Color_Sorting/Program/Python_Obstical_sorter.py +++ b/Color_Sorting/Program/Python_Obstical_sorter.py @@ -25,10 +25,11 @@ m_right.polarity = 'inversed' # use 'normal' to initialize it to run clockwise m_left.polarity = 'inversed' # Defined inputs for the given experiment / enviroment -orange = 1 # Signatur of orange dice -green = 2 # Signatur of green dice +sig_sort = 2 # Signatur of green dice +sig_let = 1 # Signatur of orange dice fast = 30 # driving speed slow = 10 # searching speed +line = 10 # min. reflection of outline # initalization state defined state = 1 # Initiate search state for beginning @@ -51,28 +52,28 @@ def search (): m_right.on(SpeedPercent(-slow)) m_left.on(SpeedPercent(slow)) # search for blocks - nr_orange, o_block = pixy2.get_blocks(orange, 2) - nr_green, g_block = pixy2.get_blocks(green, 2) - if cs.reflected_light_intensity < 15: # if outline is reached change state to edge + nr_sort, sort_block = pixy2.get_blocks(sig_sort, 2) + nr_let, let_block = pixy2.get_blocks(sig_let, 2) + if cs.reflected_light_intensity < line: # if outline is reached change state to edge tank.stop() last_state = state state = 5 elif us.distance_centimeters < 10: # if a block is currently in the arms change to sort last_state = state state = 3 - elif nr_orange > 0: # when orange block is found then change to get + elif nr_sort > 0: # when block to sort is found then change to get tank.stop() last_state = state state = 2 - elif nr_green > 0: # when green block is found then change to avoid + elif nr_let > 0: # when block to let be is found then change to avoid last_state = state state = 4 elif gyro.angle > 720: # when the robot does not find a block drive a little bit forward - while cs.reflected_light_intensity > 8 and j <= 5: + while cs.reflected_light_intensity > line and j <= 5: tank.on(fast,fast) sleep(0.2) j+=1 - if cs.reflected_light_intensity <= 8 : + if cs.reflected_light_intensity <= line : tank.stop() last_state = state state = 5 @@ -86,14 +87,14 @@ def get (): global last_state global i # drive twoards block and get it into arms - leds.set_color("LEFT", "ORANGE") + leds.set_color("LEFT", "GREEN") leds.set_color("RIGHT", "BLACK") # drive straight ahead tank.on(fast,fast) - # find the orange block again - nr_orange, o_block = pixy2.get_blocks(orange, 2) - if nr_orange > 0: # if block is found again get position - x = o_block[0].x_center #get where in the field of vision the block lays + # find the block for sorting again + nr_sort, sort_block = pixy2.get_blocks(sig_sort, 2) + if nr_sort > 0: # if block is found again get position + x = sort_block[0].x_center #get where in the field of vision the block lays i= 0 else: # when block is not found, virtually place the robot in the middle of the vision x = 130 @@ -121,7 +122,7 @@ def get (): elif x > 150: # turn right m_right.on(SpeedPercent(-slow)) m_left.on(SpeedPercent(slow)) - if cs.reflected_light_intensity < 15: # robot detected line change to edge + if cs.reflected_light_intensity < line: # robot detected line change to edge tank.stop() last_state = state state = 5 # When colorsensor detects line then switch state @@ -132,17 +133,17 @@ def sort (): global state global last_state # block is caught, drive to the edge - leds.set_color("LEFT", "ORANGE") - leds.set_color("RIGHT", "ORANGE") + leds.set_color("LEFT", "GREEN") + leds.set_color("RIGHT", "GREEN") # drive straight ahead tank.on(fast,fast) - # search for green blocks - nr_green, g_block = pixy2.get_blocks(green, 2) - if cs.reflected_light_intensity < 15: # when reached line change to edge + # search for block to let be in area + nr_let, let_block = pixy2.get_blocks(sig_let, 2) + if cs.reflected_light_intensity < line: # when reached line change to edge tank.stop() last_state = state state = 5 - elif nr_green > 0: + elif nr_let > 0: last_state = state state = 4 @@ -151,12 +152,12 @@ def avoid (): global state global last_state # found green block, do not want to kick it out - leds.set_color("LEFT", "GREEN") - leds.set_color("RIGHT", "GREEN") + leds.set_color("LEFT", "ORANGE") + leds.set_color("RIGHT", "ORANGE") # search for green block again - nr_green, g_block = pixy2.get_blocks(green, 2) - if nr_green > 0: # if block is found again get position - x = g_block[0].x_center # get where in the field of vision the block lays + nr_let, let_block = pixy2.get_blocks(sig_let, 2) + if nr_let > 0: # if block is found again get position + x = let_block[0].x_center # get where in the field of vision the block lays else: # when block is not found, virtually place the robot in the middle of the vision x = 130 # When block lays in middle of the vision drive around it