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
#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 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
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:
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
import time
file_name = 'step_test.txt'
gear_ratio = 1322
fps = 50
frame_length = 1 / fps
episode_length = 3 # in seconds
encoders = []
motors = []
timestamps = []
velocities = []
voltages = []
start_time = time.time()
start_encoder = a_star.read_encoders()
start_rot = [start_encoder[0] / gear_ratio, start_encoder[1] / gear_ratio]
previous_rot = [0, 0]
for step in range(0, fps * episode_length):
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))
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)
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
