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")