Exemplo n.º 1
0
def Keyborad_control():
    while True:
        global power_val
        key = readkey()
        if key == '6':
            if power_val <= 90:
                power_val += 10
                print("power_val:", power_val)
        elif key == '4':
            if power_val >= 10:
                power_val -= 10
                print("power_val:", power_val)
        if key == 'w':
            fc.forward(power_val)
        elif key == 'a':
            fc.turn_left(power_val)
        elif key == 's':
            fc.backward(power_val)
        elif key == 'd':
            fc.turn_right(power_val)
        else:
            fc.stop()
        if key == 'q':
            print("quit")
            break
Exemplo n.º 2
0
    def __init__(self):
        """ Init camera and wheels"""
        logging.info('Creating a PiCar-4WD...')

        fc.stop()

        logging.debug('Set up camera')
        self.camera = cv2.VideoCapture(-1)
        self.camera.set(3, self.__SCREEN_WIDTH)
        self.camera.set(4, self.__SCREEN_HEIGHT)

        ser = Servo(PWM("P0"))
        ser.set_angle(-65)

        self.lane_follower = LaneFollower(self)
        #self.traffic_sign_processor = ObjectsOnRoadProcessor(self)
        # lane_follower = DeepLearningLaneFollower()

        self.fourcc = cv2.VideoWriter_fourcc(*'XVID')
        datestr = datetime.datetime.now().strftime("%y%m%d_%H%M%S")
        self.video_orig = self.create_video_recorder(
            '../data/temp/car_video%s.avi' % datestr)
        self.video_lane = self.create_video_recorder(
            '../data/temp/car_video_lane%s.avi' % datestr)
        #     self.video_objs = self.create_video_recorder('../data/temp/car_video_objs%s.avi' % datestr)

        logging.info('Created a PiCar-4wd')
Exemplo n.º 3
0
Arquivo: car.py Projeto: voidTM/Pi-Car
    def drive_forward_cam(self, distance: int, power: int = 2):
        self.trip_meter.reset()
        coast_clear = True

        
        while(self.trip_meter.distance < distance):
            # wait for obstacles to clear
            if not self.obstacle_queue.empty():
                fc.stop()

                with self.obstacle_queue.mutex:
                    self.obstacle_queue.queue.clear()
                time.sleep(2)
                    
            else:
                fc.forward(power)



        fc.stop()
        actually_traveled = self.trip_meter.distance

        print(self.orientation, distance, actually_traveled)
        
        self.nav.update_postion(distance, self.orientation)

        return coast_clear
Exemplo n.º 4
0
def main():
    while True:
        scan_list = fc.scan_step(23)
        # print(scan_list)
        if not scan_list:
            continue

        scan_list = [str(i) for i in scan_list]
        scan_list = "".join(scan_list)
        paths = scan_list.split("2")
        length_list = []
        for path in paths:
            length_list.append(len(path))
        # print(length_list)
        if max(length_list) == 0:
            fc.stop() 
        else:
            i = length_list.index(max(length_list))
            pos = scan_list.index(paths[i])
            pos += (len(paths[i]) - 1) / 2
            # pos = int(pos)
            delta = len(scan_list) / 3
            # delta *= us_step/abs(us_step)
            if pos < delta:
                fc.turn_left(speed)
            elif pos > 2 * delta:
                fc.turn_right(speed)
            else:
                if scan_list[int(len(scan_list)/2-1)] == "0":
                    fc.backward(speed)
                else:
                    fc.forward(speed)
Exemplo n.º 5
0
def main():
    while True:
        scan_list = scan_step1(33)
        if not scan_list:
            continue
        tmp = scan_list[0][3:7]
        # print(angle_distance_list)
        # if tmp == [0,0,0,0]:
        #     fc.backward(speed)
        print(tmp)
        img = np.zeros((480, 480, 3), np.uint8)
        cv.rectangle(img, (190, 10), (290, 100), yellow, 2)
        cv.namedWindow("image")
        cv.imshow('image', img)
        cv.waitKey(500)
        draw_dot(scan_list[0], scan_list[1], img)
        print("$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$")
        if tmp != [2, 2, 2, 2]:
            fc.stop()
            # time.sleep(0.3)
            # fc.backward(speed)
            # time.sleep(0.3)
            # fc.turn_right(speed)

        else:
            fc.stop()
