def main(BP):
#Set Start- and endpoint
start = []
end = []
stop = follower.line_follower(BP,120,1,0.001,[2])
#Follow the calculated, imaginary line
#Calculate the starting angle
angle = atan((end[1]-start[1])/(end[0]-start[0]))
angle_deg = angle*180/pi
print('Starting Angle: ' + str(angle_deg))
#Calculate the distance to the endpoint
distance = sqrt((end[1]-start[1])**2 + (end[0]-start[0])**2)
print('Distance: ' + str(distance))
#Turn into the right direction
functions.turn(BP,-angle_deg)
#Set encoder and startposition
enc_right_prev = BP.get_motor_encoder(BP.PORT_A)
enc_left_prev = BP.get_motor_encoder(BP.PORT_D)
x = start[0]
y = start[1]
#While the endpoint is not reached -> Drive
endpoint = False
BP.set_motor_dps(BP.PORT_A, 300)
BP.set_motor_dps(BP.PORT_D, 300)
while endpoint is False:
time.sleep(0.1)
try:
# ... your code for recognize obstacle...
except brickpi3.SensorError as error:
print (error)
#ODOMETRY#
#Read encoders
#Calculate the distance
#Odometry for straight movement
#Set variables for the next iteration
#Check, if endpoint is reached
#Error of 5mm is allowed
print('Made it! Coordinates: ' + str(x) + ' ' + str(y))
def find_new_path(BP, shortestWay, klt_node):
#Drive forward to the center of the node and switch sensor mode
#BP.set_sensor_type(BP.PORT_3, BP.SENSOR_TYPE.EV3_COLOR_COLOR)
functions.driveforward_mm(BP, 210)
klt_color = 3 #3 = "Green"
nextColor = functions.find_pathcolor(shortestWay.pop(0))
print("nextColor: ", nextColor)
#Build a loop to find the new path and drop the klt first, if required
################
#Your code here#
################
while True:
functions.turn(BP, -4)
#time.sleep(0.05)
color = colorCheck(BP)
blackVal = BP.get_sensor(BP.PORT_3)
print("Schwarzwert:", blackVal)
if nextColor == 3: #Bis zum gruenen Drehen
if color == nextColor and blackVal < 30:
print("Stelle Klt ab...")
functions.drop_klt(BP)
nextColor = functions.find_pathcolor(shortestWay.pop(0))
continue
else:
if color == nextColor and blackVal < 30:
print("Weg gefunden")
break
#Reset sensor and drive forward to follow the correct outgoing path
#BP.set_sensor_type(BP.PORT_3, BP.SENSOR_TYPE.EV3_COLOR_REFLECTED)
functions.driveforward_mm(BP, functions.marker_offset)
def main(BP):
dist_x = 1060
dist_y = 1000
x = math.sqrt(dist_x**2 + dist_y**2)
alpha = math.atan(dist_x / dist_y) / math.pi * 180 * 0.97
follower.line_follower(BP, 250, 7, 0.01, [2])
functions.driveforward_mm(BP, 120)
functions.turn(BP, -alpha)
time.sleep(1)
fahren_bis(BP, x)
def avoid_obst(BP):
#Turn around so that the us-sensor on the side is facing the obstacle
functions.turn(BP, -80)
#Drive forward, until the distance to the obstacle is smaller than 15cm
while BP.get_sensor(BP.PORT_2) < 15:
BP.set_motor_dps(BP.PORT_A, 300)
BP.set_motor_dps(BP.PORT_D, 300)
#Start distance follower
stop = follower.distance_follower(BP, 400, 8, 0.01)
print(stop)
#If the line is reached again -> Start linefollower
if stop is "Black":
print('Line found')
#Turn, so that the robot faces the right direction again
functions.driveforward_mm(BP, 50)
functions.turn(BP, -90)
def main(BP):
beladen = False
barcode = ""
while not barcode:
barcode = functions.barcodescanner()
print("Barcode:",barcode)
stop = follower.line_follower(BP,400,7,0.01,[2])
while True:
if stop == "Blue" and not beladen:
print("Blau gefunden")
functions.driveforward_mm(BP, functions.marker_offset)
functions.turn(BP, 90)
if barcode == functions.barcodescanner():
print("Ladevorgang")
functions.lift_klt(BP)
beladen = True
functions.turn(BP, -90)
stop = follower.line_follower(BP,400,7,0.01,[5])
continue
print("Falscher KLT")
functions.turn(BP, -90)
stop = follower.line_follower(BP,400,7,0.01,[2])
if stop == "Red":
print("Fertig")
break
def main(BP, target_form):
beladen = True
stop = follower.line_follower(BP, 400, 7, 0.01, [2])
while True:
if stop == "Blue" and beladen:
print("Blau gefunden")
functions.driveforward_mm(BP, functions.marker_offset)
functions.turn(BP, 90)
if functions.shape_detection(target_form):
print("Ladevorgang")
functions.drop_klt(BP)
beladen = False
functions.turn(BP, -90)
stop = follower.line_follower(BP, 250, 7, 0.01, [5])
continue
print("Falscher Abladeort")
functions.turn(BP, -90)
stop = follower.line_follower(BP, 400, 7, 0.01, [2])
if stop == "Red":
print("Fertig")
break
else:
return False
setName("OG_Loc")
print("Started")
functions.moveUntilBlocked(1, 800)
blocked = True
stepTime = 0.5
timeOffCourse = 0
while (blocked == True):
functions.turn(90)
functions.moveForward(1, stepTime)
functions.turn(-90)
timeOffCourse += stepTime
printSensorVals()
if (noObstacle()):
functions.turn(-45)
printSensorVals()
if (noObstacle()):
blocked = False
functions.turn(45)
functions.moveForward(1, 2)
from myro import *
import functions
print("Started")
def blocked():
return (getObstacle()[0] > 1000)
functions.moveUntilBlocked(0.4, 1000)
r = 87
functions.turn(88)
slopeUp = False
if(blocked() == True):
slopeUp = True
turnTolerance = 900
max = 20
sidesTraversed = 0
actions = 0
while(sidesTraversed != 3):
if(sidesTraversed != 2):
if(slopeUp == True):
clearanceTest = functions.limitedMoveUntilBlocked(0.4, turnTolerance, 20)
from myro import *
import functions
print("Started")
def blocked():
return (getObstacle()[0] > 1000)
functions.moveUntilBlocked(0.4, 1000)
r = 87
functions.turn(88)
slopeUp = False
if (blocked() == True):
slopeUp = True
turnTolerance = 900
max = 20
sidesTraversed = 0
actions = 0
while (sidesTraversed != 3):
if (sidesTraversed != 2):
BP.set_motor_limits(BP.PORT_B, dps=400)
BP.set_sensor_type(BP.PORT_1, BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
BP.set_sensor_type(BP.PORT_2, BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
BP.set_sensor_type(BP.PORT_3, BP.SENSOR_TYPE.EV3_COLOR_REFLECTED)
BP.set_sensor_type(BP.PORT_4, BP.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(3)
target_form = 'Circle' #Init for Task7
# Main loop - start from anywhere!
try:
while True:
stop = follower.line_follower(BP, 150, 7, 0.01, [5])
# Follow the line to the next red marker
if stop == 'Red':
task_number = 0
functions.turn(BP, -90)
print('searching for task number')
# Checking task number
while task_number == 0:
task_number = int(functions.barcodescanner())
time.sleep(0.01)
print("recognized task: " + str(task_number))
functions.turn(BP, 90)
#Ensures that only for valid tasks marker is passed
for i in range(1, 8, 1):
if task_number == i:
functions.driveforward_mm(BP, functions.marker_offset)
# Jumping to task
if task_number == 1:
return True
else:
return False
setName("OG_Loc")
print("Started")
functions.moveUntilBlocked(1, 800)
blocked = True
stepTime = 0.5
timeOffCourse = 0;
while(blocked == True):
functions.turn(90)
functions.moveForward(1, stepTime)
functions.turn(-90)
timeOffCourse += stepTime
printSensorVals()
if(noObstacle()):
functions.turn(-45)
printSensorVals()
if(noObstacle()):
blocked = False
functions.turn(45)
functions.moveForward(1, 2)
def turn(self, d):
new_dir = turn(self.direction, d)
self.change_direction(new_dir)
while (1):
if (gui.startFlg == 3): break
print(1)
classes.ser1.write('1\n'.encode())
for i in a:
globals()['p{}'.format(i)] = classes.player(gui.startMoney, 1, i)
functions.pList.append(globals()['p{}'.format(i)])
if (gui.timeoutTurn == 0): gui.timeoutTurn = 500
for i in range(1, gui.timeoutTurn):
for a in functions.pList:
functions.turn(a)
if (len(functions.pList) == 1):
classes.ser1.write("2{}000000".format(
functions.pList[0].number).encode())
break
if ((len(functions.pList) > 1) and (gui.poormanFlg == 0)):
for a in functions.pList:
for b in a.estList:
a.mon += functions.calPrice(b)
d = functions.pList[0]
for a in functions.pList:
if (d.mon < a.mon):
d = a
print("{}P가 승리하였습니다.".format(d.number))
classes.ser1.write("2{}000000\n".format(d.number).encode())
from myro import *
import functions
tolerance = 300
def blocked():
return (getObstacle()[0] >= tolerance)
print("Started")
functions.moveUntilBlocked(0.2, 500) #move forward until an obstacle sensor reaches 500
while(blocked()):
functions.turn(10) #the robot will keep turning right until it becomes parallel with the box
functions.turn(50) #an additional little turn to avoid collisions with the box; now the robot should be parallel with the box's side
timeOffCourse = 0
i = 0
for i in range(0, 2): #go through the 'turn left when safe to do so' process twice
turnBlocked = True #assume turn is blocked
while(turnBlocked):
if(i == 0):
timeOffCourse += 1 #if going laterally, add time spent going the other way later
functions.moveForward(0.7, 1) #move forward slightly
functions.turn(-90) #turn left
