diff --git a/Color_Sorting/Program/Python_Obstical_sorter.py b/Color_Sorting/Program/Python_Obstical_sorter.py deleted file mode 100644 index a6ab6c7..0000000 --- a/Color_Sorting/Program/Python_Obstical_sorter.py +++ /dev/null @@ -1,203 +0,0 @@ -#!/usr/bin/env python3 - -from time import sleep - -from ev3dev2.motor import Motor, MoveTank, SpeedPercent, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D -from ev3dev2.sensor import INPUT_2,INPUT_3,INPUT_4 -from ev3dev2.sensor.lego import ColorSensor, UltrasonicSensor, GyroSensor -from ev3dev2.led import Leds -from pixycamev3.pixy2 import Pixy2 - -# Define all inputs and outputs of the Robot -# Camera and sensors are inputs -us = UltrasonicSensor(INPUT_2) -cs = ColorSensor(INPUT_3) -gyro = GyroSensor(INPUT_4) -pixy2 = Pixy2(port=1, i2c_address=0x54) -# Motors are outputs -m_left = Motor(OUTPUT_A) -m_right = Motor(OUTPUT_D) -tank = MoveTank(OUTPUT_A, OUTPUT_D) # Also define drive tank -leds = Leds() - -# The motor is initialized to run counter-clockwise (gegen den Uhrzeigersinn) -m_right.polarity = 'inversed' # use 'normal' to initialize it to run clockwise (im Uhrzeigersinn) -m_left.polarity = 'inversed' - -# Defined inputs for the given experiment / enviroment -sig_let = 1 # Signatur of orange dice -sig_sort = 2 # Signatur of green 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 -last_state = 1 # Variable for saving last state -i = 0 # conrol variable -gyro.reset() # reset gyro sensor - - -# Define class/state search -def search (): - global state - global last_state - global gyro - j=0 # control variable - # turn slowly aroud on place and search for blocks, - # if nothing is found change searchplace - leds.set_color("LEFT", "BLACK") - leds.set_color("RIGHT", "BLACK") - # turn on place to the right - m_right.on(SpeedPercent(-slow)) - m_left.on(SpeedPercent(slow)) - # search for blocks - 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_sort > 0: # when block to sort is found then change to get - tank.stop() - last_state = state - state = 2 - 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 > line and j <= 5: - tank.on(fast,fast) - sleep(0.2) - j+=1 - if cs.reflected_light_intensity <= line : - tank.stop() - last_state = state - state = 5 - gyro.reset() - j=0 # Reset control variable - - -# Define class/state get -def get (): - global state - global last_state - global i - # drive twoards block and get it into arms - leds.set_color("LEFT", "GREEN") - leds.set_color("RIGHT", "BLACK") - # drive straight ahead - tank.on(fast,fast) - # 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 - i +=1 - if 4 <= i < 6: # camera has not found block for 4 cycles - tank.on_for_seconds(-fast,-fast,0.4) # drive backwards max. 2 times - elif 6 == i: # if block is not fond drive a little bit to left - m_right.on(SpeedPercent(slow)) - m_left.on(SpeedPercent(-slow)) - sleep(0.2) - elif i > 6: # when block is still not found go back to search - i = 0 - state = 1 - gyro.reset() - # Decide what robot should do with the gathered information - if us.distance_centimeters <= 12: # block is in the arms of robot change to sort - last_state = state - state = 3 # when block distance is small switch state - else: # correct drive direction to get block into arms - # when block on left side drive more left - if x < 110: # turn left - m_right.on(SpeedPercent(slow)) - m_left.on(SpeedPercent(-slow)) - # when block on right side drive more right - elif x > 150: # turn right - m_right.on(SpeedPercent(-slow)) - m_left.on(SpeedPercent(slow)) - 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 - - -# Define class/state sort -def sort (): - global state - global last_state - # block is caught, drive to the edge - leds.set_color("LEFT", "GREEN") - leds.set_color("RIGHT", "GREEN") - # drive straight ahead - tank.on(fast,fast) - # 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_let > 0: - last_state = state - state = 4 - -# Define class/state avoid -def avoid (): - global state - global last_state - # found green block, do not want to kick it out - leds.set_color("LEFT", "ORANGE") - leds.set_color("RIGHT", "ORANGE") - # search for green block again - 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 - if 100 < x < 150: - m_right.on(SpeedPercent(-fast)) - m_left.on(SpeedPercent(fast)) - sleep(0.2) - state = last_state - last_state = 4 - -# Define class/state edge -def edge (): - global state - global last_state - global gyro - # robot reached outerline and should turn around - leds.set_color("LEFT", "BLACK") - leds.set_color("RIGHT", "RED") - gyro.reset() # reset gyro sensor - # drive straight backwards - tank.on_for_rotations(-fast,-fast,2) - while gyro.angle < 100: #turn around for 100 degrees - m_right.on(SpeedPercent(-fast)) - m_left.on(SpeedPercent(fast)) - state = 1 - gyro.reset() # reset gyro sensor again - - - -print('start') -while True: # endless-loop for switching states - - state_switch= { # dictionary of the states/classes - 1: search, - 2: get, - 3: sort, - 4: avoid, - 5: edge - } - - func = state_switch.get(state) # function that gets state - func() # activate current state - sleep(0.3) # wait for 0.3 seconds between the different states \ No newline at end of file