Exemplo n.º 6
0
async def main_func():
    global recv_dict, send_dict, gs_list
    while 1:
        gs_list = fc.get_grayscale_list()

        if recv_dict['CD'][0] == 'on':
            if fc.is_on_edge(recv_dict['CD'][1], gs_list):
                fc.backward(20)
                time.sleep(0.5)
                fc.stop()

        if recv_dict['TL'][0] == 'on':
            if fc.get_line_status(recv_dict['TL'][1], gs_list) == 0:
                fc.forward(recv_dict['PW'])
            elif fc.get_line_status(recv_dict['TL'][1], gs_list) == -1:
                fc.turn_left(recv_dict['PW'])
            elif fc.get_line_status(recv_dict['TL'][1], gs_list) == 1:
                fc.turn_right(recv_dict['PW'])

        if recv_dict['OA'] == 'on':
            scan_list = fc.scan_step(35)
            if scan_list:
                tmp = scan_list[3:7]
                if tmp != [2, 2, 2, 2]:
                    fc.turn_right(recv_dict['PW'])
                else:
                    fc.forward(recv_dict['PW'])

        elif recv_dict['OF'] == 'on':
            scan_list = fc.scan_step(23)

            if scan_list != False:
                scan_list = [str(i) for i in scan_list]
                scan_list = "".join(scan_list)
                paths = scan_list.split("2")
                length_list = []
                for path in paths:
                    length_list.append(len(path))
                if max(length_list) == 0:
                    fc.stop()
                else:
                    i = length_list.index(max(length_list))
                    pos = scan_list.index(paths[i])
                    pos += (len(paths[i]) - 1) / 2
                    delta = len(scan_list) / 3
                    if pos < delta:
                        fc.turn_left(recv_dict['PW'])
                    elif pos > 2 * delta:
                        fc.turn_right(recv_dict['PW'])
                    else:
                        if scan_list[int(len(scan_list) / 2 - 1)] == "0":
                            fc.backward(recv_dict['PW'])
                        else:
                            fc.forward(recv_dict['PW'])

        elif recv_dict['RD'] == 'on':
            fc.scan_step(35)

        await asyncio.sleep(0.01)
Exemplo n.º 7
0
def mright():
    speed4 = fc.Speed(25)
    speed4.start()
    fc.turn_right(15)
    x = 0
    for i in range(3):
        time.sleep(0.1)
    speed4.deinit()
    fc.stop()
Exemplo n.º 8
0
 def cleanup(self):
     """ Reset the hardware"""
     logging.info('Stopping the car, resetting hardware.')
     fc.stop()
     self.camera.release()
     self.video_orig.release()
     self.video_lane.release()
     # self.video_objs.release()
     cv2.destroyAllWindows()
Exemplo n.º 9
0
def move25():

    speed4 = Speed(25)
    speed4.start()
    # time.sleep(2)
    fc.forward(35)
    fc.stop()
    time.sleep(0.5)
    fc.turn_right(50)
    print('distance:', fc.us.get_distance())
Exemplo n.º 10
0
def mforward():
    speed4 = fc.Speed(25)
    speed4.start()
    fc.forward(15)
    x = 0
    for i in range(3):
        time.sleep(0.1)

    speed4.deinit()
    fc.stop()
Exemplo n.º 11
0
def mbackward():
    speed4 = fc.Speed(25)
    speed4.start()
    fc.backward(17)
    x = 0
    for i in range(10):
        time.sleep(0.1)

    speed4.deinit()
    fc.stop()
Exemplo n.º 12
0
def main():
    import sys
    if len(sys.argv) >= 2:
        print("Welcome to SunFounder PiCar-4WD.")
        command = sys.argv[1]
        if command == "soft-reset":
            print("soft-reset")
            soft_reset()
        elif command == "power-read":
            print("power-read")
            print("Power voltage: {}V".format(power_read()))
        elif command == "web-example":
            if len(sys.argv) >= 3:
                opt = sys.argv[2]
                if opt == "enable":
                    os.system(
                        "sudo update-rc.d picar-4wd-web-example defaults")
                    print("web-example start on boot is enabled")
                elif opt == "disable":
                    os.system("sudo update-rc.d picar-4wd-web-example remove")
                    print("web-example start on boot is disabled")
                else:
                    usage(command)
            else:
                print(
                    "Run: `picar-4wd web-example enable/disable` to enable/disable start on boot"
                )
                os.system(
                    "sudo python3 /home/pi/picar-4wd/examples/web/start.py")
        elif command == "test":
            from picar_4wd import forward, get_distance_at, get_grayscale_list, stop
            if len(sys.argv) >= 3:
                opt = sys.argv[2]
                if opt == "motor":
                    print("Motor test start!, Ctrl+C to Stop")
                    forward(50)
                    try:
                        while True:
                            pass
                    except KeyboardInterrupt:
                        stop()
                elif opt == "servo":
                    print(get_distance_at(0))
                elif opt == "grayscale":
                    print(get_grayscale_list())
                else:
                    usage(command)
        else:
            print('Command error, "%s" is not in list' % sys.argv[1])
            usage()
    else:
        usage()
    destroy()
