Example #1
0
def left():
    #motor.drive_l(-30)
    #motor.drive_r(100)
    motor.drive_l(-15)
    motor.drive_r(50)
    time.sleep(turningtime_left)
    motor.drive(0)
Example #2
0
def right():
    #motor.drive_l(30)
    #motor.drive_r(-100)
    motor.drive_l(15)
    motor.drive_r(-50)
    time.sleep(turningtime_right)
    motor.drive(0)
Example #3
0
def findandpickberry(cap):
    #Settings
    drivingspeed = 60  #60 or 85 # in %
    pickingspeed = 20000  # speed of the arm
    agg = 4  #4 #aggressitivity for picking # the safer the lower
    supersavedrivingspeed = 10  # in %
    supersave_agg = 0
    #testedsetting:
    # 80,200,5 works
    # 90.2500,3 works
    # 100,3000, 15 works but with overshoots
    # 90,3500,15 works better than 100

    #developersettings
    devmode = 0  #always 0, 1 for output of stream and more information

    if devmode == 1:
        import cv2
        drivingspeed = drivingspeed / 4
        window_handle = cv2.namedWindow('Calibrated camera',
                                        cv2.WINDOW_AUTOSIZE)

    y_old = int(540 - 100)
    x_old = int(720 / 2)
    reachedberry = False
    secondtry = False
    #tic = time.time()
    while True:
        berry_far_away = False
        img = cam.get_calibrated_img(cap)

        if x_old == int(720 / 2) and y_old == int(540 - 100):
            x_old, area, y_old = find.firstImage(img)

        x, area, y = find.berry(img, x_old, y_old)
        #x,area,y = find.berryAI(img)
        #x,area,y = find.berry_old(img)

        if area > 0:
            berry_far_away = True

        area = int(area / 10000)

        if area == 0 and berry_far_away == True:
            area = 1
        berry_far_away = False

        if area > 600:
            area = 600
        if area == 0:  #nothing found
            speedl = 0  #don't move
            speedr = 0
            x = int(720 / 2)
            y = int(540 - 100)

        else:
            speedl, speedr = vel_control.followstrawberry(x, area, x_old)

        if devmode == 1:
            #output image
            cv2.circle(img, (x, y), 5, (255, 255, 255), -1)
            cv2.putText(img, "centroid", (x - 25, y - 25),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
            cv2.imshow('Calibrated camera', img)

        print(
            "area: ", area, " x: ", x, " Y: ", y,
            "  | {:1.0f} %% vel left | {:2.0f} %% vel right ".format(
                speedl, speedr))
        if secondtry == True:
            print("second try:safe mode")

        if secondtry == False:
            speedl = speedl * (drivingspeed / 100)
            speedr = speedr * (drivingspeed / 100)
        else:
            speedl = speedl * (supersavedrivingspeed / 100)
            speedr = speedr * (supersavedrivingspeed / 100)

        x_old = x
        y_old = y

        motor.drive_l(speedl)
        motor.drive_r(speedr)

        if (abs(speedl) <= agg and abs(speedr) <= agg and reachedberry == True
                and area != 0
                and secondtry == False) or (abs(speedl) == supersave_agg
                                            and abs(speedr) == supersave_agg
                                            and reachedberry == True
                                            and area != 0):
            #pick.dummy()
            motor.drive(0)
            pick.berry(pickingspeed)  #safe mode
            y_old = int(540 - 100)
            x_old = int(720 / 2)
            reachedberry = False
            #doublecheck
            time.sleep(
                0.25
            )  #waitng before capturing the next image as this shows if the berry is still there or not
            x_old, area, y_old = find.firstImage(cam.get_calibrated_img(cap))
            if area / 10000 > 200:  #Berrypicking failed
                secondtry = True
                # till next picking:
                # agg = supersave_agg
                # speed = supersavespeed
                print("FAILED picking, trying again")
            else:
                secondtry = False
                print('PICKED berry')
                return True
            #end(cap)
        elif (abs(speedl) <= agg and abs(speedr) <= agg and area != 0
              and secondtry == False) or (abs(speedl) == supersave_agg
                                          and abs(speedr) == supersave_agg
                                          and area != 0):
            #berry.pick()   #fast mode
            reachedberry = True
        else:
            reachedberry = False
Example #4
0
def driveback(cap):
    motor.drive_l(-100)
    motor.drive_r(-100)
    time.sleep(driveback_time)
    safe_right(cap)
    motor.drive(0)
Example #5
0
def halfright():
    motor.drive_l(15)
    motor.drive_r(-50)
    time.sleep(turningtime_right / 2)
    motor.drive(0)
Example #6
0
def drivetomarker(cap, id, desired_distance):
    #Settings
    drivingspeed = 100
    tolerance = 2 / 100  #+- in m

    x_old = int(720 / 2)
    distance = 0
    newlylost = True
    distancereached = False
    distancecorrect = False
    while not distancereached:
        speedl, speedr, distance, x = vel_to_marker(cap, id, desired_distance,
                                                    x_old)
        print("distance to arucomarker:" + str(distance))

        if x == int(720 / 2):  #nothing found
            if newlylost == True:
                timelost = 0
                tic = time.time()
                newlylost = False
            if timelost < 5000:
                toc = time.time()
                timelost = (toc - tic) * 1000  #ms
                #print("lost since " + str(timelost) + " ms")
                if x_old > 720 / 2 + 100:  #lost it to the right
                    if timelost <= 1000:
                        speedl = 25  #turn right
                        speedr = 0
                        #print("search sequence 1")
                    elif timelost < 3000:  # turn the other way
                        #print("search sequence 2")
                        speedl = 0
                        speedr = 25
                    else:
                        #print("search sequence3")
                        speedl = 10
                        speedr = 0
                elif x_old < 720 / 2 - 100:  #lost it to the left
                    if timelost <= 1000:
                        speedl = 0  #turn left
                        speedr = 25
                    elif timelost < 3000:  # turn the other way
                        speedl = 25
                        speedr = 0
                    else:
                        speedl = 10
                        speedr = 0
                elif x_old == int(720 / 2):  #beginning
                    pass
                else:  #lost in the middle
                    speedl = -5
                    speedr = -5
        else:
            newlylost = True
            x_old = x
        speedl = speedl * (drivingspeed / 100)
        speedr = speedr * (drivingspeed / 100)
        motor.drive_l(speedl)
        motor.drive_r(speedr)
        if distance > desired_distance - tolerance and distance < desired_distance + tolerance and distancecorrect == True:
            distancereached = True
        elif distance > desired_distance - tolerance and distance < desired_distance + tolerance:
            distancecorrect = True
        else:
            distancecorrect = False
    return True