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))
Esempio n. 2
0
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)
Esempio n. 4
0
 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)
Esempio n. 5
0
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
Esempio n. 7
0
    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)
Esempio n. 8
0
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)
Esempio n. 9
0
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):
Esempio n. 10
0
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:
Esempio n. 11
0
        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)
Esempio n. 12
0
 def turn(self, d):
     new_dir = turn(self.direction, d)
     self.change_direction(new_dir)
Esempio n. 13
0
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())
Esempio n. 14
0
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