def calculate_elapsed_ticks(self):
l_ticks = gopigo.enc_read(gopigo.LEFT)
r_ticks = gopigo.enc_read(gopigo.RIGHT)
if l_ticks is not None:
old_ticks = self.elapsed_ticks_left
self.elapsed_ticks_left = l_ticks - self.elapsed_ticks_left
self.n_ticks_l = self.elapsed_ticks_left - old_ticks
else:
self.elapsed_ticks_left = self.elapsed_ticks_left
self.n_ticks_l = 0
if r_ticks is not None:
old_ticks = self.elapsed_ticks_right
self.elapsed_ticks_right = r_ticks - self.elapsed_ticks_right
self.n_ticks_r = self.elapsed_ticks_right - old_ticks
else:
self.elapsed_ticks_right = self.elapsed_ticks_right
self.n_ticks_r = 0
if self.elapsed_ticks_left >= MAX_TICK_COUNT or (
self.elapsed_ticks_right >= MAX_TICK_COUNT):
carry_over_delta = self.elapsed_ticks_left - self.elapsed_ticks_right
if carry_over_delta > 0:
self.elapsed_ticks_left = carry_over_delta
self.elapsed_ticks_right = 0
elif carry_over_delta < 0:
self.elapsed_ticks_left = 0
self.elapsed_ticks_right = -carry_over_delta
else:
self.elapsed_ticks_left = 0
self.elapsed_ticks_right = 0
def __init__(self):
rospy.init_node('gopigo_state_updater')
# Read in tangential velocity targets
self.lwheel_angular_vel_motor_sub = rospy.Subscriber('lwheel_angular_vel_motor', Float32, self.lwheel_angular_vel_motor_callback)
self.rwheel_angular_vel_motor_sub = rospy.Subscriber('rwheel_angular_vel_motor', Float32, self.rwheel_angular_vel_motor_callback)
self.lwheel_angular_vel_control_pub = rospy.Subscriber('lwheel_angular_vel_control', Float32, self.lwheel_angular_vel_control_callback)
self.rwheel_angular_vel_control_pub = rospy.Subscriber('rwheel_angular_vel_control', Float32, self.rwheel_angular_vel_control_callback)
self.lwheel_angular_vel_enc_pub = rospy.Publisher('lwheel_angular_vel_enc', Float32, queue_size=10)
self.rwheel_angular_vel_enc_pub = rospy.Publisher('rwheel_angular_vel_enc', Float32, queue_size=10)
self.rate = rospy.get_param('~rate', 10)
self.err_tick_incr = rospy.get_param('~err_tick_incr',20) # Filter out clearly erroneous encoder readings
self.time_prev_update = rospy.Time.now();
self.gopigo_on = rospy.get_param('~gopigo_on',True)
if self.gopigo_on:
import gopigo
self.lwheel_encs = [gopigo.enc_read(1)]*5
self.rwheel_encs = [gopigo.enc_read(0)]*5
self.R = rospy.get_param('~robot_wheel_radius', .03)
# Need a little hack to incorporate direction wheels are spinning
self.lwheel_dir = 1;
self.rwheel_dir = 1;
self.rwheel_angular_vel_control = 0;
self.lwheel_angular_vel_control = 0;
def update(self):
if self.gopigo_on: # Running on actual robot
import gopigo
lwheel_enc = self.lwheel_dir * gopigo.enc_read(1) * .01 # cm's moved
rwheel_enc = self.rwheel_dir * gopigo.enc_read(0) * .01 # cm's moved
self.lwheel_encs = self.lwheel_encs[1:] + [lwheel_enc]
self.rwheel_encs = self.rwheel_encs[1:] + [rwheel_enc]
# History of past three encoder reading
time_curr_update = rospy.Time.now()
dt = (time_curr_update - self.time_prev_update).to_sec()
# Compute angular velocity in rad/s
lwheel_enc_delta = abs(self.lwheel_encs[-1]) - abs(self.lwheel_encs[-2])
rwheel_enc_delta = abs(self.rwheel_encs[-1]) - abs(self.rwheel_encs[-2])
lwheel_angular_vel_enc = self.enc_2_rads(lwheel_enc_delta) / dt
rwheel_angular_vel_enc = self.enc_2_rads(rwheel_enc_delta) / dt
# Adjust sign
if self.lwheel_encs[-1] < 0: lwheel_angular_vel_enc = -lwheel_angular_vel_enc
if self.rwheel_encs[-1] < 0: rwheel_angular_vel_enc = -rwheel_angular_vel_enc
self.lwheel_angular_vel_enc_pub.publish(lwheel_angular_vel_enc)
self.rwheel_angular_vel_enc_pub.publish(rwheel_angular_vel_enc)
self.time_prev_update = time_curr_update
else: # Running in simulation -- blindly copy from target assuming perfect execution
self.lwheel_angular_vel_enc_pub.publish(self.lwheel_angular_vel_control)
self.rwheel_angular_vel_enc_pub.publish(self.rwheel_angular_vel_control)
def read_enc_ticks(initial_ticks_left, initial_ticks_right):
"""
Reads the encoder ticks from the left and right motors and returns the
number of ticks that have occurred from each since the start of the ACC.
Uses time.sleep calls to try to reduce issues with reading and writing to
pins to communicate with the rover, as the official gopigo library does
this.
If an error is returned from either of the encoder readings, then it tries
to take more readings until either a valid pair of readings is made or a
try limit is exceeded.
:param int initial_ticks_left: The number of left motor ticks recorded at
the start of the ACC. (ticks)
:param int initial_ticks_right: The number of right motor ticks recorded at
the start of the ACC. (ticks)
:return: The number of ticks for the left and right motor that have
occurred since the start of the ACC. (ticks)
:rtype: tuple[int, int]
"""
found_good_reading = False
tries = 0
while not found_good_reading and tries < ENCODER_READ_TRIES:
time.sleep(0.01)
elapsed_ticks_left = gopigo.enc_read(gopigo.LEFT) - initial_ticks_left
time.sleep(0.01)
elapsed_ticks_right = gopigo.enc_read(gopigo.RIGHT) - initial_ticks_right
tries += 1
if elapsed_ticks_left >= 0 and elapsed_ticks_right >= 0:
found_good_reading = True
return elapsed_ticks_left, elapsed_ticks_right
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 oneRun():
leftStart = gpg.enc_read(0)
rightStart = gpg.enc_read(1)
gpg.motor_fwd()
sleep(2)
stop()
sleep(0.5)
leftEnd = gpg.enc_read(0)
rightEnd = gpg.enc_read(1)
return (leftEnd - leftStart, rightEnd - rightStart)
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 slowThread():
global us_pos
global enc_pos
global cam_pos
global scale
global running
while running:
ENC1_MOVED_TICKS = go.enc_read(0) - enc_start0
ENC2_MOVED_TICKS = go.enc_read(1) - enc_start1
MOVED_TICKS = int((ENC1_MOVED_TICKS + ENC2_MOVED_TICKS) / 2)
enc_pos = (1478 - MOVED_TICKS * 11.34)
# Slower code goes here
drawMap(enc_pos, us_pos, cam_pos)
#read the image from the camera
ret, frame = video.read()
lowerLimits = np.array([lB, lG, lR])
upperLimits = np.array([hB, hG, hR])
blur = cv2.blur(frame, (kernel, kernel))
color = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
thresh = cv2.inRange(color, lowerLimits, upperLimits)
opening = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel2)
cv2.createTrackbar("lB", "Processed", lB, 255, updateValue)
cv2.createTrackbar("lG", "Processed", lG, 255, updateValue_2)
cv2.createTrackbar("lR", "Processed", lR, 255, updateValue_3)
cv2.createTrackbar("hB", "Processed", hB, 255, updateValue_4)
cv2.createTrackbar("hR", "Processed", hG, 255, updateValue_5)
cv2.createTrackbar("hG", "Processed", hR, 255, updateValue_6)
keypoints = detector.detect(opening)
vid_new = cv2.drawKeypoints(frame, keypoints, np.array([]),
(0, 0, 255),
cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
for keypoint in keypoints: #
x = int(keypoint.pt[0]) #Let's get coordinate locations
y = int(keypoint.pt[1])
size = round(keypoint.size)
cam_pos = getDistanceWithCam(size)
#Write some text onto the frame
vid_new = cv2.resize(vid_new, (0, 0), fx=0.5, fy=0.5)
opening = cv2.resize(opening, (0, 0), fx=0.5, fy=0.5)
cv2.imshow('Original', vid_new)
cv2.imshow('Processed', opening)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def determine_safety_values(self, dt):
self.right_rotation_rate = (gopigo.enc_read(gopigo.RIGHT) -
self.elapsed_ticks_right) / dt
self.left_rotation_rate = (gopigo.enc_read(gopigo.LEFT) -
self.elapsed_ticks_left) / dt
self.caculate_current_speed()
self.observe_obstacle(dt)
self.critical_distance = (
MAX_STOPPING_DISTANCE / MAX_POWER_VALUE) * max(
self.current_speed_left, self.current_speed_right)
self.mimum_settable_safe_distance = self.critical_distance + BUFFER_DISATANCE
self.alert_distance = self.safe_distance + max(
self.current_speed_left, self.current_speed_right) * SLOWDOWN_TIME
self.perform_obstalce_based_acceleratioin_determination()
def read_encoders():
return (go.enc_read(0), go.enc_read(1))
import gopigo as go
def set_motors(s1, s2):
go.motor1(1, s1)
go.motor2(1, s2)
lwt = go.enc_read(0)
rwt = go.enc_read(1)
target_speed = input("Speed?")
print "Starting"
lws = target_speed
rws = target_speed
set_motors(target_speed, target_speed)
while True:
lwt -= go.enc_read(0)
rwt -= go.enc_read(1)
error = rwt - lwt
lws += error / 2
rws += error / 2
if error > 0:
if rws == target_speed:
lws += 2
go.motor1(1, lws)
else:
rws -= 2
go.motor2(1, rws)
import gopigo as go
import time
import math
# 0 - Right motor
# 1 - Left motor
# 0 - Left encoder
# 1 - Right encoder
go.set_speed(150)
go.forward()
prevLeft = go.enc_read(0)
prevRight = go.enc_read(1)
def set_slave(speed):
#go.set_right_speed(speed)
go.set_left_speed(speed)
while True:
time.sleep(0.25)
left = go.enc_read(0)
right = go.enc_read(1)
leftDiff = left - prevLeft
rightDiff = right - prevRight
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 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 ""
import cv2
import json
import os
import time
import numpy as np
import _thread
img_orig = cv2.imread('map.png')
gospeed = 40
us_pos = 0
enc_pos = 0
cam_pos = 0
scale = 0.5
enc_start0 = go.enc_read(0)
enc_start1 = go.enc_read(1)
video = cv2.VideoCapture(0)
time_start = 0
file = open("new.txt", "r")
text = file.read().splitlines()
lB = int(text[0])
lG = int(text[1])
lR = int(text[2])
hB = int(text[3])
hG = int(text[4])
hR = int(text[5])
kernel = int(text[6])
def enc_read(kargs):
r = {'return_value': gopigo.enc_read(int(kargs['motor']))}
return r
