def turn_slight_right():
if msg_en:
print("Turn slight right")
if gpg_en:
gopigo.set_right_speed(fwd_speed)
gopigo.set_left_speed(slight_turn_speed)
gopigo.fwd()
def set_left_speed(self,new_speed):
_ifMutexAcquire(self.use_mutex)
try:
gopigo.set_left_speed(new_speed)
except:
pass
_ifMutexRelease(self.use_mutex)
def turn_fast_right():
if msg_en:
print("Turn fast left")
if gpg_en:
gopigo.set_right_speed(fast_turn_speed)
gopigo.set_left_speed(fwd_speed)
gopigo.fwd()
def set_left_speed(self, new_speed):
_grab_read()
try:
gopigo.set_left_speed(new_speed)
except:
pass
_release_read()
def actualize_power(self):
if self.stop:
gopigo.set_left_speed(0)
gopigo.set_right_speed(0)
else:
gopigo.set_left_speed(self.left_power)
gopigo.set_right_speed(self.right_power)
def setMotorJoystickPos( self, joystickX, joystickY ):
joystickX, joystickY = self.normaliseJoystickData( joystickX, joystickY )
if debug:
print "Left joy",joystickX, joystickY
print self.speed_l*joystickY
gopigo.set_left_speed(int(self.speed_l*joystickY))
gopigo.fwd()
def __init__(self, recordFreq=0.5, camera=None, stream=None):
"""this function intializes everything, including setting the speed"""
go.set_right_speed(50)
go.set_left_speed(50)
go.enable_encoders()
self.camera = camera
self.stream = stream
def set_left_speed(self,new_speed):
_ifMutexAcquire(self.use_mutex)
try:
gopigo.set_left_speed(new_speed)
except:
pass
_ifMutexRelease(self.use_mutex)
def set_speed_lr(speed, l_diff, r_diff):
if speed >= MIN_SPEED:
gopigo.set_left_speed(int(speed + l_diff))
gopigo.set_right_speed(int(speed + r_diff))
else:
gopigo.set_left_speed(0)
gopigo.set_right_speed(0)
def turn_slight_left(): if msg_en: print "Turn slight left" if gpg_en: gopigo.set_right_speed(slight_turn_speed) gopigo.set_left_speed(fwd_speed) gopigo.fwd()
def rescue():
gopigo.stop()
print("Rescue")
time.sleep(1)
gopigo.set_left_speed(130)
gopigo.set_right_speed(130)
gopigo.fwd()
time.sleep(0.2)
def rotate_left(self):
_grab_read()
try:
gopigo.set_right_speed(25)
gopigo.set_left_speed(0)
except Exception as e:
print("idk wtf")
pass
def forwardTicks(distance, speed):
distance = max(1, min(distance, 500))
speed = max(50, min(speed, 200))
leftTicks = distance
rightTicks = distance
right_speed = speed
left_speed = speed
set_left_speed(left_speed)
set_right_speed(right_speed)
leftStart = gpg.enc_read(0)
rightStart = gpg.enc_read(1)
leftTarget = leftStart + leftTicks
rightTarget = rightStart + rightTicks
isLeftMoving = False
isRightMoving = False
adjustment_interval = 1
last_left_check = leftStart
last_right_check = rightStart
while(True):
leftReading = gpg.enc_read(0)
rightReading = gpg.enc_read(1)
leftToEnd = leftTarget - leftReading
rightToEnd = rightTarget - rightReading
if leftReading >= leftTarget and rightReading >= rightTarget:
gpg.stop()
break
elif leftReading < leftTarget and rightReading < rightTarget:
new_left_speed = speed
new_right_speed = speed
if (leftToEnd > rightToEnd + 1):
extraFactor = float(leftToEnd - rightToEnd) / leftToEnd
extraFactor = max(0.02, min(0.15, extraFactor))
new_left_speed = int(speed * (1.0 + extraFactor))
elif (rightToEnd > leftToEnd + 1):
extraFactor = float(rightToEnd - leftToEnd) / rightToEnd
extraFactor = max(0.02, min(0.15, extraFactor))
new_right_speed = int(speed * (1.0 + extraFactor))
if (left_speed != new_left_speed):
set_left_speed(new_left_speed)
left_speed = new_left_speed
if (right_speed != new_right_speed):
set_right_speed(new_right_speed)
right_speed = new_right_speed
if (not isLeftMoving) or (not isRightMoving):
print "Forward!"
gpg.motor_fwd()
isLeftMoving = True
isRightMoving = True
def get(self):
left = int(self.get_argument("left"))
right = int(self.get_argument("right"))
gopigo.set_left_speed(abs(left))
gopigo.set_right_speed(abs(right))
if (left > 0):
gopigo.fwd()
else:
gopigo.bwd()
def main(self, motor1, motor2):
"""
Takes in 2 ints and rotates accordingly
"""
gopigo.set_left_speed(motor1)
gopigo.set_right_speed(motor2)
if motor1 >= 0 and motor2 >= 0:
gopigo.fwd()
else:
gopigo.bwd()
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 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 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 around():
gopigo.set_left_speed(250)
gopigo.set_right_speed(250)
gopigo.left_rot()
time.sleep(1.30)
while True:
time.sleep(0.2)
marker, t = aruco.get_result()
if t > 0.01:
break
gopigo.stop()
gopigo.set_left_speed(250)
gopigo.set_right_speed(250)
gopigo.left_rot()
time.sleep(0.2)
gopigo.stop()
time.sleep(0.2)
def turn_around(self):
gopigo.set_right_speed(35)
gopigo.set_left_speed(35)
gopigo.backward()
time.sleep(1)
gopigo.stop()
time.sleep(1)
gopigo.left_rot()
time.sleep(2)
while True:
self.read_position()
self.read_position()
pos = self.read_position()
print(pos)
debug(pos)
if pos == "center":
gopigo.stop()
break
def __actualize_power(self, l_diff, r_diff):
"""
Applies the decided motor powers to the motors of the rover.
If the desired motor power is below the minimum allowed motor power,
then the motors are stopped to prevent issues with motor differences at
low motor powers.
:param l_diff: The additional motor power to apply to just the left
motor. (power units)
:param r_diff: The additional motor power to apply to just the right
motor. (power units)
"""
if self.speed >= MIN_SPEED:
gopigo.set_left_speed(int(self.speed + l_diff))
gopigo.set_right_speed(int(self.speed + r_diff))
else:
gopigo.set_left_speed(0)
gopigo.set_right_speed(0)
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 do_turn(d):
global turn_angle, compass
gopigo.set_left_speed(210)
gopigo.set_right_speed(210)
time.sleep(0.1)
compass.turn_c(d)
if d == "left":
gopigo.turn_left_wait_for_completion(turn_angle)
elif d == "around":
around()
gopigo.stop()
else:
try:
gopigo.turn_right_wait_for_completion(turn_angle)
except TypeError:
do_turn(d)
time.sleep(0.1)
gopigo.fwd()
drive_forwards(0.5)
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 correctLeft(speed):
print "Correcting LEFT!"
go.set_left_speed(speed - correctionNeeded(speed))
#Corrects wheel size offset by multiplying by 2.
go.set_right_speed(speed + correctionNeeded(speed) * 2)
def correctRight(speed):
print "Correcting RIGHT!"
go.set_left_speed(speed + correctionNeeded(speed))
go.set_right_speed(speed - correctionNeeded(speed))
def set_left_speed(speed=100):
go.set_left_speed(speed)
def set_slave(speed):
#go.set_right_speed(speed)
go.set_left_speed(speed)
def main():
global network, state, prev_marker, algoInstance
if network is None:
# Setup network
ip = "localhost"
with open("server.ip") as f:
ip = f.read()
x, y = 1, 0
network = netemuclient.NetEmuClient(rec, ip, 8080)
network.start()
network.waitForMaze()
network.position(x, y)
network.txpower(0.02)
if algoInstance is None:
# If the program is started with arguments, this will load the last saved state
if len(sys.argv) > 1:
with open("last_state.pickle", "rb") as f_pickle:
algoInstance = pickle.loads(f_pickle.read())
algoInstance.restoreState(network)
network.position(*algoInstance.position)
# Start the algorithm without a saved state
else:
algoInstance = algo.Algorithm(network, (x, y))
# Give everything time to warm up
time.sleep(2)
gopigo.set_left_speed(0)
gopigo.set_right_speed(0)
gopigo.fwd()
save_timer = time.time()
# Save the latest encoder reading
save_enc = (0, 0)
while True:
# Move in the alorithm
algoInstance.step()
# Call the latest camera results
(marker, t) = aruco.get_result()
# GoPiGo is not very stable, this block is just to make it stable
if save_timer + 3 < time.time():
try:
new_enc = (gopigo.enc_read(0), gopigo.enc_read(1))
except TypeError:
print(
"GoPiGo breaks when you enc read sometimes just restart the main, the state should be fine"
)
gopigo.stop()
time.sleep(0.2)
gopigo.stop()
main()
# We have been stopping while we should be driving
if new_enc == save_enc and state == State.DRIVE:
rescue()
save_enc = new_enc
# Update the state
state = change_state(marker, t)
if state == State.DRIVE:
drive_forwards(t)
elif state == State.STOP:
gopigo.stop()
elif state == State.TURN_LEFT:
do_turn("left")
elif state == State.TURN_RIGHT:
do_turn("right")
elif state == State.TURN_AROUND:
do_turn("around")
time.sleep(0.001)
else:
raise ValueError
def turn_right_around(self):
gopigo.set_right_speed(0)
gopigo.set_left_speed(50)
time.sleep(5)
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 do_command(command=None):
logging.debug(command)
if command in ["forward", "fwd"]:
gopigo.fwd()
elif command == "left":
gopigo.left()
elif command == "left_rot":
gopigo.left_rot()
elif command == "right":
gopigo.right()
elif command == "right_rot":
gopigo.right_rot()
elif command == "stop":
gopigo.stop()
elif command == "leftled_on":
gopigo.led_on(0)
elif command == "leftled_off":
gopigo.led_off(0)
elif command == "rightled_on":
gopigo.led_on(1)
elif command == "rightled_off":
gopigo.led_off(1)
elif command in ["back", "bwd"]:
gopigo.bwd()
elif command == "speed":
logging.debug("speed")
speed = flask.request.args.get("speed")
logging.debug("speed:" + str(speed))
if speed:
logging.debug("in if speed")
gopigo.set_speed(int(speed))
left_speed = flask.request.args.get("left_speed")
logging.debug("left_speed:" + str(left_speed))
if left_speed:
logging.debug("in if left_speed")
gopigo.set_left_speed(int(left_speed))
right_speed = flask.request.args.get("right_speed")
logging.debug("right_speed:" + str(right_speed))
if right_speed:
logging.debug("in if right_speed")
gopigo.set_right_speed(int(right_speed))
speed_result = gopigo.read_motor_speed()
logging.debug(speed_result)
return flask.json.jsonify({"speed": speed_result, "right": speed_result[0], "left": speed_result[1]})
elif command == "get_data":
speed_result = gopigo.read_motor_speed()
enc_right = gopigo.enc_read(0)
enc_left = gopigo.enc_read(1)
volt = gopigo.volt()
timeout = gopigo.read_timeout_status()
return flask.json.jsonify(
{
"speed": speed_result,
"speed_right": speed_result[0],
"speed_left": speed_result[1],
"enc_right": enc_right,
"enc_left": enc_left,
"volt": volt,
"timeout": timeout,
"fw_ver": gopigo.fw_ver(),
}
)
elif command in ["enc_tgt", "step"]:
tgt = flask.request.args.get("tgt")
direction = flask.request.args.get("dir")
if tgt:
gopigo.gopigo.enc_tgt(1, 1, int(tgt))
if dir:
if dir == "bwd":
gopigo.bwd()
else:
gopigo.fwd()
else:
gopigo.fwd()
return ""
def drive_forwards(target):
left, right = get_control_out(target)
time.sleep(0.001)
gopigo.set_left_speed(int(left))
gopigo.set_right_speed(int(right))
dist = data['us1']
except Exception as e: # Something went wrong extracting the JSON.
dist = -1 # Handle the situation.
print(e)
pass
if dist != -1: # If a JSON was correctly extracted, continue.
# Print received to the console
print("LS1: ", ls1, "LS2: ", ls2, "LS3: ", ls3, "LS4: ", ls4,
"LS5: ", ls5, "DIST: ", dist)
# 0 = black
# 1 = white
# Line following logic goes here
if ls5 == 1 and ls4 == 1:
go.set_left_speed(gospeed)
go.set_right_speed(gospeed)
go.fwd()
elif ls5 == 1 and ls4 != 1:
go.set_left_speed(gospeed)
go.set_right_speed(gospeed + 15)
go.fwd()
elif ls5 != 1 and ls4 == 1:
go.set_left_speed(gospeed + 15)
go.set_right_speed(gospeed)
go.fwd()
else:
go.fwd()
def defaultCorrection(speed):
print "Correcting!"
go.set_left_speed(speed - correctionNeeded(speed))
go.set_right_speed(speed + correctionNeeded(speed))
right_speed -= rotate_value
right_speed = min(max(right_speed, -1), 1)
left_speed = min(max(left_speed, -1), 1)
left_speed *= copysign(255, move_value)
right_speed *= copysign(255, move_value)
return [left_speed, right_speed]
while(True):
left_speed = 0
right_speed = 0
if controller.read():
move_value = controller.y
rotate_value = controller.x
[left_speed, right_speed] = calc_speed(move_value, rotate_value)
if left_speed != 0 or right_speed != 0:
print "controls: ({0}, {1})".format(controller.x, controller.y)
dir = "forward" if move_value >= 0 else "back"
print "{0} left:{1} right:{2}".format(dir, left_speed, right_speed)
gopigo.set_left_speed(int(abs(left_speed)))
gopigo.set_right_speed(int(abs(right_speed)))
if left_speed > 0:
if right_speed > 0:
gopigo.fwd()
else:
gopigo.right_rot()
else:
if right_speed > 0:
gopigo.left_rot()
else:
gopigo.bwd()
def set_left_speed(kargs):
r = {'return_value': gopigo.set_left_speed(int(kargs['speed']))}
return r
conn.send("Turning right")
elif data=='i':
gopigo.increase_speed()
conn.send("Increase speed")
elif data=='d':
gopigo.decrease_speed()
conn.send("Decrease speed")
elif data=='ledOn':
gopigo.led_on()
conn.send("Led ON")
elif data=='ledOff':
gopigo.led_off()
conn.send("Led OFF")
elif data=='set_right_speed':
gopigo.set_right_speed()
conn.send("Set Right Speed")
elif data=='set_left_speed':
gopigo.set_left_speed()
conn.send("Set Left Speed")
else:
print ("Invalid command")
conn.send("Invalid command")
conn.close()
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()
#!/usr/bin/env python # arguemtn : speed : 0 - 255 import gopigo gopigo.set_left_speed(sys.argv[1])
while dist < SAFE_DISTANCE:
dist = us_dist(USS)
time.sleep(0.01)
gopigo.fwd()
#time.sleep(0.1)
elapsed_ticks_left = enc_read(LEFT) - initial_ticks_left
#time.sleep(0.1)
elapsed_ticks_right = enc_read(RIGHT) - initial_ticks_right
print("L: " + str(elapsed_ticks_left) + "\tR: " +
str(elapsed_ticks_right))
if elapsed_ticks_left > elapsed_ticks_right:
print("RIGHT SLOW")
if CORRECTION:
gopigo.set_left_speed(SPEED - INC)
gopigo.set_right_speed(SPEED + INC)
elif elapsed_ticks_left < elapsed_ticks_right:
print("LEFT SLOW")
if CORRECTION:
gopigo.set_left_speed(SPEED + INC)
gopigo.set_right_speed(SPEED - INC)
else:
print("Equal")
if CORRECTION:
gopigo.set_left_speed(SPEED)
gopigo.set_right_speed(SPEED)
print(time.time() - t)
except Exception, e:
print(e)
