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