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
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)
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
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)
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
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
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
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
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
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
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
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
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
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
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
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")
def dist():
distances = []
for i in range(3):
distances.append(gpg.us_dist(15))
distances.sort()
return distances[1]
def read(self):
return gopigo.us_dist(PORTS[self.port])
def is_too_close(self):
if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
return True
return False
def distanceToObstacle():
global CURR_DIST
dist = go.us_dist(15)
if(dist != 0):
CURR_DIST = dist
return CURR_DIST
#!/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()
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()
def is_too_close(self):
if gopigo.us_dist(PORTS[self.port]) < self.get_safe_distance():
return True
return False
def read(self):
return gopigo.us_dist(PORTS[self.port])
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
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()
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
def distanceToObstacle():
return go.us_dist(15)
def on_loop(self):
if self._usdist:
self.dist = gopigo.us_dist(15)
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
def us_dist(kargs):
r = {'return_value': gopigo.us_dist(int(kargs['pin']))}
return r
