Beispiel #1
0
def move_till(destination,direction):
	#determine whether right or left wall
	if direction=='right':
		first='right'
		second='left'
	elif direction=='left':
		first='left'
		second='left'

	#if first open turn right
	if dist.check(first):
		motion.turn(first)
	if callable(motion.forward):
        dist.thread_forward(first)

     #if front blocked, check for destination
    if not check('front'):
    	motion.stop()
	    if destination=='qr':
		    if dist.check_for_qr:
		    	if vision.scan_qr():
		    		wait_for_path(direction)
		    	return True
		elif destination=='box' or destination=='block':
			if vision.check_for_block():
				wait_for_path(direction)
				return True
		elif destination=='end':
			if vision.ground_is_white():
				return True
	#turn second if nothing found
	else:
		motion.turn(second)
	#call func again until destination reached
	return move_till(destination,direction)
Beispiel #2
0
def parseServerData(data):
    #####################################
    # TCP NETWORK SIGNALS
    #####################################
    if data:
        if data != "env":
            conn.send(data)  # echo
        if data == "close":
            CONNECTION_MADE = 0
            conn.close()
        if data == "shutdown":
            conn.send("close")
            conn.close()
            shutdown()
        ##################################
        # OTHER COMMANDS
        ##################################

        if data == "snap":
            os.system("raspistill -o test.jpg")
        if data == "env":
            STR = 'IP: ' + ip_address + 'Lights are ' + lights + "\n" + 'Temperature:' + str(
                int(temp)) + 'C' + "\n" + 'Status: ' + STATUS
            conn.send(STR)
        if data == "a":
            motion.left()
        if data == "d":
            motion.right()
        if data == "w":
            motion.forward()
        if data == "s":
            motion.backward()
        if data == "x":
            motion.stop()
        if data:
            if data[0] == 'r':
                GPIO.setup(10, GPIO.OUT)
                rotate = GPIO.PWM(10, 50)
                rotate.start(float(data[1:]))
                time.sleep(1)
                GPIO.cleanup(10)
            if data[0] == 't':
                GPIO.setup(9, GPIO.OUT)
                tilt = GPIO.PWM(9, 50)
                tilt.start(float(data[1:]))
                time.sleep(1)
                GPIO.cleanup(9)
        if data == "cool":
            disp.begin()

            disp.clear()
            disp.display()

            image = Image.open('test.jpg').resize((disp.width, disp.height),
                                                  Image.ANTIALIAS).convert('1')

            disp.image(image)
            disp.display()
            time.sleep(5)
Beispiel #3
0
def move():

	#measure time elapsed
        time=time.time()-time0
	#right check
	if distances.box_on_right:
		motion.turn('right')
		path.append(['right',time])

	elif distances.right_open:
		motion.turn(right)
		path.append(['left',time])
		motion.forward()
		path.append['forward',time]

	#front check
	elif distances.front_blocked:

		if vision.check_for_box():
			motion.stop()
			
			#stores box coordinate in path
			box_coordinates.append(len(path)-1)
			
			
			wait_for_path()

		elif distances.check_for_qr():
			motion.stop()
			qr_coordinates.append(len(path)-1)
			number_of_boxes_found=len(box_coordinates)

			if number_of_boxes_found<2:
				motion.turn('left')
			else: wait_for_path()	

		else:
			motion.turn('left')
			path.append(['left',time])

	#define start-end coordinates
	if vision.ground_is_white:
		if len(start_coordinates)<2:
			start_coordinates.append(len(path)-1)
		else: 
			end_coordinates.append(len(path)-1)
		if len(start_coordinates)==2:
			path.clear()
			follow_path=min_path(path_trim(path,start[0],qr_coordinates[0]),path_trim(start[1],qr_coordinates[0]))
			time=0
			

	#call method again
	move()		
