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 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 rotate_servo(self, servo_position):
if servo_position > 180:
servo_position = 180
if servo_position < 0:
servo_position = 0
gopigo.servo(servo_position)
def rotate_usd(self,servo_pos):
#print "rotate usd ",servo_pos
gopigo.servo(90+servo_pos)
sleep(self.delay)
def rotate_usd(self,servo_pos):
print "rotate ",servo_pos
gopigo.servo(90+servo_pos)
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 on_loop(self):
if self._servo:
if self.servo_pos!=self.old_servo_pos:
gopigo.servo(self.servo_pos)
self.old_servo_pos = self.servo_pos
pygame.time.delay(50)
def rotate_servo(self, servo_position):
if servo_position > 180:
servo_position = 180
if servo_position < 0:
servo_position = 0
gopigo.servo(servo_position)
def servo(kargs):
r = {'return_value': gopigo.servo(int(kargs['position']))}
return r