Exemplo n.º 13
0
def Remote_control(control_flag, speed=50):
    speed = int(speed)
    if control_flag == 'forward':
        fc.forward(speed)
    elif control_flag == 'backward':
        fc.backward(speed)
    elif control_flag == 'turn_left':
        fc.turn_left(speed)
    elif control_flag == 'turn_right':
        fc.turn_right(speed)
    else:
        fc.stop()
Exemplo n.º 14
0
def naviCar(turns):
	dir = []
	o = start
	d = 'F'

	for t in turns:
		
		dy = t[0] - o[0]
		dx = t[1] - o[1]
		
		if dy == 0 and dx > 0:
			dir.append('R')
			if d == 'B' or d == 'F':
				fc.turn_right(pow)
				time.sleep(agR)
			d = 'R'
			
		if dy == 0 and dx < 0:
			dir.append('L')
			if d == 'F' or d == 'F':	
				fc.turn_left(pow)
				time.sleep(agL)
			d = 'L'
			
		if dy > 0 and dx == 0:
			dir.append('B')
			if d == 'L':
				fc.turn_right(pow)
				time.sleep(agR)
			if d == 'R':	
				fc.turn_left(pow)
				time.sleep(agL)
			d = 'B'
			
		if dy < 0 and dx == 0:
			dir.append('F')
			if d == 'L':
				fc.turn_right(pow)
				time.sleep(agR)
			if d == 'R':	
				fc.turn_left(pow)
				time.sleep(agL)
			d = 'F'
		
		fc.stop()	
		fc.forward(pow)
		time.sleep(fcm * abs(dy+dx))
		
		o = t
	
	fc.stop()

	print("Direction: ", dir)
Exemplo n.º 15
0
Arquivo: car.py Projeto: voidTM/Pi-Car
    def drive_backward(self, distance = 10, power = 5):
        self.trip_meter.reset()
        fc.backward(power)

        while(self.trip_meter.distance < distance):
            continue
        
        fc.stop()

        actually_traveled = self.trip_meter.distance

        return actually_traveled
Exemplo n.º 16
0
def main():
    time.sleep(5)
    fc.forward(pow)
    while True:
        dist = fc.get_distance_at(ang)
        print(dist)
        if dist != -2:
            if dist <= 60:
                fc.forward(pow / 5)
            if dist <= 20:
                fc.stop()
                break
    fc.stop()
Exemplo n.º 17
0
def drive_n_stop(speed: int = 10):
    clear = True
    fc.forward(speed)
    while clear:
        scan_list = fc.scan_step()
        if not scan_list:
            continue

        ahead = scan_list[2:8]
        # coast clear full speed ahead
        if min(ahead) < 2:
            #print("Coast Clear")
            clear = False
            fc.stop()
Exemplo n.º 18
0
Arquivo: car.py Projeto: voidTM/Pi-Car
    def drive_step(self, distance: int, power: int = 2):
        self.trip_meter.reset()

        while(self.cam.detect_traffic() == True):
            fc.stop()
            print("Obstacle detected")
            time.sleep(2)
        
        while(self.trip_meter.distance < distance):

            fc.forward(2)

        fc.stop()
        print(self.trip_meter.distance, distance)
Exemplo n.º 19
0
 def move25():
     speed4 = Speed(25)
     speed4.start()
     # time.sleep(2)
     fc.backward(100)
     x = 0
     for i in range(1):
         time.sleep(0.1)
         speed = speed4()
         x += speed * 0.1
         print("%smm/s"%speed)
     print("%smm"%x)
     speed4.deinit()
     fc.stop()
Exemplo n.º 20
0
def test3():
    speed4 = Speed(25)
    speed4.start()
    # time.sleep(2)
    fc.forward(100)
    x = 0
    for i in range(20):
        time.sleep(0.1)
        speed = speed4()
        x += speed * 0.1
        print("%smm/s" % speed)
    print("%smm" % x)
    speed4.deinit()
    fc.stop()
Exemplo n.º 21
0
def turnRight():
    speed4 = Speed(25)
    speed4.start()
    # time.sleep(2)
    fc.turn_right(80)
    x = 0
    for i in range(15):
        time.sleep(0.1)
        speed = speed4()
        x += speed * 0.1
        print("%smm/s" % speed)
    print("%smm" % x)
    speed4.deinit()
    fc.stop()