Beispiel #4
0
def check_for_qr():
    #checks if a qr bock is present whithin 35 cm of stand
    if dist(upper) < 35:
        #goes to the required range kept 1 cm extra as error due to no brakes.
        if dist.upper >= 26:
            motion.forward()
            check_for_qr()
        elif dist.upper <= 24:
            motion.backward()
        #stops if already in range
        else:
            motion.stop()
        return True
Beispiel #5
0
def signal_handler(signal, frame):
    print('SIGINT RECIEVED')
    LCD1602.write(0, 0, '                ')
    LCD1602.write(1, 1, '                ')
    motion.stop()
    disp = Adafruit_SSD1306.SSD1306_128_32(0)
    disp.clear()
    setColor(0xff0000)
    global CONNECTION_MADE
    if CONNECTION_MADE:
        CONNECTION_MADE = 0
        conn.send("close")
        conn.close()
    global RUN_SERVER
    exit(0)
    RUN_SERVER = 0
Beispiel #6
0
def _handle_odom_timer():
    global _ctrl_on, _velocities, _set_speed, _xhat, _yhat, _thetahat
    if _odom_on:
        odom = Odometry.update(_odom_timer_period, _velocityhat)
        print "{}\r".format(odom)

        _xhat = odom[0]
        _yhat = odom[1]
        _thetahat = odom[2]

        if _ctrl_on:
            x_c, y_c, theta_c = Controller.get_commanded_position()
            if _close(_xhat, x_c) and _close(_yhat, y_c) and \
                    _close(_thetahat, theta_c, tolerance=8):
                _ctrl_on = False
                print("\r\n*** Reached Set Point within Tolerances ***\r\n")
                _motion_timer.stop()
                motion.stop()
                time.sleep(0.5)
                _velocities = (0, 0, 0)
                _set_speed = True
                _motion_timer.start()
