コード例 #1
0
def balance():

    # init object from complementaryfilter class
    myFilter = ComplementaryFilter.ComplementaryFilter()
    myFilter.initual_filter_compute()

    counter = 0
    RobotCali.setup()

    # stop this while loop when we finish moving forward or backward task
    # by setting running to False ---> interupt.py knows when the task is done
    while running:
        # note in the case we are not using y_value
        x_value, y_value = myFilter.filter_compute()
        #print ("x val", x_value)

        if counter == 0:

            print("here is my x", x_value)

            if x_value > 1 or x_value < -1:
             # we have to calibrate the robot
                RobotCali.calibrate(x_value)

        counter += 1

        if counter == 5:
            counter = 0
    print("balance is done")
コード例 #2
0
def detect_obstacle():

    # init object from complementaryfilter class
    myFilter = ComplementaryFilter.ComplementaryFilter()
    myFilter.initual_filter_compute()

    counter = 0
    RobotCali.setup()

    # stop this while loop when we finish moving forward or backward task
    # by setting running to False ---> interupt.py knows when the task is done
    while running:
        # note in the case we are not using y_value
        x_value, y_value = myFilter.filter_compute()

        if counter == 0:

            print("here is my x", x_value)

            if y_value > 40 or y_value < -15:
                # obstacle found!! report to ratthanan
                ipc.write("ERROR")

        counter += 1

        if counter == 5:
            counter = 0
    print("obstacle is found")