Delete Old Program
This commit is contained in:
parent
5dec438d78
commit
6f21bb3d03
@ -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
|
|
||||||
Loading…
Reference in New Issue
Block a user