Beispiel #7
0
def test_square_calibration(velocity=0.6,
                            sleep_time=1.5,
                            M1QPPS=None,
                            M2QPPS=None,
                            M3QPPS=None):
    global wheelbase_configured

    # Make sure everything is init'd
    if not wheelbase_configured:
        w.init()
        wheelbase_configured = True

    _m1qpps = M1QPPS if M1QPPS is not None else qppsM1
    _m2qpps = M2QPPS if M2QPPS is not None else qppsM2
    _m3qpps = M3QPPS if M3QPPS is not None else qppsM3

    if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
        w.SetVelocityPID(w.M1, kp, ki, kd, _m1qpps)
        w.SetVelocityPID(w.M2, kp, ki, kd, _m2qpps)
        w.SetVelocityPID(w.M3, kp, ki, kd, _m3qpps)

    # Right
    motion.drive(velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Up
    motion.drive(0, velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Left
    motion.drive(-velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Down
    motion.drive(0, -velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
Beispiel #8
0
def test_square_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None):
    global wheelbase_configured

    # Make sure everything is init'd
    if not wheelbase_configured:
        w.init()
        wheelbase_configured = True

    _m1qpps = M1QPPS if M1QPPS is not None else qppsM1
    _m2qpps = M2QPPS if M2QPPS is not None else qppsM2
    _m3qpps = M3QPPS if M3QPPS is not None else qppsM3

    if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
        w.SetVelocityPID(w.M1, kp, ki, kd, _m1qpps)
        w.SetVelocityPID(w.M2, kp, ki, kd, _m2qpps)
        w.SetVelocityPID(w.M3, kp, ki, kd, _m3qpps)

    # Right
    motion.drive(velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Up
    motion.drive(0, velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Left
    motion.drive(-velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Down
    motion.drive(0, -velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
Beispiel #9
0
def square():
    motion.drive(.5, 0, 0)
    time.sleep(1)

    motion.stop()
    time.sleep(0.5)

    motion.drive(0, .5, 0)
    time.sleep(1)

    motion.stop()
    time.sleep(0.5)

    motion.drive(-.5, 0, 0)
    time.sleep(1)

    motion.stop()
    time.sleep(0.5)

    motion.drive(0, -.5, 0)
    time.sleep(1)

    motion.stop()
Beispiel #10
0
def square():
	motion.drive(.5, 0, 0)
	time.sleep(1)

	motion.stop()
	time.sleep(0.5)

	motion.drive(0, .5, 0)
	time.sleep(1)

	motion.stop()
	time.sleep(0.5)

	motion.drive(-.5, 0, 0)
	time.sleep(1)

	motion.stop()
	time.sleep(0.5)

	motion.drive(0, -.5, 0)
	time.sleep(1)

	motion.stop()
Beispiel #11
0
def _go_home():
    # Get current robot position, rounded to 1 decimal place
    xhat = round(_xhat, 1)
    yhat = round(_yhat, 1)
    thetahat = round(_thetahat, 1)

    # Set a tolerance
    tolerance = 0.1

    if abs(xhat) > tolerance:
        dt = abs(xhat / _vx)
        sign = -1 if xhat > 0 else 1
        print("motion.drive({},{},{}) for {} s\r".format(sign*_vx, 0, 0, dt))
        motion.drive(sign*_vx, 0, 0)
        time.sleep(dt)
        motion.stop()
        time.sleep(0.5)

    if abs(yhat) > tolerance:
        dt = abs(yhat / _vy)
        sign = -1 if yhat > 0 else 1
        print("motion.drive({},{},{}) for {} s\r".format(0, sign*_vy, 0, dt))
        motion.drive(0, sign*_vy, 0)
        time.sleep(dt)
        motion.stop()
        time.sleep(0.5)

    if abs(thetahat) > tolerance*100:
        # since it's periodic...
#        thetahat = round(thetahat%(360) ,2)
        dt = abs(thetahat / _w)
        sign = -1 if thetahat > 0 else 1
        print("motion.drive({},{},{}) for {} s\r".format(0, 0, sign*_w, dt))
        motion.drive(0, 0, sign*_w)
        time.sleep(dt)
        motion.stop()
        time.sleep(0.5)
Beispiel #12
0
def main():
    global _motion_timer
    _motion_timer = RepeatedTimer(_motion_timer_period, _handle_motion_timer)
    _motion_timer.start()

    global _odom_timer
    _odom_timer = RepeatedTimer(_odom_timer_period, _handle_odom_timer)
    _odom_timer.start()

    global _ctrl_timer
    _ctrl_timer = RepeatedTimer(_ctrl_timer_period, _handle_ctrl_timer)
    _ctrl_timer.start()

    use_rcv3 = os.environ['USE_RCV3'] == 'true'
    w.init(use_rcv3=use_rcv3)

    # TODO: Okay, so Controller.init(None) will automagically load in some PID 
    # values for each of the x, y, theta position controllers. However, since
    # `teleop` is run in real life on different robots, really it should read
    # the correct YAML robot config file and load in the PID constants and
    # pass them into Controller.init(gains) as in the controller_node.py.
    # Two ways to do this: (1) do like os.environ['USE_RCV3'], where the odroid_setup.bash
    # script loads up the PID constants as environment variables (this seems messy).
    # (2) find a python YAML reading library and just read the file directly from here
    # based on which robot this is (you can use os.environ['ROBOT'])
    Controller.init()
    
    print 'Started.'


    global _velocities, _set_speed, _smooth, _odom_on, _previous_action, _ctrl_on

    while(1):
        action = get_action()
        if action == 'UP':
            _set_speed = True
            _velocities = (0, _vy, 0)

        elif action == 'DOWN':
            _set_speed = True
            _velocities = (0, -_vy, 0)

        elif action == 'RIGHT':
            _set_speed = True
            _velocities = (_vx, 0, 0)

        elif action == 'LEFT':
            _set_speed = True
            _velocities = (-_vx, 0, 0)

        elif action == 'SPIN_CW':
            _set_speed = True
            _velocities = (0, 0, _w)

        elif action == 'SPIN_CCW':
            _set_speed = True
            _velocities = (0, 0, -_w)

        elif action == 'SET_HOME':
            Odometry.init()

        elif action == 'GO_HOME':
            _motion_timer.stop()
            motion.stop()
            time.sleep(1)

            _go_home()

            time.sleep(1)
            _set_speed = True
            _velocities = (0, 0, 0)
            _motion_timer.start()

        elif action == 'GO_TO_POINT':
            _toggle_timers(False)
            motion.stop()
            time.sleep(1)

            _ask_for_point()

            time.sleep(1)
            _ctrl_on = True
            _odom_on = True
            _toggle_timers(True)

        elif action == 'TOGGLE_CNTRL':
            _ctrl_on = not _ctrl_on
            print("Controller: {}".format(_ctrl_on))

        elif action == 'TOGGLE_SMOOTH':
            _smooth = not _smooth
            print("Smooth: {}".format(_smooth))

        elif action == 'TOGGLE_ODOM':
            _odom_on = not _odom_on
            print("Odom: {}".format(_odom_on))

        elif action == 'BATTERY':
            print("\n\r*** Battery: {} ***\n\r".format(get_battery()))

        elif action == 'BREAKPOINT':
            _toggle_timers(False)
            import ipdb; ipdb.set_trace()
            _toggle_timers(True)

        elif action == 'CALIBRATION_MODE':
            _toggle_timers(False)

            _deal_with_calibration()

            time.sleep(1)
            _toggle_timers(True)

        elif action == 'DIE':
            _toggle_timers(False)
            motion.stop()
            w.kill()
            return sys.exit(0)

        else:
            _set_speed = True
            _velocities = (0, 0, 0)

        # handle stopping before changing directions
        if _action_requires_stop(action):
            _set_speed = False
            motion.stop()
            time.sleep(0.4)
            _set_speed = True

        _previous_action = action

        print "{}\r".format(_velocities)
Beispiel #13
0
def test_calibration(velocity=0.6,
                     sleep_time=1.5,
                     M1QPPS=None,
                     M2QPPS=None,
                     M3QPPS=None):
    global wheelbase_configured

    # Make sure everything is init'd
    if not wheelbase_configured:
        w.init()
        wheelbase_configured = True

    _m1qpps = M1QPPS if M1QPPS is not None else qppsM1
    _m2qpps = M2QPPS if M2QPPS is not None else qppsM2
    _m3qpps = M3QPPS if M3QPPS is not None else qppsM3

    if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
        w.UpdateVelocityPID(w.M1, q=_m1qpps)
        w.UpdateVelocityPID(w.M2, q=_m2qpps)
        w.UpdateVelocityPID(w.M3, q=_m3qpps)

    _print_stats()

    print("velocity: {}\r".format(velocity))

    # Forward
    motion.drive(velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Backward
    motion.drive(-velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Left back
    motion.drive(velocity * np.cos(2 * np.pi / 3),
                 velocity * np.sin(2 * np.pi / 3), 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right forward
    motion.drive(-np.cos(2 * np.pi / 3) * velocity,
                 -np.sin(2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right back
    motion.drive(
        np.cos(-2 * np.pi / 3) * velocity,
        np.sin(-2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right Forward
    motion.drive(-np.cos(-2 * np.pi / 3) * velocity,
                 -np.sin(-2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
Beispiel #14
0
def move(argument):

    global number_of_qr
    global number_of_boxes
    global box_coordinates
    global qr_coordinates
    global t
    global time0
    #global nodes

    #update variables required

    print('bot: (move called again) right wall dist=',
          us.dist(us.right_trig, us.right_echo))
    # if right is open, it will stop, turn right
    if check('right'):

        print('bot:right is open ')

        motion.stop()
        motion.turn('right')
    #move forward
    if callable(motion.forward):
        thread_forward('right')

    if not check('front'):
        #if front is blocked, it will check for boxes
        motion.stop()
        if check_for_qr():
            number_of_qr += 1
            #if we are still in the first loop num will be <2
            if number_of_qr == 1:
                #if we have not detected all boxes in the first loop
                #it will continue mapping 1st loop
                t += time.time() - time0

                qr_coordinates.append(t)

                motion.turn('left')

                time0 = time.time()
            #if we have detected all boxes, move to next loop
            #we have finished mapping 1st loop
            elif number_of_qr == 2:

                wait_for_path()

                time0 = time.time()
            #second loop
            elif number_of_qr == 3:
                t += time.time() - time0
                qr_coordinates.append(t)
                wait_for_path()
                time0 = time.time()

        elif vision.check_for_block():
            t += time.time() - time0
            box_coordinates.append(t)
            wait_for_path()
            time0 = time.time()
        #if nothing found it will turn left and check the ground
        else:
            motion.turn('left')

            if vision.ground_is_white():
                t += time.time() - time0
                if number_of_qr >= 2:
                    end_coordinates.append(t)
                    #nodes.append('end')

                else:
                    start_coordinates.append(t)
                    #now we have to go to the qr block...
                    #to move to the next loop
                    #set t=0 for next loop
                    if qr_coordinates[0] > start[1] - qr_coordinates[0]:
                        wet_run.uturn()
                        if wet_run.move_till('qr', 'left'):
                            t = 0
                    else:
                        if wet_run.move_till('qr', 'right'):
                            t = 0

    #elif callable(motion.forward):

    #put condition for finishing mapping here

    if (len(qr_coordinates) == 2) + (number_of_boxes
                                     == 3) + (len(end_coordinates) == 1) < 3:
        return move()
    else:
        return [
            qr_coordinates, box_coordinates, start_coordinates, end_coordinates
        ]
Beispiel #15
0
import motion 
import time
import particles

for i in range(4):
  motion.fwd_amt(50)
  motion.stop()
  motion.turn(90, "r")
  motion.stop()

'''
particles.initialise()
particles.draw()
time.sleep(1)

particles.update_forward(100)
particles.draw()
time.sleep(1)

particles.update_forward(100)
particles.draw()
time.sleep(1)

particles.update_forward(100)
particles.draw()
time.sleep(1)

particles.update_rotate(90)
particles.draw()
time.sleep(1)
Beispiel #16
0
def spin():
    motion.drive(0, 0, 2 * np.pi)

    time.sleep(3)

    motion.stop()
Beispiel #17
0
def spin():
	motion.drive(0, 0, 2*np.pi)

	time.sleep(3)

	motion.stop()
Beispiel #18
0
def test_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None):
    global wheelbase_configured

    # Make sure everything is init'd
    if not wheelbase_configured:
        w.init()
        wheelbase_configured = True

    _m1qpps = M1QPPS if M1QPPS is not None else qppsM1
    _m2qpps = M2QPPS if M2QPPS is not None else qppsM2
    _m3qpps = M3QPPS if M3QPPS is not None else qppsM3

    if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
        w.UpdateVelocityPID(w.M1, q=_m1qpps)
        w.UpdateVelocityPID(w.M2, q=_m2qpps)
        w.UpdateVelocityPID(w.M3, q=_m3qpps)

    _print_stats()

    print ("velocity: {}\r".format(velocity))

    # Forward
    motion.drive(velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Backward
    motion.drive(-velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Left back
    motion.drive(velocity * np.cos(2 * np.pi / 3), velocity * np.sin(2 * np.pi / 3), 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right forward
    motion.drive(-np.cos(2 * np.pi / 3) * velocity, -np.sin(2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right back
    motion.drive(np.cos(-2 * np.pi / 3) * velocity, np.sin(-2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right Forward
    motion.drive(-np.cos(-2 * np.pi / 3) * velocity, -np.sin(-2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()