예제 #1
0
 def acquire_usd(self):
     sleep(500/1000)
     dist1 = gopigo.us_dist(15)
     sleep(500/1000)
     dist2 = gopigo.us_dist(15)
     #dist = randint(1, 200)
     dist = (dist1+dist2)/2
     print "acquire usd",dist
     return dist
예제 #2
0
    def read(self):
        '''
        Limit the ultrasonic sensor to a distance of 5m.
        Take 3 readings, discard any that's higher than 5m
        If we discard 5 times, then assume there's nothing in front
            and return 501
        '''
        return_reading = 0
        readings =[]
        skip = 0
        while len(readings) < 3:
            value = gopigo.us_dist(PORTS[self.port])
            if value < 501 and value > 0:
                readings.append(value)
            else:
                skip +=1
                if skip > 5:
                    break

        if skip > 5:
            return(501)

        for reading in readings:
            return_reading += reading

        return_reading = int(return_reading // len(readings))

        return (return_reading)
예제 #3
0
 def is_too_close(self):
     too_close = False
     I2C_Mutex_Acquire()
     if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
         too_close = True
     I2C_Mutex_Release()
     return too_close
예제 #4
0
    def read(self):
        '''
        Limit the ultrasonic sensor to a distance of 5m.
        Take 3 readings, discard any that's higher than 5m
        If we discard 5 times, then assume there's nothing in front
            and return 501
        '''
        return_reading = 0
        readings = []
        skip = 0
        while len(readings) < 3:
            value = gopigo.us_dist(PORTS[self.port])
            if value < 501 and value > 0:
                readings.append(value)
            else:
                skip += 1
                if skip > 5:
                    break

        if skip > 5:
            return (501)

        for reading in readings:
            return_reading += reading

        return_reading = int(return_reading // len(readings))

        return (return_reading)
예제 #5
0
 def is_too_close(self):
     too_close = False
     _grab_read()
     try:
         if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
             too_close = True
     except:
         pass
     _release_read()
     return too_close
예제 #6
0
    def is_too_close(self):
        _wait_for_read()

        if _is_read_open():
            _grab_read()
            if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
                _release_read()
                return True
        _release_read()
        return False
예제 #7
0
    def is_too_close(self):
        _wait_for_read()

        if _is_read_open():
            _grab_read()
            if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
                _release_read()
                return True
        _release_read()
        return False
예제 #8
0
 def is_too_close(self):
     too_close = False
     _ifMutexAcquire(self.use_mutex)
     try:
         if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
             too_close = True
     except:
         pass
     finally:
         _ifMutexRelease(self.use_mutex)
     return too_close
예제 #9
0
 def is_too_close(self):
     too_close = False
     _ifMutexAcquire(self.use_mutex)
     try:
         if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
             too_close = True
     except:
         pass
     finally:
         _ifMutexRelease(self.use_mutex)
     return too_close
예제 #10
0
    def acquire_usd(self,num_sample):
        all_sample=[]
        sumdist = 0
        for test_num_sample in range(num_sample):
            dist = gopigo.us_dist(15)
            all_sample.append(dist)
            if dist<self.lim and dist>=0:
                sumdist=dist+sumdist
            else:
                sumdist=self.lim+sumdist


        dist = 	sumdist/num_sample
        #print "acquire usd mid :",dist
        return all_sample
예제 #11
0
파일: acc.py 프로젝트: mkolacki/gopigo
    def observe_obstacle(self, dt):
        prev_dist = self.obstacle_distance
        self.obstacle_distance = gopigo.us_dist(gopigo.USS)

        if self.obstacle_distance is 0:
            self.obstacle_distance = None
        elif self.obstacle_distance is -1:
            self.obstacle_distance = None
        else:
            if prev_dist is not None:
                self.obstacle_velocity = (self.obstacle_distance -
                                          prev_dist) / dt + (
                                              self.current_speed_right +
                                              self.current_speed_left) / 2.0
            else:
                self.obstacle_velocity = None
예제 #12
0
파일: acc.py 프로젝트: xmorton/gopigo
    def __process_commands(self):
        """
        Processes the next command in the ACC's command queue if there are any
        queued commands.
        """
        if not self.command_queue.empty():
            command = self.command_queue.get()
            if isinstance(command, commands.ChangeSettingsCommand):
                if command.userSetSpeed is not None:
                    self.user_set_speed = command.userSetSpeed
                else:
                    motor_speeds = gopigo.read_motor_speed()
                    self.user_set_speed = (motor_speeds[0] + motor_speeds[1]) / 2.0

                if command.safeDistance is not None:
                    self.safe_distance = command.safeDistance
                else:
                    self.safe_distance = gopigo.us_dist(gopigo.USS)

            if isinstance(command, commands.TurnOffCommand):
                self.power_on = False
예제 #13
0
파일: acc.py 프로젝트: xmorton/gopigo
def get_dist():
    """
    Measures the distance of the obstacle from the rover.

    Uses a time.sleep call to try to prevent issues with pin writing and
    reading. (See official gopigo library)

    Returns error strings in the cases of measurements of -1 and 0, as -1
    indicates and error, and 0 seems to also indicate a failed reading.

    :return: The distance of the obstacle. (cm)
    :rtype: either[int, str]
    """
    time.sleep(0.01)
    dist = gopigo.us_dist(gopigo.USS)

    if dist == -1:
        return USS_ERROR
    elif dist == 0 or dist == 1:
        return NOTHING_FOUND
    else:
        return dist
예제 #14
0
파일: empathybot.py 프로젝트: CleoQc/GoPiGo
def wait_for_button():
	gopigo.stop()
	while (gopigo.digitalRead(button_pin) != 1):
		try:
			time.sleep(.5)
		except IOError:
			print ("Error")
	print "Button pressed!"
	
	gopigo.set_speed(gopigo_speed)
	distance = 100
	while (distance > distance_from_body):
		try:
			distance = gopigo.us_dist(distance_sensor_pin)
			print ("Distance: " + str(distance))
			time.sleep(.1)
			gopigo.fwd()
		except IOError:
			print ("Error")
	
	gopigo.stop()
	
	sound("Hello!")
def make_sweep():

    obstacle_range = numpy.zeros(NUMBER_OF_SCANS)
    index = 0
    gopigo.enable_servo()

    #Scan the vicinity in front
    for angle in range(SCAN_START, SCAN_END + SCAN_STEP, SCAN_STEP):

        gopigo.servo(angle)
        time.sleep(SLEEP_TIME_BETWEEN_STEPS)
        distance = gopigo.us_dist(PORT_A1)

        #Take into account sense lmits
        if distance < SENSE_LIMIT and distance >= 0:
            obstacle_range[index] = distance - AVERAGE_DISTANCE_ERROR
        else:
            obstacle_range[index] = SENSE_LIMIT

        index += 1

    gopigo.disable_servo()
    return obstacle_range
예제 #16
0
def wait_for_button():
    gopigo.stop()
    while (gopigo.digitalRead(button_pin) != 1):
        try:
            time.sleep(.5)
        except IOError:
            print("Error")
    print "Button pressed!"

    gopigo.set_speed(gopigo_speed)
    distance = 100
    while (distance > distance_from_body):
        try:
            distance = gopigo.us_dist(distance_sensor_pin)
            print("Distance: " + str(distance))
            time.sleep(.1)
            gopigo.fwd()
        except IOError:
            print("Error")

    gopigo.stop()

    sound("Hello!")
def make_sweep():
	
	obstacle_range = numpy.zeros(NUMBER_OF_SCANS)
	index = 0
	gopigo.enable_servo()

	#Scan the vicinity in front
	for angle in range(SCAN_START, SCAN_END + SCAN_STEP, SCAN_STEP):

		gopigo.servo(angle)
		time.sleep(SLEEP_TIME_BETWEEN_STEPS)
		distance = gopigo.us_dist(PORT_A1)
		
		#Take into account sense lmits
		if distance < SENSE_LIMIT and distance >= 0:
			obstacle_range[index] = distance - AVERAGE_DISTANCE_ERROR
		else:
			obstacle_range[index] = SENSE_LIMIT

		index += 1		

	gopigo.disable_servo()
	return obstacle_range
예제 #18
0
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
'''

import time
import gopigo

# Connect the Grove distance sensor to digital port D10 or A1
#   The pin should be 15 for A11 port and 10 for the D10 digital port or you can call the analogPort or digitalPort definitions from the GoPiGo library
#   The distance sensor won't work on any other port
#   The data form the digitalPort is much more cleaner than the analog port

distance_sensor_pin = gopigo.digitalPort
# distance_sensor_pin = gopigo.analogPort

while True:
    try:
        print (gopigo.us_dist(distance_sensor_pin))
        time.sleep(.5)

    except IOError:
        print ("Error")
예제 #19
0
파일: __init__.py 프로젝트: tsceats/gopigo
def dist():
    distances = []
    for i in range(3):
        distances.append(gpg.us_dist(15))
    distances.sort()
    return distances[1]
예제 #20
0
 def read(self):
     return gopigo.us_dist(PORTS[self.port])
예제 #21
0
 def is_too_close(self):
     if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
         return True
     return False
예제 #22
0
def distanceToObstacle():
    global CURR_DIST
    dist = go.us_dist(15)
    if(dist != 0):
        CURR_DIST = dist
    return CURR_DIST
예제 #23
0
#!/usr/bin/env python
# Guardian

import gopigo, time, soundplayer

# Should go forward until distance less than 10

while True:
    distance = gopigo.us_dist(15)
    if distance < 10:
        gopigo.stop()
        soundplayer.main(["~/sounds/brownalert.mp3"])
    else:
        gopigo.fwd()
예제 #24
0
    def process_command(self, command):
        parts = command.split("/")

        if parts[1] == "poll":
            print "poll"
            self.us_dist = gopigo.us_dist(usdist_pin)
            self.enc_status = gopigo.read_status()[0]
            self.volt = gopigo.volt()
            self.fw_ver = gopigo.fw_ver()
            self.trim = gopigo.trim_read() - 100

            if self.enc_status == 0:
                self.waitingOn = None
        elif parts[1] == "stop":
            gopigo.stop()
        elif parts[1] == "trim_write":
            gopigo.trim_write(int(parts[2]))
            self.trim = gopigo.trim_read()
        elif parts[1] == "trim_read":
            self.trim = gopigo.trim_read() - 100
        elif parts[1] == "set_speed":
            if parts[2] == "left":
                self.left_speed = int(parts[3])
            elif parts[2] == "right":
                self.right_speed = int(parts[3])
            else:
                self.right_speed = int(parts[3])
                self.left_speed = int(parts[3])
            gopigo.set_left_speed(self.left_speed)
            gopigo.set_right_speed(self.right_speed)
        elif parts[1] == "leds":
            val = 0
            if parts[3] == "on":
                val = 1
            elif parts[3] == "off":
                val = 0
            elif parts[3] == "toggle":
                val = -1

            if parts[2] == "right" or parts[2] == "both":
                if val >= 0:
                    self.ledr = val
                else:
                    self.ledr = 1 - self.ledr

            if parts[2] == "left" or parts[2] == "both":
                if val >= 0:
                    self.ledl = val
                else:
                    self.ledl = 1 - self.ledl

            gopigo.digitalWrite(ledr_pin, self.ledr)
            gopigo.digitalWrite(ledl_pin, self.ledl)
        elif parts[1] == "servo":
            gopigo.servo(int(parts[2]))
        elif parts[1] == "turn":
            self.waitingOn = parts[2]
            direction = parts[3]
            amount = int(parts[4])
            encleft = 0 if direction == "left" else 1
            encright = 1 if direction == "left" else 0
            gopigo.enable_encoders()
            gopigo.enc_tgt(encleft, encright, int(amount / DPR))
            if direction == "left":
                gopigo.left()
            else:
                gopigo.right()
        elif parts[1] == "move":
            self.waitingOn = int(parts[2])
            direction = parts[3]
            amount = int(parts[4])
            gopigo.enable_encoders()
            gopigo.enc_tgt(1, 1, amount)
            if direction == "backward":
                gopigo.bwd()
            else:
                gopigo.fwd()
        elif parts[1] == "beep":
            gopigo.analogWrite(buzzer_pin, self.beep_volume)
            time.sleep(self.beep_time)
            gopigo.analogWrite(buzzer_pin, 0)
        elif parts[1] == "reset_all":
            self.ledl = 0
            self.ledr = 0

            gopigo.digitalWrite(ledl_pin, self.ledl)
            gopigo.digitalWrite(ledr_pin, self.ledr)
            gopigo.analogWrite(buzzer_pin, 0)
#           gopigo.servo(90)
            gopigo.stop()
예제 #25
0
 def is_too_close(self):
     if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
         return True
     return False
예제 #26
0
 def read(self):
     return gopigo.us_dist(PORTS[self.port])
예제 #27
0
import gopigo as go

speed = input("Enter target speed")
stop_dist = input("Enter target stop distance")

trim_write(speed)
go.forward()

stopped = False

while True:
    dist = go.us_dist(15)
    if stopped and dist > stop_dist:
        go.forward()
    elif dist < stop_dist:
        go.stop()
        stopped = True
예제 #28
0
import gopigo as go
import time

go.set_speed(100)
#go.trim_write(-5)
while True:

    DISTANCE_TO_OBJECT = go.us_dist(15)
    go.forward()

    if (DISTANCE_TO_OBJECT <= 20):
        #go.trim_write(-5)
        #go.forward()
        #time.sleep(3)
        go.left()
        time.sleep(1)
        #go.set_speed(200)
        go.forward()
        time.sleep(1)
        go.right()
        time.sleep(1)
        #go.set_speed(200)
        go.forward()
        #time.sleep(2)
        #go.right()
        #time.sleep(1)
        #go.forward()
        #time.sleep(1)
        #go.left()
        #time.sleep(1)
        #go.forward()
예제 #29
0
import gopigo as go

critical_distance = input("Enter Critical Stop Distance: ")
slowdown_distance = input("Enter Distance where Slowdown begins: ")
initial_speed = input("Input starting speed: ")
target_speed = input("Input target speed: ")

temp_speed = initial_speed

go.set_speed(initial_speed)
go.forward()


while True:
    
    us_Distance = go.us_dist(15)


    
    if (us_Distance <= critical_distance):
    	go.stop()
    elif ((us_Distance <= slowdown_distance)): 
        if(target_speed < initial_speed):
	    while(temp_speed > target_speed):
                us_Distance = go.us_dist(15)
               # print "Distance to object: ", us_Distance, " Critical Distance: ", critical_distance
                if (us_Distance <= critical_distance):
                #    print "INSIDE: Distance to object: ", us_Distance, " Critical Distance: ", critical_distance
    	            break
        	temp_speed = temp_speed - 2
                #print "Current TempSpeed: ", temp_speed, "Current TargetSpeed: ", target_speed
예제 #30
0
파일: rover.py 프로젝트: GDave50/GoPiGo
def distanceToObstacle():
    return go.us_dist(15)
예제 #31
0
 def on_loop(self):
     if self._usdist:
         self.dist = gopigo.us_dist(15)      
예제 #32
0
import gopigo as go

targetSpeed = input("Enter target speed: ")
stop_dist = input("Enter target stop distance: ")

speed = 90

go.set_speed(speed)

max_speed = 150

go.forward()

stopped = False

while True:
    dist = go.us_dist(25)
    if speed < targetSpeed:
        go.increase_speed()
    if (speed == max_speed):
        go.set_speed(max_speed)
    elif dist < stop_dist:
        go.stop()
        stopped = True
    print "Speed = ", speed, "|", "Sonar = ", dist
예제 #33
0
파일: gopi_proxy.py 프로젝트: sumsted/gopi
def us_dist(kargs):
    r = {'return_value': gopigo.us_dist(int(kargs['pin']))}
    return r