コード例 #1
0
ファイル: main3.py プロジェクト: mvernacc/maslab-staff-2013
def main(argv):
    # side = "red"

    # try:
    #     opts, args = getopt.getopt(argv,'', ['color='])
    # except getopt.GetoptError:
    #     print "usage: 'main.py --color=<c>', where <c> is 'red' or 'green'"
    # for opt, arg in opts:
    #     if opt == '--color':
    #         if arg == 'green':
    #             side = "green"
    #             print 'looking for green balls'
    #         elif arg == 'red':
    #             print 'looking for red balls'

    ardu = arduino.Arduino()
    motor_left = arduino.Motor(ardu, 0, 9, 8) # current pin, direction pin, pwm pin
    motor_right = arduino.Motor(ardu, 0, 7, 6)
    motor_pickup = arduino.Motor(ardu, 0, 12, 11)
    bump_right = arduino.DigitalInput(ardu, 22)
    bump_left = arduino.DigitalInput(ardu, 23)
    ardu.run()

    vis = Vision(True)
    end_now = False

    start_code = bump_left.getValue() - bump_right.getValue()
    while start_code == 0:
        start_code = bump_left.getValue() - bump_right.getValue()
    if start_code > 0:
        side = "green"
        print "green"
    else:
        side = "red"
        print "red"
    cv2.waitKey(1000)

    motor_pickup.setSpeed(60)

    start_time = time.time()
    try:
        while not end_now:
            time_elapsed = time.time() - start_time
            if bump_right.getValue():
                print 'right bump'
                motor_right.setSpeed(-200)
                motor_left.setSpeed(-50)
                cv2.waitKey(1000)
                motor_right.setSpeed(-100)
                motor_left.setSpeed(100)
                cv2.waitKey(500)
                motor_right.setSpeed(0)
                motor_left.setSpeed(0)
            if bump_left.getValue():
                print 'left bump'
                motor_right.setSpeed(-50)
                motor_left.setSpeed(-200)
                cv2.waitKey(1000)
                motor_right.setSpeed(100)
                motor_left.setSpeed(-100)
                cv2.waitKey(500)
                motor_right.setSpeed(0)
                motor_left.setSpeed(0)
            vis.grab_frame()
            if time_elapsed < 120:
                feat_pos = vis.detect_balls(side)
            else:
                feat_pos = vis.detect_walls()
            if feat_pos != None:
                angle = int(feat_pos)
                print angle 
                motor_right.setSpeed(60 - angle)
                motor_left.setSpeed(60 + angle)
            else:
                print "No ball"
                motor_right.setSpeed(-40)
                motor_left.setSpeed(40)
            cv2.waitKey(20)
            if time_elapsed > 170:
                end_now = True
                break
    except KeyboardInterrupt:
        print "ending..."
    time.sleep(0.1)
    motor_right.setSpeed(0)
    motor_left.setSpeed(0)
    motor_pickup.setSpeed(0)
    time.sleep(0.1)
    ardu.stop()