parent
d67545c574
commit
384adae17c
203
Color_Sorting/Program/Python_Obstical_sorter.py
Normal file
203
Color_Sorting/Program/Python_Obstical_sorter.py
Normal file
@ -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
|
||||||
Loading…
Reference in New Issue
Block a user