Delete 'Color_Sorting/Test_Python_Obstical_sorter.py'
This commit is contained in:
parent
ca6e125269
commit
2d92c52e40
@ -1,187 +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
|
||||
# Sensors are inputs
|
||||
us = UltrasonicSensor(INPUT_2)
|
||||
cs = ColorSensor(INPUT_3)
|
||||
gyro = GyroSensor(INPUT_4)
|
||||
gyro.reset()
|
||||
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)
|
||||
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
|
||||
orange = 1 # Signatur of orange dice
|
||||
green = 2 # Signatur of green dice
|
||||
fast = 30 # driving speed
|
||||
slow = 10 # searching speed
|
||||
|
||||
# inital state defined
|
||||
state = 1 # Initiate search state for beginning
|
||||
last_state = 1
|
||||
i = 0 # conrol variable
|
||||
|
||||
|
||||
# Define state search
|
||||
def search ():
|
||||
global state
|
||||
global last_state
|
||||
global gyro
|
||||
j=0 # control variable
|
||||
leds.set_color("LEFT", "BLACK")
|
||||
leds.set_color("RIGHT", "BLACK")
|
||||
# turn slowly aroud on place until blocks are found
|
||||
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
|
||||
last_state = state
|
||||
state = 5
|
||||
elif us.distance_centimeters < 10:
|
||||
last_state = state
|
||||
state = 3
|
||||
elif nr_orange > 0: # when orange block is found then change to get
|
||||
last_state = state
|
||||
state = 2
|
||||
elif nr_green > 0: # when green block 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:
|
||||
tank.on(fast,fast)
|
||||
sleep(0.2)
|
||||
j+=1
|
||||
gyro.reset()
|
||||
j=0
|
||||
|
||||
|
||||
def get (): # Define state get
|
||||
global state
|
||||
global last_state
|
||||
global i
|
||||
# block is found drive twoards it
|
||||
leds.set_color("LEFT", "ORANGE")
|
||||
leds.set_color("RIGHT", "BLACK")
|
||||
tank.on(fast,fast)
|
||||
# find the orange block again
|
||||
nr_orange, o_block = pixy2.get_blocks(orange, 2)
|
||||
if nr_orange > 0: # get where in the field of vision the block lays
|
||||
x = o_block[0].x_center
|
||||
i= 0
|
||||
else:
|
||||
x = 130
|
||||
i +=1
|
||||
if 4 <= i < 6: # when the block is not found, virtually place the robot in the middle of the vision
|
||||
tank.on_for_seconds(-fast,-fast,0.4) # drive backwards if block is not found 3 times in a row
|
||||
elif 6 == i:
|
||||
m_right.on(SpeedPercent(slow))
|
||||
m_left.on(SpeedPercent(-slow))
|
||||
sleep(0.2)
|
||||
elif i > 6:
|
||||
i = 0
|
||||
state = 1
|
||||
gyro.reset()
|
||||
|
||||
# Decide what robot should do with the gathered information
|
||||
if us.distance_centimeters <= 12:
|
||||
last_state = state
|
||||
state = 3 # when block distance is small switch state
|
||||
else:
|
||||
# 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 < 15: # When colorsensor detects line then switch state
|
||||
last_state = state
|
||||
state = 5
|
||||
|
||||
|
||||
def sort (): # Define state sort
|
||||
global state
|
||||
global last_state
|
||||
leds.set_color("LEFT", "ORANGE")
|
||||
leds.set_color("RIGHT", "ORANGE")
|
||||
m_right.on(SpeedPercent(fast))
|
||||
m_left.on(SpeedPercent(fast))
|
||||
nr_green, g_block = pixy2.get_blocks(green, 2)
|
||||
# block is caught, drive to the edge
|
||||
if cs.reflected_light_intensity < 15:
|
||||
last_state = state
|
||||
state = 5
|
||||
elif nr_green > 0:
|
||||
last_state = state
|
||||
state = 4
|
||||
|
||||
|
||||
def avoid ():
|
||||
global state
|
||||
global last_state
|
||||
leds.set_color("LEFT", "GREEN")
|
||||
leds.set_color("RIGHT", "GREEN")
|
||||
# found block but do not want to kick it out
|
||||
nr_green, g_block = pixy2.get_blocks(green, 2)
|
||||
if nr_green > 0:
|
||||
x = g_block[0].x_center
|
||||
else:
|
||||
x = 130
|
||||
# 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
|
||||
|
||||
|
||||
def edge ():
|
||||
global state
|
||||
global last_state
|
||||
global gyro
|
||||
leds.set_color("LEFT", "BLACK")
|
||||
leds.set_color("RIGHT", "RED")
|
||||
gyro.reset()
|
||||
tank.on_for_rotations(-fast,-fast,2) # drive backwards
|
||||
while gyro.angle < 100: #turn around for 180 degrees
|
||||
m_right.on(SpeedPercent(-fast))
|
||||
m_left.on(SpeedPercent(fast))
|
||||
state = 1
|
||||
# reset gyro sensor
|
||||
gyro.reset()
|
||||
|
||||
|
||||
|
||||
print('start')
|
||||
while True: # endless-loop for switching states
|
||||
|
||||
state_switch= { # dictionary of the states
|
||||
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)
|
||||
Loading…
Reference in New Issue
Block a user