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