From 384adae17c40ed039cfd9559dbcdbaae90e5e4d6 Mon Sep 17 00:00:00 2001 From: Tanja Sukal Date: Tue, 27 Aug 2024 08:33:48 +0000 Subject: [PATCH] revert d67545c574312551525a71e4b3c5c8787f4f2768 revert Delete Old Program --- .../Program/Python_Obstical_sorter.py | 203 ++++++++++++++++++ 1 file changed, 203 insertions(+) create mode 100644 Color_Sorting/Program/Python_Obstical_sorter.py diff --git a/Color_Sorting/Program/Python_Obstical_sorter.py b/Color_Sorting/Program/Python_Obstical_sorter.py new file mode 100644 index 0000000..88b0900 --- /dev/null +++ b/Color_Sorting/Program/Python_Obstical_sorter.py @@ -0,0 +1,203 @@ +#!/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