def trim_write(self, set_trim_to):
_grab_read()
try:
gopigo.trim_write(int(set_trim_to))
except:
pass
_release_read()
def trim_write(self,set_trim_to):
_ifMutexAcquire(self.use_mutex)
try:
gopigo.trim_write(int(set_trim_to))
except:
pass
_ifMutexRelease(self.use_mutex)
def trim_write(self,set_trim_to):
_ifMutexAcquire(self.use_mutex)
try:
gopigo.trim_write(int(set_trim_to))
except:
pass
_ifMutexRelease(self.use_mutex)
def __power_on(self):
"""
Initializes the state of the rover and ACC in order to prepare for the
ACC to start controlling the rover.
time.sleep calls are used here to try to reduce the chance of issues
with writing to and reading from the pins used for interacting with
the rover. The developers of the official python gopigo library did
this in, so we figured it would be a good idea to do it in this
function as it may reduce the chance of bad initial readings while only
having a one time reduction in the operation time of the ACC.
"""
self.power_on = True
# Set the trim value for the rover's motors. Even if the trim value is
# 0 it should still be set in order to clear out any previous trim
# setting.
gopigo.trim_write(TRIM)
# Get battery voltage in order to make it easy to tell when the battery
# is getting low, and could thus effect performance of the rover
time.sleep(0.1)
volt = gopigo.volt()
self.system_info.setStartupVoltage(volt)
# Read the encoder tick amounts at startup to allow all future encoder
# tick readings to be relative to the rover's state at the startup of
# the ACC. This allows the ACC to maintain the rover's direction at
# startup as straight.
time.sleep(0.1)
self.initial_ticks_left = gopigo.enc_read(gopigo.LEFT)
time.sleep(0.1)
self.initial_ticks_right = gopigo.enc_read(gopigo.RIGHT)
def calibrate(speed):
gpg.trim_write(0)
sleep(2)
set_left_speed(speed)
set_right_speed(speed)
left = -1
right = 1
sp = 0
while left != right:
both = oneRun()
both2 = oneRun()
left = both[0] + both2[0]
right = both[1] + both2[1]
if left > right + 1:
sp -= 1
elif (left + 1 < right):
sp += 1
print left, right, sp
gpg.trim_write(sp)
sleep(2)
def trim_write_button_OnButtonClick(self, event):
global slider_val
gopigo.trim_write(slider_val)
def trim_write_button_OnButtonClick(self,event):
global slider_val
gopigo.trim_write(slider_val)
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 main():
speed = INITIAL_SPEED
gopigo.trim_write(TRIM)
time.sleep(0.1)
print("Volt: " + str(gopigo.volt()))
time.sleep(0.1)
initial_ticks_left = enc_read(LEFT)
time.sleep(0.1)
initial_ticks_right = enc_read(RIGHT)
print("Initial\tL: " + str(initial_ticks_left) + "\tR: " +
str(initial_ticks_right))
print("Critical: " + str(CRITICAL_DISTANCE))
print("Safe: " + str(SAFE_DISTANCE))
print("Alert: " + str(ALERT_DISTANCE))
dists = collections.deque(maxlen=SAMPLE_SIZE)
dts = collections.deque(maxlen=SAMPLE_SIZE)
elapsed_ticks_left = 0
elapsed_ticks_right = 0
try:
gopigo.set_speed(0)
gopigo.fwd()
t = time.time()
while True:
if speed < 0:
speed = 0
print("========================")
#if not command_queue.empty():
# comm = command_queue.get()
# global MAX_SPEED
# global SAFE_DISTANCE
# MAX_SPEED = comm[0]
# SAFE_DISTANCE = comm[1]
# print(comm)
dt = time.time() - t
t = time.time()
#time.sleep(0.1)
print("Time: " + str(dt))
dist = get_dist()
print("Dist: " + str(dist))
if not isinstance(dist, str):
dists.append(float(dist))
dts.append(float(dt))
rel_speed = None
if len(dists) > 9:
rel_speed = calculate_relative_speed(dists, dts)
print("Rel speed: " + str(rel_speed))
if (isinstance(dist, str)
and dist != NOTHING_FOUND) or dist < CRITICAL_DISTANCE:
print("< Critical")
stop_until_safe_distance()
speed = 0
t = time.time()
elif dist < SAFE_DISTANCE:
print("< Safe")
if speed > STOP_THRESHOLD:
#speed = speed - dt * SPEED_DECCELLERATION
speed = speed - dt * get_deccelleration(speed)
else:
speed = 0
elif speed > MAX_SPEED:
print("Slowing down")
speed = speed - dt * SLOWING_DECCELLERATION
elif dist < ALERT_DISTANCE and rel_speed is not None:
speed = handle_alert_distance(speed, rel_speed, dt)
elif speed < MAX_SPEED:
print("Speeding up")
speed = speed + dt * SPEED_ACCELERATION
#speed = speed - dt * get_deccelleration(speed)
elapsed_ticks_left, elapsed_ticks_right = \
read_enc_ticks(initial_ticks_left, initial_ticks_right)
print("L: " + str(elapsed_ticks_left) + "\tR: " +
str(elapsed_ticks_right))
l_diff, r_diff = straightness_correction(speed, elapsed_ticks_left,
elapsed_ticks_right)
if elapsed_ticks_left >= 0 and elapsed_ticks_right >= 0:
set_speed_lr(speed, l_diff, r_diff)
else:
set_speed_lr(speed, 0, 0)
print("Speed: " + str(speed))
except (KeyboardInterrupt, Exception):
traceback.print_exc()
gopigo.stop()
gopigo.stop()
SPEED = int(sys.argv[1])
INC = (SPEED / 100) * 10 #7#15
SAFE_DISTANCE = 45
USS = 15
TRIM = int(sys.argv[2])
CORRECTION = sys.argv[3] == "true"
print(CORRECTION)
# Might as well try this out, probably doesn't do anything though
gopigo.disable_encoders()
gopigo.enable_encoders()
gopigo.trim_write(TRIM)
ADDRESS = 0x08
ENC_READ_CMD = [53]
import smbus
bus = smbus.SMBus(1)
def write_i2c_block(address, block):
try:
op = bus.write_i2c_block_data(address, 1, block)
time.sleep(0.005)
return op
except IOError:
return -1
# Sets the speed of the motors.
# speed1: speed of motor1
# speed2: speed of motor2
def set_motors(speed1, speed2):
go.motor1(1, speed1)
go.motor2(1, speed2)
# Get program parameters
target_speed = input("Enter target speed ")
stop_dist = input("Enter target stop distance ")
#trim = input ("Enter trim ")
print "Starting rover"
go.trim_write(-10)
#Make rover start moving
set_motors(target_speed, target_speed)
stopped = False
while True:
dist = go.us_dist(15)
# If stopped but no object is too close, resume
# Else if moving and an object is too close, stop
if stopped and dist > stop_dist:
set_motors(target_speed, target_speed)
stopped = False
elif not stopped and dist < stop_dist:
import gopigo as go
import time
def set_motors(s1, s2):
go.motor1(1, s1)
go.motor2(1, s2)
go.trim_write(0)
lwt = go.enc_read(0)
rwt = go.enc_read(1)
oldlwt = lwt
oldrwt = rwt
target_speed = input("Speed?")
lms = target_speed
rms = target_speed
print "Starting"
set_motors(target_speed, target_speed)
while True:
oldlwt = lwt
oldrwt = rwt
lwt = go.enc_read(0)
rwt = go.enc_read(1)
lwtdiff = lwt - oldlwt
rwtdiff = rwt - oldrwt
error = rwtdiff - lwtdiff
print "L, R, E: ", lwtdiff, rwtdiff, error
def trim_write(kargs):
r = {'return_value': gopigo.trim_write(int(kargs['value']))}
return r
