def handleCommand(command, keyPosition, price=0):
# only uses pressing down of keys
if keyPosition != "down":
return
print("handle command", command, keyPosition)
if command == 'L':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'R':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'F':
gopigo.forward()
time.sleep(0.4)
gopigo.stop()
if command == 'B':
gopigo.backward()
time.sleep(0.3)
gopigo.stop()
robot_util.handleSoundCommand(command, keyPosition, price)
def forward(self):
_grab_read()
try:
gopigo.forward()
except:
pass
_release_read()
def command_handler(Line_list):
print(Line_list)
for command in Line_list:
print(command)
if command == "Forward":
curr = absolute_line_pos()
while True:
curr = absolute_line_pos()
if curr == mid or curr == small_l1 or curr == small_l or curr == small_r or curr == small_r1:
run_gpg(curr)
if curr == all_white:
gopigo.stop()
time.sleep(1)
temp = absolute_line_pos()
temp = absolute_line_pos()
if temp == all_white:
print("White found")
break
else:
gopigo.forward()
if curr == all_black:
break
print(curr)
elif command == "Left":
turn_left()
time.sleep(1)
elif command == "Right":
turn_right()
time.sleep(1)
elif command == "Turn Around":
turn_around()
time.sleep(1)
def bang_bang(linelocation):
if (linelocation) < 300:
go.left()
elif (linelocation) > 300:
go.right()
else:
go.forward()
# This function should use the line location to implement a simple bang-bang controller.
# YOUR CODE HERE
return
def bang_bang_with_hysteresis(linelocation):
higher = 360
lower = 280
if (linelocation) < higher:
go.left()
elif (linelocation) > lower:
go.right()
else:
go.forward()
# This function should use the line location to implement bang-bang controller with hysteresis.
# YOUR CODE HERE
return
def turn_right(self):
gopigo.set_right_speed(0)
gopigo.set_left_speed(25)
gopigo.forward()
time.sleep(2)
while True:
self.read_position()
self.read_position()
self.read_position()
pos = self.read_position()
print(pos)
debug(pos)
if pos == "center":
gopigo.stop()
break
def forward(self):
_ifMutexAcquire(self.use_mutex)
try:
val = gopigo.forward()
except Exception as e:
print("easygopigo fwd: {}".format(e))
pass
_ifMutexRelease(self.use_mutex)
def follow_line(self, fwd_speed = 80):
slight_turn_speed=int(.7*fwd_speed)
while True:
pos = self.read_position()
debug(pos)
if pos == "center":
gopigo.forward()
elif pos == "left":
gopigo.set_right_speed(0)
gopigo.set_left_speed(slight_turn_speed)
elif pos == "right":
gopigo.set_right_speed(slight_turn_speed)
gopigo.set_left_speed(0)
elif pos == "black":
gopigo.stop()
elif pos == "white":
gopigo.stop()
def forward(self):
_ifMutexAcquire(self.use_mutex)
try:
val = gopigo.forward()
except Exception as e:
print("easygopigo fwd: {}".format(e))
pass
_ifMutexRelease(self.use_mutex)
def run(self, command):
if command == 'L':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
elif command == 'R':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
elif command == 'F':
gopigo.forward()
time.sleep(0.35)
gopigo.stop()
elif command == 'B':
gopigo.backward()
time.sleep(0.35)
gopigo.stop()
def moveGoPiGo2(command):
if command == 'L':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'R':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'F':
gopigo.forward()
time.sleep(0.35)
gopigo.stop()
if command == 'B':
gopigo.backward()
time.sleep(0.35)
gopigo.stop()
def follow_line(self, fwd_speed = 80):
slight_turn_speed=int(.7*fwd_speed)
while True:
pos = self.read_position()
debug(pos)
if pos == "center":
gopigo.forward()
elif pos == "left":
gopigo.set_right_speed(0)
gopigo.set_left_speed(slight_turn_speed)
elif pos == "right":
gopigo.set_right_speed(slight_turn_speed)
gopigo.set_left_speed(0)
elif pos == "black":
gopigo.stop()
elif pos == "white":
gopigo.stop()
def forward(self):
_grab_read()
try:
val = gopigo.forward()
except Exception as e:
print("easygopigo fwd: {}".format(e))
pass
_release_read()
def move(args):
command = args['command']
if command == 'L':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'R':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'F':
gopigo.forward()
time.sleep(0.35)
gopigo.stop()
if command == 'B':
gopigo.backward()
time.sleep(0.35)
gopigo.stop()
def move(args):
command = args['button']['command']
if command == 'l':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'r':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'f':
gopigo.forward()
time.sleep(0.35)
gopigo.stop()
if command == 'b':
gopigo.backward()
time.sleep(0.35)
gopigo.stop()
def follow_line(self, fwd_speed=30):
slight_turn_speed = int(35)
print("FOLLOWING LINE")
while True:
self.read_position()
self.read_position()
#print(self.read())
pos = self.read_position()
#print(pos)
debug(pos)
if pos == "center":
gopigo.set_speed(30)
gopigo.forward()
elif pos == "wayleft":
gopigo.set_right_speed(25)
gopigo.set_left_speed(40)
elif pos == "wayright":
gopigo.set_right_speed(40)
gopigo.set_left_speed(25)
elif pos == "left":
gopigo.set_right_speed(25)
gopigo.set_left_speed(slight_turn_speed)
elif pos == "right":
gopigo.set_right_speed(slight_turn_speed)
gopigo.set_left_speed(25)
elif pos == "white":
print("white Brake")
gopigo.stop()
break
elif pos == "intersection":
gopigo.stop()
break
elif pos == "unknown":
print("Unknown???")
gopigo.stop()
break
else:
break
def main():
go.set_speed(MIN_SPEED)
INIT_LEFT_ENC, INIT_RIGHT_ENC = read_encoders()
print('Left: ', INIT_LEFT_ENC, ' Right: ', INIT_RIGHT_ENC)
time.sleep(5)
go.forward()
while True:
try:
if STATE == 3:
go.stop()
else:
go.forward()
print 'MOTORS: ', go.read_motor_speed()
rs.rs(INIT_LEFT_ENC, INIT_RIGHT_ENC)
stateCheck()
# Shuts the ACC down when a Ctrl + c command is issued
except KeyboardInterrupt:
print '\nACC shut off'
go.stop()
sys.exit()
def slowing(state):
if (getCurrentSpeed() >= MIN_SPEED):
go.set_speed(getCurrentSpeed() - 10)
go.forward()
else:
state.state = 0
import gopigo as gpg import time import time gpg.forward() time.sleep(10) gpg.stop() gpg.right() time.sleep(1) gpg.stop() gpg.forward() time.sleep(10) gpg.stop() gpg.left() time.sleep(5) gpg.stop() gpg.forward() time.sleep(5) gpg.stop()
def accelerating(state):
if (getCurrentSpeed() < MAX_SPEED):
go.set_speed(getCurrentSpeed() + 10)
go.forward()
else:
state.state = 0
def normal():
go.forward()
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
def follow_line(self):
slight_turn_speed = int(50)
default_speed = int(35)
wayoff_turn_speed = int(115)
print("FOLLOWING LINE")
gopigo.set_speed(default_speed)
gopigo.forward()
time.sleep(1)
while True:
self.read_position()
self.read_position()
#print(self.read())
pos = self.read_position()
#print(pos)
debug(pos)
if pos == "center":
gopigo.set_speed(default_speed)
gopigo.forward()
elif pos == "wayleft":
gopigo.set_right_speed(default_speed)
gopigo.set_left_speed(wayoff_turn_speed)
elif pos == "wayright":
gopigo.set_right_speed(wayoff_turn_speed)
gopigo.set_left_speed(default_speed)
elif pos == "left":
gopigo.set_right_speed(default_speed)
gopigo.set_left_speed(slight_turn_speed)
elif pos == "right":
gopigo.set_right_speed(slight_turn_speed)
gopigo.set_left_speed(default_speed)
elif pos == "white":
print("white Brake")
gopigo.stop()
time.sleep(1)
whiteTest = self.read_position()
whiteTest = self.read_position()
if whiteTest == "white":
print("really white")
break
else:
gopigo.forward()
elif pos == "intersection":
gopigo.stop()
time.sleep(1)
gopigo.set_right_speed(40)
gopigo.set_left_speed(40)
gopigo.forward()
time.sleep(1.2)
gopigo.stop()
time.sleep(1)
intersectionTest = self.read_position()
intersectionTest = self.read_position()
print("Reading found!" + intersectionTest)
gopigo.backward()
time.sleep(1.5)
gopigo.stop()
if intersectionTest != "white":
print("interesection hit!")
else:
print("T intersection hit!")
break
elif pos == "left corner" or "right corner":
gopigo.stop()
time.sleep(1)
gopigo.set_right_speed(40)
gopigo.set_left_speed(40)
gopigo.forward()
time.sleep(1.2)
gopigo.stop()
time.sleep(1)
intersectionTest = self.read_position()
intersectionTest = self.read_position()
print("Reading found! " + intersectionTest)
gopigo.backward()
time.sleep(1.5)
gopigo.stop()
if intersectionTest != "white":
print("T interesection hit!")
break
else:
print("corner! turning!")
if pos == "left corner":
self.turn_left()
else:
self.turn_right()
gopigo.forward()
time.sleep(.5)
elif pos == "unknown":
print("Unknown???")
#gopigo.stop()
#break
else:
break
print(pos)
gopigo.stop()
def forward():
I2C_Mutex_Acquire()
gopigo.forward()
I2C_Mutex_Release()
def forward():
_wait_for_read()
_grab_read()
gopigo.forward()
_release_read()
def forward():
_wait_for_read()
_grab_read()
gopigo.forward()
_release_read()
