class Follower:
def __init__(self):
self.bridge = cv_bridge.CvBridge()
# cv2.namedWindow("window", 1)
self.image_sub = rospy.Subscriber('cv_camera/image_raw', Image,
self.image_callback)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.twist = Twist()
self.a_star = AStar()
def image_callback(self, msg):
image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_black = numpy.array([0, 0, 0])
upper_black = numpy.array([120, 100, 80])
mask = cv2.inRange(hsv, lower_black, upper_black)
h, w, d = image.shape
search_top = 3 * h / 4
search_bot = 3 * h / 4 + 20
mask[0:search_top, 0:w] = 0
mask[search_bot:h, 0:w] = 0
M = cv2.moments(mask)
if M['m00'] > 0:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
# cv2.circle(image, (cx, cy), 20, (0,0,255), -1)
cv2.circle(mask, (cx, cy), 20, (0, 0, 255), -1)
# BEGIN CONTROL
err = cx - w / 2
self.twist.linear.x = 0.2
self.twist.angular.z = -float(err) / 100
self.cmd_vel_pub.publish(self.twist)
self.a_star.motors(int(40 + 0.3 * err), int(40 - 0.3 * err))
def keys_mtr(kmsg):
a_star = AStar()
if len(kmsg.data) == 0 or not key_mapping.has_key(kmsg.data[0]):
return
mtrs = key_mapping[kmsg.data[0]]
mtrs = np.array(mtrs, dtype='int16')
mleft = mtrs[0]
mright = mtrs[1]
# print(mleft, mleft.dtype)
# print(mright, mright.dtype)
a_star.motors(mleft, mright)
time.sleep(0.1)
def callback(directions):
a_star = AStar()
if directions.forward:
a_star.motors(200, 200)
elif directions.back:
a_star.motors(-200, -200)
elif directions.left:
a_star.motors(200, -200)
elif directions.right:
a_star.motors(-200, 200)
else:
a_star.motors(0, 0)
#Test code for reading data from the Encoders
#Written by Nicholas Gregg
import time
import os
from a_star import AStar
a_star = AStar()
a_star.motors(50, -50)
try:
while True:
encoders = a_star.read_encoders()
print(encoders[0], encoders[1])
time.sleep(.1)
os.system('clear')
except KeyboardInterrupt:
a_star.motors(0, 0)
class DeepQPiCar(object):
""" """
ACTIONS_COUNT = 20 # number of valid actions. In this case forward, backward, right, left
MEMORY_SIZE = 5000 # number of observations to remember
STATE_FRAMES = 4 # number of frames to store in the state
OBSERVATION_STEPS = 75 # time steps to observe before training
CHOICES = [(False, 25), (True, 75)]
ACTIONS = [(65, 65), (75, 35), (35, 75), (-65, -65)]
ACTION_CHOICES = [
((25, 25), 5),
((-25, -25), 5),
((45, 45), 5),
((-45, -45), 5),
((65, 65), 5),
((-65, -65), 5),
((100, 100), 5),
((-100, -100), 5),
((35, 75), 5),
((-35, -75), 5),
((75, 35), 5),
((-75, -35), 5),
((35, 90), 5),
((-35, -90), 5),
((90, 35), 5),
((-90, -35), 5),
((25, 100), 5),
((-25, -100), 5),
((100, 25), 5),
((-100, -25), 5),
]
calc_error_in_position = .01
calc_error_in_velocity = .0001
obs_error_in_position = .1
obs_error_in_velocity = .001
_tf = TensorFlowUtils()
_kf = KalmanFilter(
1, # delta t is one second
17., # approaximate distance of starting point
.05,
.001,
calc_error_in_position,
calc_error_in_velocity,
obs_error_in_position,
obs_error_in_velocity)
def __init__(self, is_training=True):
""" """
rospy.init_node('deepq_carnode')
self.rate = rospy.Rate(2) # send 2 observations per second
self.camera = picamera.PiCamera()
# self.camera.vflip = True
# self.camera.hflip = True
self.output = picamera.array.PiRGBArray(self.camera)
self.last_state = None
self.crashed = False
self.dead_counter = 0
self.observations = deque()
self.width = 320
self.height = 240
self.camera.resolution = (self.width, self.height)
self.publish_train = False
self.terminal_frame = False
self.trainer = rospy.Publisher('/pi_car/start_training',
Bool,
queue_size=1)
self.pub = rospy.Publisher('/pi_car/observation',
String,
queue_size=15,
latch=True)
rospy.Subscriber('/pi_car/cmd_resume', Bool, self.resume)
self._run = True
self._wait = False
self.driver = AStar()
self.previous_controls = (65, 65)
self.current_controls = (75, 35)
self.distance_from_router = 1.0
self.recovery_count = 0
self.reward_is_stuck = -.005
self.global_counter = 0
self.recovery_at_counter = 0
self._is_training = True
if not self._is_training:
self._tf._create_convolutional_network()
def resume(self, msg):
""" """
self._wait = False
self._run = True
def _get_normalized_frame(self):
""" """
self.camera.capture(self.output, 'rgb')
frame = np.array(self.output.array)
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
gray = gray[-90:, :]
frame = (gray - np.min(gray)) / (np.max(gray) - np.min(gray))
self.output.truncate(0)
return frame, gray
def _calculate_reward(self):
""" """
observed_distance = round(utils.get_distance_from_router(), 3)
current_distance = self._kf.get_current_position(
observed_distance, utils.get_velocity_from_motors((65, 65)),
utils.get_acceleration_from_motors((65, 65), (75, 35)))
current_distance = round(current_distance, 3)
self.reward = round(current_distance - self.distance_from_router, 3)
self.distance_from_router = float(current_distance)
if self.reward < self.reward_is_stuck:
self.dead_counter += 1
if self.dead_counter >= 3:
print('I am stuck!')
self.terminal_frame = True
self.dead_counter = 0
else:
self.terminal_frame = False
self.dead_counter = 0
if self.reward > 0.:
self.reward *= 1.5
# else:
# self.reward *= -.1
print('my reward', self.reward, 'observed distance', observed_distance,
'current distance', current_distance)
def _set_state(self):
""" """
# first frame must be handled differently
if self.last_state is None:
self.last_action = np.zeros(self.ACTIONS_COUNT)
self.last_action[0] = 1 # move forward
self.last_state = np.stack(tuple(
self._get_normalized_frame()[0]
for _ in range(self.STATE_FRAMES)),
axis=2)
frame, img = self._get_normalized_frame()
frame = np.reshape(frame, (90, self.width, 1))
current_state = np.append(self.last_state[:, :, 1:], frame, axis=2)
return current_state, img
def _set_action(self):
# if self.terminal_frame and (self.global_counter - self.recovery_at_counter > 3):
# self.recovery_at_counter = int(self.global_counter)
# print('attempting recovery')
# self._set_recovery_action()
# if self.recovery_count > 1:
# print('done with recovery')
# self.recovery_count = 0
# self.terminal_frame = False
# if self.recovery_count:
# return
self.change = weighted_choice(self.CHOICES)
if self.change:
self.last_action = np.zeros(self.ACTIONS_COUNT)
self.current_controls = weighted_choice(self.ACTION_CHOICES)
index = self.ACTION_CHOICES.index((self.current_controls, ) +
(5, ))
self.last_action[index] = 1
print('current controls', self.current_controls)
def _set_recovery_action(self):
self.recovery_count += 1
self.last_action = np.zeros(self.ACTIONS_COUNT)
self.current_controls = (-65, -65)
index = self.ACTION_CHOICES.index((self.current_controls, ) + (5, ))
self.last_action[index] = 1
def _publish_observation(self):
""" """
current_state, img = self._set_state()
self._set_action()
self._calculate_reward()
observation = (self.last_state, self.last_action, self.reward,
current_state, img, self.terminal_frame)
self.observations.append(observation)
self.last_state = current_state
if len(self.observations) > self.MEMORY_SIZE:
self.observations.popleft()
self._publish(observation)
if len(self.observations) > 0 and len(
self.observations) % self.OBSERVATION_STEPS == 0:
# stop the car while training
self.move(0, 0)
self.publish_train = True
self._wait = True
while self._wait:
self._train()
time.sleep(5)
self.publish_train = False
def _publish(self, obs):
""" """
msg = String()
data = pickle.dumps(obs)
msg.data = base64.b64encode(data)
self.pub.publish(msg)
self.rate.sleep()
def _train(self):
msg = Bool()
msg.data = True
if self.publish_train:
self.trainer.publish(msg)
def _move(self, action):
self.move(*action)
def move(self, left, right):
try:
self.driver.motors(left, right)
except IOError as ioerror:
print("io error")
except Exception as e:
print(e)
def _training(self):
if not self._wait:
self._publish_observation()
self._move(self.current_controls)
self.global_counter += 1
def _autonomous(self):
current_state, img = self._set_state()
agents_reward_per_action = self._tf.choose_next_action(current_state)
print('agents reward per action', agents_reward_per_action)
action_index = np.argmax(agents_reward_per_action)
action = self.ACTIONS[action_index]
print('robot chose action', action)
self._move(action)
def run(self):
""" """
while self._run:
if self._is_training:
self._training()
else:
self._autonomous()
time.sleep(1.0)
import time
import os
import math
from a_star import AStar
a_star = AStar()
a_star.motors(25,-25)
theta_initial = 0.0
theta_new = 0.0
def displacement(right_encoder,left_encoder): #velocity: ft/s, position:
global theta_initial
global theta_new
pi = math.pi
dist_between_wheels = 0.4791667
right_wheel_rotations = right_encoder/float(1440) #converts encoder counts to rotations
left_wheel_rotations = left_encoder/float(1440)
print("rightrotations = %s rotations" % right_wheel_rotations)
print("leftrotations = %s rotations" % left_wheel_rotations)
right_displacement = right_wheel_rotations*float(2)*pi*.114829 #calculates displacement of right, left and center wheels
left_displacement = left_wheel_rotations*float(2)*pi*.114829
center_displacement = (right_displacement + left_displacement)/float(2)
alpha_left_turn_radians = (right_displacement - left_displacement)/dist_between_wheels #calculates the change of the angle by a turn
alpha_left_turn_degrees = alpha_left_turn_radians * float(180)/pi #converts to degrees
theta_new = theta_initial + alpha_left_turn_degrees #appends initial theta to new theta
if theta_new >= float(360):
theta_new = theta_new - float(360)
theta_initial = theta_new
elif theta_new < float(0):
theta_new = float(360) + theta_new
theta_initial = theta_new
class Balancer:
def __init__(self):
self.a_star = AStar()
self.imu = LSM6()
self.g_y_zero = 0
self.angle = 0 # degrees
self.angle_rate = 0 # degrees/s
self.distance_left = 0
self.speed_left = 0
self.drive_left = 0
self.last_counts_left = 0
self.distance_right = 0
self.speed_right = 0
self.drive_right = 0
self.last_counts_right = 0
self.motor_speed = 0
self.balancing = False
self.calibrated = False
self.running = False
self.next_update = 0
self.update_thread = None
def setup(self):
self.imu.enable()
time.sleep(1) # wait for IMU readings to stabilize
# calibrate the gyro
total = 0
for _ in range(CALIBRATION_ITERATIONS):
self.imu.read()
total += self.imu.g.y
time.sleep(0.001)
self.g_y_zero = total / CALIBRATION_ITERATIONS
self.calibrated = True
def start(self):
if self.calibrated:
if not self.running:
self.running = True
self.next_update = time.clock_gettime(time.CLOCK_MONOTONIC_RAW)
self.update_thread = threading.Thread(target=self.update_loop,
daemon=True)
self.update_thread.start()
else:
raise RuntimeError(
"IMU not enabled/calibrated; can't start balancer")
def stop(self):
if self.running:
self.running = False
self.update_thread.join()
def stand_up(self):
if self.calibrated:
sign = 1
if self.imu.a.z < 0:
sign = -1
self.stop()
self.reset()
self.imu.read()
self.a_star.motors(-sign * MOTOR_SPEED_LIMIT,
-sign * MOTOR_SPEED_LIMIT)
time.sleep(0.4)
self.a_star.motors(sign * MOTOR_SPEED_LIMIT,
sign * MOTOR_SPEED_LIMIT)
for _ in range(40):
time.sleep(UPDATE_TIME)
self.update_sensors()
print(self.angle)
if abs(self.angle) < 60:
break
self.motor_speed = sign * MOTOR_SPEED_LIMIT
self.reset_encoders()
self.start()
else:
raise RuntimeError("IMU not enabled/calibrated; can't stand up")
def update_loop(self):
while self.running:
self.update_sensors()
self.do_drive_ticks()
if self.imu.a.x < 2000:
# If X acceleration is low, the robot is probably close to horizontal.
self.reset()
self.balancing = False
else:
self.balance()
self.balancing = True
# Perform the balance updates at 100 Hz.
self.next_update += UPDATE_TIME
now = time.clock_gettime(time.CLOCK_MONOTONIC_RAW)
time.sleep(max(self.next_update - now, 0))
# stop() has been called and the loop has exited. Stop the motors.
self.a_star.motors(0, 0)
def update_sensors(self):
self.imu.read()
self.integrate_gyro()
self.integrate_encoders()
def integrate_gyro(self):
# Convert from full-scale 1000 deg/s to deg/s.
self.angle_rate = (self.imu.g.y - self.g_y_zero) * 35 / 1000
self.angle += self.angle_rate * UPDATE_TIME
def integrate_encoders(self):
(counts_left, counts_right) = self.a_star.read_encoders()
self.speed_left = subtract_16_bit(counts_left, self.last_counts_left)
self.distance_left += self.speed_left
self.last_counts_left = counts_left
self.speed_right = subtract_16_bit(counts_right,
self.last_counts_right)
self.distance_right += self.speed_right
self.last_counts_right = counts_right
def drive(self, left_speed, right_speed):
self.drive_left = left_speed
self.drive_right = right_speed
def do_drive_ticks(self):
self.distance_left -= self.drive_left
self.distance_right -= self.drive_right
self.speed_left -= self.drive_left
self.speed_right -= self.drive_right
def reset(self):
self.motor_speed = 0
self.reset_encoders()
self.a_star.motors(0, 0)
if abs(self.angle_rate) < 2:
# It's really calm, so assume the robot is resting at 110 degrees from vertical.
if self.imu.a.z > 0:
self.angle = 110
else:
self.angle = -110
def reset_encoders(self):
self.distance_left = 0
self.distance_right = 0
def balance(self):
# Adjust toward angle=0 with timescale ~10s, to compensate for
# gyro drift. More advanced AHRS systems use the
# accelerometer as a reference for finding the zero angle, but
# this is a simpler technique: for a balancing robot, as long
# as it is balancing, we know that the angle must be zero on
# average, or we would fall over.
self.angle *= 0.999
# This variable measures how close we are to our basic
# balancing goal - being on a trajectory that would cause us
# to rise up to the vertical position with zero speed left at
# the top. This is similar to the fallingAngleOffset used
# for LED feedback and a calibration procedure discussed at
# the end of Balancer.ino.
#
# It is in units of degrees, like the angle variable, and
# you can think of it as an angular estimate of how far off we
# are from being balanced.
rising_angle_offset = self.angle_rate * ANGLE_RATE_RATIO / 1000 + self.angle
# Combine risingAngleOffset with the distance and speed
# variables, using the calibration constants defined in
# Balance.h, to get our motor response. Rather than becoming
# the new motor speed setting, the response is an amount that
# is added to the motor speeds, since a *change* in speed is
# what causes the robot to tilt one way or the other.
self.motor_speed += (
+ANGLE_RESPONSE * 1000 * rising_angle_offset + DISTANCE_RESPONSE *
(self.distance_left + self.distance_right) + SPEED_RESPONSE *
(self.speed_left + self.speed_right)) / 100 / GEAR_RATIO
if self.motor_speed > MOTOR_SPEED_LIMIT:
self.motor_speed = MOTOR_SPEED_LIMIT
if self.motor_speed < -MOTOR_SPEED_LIMIT:
self.motor_speed = -MOTOR_SPEED_LIMIT
# Adjust for differences in the left and right distances; this
# will prevent the robot from rotating as it rocks back and
# forth due to differences in the motors, and it allows the
# robot to perform controlled turns.
distance_diff = self.distance_left - self.distance_right
self.a_star.motors(
int(self.motor_speed +
distance_diff * DISTANCE_DIFF_RESPONSE / 100),
int(self.motor_speed -
distance_diff * DISTANCE_DIFF_RESPONSE / 100))
class Robot:
# initialize robot
def __init__(self):
self.directionFacing = 'north'
self.maxSpeed = 100
self.aStar = AStar()
self.tcs = Adafruit_TCS34725.TCS34725()
self.initialLeftCount = self.aStar.read_encoders()[0]
self.initialRightCount = self.aStar.read_encoders()[1]
# makes multiple readings with RGB sensor and returns the average values
def getColorAverage(self):
numIterations = 3
averageG = averageR = averageB = 0
for i in range(0,numIterations):
r, g, b, c = self.tcs.get_raw_data()
averageG += g
averageR += r
averageB += b
averageG /= numIterations
averageR /= numIterations
averageB /= numIterations
return (averageR, averageG, averageB)
# check whether RGB sensor is reading green
def checkGreen(self):
averageColors = self.getColorAverage()
return (averageColors[1] > averageColors[0]) and (averageColors[1] > averageColors[2]) and averageColors[1] > 20
# check whether RGB sensor is reading red
def checkRed(self):
averageColors = self.getColorAverage()
return (averageColors[0] > averageColors[1]) and (averageColors[0] > averageColors[2]) and averageColors[0] > 20
# check whether RGB sensor is reading purple
def checkPurple(self):
averageColors = self.getColorAverage()
return (math.fabs(averageColors[0] - averageColors[2]) < 3) and (averageColors[1] / averageColors[0] < 0.6)
# grab tuple containing RGB values (r, g, b, clear)
def getColors(self):
return self.tcs.get_raw_data()
# set motor speeds of robot wheels
def motors(self, lspeed, rspeed):
self.aStar.motors(lspeed, rspeed)
# set both motor speeds to max speed forward direction
def goForward(self):
self.motors(self.maxSpeed, self.maxSpeed)
# set both motor speeds to zero
def stop(self):
self.motors(0, 0)
# function used to getting the current time
def getTime(self):
return time.time()
# play music notes on arduino buzzer
def playNotes(self):
self.aStar.play_notes("o4l16ceg>c8")
# turn leds on or off
def leds(self, red, yellow, green):
self.aStar.leds(red, yellow, green)
# reads analog information of arduino
def readAnalog(self):
return self.aStar.read_analog()
# get the current reading of the robot battery
def readBatteryMillivolts(self):
return self.aStar.read_battery_millivolts()
# get the boolean values of buttons on the arduino (A, B, C)
def readButtons(self):
return self.aStar.read_buttons()
# reads the encoder information on the arduino wheels and returns a tuple (INT, INT)
# uses relative encoder information based on class initialization
def readEncoders(self):
encoders = self.aStar.read_encoders()
return (encoders[0] - self.initialLeftCount, encoders[1] - self.initialRightCount)
# print r, g, b, temperature, and lux values of rgb sensor
def printColorInfo(self):
r, g, b, c = self.tcs.get_raw_data()
temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b)
lux = Adafruit_TCS34725.calculate_lux(r, g, b)
print('Color: r={0} g={1} b={2} temp={3} lux={4}'.format(r, g, b, temp, lux))
def turnOffInterruptsRGB(self):
self.tcs.set_interrupt(False)
def turnOnInterruptsRGB(self):
self.tcs.set_interrupt(True)
# rgb sensor is enabled by default in the constructor method
def enableRGB(self):
self.tcs.enable()
def disableRGB(self):
self.tcs.disable()
# used to calibrate direction to green facing
def calibrateTwo(self):
self.sweepsForGreen(100)
# rotate bot a specified number of encoder counts
def turnCounts(self, direction, counts):
initEncoders = self.readEncoders()
if(direction == 'left'):
while( self.readEncoders()[0] > initEncoders[0] - counts
and self.readEncoders()[1] < initEncoders[1] + counts):
self.motors(self.maxSpeed * (-1), self.maxSpeed)
elif(direction == 'right'):
while( self.readEncoders()[1] > initEncoders[1] - counts
and self.readEncoders()[0] < initEncoders[0] + counts):
self.motors(self.maxSpeed, self.maxSpeed * (-1))
else:
print("Invalid turn direction")
self.stop()
# rotate robot 90 degrees left
def turn90Left(self):
self.turnCounts('left', 744)
def turn90Right(self):
self.turnCounts('right', 744)
# move robot forward while calibrating the difference between the
# wheel encoders to be within an epsilon of equivalence
def goForwardtwo(self, offset):
x = self.maxSpeed
self.motors(int(100),int(100))
grabEncoders = self.updateEncoders(offset)
diff = math.fabs(grabEncoders[0] - grabEncoders[1])
# print(diff)
while(diff > 3):
grabEncoders = self.updateEncoders(offset)
diff = math.fabs(grabEncoders[0] - grabEncoders[1])
x-=1.0
if(grabEncoders[0] < grabEncoders[1]):
self.motors(int(self.maxSpeed), int(x))
time.sleep(2.0/1000.0)
elif(grabEncoders[0] > grabEncoders[1]):
self.motors(int(x), int(self.maxSpeed))
time.sleep(2.0/1000.0)
self.motors(int(self.maxSpeed), int(self.maxSpeed))
# get encoder offset (if one wheel is negative and the other is positive after a rotation)
def zeroEncoders(self):
return([self.aStar.read_encoders()[0]%32768, self.aStar.read_encoders()[1]%32768])
# used for iterative tracking of encoder information from some original state
# e.g. baseline = self.updateEncoders()...do loop...currentState = self.updateEncoders... compare against baseline
def updateEncoders(self, offset):
return([(self.aStar.read_encoders()[0]-offset[0])%32768, (self.aStar.read_encoders()[1]-offset[1])%32768])
# look for green a number of rotation encoder counts in a particular direction
def sweep(self, direction, counts):
interval = 10
iterations = math.ceil(counts / interval)
for i in range (1, iterations):
self.turnCounts(direction, interval)
if(self.checkGreen()):
return True
return False
# return true is green is found within a sweep distance, false otherwise
def sweepsForGreen(self, counts):
# counts is equal to the number of encoders counts at both sides of
# facing direction e.g. count = 50 searches 50 counts left and 50 right
initEncoders = self.readEncoders()
self.sweep('left', counts)
self.turnCounts('right', self.readEncoders()[1] - initEncoders[1])
self.sweep('right', counts)
self.turnCounts('left', self.readEncoders()[0] - initEncoders[0])
return False
# check where there are paths at a given intersection, return a list
def getValidPaths(self):
directions = list()
self.calibrateTwo()
if(self.checkGreen()):
directions.append('forward')
self.turn('left')
if(self.checkGreen()):
directions.append('left')
self.turn('backward')
if(self.checkGreen()):
directions.append('right')
self.turn('left')
print(directions)
return directions
# center robot at intersection after intersection is detected
def adjustIntersection(self):
saveEncoders = self.readEncoders()
while(self.readEncoders()[0] - saveEncoders[0] < 700
and self.readEncoders()[1] - saveEncoders[1] < 700):
self.goForward()
print("Centered at intersection")
# takes initial encoder values and newly recorded encoder values
# and calculates the average of both to estimate the total forward distance
def calculateForwardDistance(self, initialEncoderInfo):
newEncoders = self.readEncoders()
leftDistance = newEncoders[0] - initialEncoderInfo[0]
rightDistance = newEncoders[1] - initialEncoderInfo[1]
averageDistance = (leftDistance / 2) + (rightDistance / 2)
return averageDistance
# calculates current location based on a coordinate system
def calculatePosition(self, lastPosition, directionFacing, forwardDistance):
x = lastPosition[0]
y = lastPosition[1]
if(directionFacing == 'north'):
return (x, y + forwardDistance)
elif(directionFacing == 'west'):
return (x - forwardDistance, y)
elif(directionFacing == 'east'):
return (x + forwardDistance, y)
elif(directionFacing == 'south'):
return (x, y - forwardDistance)
else:
print('invalid direction in calculatePosition()')
return (0, 0) # we should never end up back at the starting position
# calculate the new facing direction after turn
def directionFacingAfterTurn(self, directionTurning, directionCurrentlyFacing):
directions = ['north', 'west', 'south', 'east']
index = 0
if(directionTurning == 'left'):
index = (directions.index(directionCurrentlyFacing) + 1) % 4
elif(directionTurning == 'right'):
index = (directions.index(directionCurrentlyFacing) - 1) % 4
elif(directionTurning == 'forward'):
index = (directions.index(directionCurrentlyFacing) + 0) % 4
elif(directionTurning == 'backward'):
index = (directions.index(directionCurrentlyFacing) + 2) % 4
else:
print('Error in directionFacingAfterTurn, invalid directionCurrentlyFacing value')
return directions[index]
# turn robot in a specified direction
def turn(self, direction):
if(direction == 'left'):
self.turn90Left()
self.calibrateTwo()
elif(direction == 'right'):
self.turn90Right()
self.calibrateTwo()
elif(direction == 'forward'):
pass
elif(direction == 'backward'):
self.turn90Left()
self.turn90Left()
self.calibrateTwo()
else:
print("invalid direction in turn()")
self.directionFacing = self.directionFacingAfterTurn(direction, self.directionFacing)
# head back to previous intersection
def goToPreviousIntersection(self):
while(not (self.checkRed())):
if(self.checkGreen()):
self.goForwardtwo(offset)
else:
self.calibrateTwo()
offset = self.zeroEncoders()
self.adjustIntersection()
offset = self.zeroEncoders()
# traverse through a maze using backtracking
def completeMaze(self, previousPositions = list()):
mazeSolved = False
initialEncoderInfo = self.readEncoders()
forwardDistance = 0
# record initial position as a coordinate if list is empty, it should never be removed
# and will always be the only position left in the list if all positions are popped
offset = self.zeroEncoders()
if (not previousPositions):
previousPositions.append((0, 0))
self.directionFacing = 'north'
# continue until maze exit is found
while(not mazeSolved):
# follow the green-brick road
if(self.checkGreen()):
self.goForwardtwo(offset)
# found intersection
elif(self.checkRed()):
self.adjustIntersection()
forwardDistance = self.calculateForwardDistance(initialEncoderInfo)
# In python, the [-1] references the last index in a list
currentPosition = self.calculatePosition(previousPositions[-1], self.directionFacing, forwardDistance)
# if found end of maze, return
if(self.checkPurple()):
return True
paths = self.getValidPaths()
offset = self.zeroEncoders()
# if no paths were detected, must be deadend
if(not paths):
pass
else:
# try eacn path (alter state), recurse, go back to starting position (revert state)
for path in paths:
self.turn(path) # change state
offset = self.zeroEncoders()
mazeSolved = self.completeMaze(previousPositions) # recursive call
if(mazeSolved):
return True
# in order to get back to the original facing position, repeat original turn direction
if(path != 'forward'):
self.turn(path) # revert state
offset = self.zeroEncoders()
# forward is an exception, if returning from forward, stop at original position and turn around
else:
self.turn('backward')
offset = self.zeroEncoders()
self.turn('backward') # turn towards last intersection
offset = self.zeroEncoders()
self.goToPreviousIntersection()
return False
# end if checkRed()
else:
self.calibrateTwo()
offset = self.zeroEncoders()
# end while
return False
msg_pose.x = center_displacement
msg_pose.theta = angle
msg_vel.angular.x = 0
msg_vel.angular.y = 0
msg_vel.angular.z = aVel
msg_vel.linear.x = (center_displacement / sampleRate) * (1 / 3.2808)
msg_vel.linear.y = 0
msg_vel.linear.z = 0
# data.angle = angle
# data.cDisplacement = center_displacement
# data.aVel = aVel
# data.lvel = (center_displacement/sampleRate)*(1/3.2808)
# print(angle_msg)
# print(sampleRate)
pub_vel.publish(msg_vel)
pub_pose.publish(msg_pose)
rate.sleep(
) #Make sure this is equal to the output of the sample rate, DO NOT USE THE VARIABLE
# sampleRate = timeit.default_timer() - start_time
sampleRate1 = rospy.Time.now() - start_time
sampleRate = sampleRate1.to_sec()
if __name__ == '__main__':
try:
talker()
except KeyboardInterrupt:
a_star.motors(0, 0)
#import sys
#sys.path.insert(0, '/home/pi/pololu-rpi-slave-arduino-library-2.0.0/pi')
import time
from a_star import AStar
a_star = AStar()
while True:
a_star.motors(200,200)
time.sleep(5)
a_star.motors(-200,-200)
time.sleep(5)
a_star.motors(-200,200)
time.sleep(5)
a_star.motors(200,-200)
time.sleep(5)
a_star.motors(0,0)
time.sleep(5)
timestamps.append(time.time() - start_time)
current_encoder = a_star.read_encoders()
current_rot = [
current_encoder[0] / gear_ratio - start_rot[0],
current_encoder[1] / gear_ratio - start_rot[1]
]
encoders.append(current_rot)
velocities.append(((current_rot[0] - previous_rot[0]) / frame_length,
(current_rot[1] - previous_rot[1]) / frame_length))
previous_rot = current_rot
voltages.append(a_star.read_battery_millivolts()[0] / 1000)
if step < 1 * fps:
a_star.motors(0, 0)
motors.append((0, 0))
elif step < 2 * fps:
a_star.motors(400, 400)
motors.append((400, 400))
elif step < 3 * fps:
a_star.motors(0, 0)
motors.append((0, 0))
delay = start_time + (step + 1) * frame_length - time.time()
time.sleep(delay)
# a_star.motors(0, 0)
# a_star.motors(400, 0)
# time.sleep(1)
# a_star.motors(0, 0)
import sys
from a_star import AStar
import time
a_star = AStar()
speed1 = int(sys.argv[1])
speed2 = int(sys.argv[2])
print("Setting left motor to " + str(speed1) + " and right motor to " +
str(speed2))
a_star.motors(speed1, speed2)
while 1:
time.sleep(0.1)
encoders = a_star.read_encoders()
print("Left: " + str(encoders[0]))
print("Right: " + str(encoders[1]))
class RobotController:
def __init__(self):
self.a_star = AStar()
self.reset_encoder_states()
def transition(self, prev_state, state):
if state == StateName.CHASE:
print("CHASE")
self.a_star.motors(SPEED, SPEED)
elif state == StateName.SEARCH:
print("SEARCH: start rotating")
# Record temperature every 5 degrees
# After spinning 360 degrees, follow the highest
self.a_star.motors(SPEED, -SPEED)
elif state == StateName.FILM:
print("FILM: spying cats")
self.a_star.motors(0, 0)
elif state == StateName.TERMINATE:
print("TERMINATE: stop the robot")
self.a_star.motors(0, 0)
return
def reset_encoder_states(self):
self.encoders = (0, 0, 0, 0)
self.threshold = float('inf')
self.encoder_index = STATIONARY
self.a_star.reset_encoders(True)
def update(self):
self.encoders = a_star.read_encoders()
if self.encoders[encoder_index] >= threshold:
print("stopping motors at threshold = {}".format(threshold))
self.reset_encoder_states()
return True
return False
def turn_right(self, degrees=90, stop=True):
self.threshold = self._motor_rotations(degrees)
self.encoder_index = LEFT_FORWARD
self.a_star.reset_encoders(False)
self.a_star.motors(SPEED, -SPEED)
def turn_left(self, degrees=90, stop=True):
self.threshold = _motor_rotations(degrees)
self.encoder_index = LEFT_FORWARD
self.a_star.reset_encoders(False)
self.a_star.motors(SPEED, -SPEED)
def turn_left(self, degrees=90, stop=True):
self._turn(degrees, stop, 'left')
def forward(self, distance, stop=True):
self._move(distance, stop, FORWARD)
def backward(self, distance, stop=True):
self._move(distance, stop, BACKWARD)
def _move(self, distance, stop, direction):
threshold = self._motor_move(distance)
if direction == FORWARD:
self.a_star.motors(SPEED, SPEED)
else:
self.a_star.motors(-SPEED, -SPEED)
self._read_encoder_until(threshold, direction)
if stop:
self.a_star.motors(0, 0)
def _motor_move(self, distance):
return (distance / (2 * math.pi * WHEEL_RADIUS)) * WHEEL_TO_MOTOR
def _motor_rotations(self, degrees):
wheelRotations = ROBOT_RADIUS * (abs(degrees) / 360) / WHEEL_RADIUS
return wheelRotations * WHEEL_TO_MOTOR
def _read_encoder_until(self, count, index):
self.a_star.reset_encoders(False)
while True:
sleep(ENCODER_INTERVAL)
reading = self.a_star.read_encoders(index)
if reading >= count:
print("reading = {}, threshold = {}".format(reading, count))
self.a_star.reset_encoders(True)
break
def _turn(self, degrees, stop, orientation):
threshold = self._motor_rotations(degrees)
if orientation == 'left':
self.a_star.motors(-SPEED, SPEED)
self._read_encoder_until(threshold, RIGHT_FORWARD)
else:
self.a_star.motors(SPEED, -SPEED)
self._read_encoder_until(threshold, LEFT_FORWARD)
if stop:
self.a_star.motors(0, 0)
thermal = ThermalSensor()
aStar = AStar()
SPEED = 50
print("Battery voltage: {}".format(aStar.read_battery_millivolts()))
with Input(keynames='curses') as input_generator:
while True:
pixels = thermal.read()
thermal.pretty_print(pixels)
print("Is cat? {}".format(is_cat(pixels)))
for c in input_generator:
if c == 'w':
print("Pressed W: Forward")
aStar.motors(SPEED, SPEED)
elif c == "s":
print("Pressed S: Backward")
aStar.motors(-SPEED, -SPEED)
elif c == "q":
print("Pressed Q: Turn Left")
aStar.motors(-SPEED, SPEED)
elif c == "e":
print("Pressed E: Turn Right")
aStar.motors(SPEED, -SPEED)
elif c == "z":
print("Pressed Z: Stop")
aStar.motors(0, 0)
break
sleep(0.1)
class Robot:
def __init__(self):
self.isLost = False
self.nextSearchDirection = 0 # (left: 0, right: 1)
self.veerSpeed = 30
self.maxSpeed = 70
self.aStar = AStar()
self.tcs = Adafruit_TCS34725.TCS34725()
self.initialLeftCount = self.aStar.read_encoders()[0]
self.initialRightCount = self.aStar.read_encoders()[1]
# check if currently reading green tape
def checkGreen(self):
r, g, b, c = self.tcs.get_raw_data()
return (g > r and g > b)
def checkRed(self):
r, g, b, c = self.tcs.get_raw_data()
return (r > 1.6 * g or r > 1.6 * b) and r > 50
# set motor speeds
def motors(self, lspeed, rspeed):
self.aStar.motors(lspeed, rspeed)
def goForward(self):
self.motors(self.maxSpeed, self.maxSpeed)
def stop(self):
self.motors(0, 0)
def veerLeft(self):
self.motors(self.veerSpeed, self.maxSpeed)
def veerRight(self):
self.motors(self.maxSpeed, self.veerSpeed)
def getTime(self):
return time.time()
def playNotes(self):
self.aStar.play_notes("o4l16ceg>c8")
# turn leds on or off
def leds(self, red, yellow, green):
self.aStar.leds(red, yellow, green)
def readAnalog(self):
return self.aStar.read_analog()
def readBatteryMillivolts(self):
return self.aStar.read_battery_millivolts()
def readButtons(self):
return self.aStar.read_buttons()
def readEncoders(self):
encoders = self.aStar.read_encoders()
return (encoders[0] - self.initialLeftCount,
encoders[1] - self.initialRightCount)
def printColorInfo(self):
r, g, b, c = self.tcs.get_raw_data()
temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b)
lux = Adafruit_TCS34725.calculate_lux(r, g, b)
print('Color: r={0} g={1} b={2} temp={3} lux={4}'.format(
r, g, b, temp, lux))
def turnOffInterruptsRGB(self):
self.tcs.set_interrupt(False)
def turnOnInterruptsRGB(self):
self.tcs.set_interrupt(True)
# rgb sensor is enabled by default in the constructor method
def enableRGB(self):
self.tcs.enable()
def disableRGB(self):
self.tcs.disable()
def turnLeft(self, count):
while (self.readEncoders()[0] % 744 > (-1) * count
and self.readEncoders()[1] % 744 < count):
if (self.checkGreen()):
return 1
break
self.motors(0, self.maxSpeed)
self.stop()
return 0
def turnRight(self, count):
while (self.readEncoders()[0] % 744 < count
and self.readEncoders()[1] % 744 > (-1) * count):
if (self.checkGreen()):
return 1
break
self.motors(self.maxSpeed, 0)
self.stop()
return 0
def changeCount(self, count):
if (count != 0):
count = count / 2
def calibrate(self):
count = 744
while (self.checkGreen() == False):
if (self.turnLeft(count)):
pass
elif (self.turnRight(count)):
pass
self.changeCount(count)
# end while
self.goForwardtwo()
def greenIsLeft(self):
initEncoders = self.readEncoders()
while (self.readEncoders()[0] > initEncoders[0] - 100
and self.readEncoders()[1] < initEncoders[1] + 100
and self.checkGreen() == False):
self.motors(-50, 50)
self.stop()
if (self.checkGreen()):
return True
else:
return False
def greenIsRight(self):
initEncoders = self.readEncoders()
while (self.readEncoders()[1] > initEncoders[1] - 100
and self.readEncoders()[0] < initEncoders[0] + 100
and self.checkGreen() == False):
self.motors(50, -50)
self.stop()
if (self.checkGreen()):
return True
else:
return False
def adjustSlightly(self, direction):
counts = 50
initEncoders = self.readEncoders()
if (direction == 'left'):
while (self.readEncoders()[1] < initEncoders[1] + counts):
self.motors(-50, 50)
else:
while (self.readEncoders()[0] < initEncoders[0] + counts):
self.motors(50, -50)
def calibrateTwo(self):
self.stop()
if (self.greenIsLeft()):
self.adjustSlightly('left')
elif (self.greenIsRight()):
self.adjustSlightly('right')
elif (self.checkRed()):
pass
else:
print("Unable to find green")
'''
def calibrateTwo(self):
self.sweepsForGreen(80)
'''
def turnCounts(self, direction, counts):
initEncoders = self.readEncoders()
if (direction == 'left'):
while (self.readEncoders()[0] > initEncoders[0] - counts
and self.readEncoders()[1] < initEncoders[1] + counts):
self.motors(self.maxSpeed * (-1), self.maxSpeed)
elif (direction == 'right'):
while (self.readEncoders()[1] > initEncoders[1] - counts
and self.readEncoders()[0] < initEncoders[0] + counts):
self.motors(self.maxSpeed, self.maxSpeed * (-1))
else:
print("Invalid turn direction")
self.stop()
def turn90Left(self):
self.turnCounts('left', 744)
def turn90Right(self):
self.turnCounts('right', 744)
# return true is green is found within a sweep distance, false otherwise
def sweepsForGreen(self, counts):
# counts is equal to the number of encoders counts at both sides of
# facing direction e.g. count = 50 searches 50 counts left and 50 right
interval = 10
iterations = math.ceil(counts / interval)
initEncoders = self.readEncoders()
# sweep left
for i in range(1, iterations):
self.turnCounts('left', interval)
if (self.checkGreen()):
return True
#print(self.readEncoders()[1] - initEncoders[1])
#self.turnCounts('right', self.readEncoders()[1] - initEncoders[1])
# sweep right
for i in range(1, iterations):
self.turnCounts('right', interval)
if (self.checkGreen()):
return True
#print(self.readEncoders()[0] - initEncoders[0])
#self.turnCounts('left', self.readEncoders()[0] - initEncoders[0])
return False
def isPathInDirection(self, direction):
if (direction == 'forward'):
pass
elif (direction == 'left'):
self.turn90Left()
elif (direction == 'right'):
self.turn90Right()
return self.sweepsForGreen(120)
def getValidPaths(self):
forward = self.isPathInDirection('forward')
self.stop()
time.sleep(0.5)
left = self.isPathInDirection('left')
self.stop()
time.sleep(0.5)
self.turn90Right()
right = self.isPathInDirection('right')
self.stop()
time.sleep(0.5)
self.turn90Left()
time.sleep(0.5)
return (left, forward, right)
def noValidPaths(pathTuple):
return not (pathTuple[0] or pathTuple[1] or pathTuple[2])
def goForwardtwo(self):
self.motors(self.maxSpeed, self.maxSpeed)
x = self.maxSpeed
diff = math.fabs(self.readEncoders()[1] - self.readEncoders()[0])
while (diff > 2):
diff = math.fabs(self.readEncoders()[1] - self.readEncoders()[0])
x -= 1
if (self.readEncoders()[1] > self.readEncoders()[0]):
self.motors(self.maxSpeed, x)
time.sleep(1.0 / 1000.0)
elif (self.readEncoders()[0] > self.readEncoders()[1]):
self.motors(x, self.maxSpeed)
time.sleep(1.0 / 1000.0)
self.motors(self.maxSpeed, self.maxSpeed)
'''
def calibrate(self):
while(self.checkGreen() == False):
for x in range(100, -20, -1):
self.motors(100 if x+40>100 else x+40,x)
'''
def adjustIntersection(self):
saveEncoders = self.readEncoders()
while (self.readEncoders()[0] - saveEncoders[0] < 700
and self.readEncoders()[1] - saveEncoders[1] < 700):
self.goForwardtwo()
print("Centered at intersection")
# recalibrate the robot onto the path
def calibrateDirection(self):
calibrated = False
turnAmount = 500 # initial encoder count
print("in calibrate")
while (calibrated == False):
# turn the robot one direction
if (self.nextSearchDirection == 0):
self.veerLeft()
else:
self.veerRight()
# switch turn direction for next iteration
self.nextSearchDirection = self.nextSearchDirection ^ 1
print("in first while")
recordEncoder = self.readEncoders()[self.nextSearchDirection]
# wait to see if direction was correct
while (self.readEncoders()[self.nextSearchDirection] -
recordEncoder < turnAmount):
print("in second while")
if (self.checkGreen()):
# creep forward until centered
self.adjustIntersection()
print("done centering")
# reposition front of robot onto green edge
while (self.checkGreen() == False):
if (self.nextSearchDirection == 0):
print("repositioning [left] to green edge")
self.veerLeft()
else:
print("repositioning [right] to green edge")
self.veerRight()
print("done repositioning to green edge")
calibrated = True
'''
# rotate half an inch until sensor is centered on tape
slapEncoders = self.readEncoders()
if(self.nextSearchDirection == 0):
while(self.readEncoders()[1] - slapEncoders[1] < 81):
print("rotating left half an inch")
self.motors(-100, 100)
self.stop()
else:
while(self.readEncoders()[0] - slapEncoders[0] < 81):
print("rotating right half an inch")
self.motors(100, -100)
self.stop()
'''
break
else:
pass # not green, continue to veer
driveLeft = -upDown
driveRight = -upDown
if leftRight < -0.05:
# Turning left
driveLeft *= 1.0 + (2.0 * leftRight)
elif leftRight > 0.05:
# Turning right
driveRight *= 1.0 - (2.0 * leftRight)
# Check for button presses
if joystick.get_button(buttonResetEpo):
pass
if joystick.get_button(buttonSlow):
driveLeft *= slowFactor
driveRight *= slowFactor
# Set the motors to the new speeds
# PBR.SetMotor1(driveRight * maxPower)
# PBR.SetMotor2(-driveLeft * maxPower)
# print(driver.joystck(driveRight * maxPower), driver.joystck(-driveLeft * maxPower))
driver.motors(driver.joystck(driveRight * maxPower),
driver.joystck(driveLeft * maxPower))
# Change the LED to reflect the status of the EPO latch
# PBR.SetLed(PBR.GetEpo())
# Wait for the interval period
time.sleep(interval)
# Disable all drives
# PBR.MotorsOff()
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
# PBR.MotorsOff()
pass
class Robot:
def __init__(self):
self.isLost = False
self.nextSearchDirection = 0 # (left: 0, right: 1)
self.veerSpeed = 40
self.maxSpeed = 100
self.aStar = AStar()
self.tcs = Adafruit_TCS34725.TCS34725()
self.initialLeftCount = self.aStar.read_encoders()[0]
self.initialRightCount = self.aStar.read_encoders()[1]
# check if currently reading green tape
def checkGreen(self):
r, g, b, c = self.tcs.get_raw_data()
return (g > r and g > b)
def checkRed(self):
r, g, b, c = self.tcs.get_raw_data()
return (r > g and r > b)
# set motor speeds
def motors(self, lspeed, rspeed):
self.aStar.motors(lspeed, rspeed)
def goForward(self):
self.motors(self.maxSpeed, self.maxSpeed)
def stop(self):
self.motors(0, 0)
def veerLeft(self):
self.motors(self.veerSpeed, self.maxSpeed)
def veerRight(self):
self.motors(self.maxSpeed, self.veerSpeed)
def getTime(self):
return time.time()
def playNotes(self):
self.aStar.play_notes("o4l16ceg>c8")
# turn leds on or off
def leds(self, red, yellow, green):
self.aStar.leds(red, yellow, green)
def readAnalog(self):
return self.aStar.read_analog()
def readBatteryMillivolts(self):
return self.aStar.read_battery_millivolts()
def readButtons(self):
return self.aStar.read_buttons()
def readEncoders(self):
encoders = self.aStar.read_encoders()
return (encoders[0] - self.initialLeftCount,
encoders[1] - self.initialRightCount)
def printColorInfo(self):
r, g, b, c = self.tcs.get_raw_data()
temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b)
lux = Adafruit_TCS34725.calculate_lux(r, g, b)
print('Color: r={0} g={1} b={2} temp={3} lux={4}'.format(
r, g, b, temp, lux))
def turnOffInterruptsRGB(self):
self.tcs.set_interrupt(False)
def turnOnInterruptsRGB(self):
self.tcs.set_interrupt(True)
# rgb sensor is enabled by default in the constructor method
def enableRGB(self):
self.tcs.enable()
def disableRGB(self):
self.tcs.disable()
def turnLeft(self, count):
while (self.readEncoders()[0] % 744 > (-1) * count
and self.readEncoders()[1] % 744 < count):
if (self.checkGreen()):
return 1
break
self.motors(0, self.maxSpeed)
self.stop()
return 0
def turnRight(self, count):
while (self.readEncoders()[0] % 744 < count
and self.readEncoders()[1] % 744 > (-1) * count):
if (self.checkGreen()):
return 1
break
self.motors(self.maxSpeed, 0)
self.stop()
return 0
def changeCount(self, count):
if (count != 0):
count = count / 2
def calibrate(self):
count = 744
while (self.checkGreen() == False):
if (self.turnLeft(count)):
pass
elif (self.turnRight(count)):
pass
self.changeCount(count)
# end while
self.goForwardtwo()
def turn90Left(self):
initEncoders = self.readEncoders()
while (self.readEncoders()[0] > initEncoders[0] - 744
and self.readEncoders()[1] < initEncoders[1] + 744):
self.motors(self.maxSpeed * (-1), self.maxSpeed)
self.stop()
def turn90Right(self):
initEncoders = self.readEncoders()
while (self.readEncoders()[1] > initEncoders[1] - 744
and self.readEncoders()[0] < initEncoders[0] + 744):
self.motors(self.maxSpeed, self.maxSpeed * (-1))
self.stop()
def goForwardtwo(self):
self.motors(self.maxSpeed, self.maxSpeed)
x = self.maxSpeed
diff = math.fabs(self.readEncoders()[1] - self.readEncoders()[0])
while (diff > 2):
diff = math.fabs(self.readEncoders()[1] - self.readEncoders()[0])
x -= 1
if (self.readEncoders()[1] > self.readEncoders()[0]):
self.motors(self.maxSpeed, x)
time.sleep(1.0 / 1000.0)
elif (self.readEncoders()[0] > self.readEncoders()[1]):
self.motors(x, self.maxSpeed)
time.sleep(1.0 / 1000.0)
self.motors(self.maxSpeed, self.maxSpeed)
'''
def calibrate(self):
while(self.checkGreen() == False):
for x in range(100, -20, -1):
self.motors(100 if x+40>100 else x+40,x)
'''
def adjustIntersection(self):
saveEncoders = self.readEncoders()
while (self.readEncoders()[0] - saveEncoders[0] < 648
or self.readEncoders()[1] - saveEncoders[1] < 648):
self.goForwardtwo()
# recalibrate the robot onto the path
def calibrateDirection(self):
calibrated = False
turnAmount = 200 # initial encoder count
inRed = False
while (self.checkGreen() == False):
# turn the robot one direction
if (self.nextSearchDirection == 0):
self.veerLeft()
else:
self.veerRight()
# switch turn direction for next iteration
self.nextSearchDirection = self.nextSearchDirection ^ 1
# wait to see if direction was correct
while (self.readEncoders()[self.nextSearchDirection] < turnAmount):
if (self.checkGreen()):
self.adjustIntersection()
while (not self.checkGreen()):
if (self.nextSearchDirection == 0):
self.motors(-100, 100)
else:
self.motors(100, -100)
slapEncoders = self.readEncoders()
if (self.nextSearchDirection == 0):
while (self.readEncoders()[0] - slapEncoders[0] < 81):
self.motors(-100, 100)
self.stop()
else:
while (self.readEncoders()[1] - slapEncoders[1] < 81):
self.motors(100, -100)
self.stop()
break
class RosRomiNode:
def __init__(self):
#Create a_star instance to talk to arduino based a_star Romi board
self.a_star = AStar()
#self.a_star = MockBot()
self.encoder_resolution = 12 #according to pololu encoder count
self.gear_reduction = 120 #according to romi website
self.wheel_diameter = 0.07 #70mm diameter wheels according to website
self.wheel_track = 0.141 #149mm outside to outside according to drawing, 8mm thick tires
self.motorMax = 400
#internal data
self.leftEncoder = None #last encoder state variables for odometry
self.rightEncoder = None
self.x = 0
self.y = 0
self.th = 0
self.leftMotor = 0
self.rightMotor = 0
#time variables for odometry pulled from http://github.com/hbrobotics/ros_arduino_bridge
self.rate = float(50) #hard coding this for now, will be param'd later
self.tickes_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)
now = rospy.Time.now()
self.then = now
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
rospy.Subscriber("lmotor_cmd", Float32, self.lmotorCallback)
rospy.Subscriber("rmotor_cmd", Float32, self.rmotorCallback)
self.lwheelPub = rospy.Publisher('lwheel', Int16, queue_size=5)
self.rwheelPub = rospy.Publisher('rwheel', Int16, queue_size=5)
def lmotorCallback(self, msg):
leftMotor = msg.data * self.motorMax
if leftMotor > self.motorMax:
leftMotor = self.motorMax
elif leftMotor < -self.motorMax:
leftMotor = -self.motorMax
self.leftMotor = int(leftMotor)
self.updateMotors()
def rmotorCallback(self, msg):
rightMotor = msg.data * self.motorMax
if rightMotor > self.motorMax:
rightMotor = self.motorMax
elif rightMotor < -self.motorMax:
rightMotor = -self.motorMax
self.rightMotor = int(rightMotor)
self.updateMotors()
def updateMotors(self):
print("Left: ", self.leftMotor, "Right: ", self.rightMotor)
self.a_star.motors(self.leftMotor,self.rightMotor)
def pollEncoders(self):
now = rospy.Time.now()
if now > self.t_next:
try:
leftEnc, rightEnc = self.a_star.read_encoders()
self.lwheelPub.publish(leftEnc)
self.rwheelPub.publish(rightEnc)
except:
return
self.t_next = now + self.t_delta
print("Test PixyCam")
pixy.init()
pixy.change_prog("color_connected_components")
class Blocks(Structure):
_fields_ = [("m_signature", c_uint), ("m_x", c_uint), ("m_y", c_uint),
("m_width", c_uint), ("m_height", c_uint), ("m_angle", c_uint),
("m_index", c_uint), ("m_age", c_uint)]
blocks = BlockArray(100)
frame = 0
pixy.set_lamp(0, 0)
a_star.motors(70, 70) # move straight forward, need to use pi control!!!
print("begin obstacle search")
while 1:
count = pixy.ccc_get_blocks(100, blocks) # 100 blocks probably overkill...
if (count > 0):
# make sure block is large enough to be relevant
if (blocks[0].m_width > 50 and blocks[0].m_height > 50):
a_star.motors(0, 0) # stops if a block is detected
pixy.set_lamp(1, 1) # turns on lamp if a block is detected
time.sleep(2)
pixy.set_lamp(0, 0)
print("obstacle detected, motor stopped")