示例#1
0
def main():
    # read arguments
    ap = argparse.ArgumentParser()
    ap.add_argument("-p", "--port",
                    help="path for serial port")
    args = vars(ap.parse_args())

    # inits IMU
    port = args.get('port', None)
    if port is None:
        port = '/dev/ttyUSB_IMU'
    imu = IMU(port)

    # start subprocess
    imu.start()

    try:
        while True:
            print("yaw = ", imu.get_yaw())
            print("yaw2 = ", imu.get_yaw2())
            print("pitch = ", imu.get_pitch())
            print("roll = ", imu.get_roll())

            print("angx = ", imu.get_angx())
            print("angy = ", imu.get_angy())
            print("angz = ", imu.get_angz())

            print("accx = ", imu.get_accx())
            print("accy = ", imu.get_accy())
            print("accz = ", imu.get_accz())

            print("magx = ", imu.get_magx())
            print("magy = ", imu.get_magy())
            print("magz = ", imu.get_magz())

            print()
            time.sleep(1)
    except KeyboardInterrupt:
        pass
    finally:
        print("Stopping remaining threads...")
        imu.stop()
示例#2
0
def main():
    # read arguments
    ap = argparse.ArgumentParser()
    ap.add_argument("-v", "--video", help="path to the (optional) video file")
    ap.add_argument("-c", "--camera", help="index of camera")
    ap.add_argument("-o", "--output", help="path to save the video")
    ap.add_argument("-s", "--speed")
    args = vars(ap.parse_args())

    if args.get("video", False):
        vs = args.get("video", False)
    elif args.get("camera", False):
        vs = int(args.get("camera", False))
    else:
        vs = 0

    set_speed = args.get('speed', 0)
    if set_speed is None:
        set_speed = 0
    set_speed = float(set_speed)
    speed = 0

    # inits CV
    cv = CVManager(
        vs,  # choose the first web camera as the source
        enable_imshow=True,  # enable image show windows
        server_port=3333,  # start stream server at port 3333
        delay=5,
        outputfolder=args.get("output"))
    cv.add_core("Flare", Flare(), True)

    # inits MCU
    mcu = MCU(2222)

    # inits IMU
    imu = IMU("/dev/ttyUSB_IMU")

    # start subprocess
    cv.start()
    imu.start()
    mcu.start()

    time.sleep(2)
    imu.delta_yaw = imu.get_yaw()
    print('Target yaw: ', imu.delta_yaw)
    mcu.wait()

    pidR = pidRoll(1, 0, 0)  # 5, 0.1 , 5
    pidP = pidPitch(0.6, 0, 0)  # 5 ,0.1 ,8
    pidD = pidDepth(1, 0, 0)
    pidY = pidYaw(1, 0.1, 0)
    motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0

    try:
        motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
        counter = 0
        last_cv_gate = 0
        while True:
            counter += 1
            gate, _, gate_size = cv.get_result("Flare")
            depth = mcu.get_depth()
            pinger = mcu.get_angle()
            pitch = imu.get_pitch()
            roll = imu.get_roll()

            if gate is None:  # just go straight
                yaw = imu.get_yaw2(0)
            else:
                if gate != last_cv_gate:
                    imu.reset_yaw2(-gate * 0.1, 1)
                    last_cv_gate = gate
                else:
                    yaw = imu.get_yaw2(1)

            if abs(yaw) > 10:
                speed = 0
            else:
                speed = set_speed

            pidR.getSetValues(roll)
            pidP.getSetValues(pitch)
            pidD.getSetValues(70 - depth)
            pidY.getSetValues(-yaw)
            finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(),
                                      pidY.start())

            sentValues = []
            for values in finalPidValues:
                subValues = values
                sentValues.append(subValues)

            motor_fl = sentValues[0]
            motor_fr = sentValues[1]
            motor_bl = sentValues[2] + set_speed
            motor_br = sentValues[3] + set_speed
            motor_t = sentValues[4]

            # Put control codes here

            mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)

            if counter % 20 == 0:
                print('Gate', gate)
                print('GateSize', gate_size)
                print('Depth:', depth)
                print('Pinger:', pinger)
                print('Pitch:', pitch)
                print('Roll:', roll)
                print('Yaw:', imu.get_yaw2())
                print('Yaw_sent:', yaw)
                print('Motors: %.2f %.2f %.2f %.2f %.2f' %
                      (motor_fl, motor_fr, motor_bl, motor_br, motor_t))
                print()
            time.sleep(0.1)
    except KeyboardInterrupt:
        pass
    finally:
        print("Stopping remaining threads...")
        cv.stop()
        imu.stop()
        mcu.stop()