Exemplo n.º 22
0
def main_loop():
    if lettura_media(0) > MIN_DIST_FOLLOW:
        fc.forward(20)
        while lettura_media(0) > MIN_DIST_FOLLOW:
            pass
    fc.stop()
    lval = scan_sx()
    rval = scan_dx()
    turn_time = 0.5 + random.random() * 1
    if rval < lval:
        fc.turn_right(20)
    else:
        fc.turn_left(20)
    time.sleep(turn_time)
Exemplo n.º 23
0
def drive_n_stop(speed: int = 5):
    clear = True
    fc.forward(speed)

    while clear:
        # scan list returns false until a full 180 deg scan is performed.
        scan_list = fc.scan_step(35)
        if not scan_list:
            continue

        ahead = scan_list[2:8]
        if min(ahead) < 2:
            clear = False
            fc.stop()
Exemplo n.º 24
0
def main():
    time.sleep(5)

    fc.forward(pow)
    time.sleep(3)

    fc.backward(pow)
    time.sleep(2)

    fc.turn_left(pow)
    time.sleep(2)

    fc.turn_right(pow)
    time.sleep(2)

    fc.stop()
Exemplo n.º 25
0
def roomba(speed=10):

    while True:

        # get scan by distance
        scan_list = scanner.scan_step_dist()
        if not scan_list:
            continue

        # the preprocessing limits the max distance to 200cm
        # since any -2 means no response set the value to 200cm
        scan_list = [200 if d == -2 else d for d in scan_list]

        # readings that are infront on the Picar
        # -54 degrees to 54 degrees
        ahead = scan_list[2:7]

        # coast clear full speed ahead
        if min(ahead) > 35:
            fc.forward(speed)
            continue

        logging.info("Too close stopping")
        fc.stop()

        left = scan_list[:5]
        right = scan_list[5:]

        # -1 = turn left, 0 forward, 1 turn right
        direction = 0
        # evaluates which direction turn
        # turns in the direction with the most open space

        if (sum(right) > sum(left)):
            direction = 1
        else:
            direction = -1

        #print(sum(left), sum(right))
        #print( direction)

        if direction == 1:
            logging.info("Turning right")
            fc.turn_right(speed)
        elif direction == -1:
            logging.info("Turning left")
            fc.turn_left(speed)
Exemplo n.º 26
0
Arquivo: car.py Projeto: voidTM/Pi-Car
    def turn_right(self, angle: int, power: int = 5):
        # need to adjust slippage for turning
        slippage = 2.2 
        dist = utils.angle_to_dist(angle) * slippage


        self.trip_meter.reset()        
        fc.turn_right(power)
        while(self.trip_meter.distance < dist):
            continue
        
        fc.stop()

        self.orientation = update_angle(self.orientation, angle)
        print("new car orientation", self.orientation)

        return self.orientation
Exemplo n.º 27
0
Arquivo: car.py Projeto: voidTM/Pi-Car
    def drive_forward(self, distance: int, power: int = 5):

        self.trip_meter.reset()
        fc.forward(power)
        coast_clear = True
        while(self.trip_meter.distance < distance and coast_clear):
            if (fc.get_status_at(0, 20) != 2):
                coast_clear = False
                break
            continue
        
        fc.stop()
        print(self.trip_meter.distance, distance)



        return coast_clear
Exemplo n.º 28
0
def main():
    while True:
        scan_list = scan_step1(25)
        if not scan_list:
            continue
        tmp = scan_list[3:7]
        # detect()
        # print(tmp)
        print(detect())

        if 1 not in tmp:
            print(tmp)
            # fc.forward(speed)
            fc.stop()
        else:
            print(tmp)
            fc.stop()
Exemplo n.º 29
0
def main():
    while True:
        scan_list = scan_step1(40)
        if not scan_list:
            continue
        tmp = scan_list[3:7]
        # if tmp == [0,0,0,0]:
        #     fc.backward(speed)

        if 1 not in tmp:
            print(tmp)
            fc.forward(speed)
        else:
            print(tmp)
            fc.stop()
            time.sleep(0.3)
            fc.backward(speed)
            time.sleep(0.3)
            fc.turn_right(speed)
Exemplo n.º 30
0
def test1():
    # import fwd as nc
    fc.forward(100)

    speed3 = Speed(25)
    speed4 = Speed(4)
    speed3.start()
    speed4.start()
    try:
        # nc.stop()
        while 1:
            # speed_counter
            # = 0
            print(speed3())
            print(speed4())
            print(" ")
            time.sleep(0.5)
    finally:
        speed3.deinit()
        speed4.deinit()
        fc.stop()