def setup(self): if self.option == "a": # --- Import local libraries --- import direct.directbase.DirectStart from RobotModel3D import Robot from main_simulation3d import Simulation self.sim = Simulation() j = 1 # For the number of a collision sphere for i in numpy.arange(0, COLLISION_DIST / 30, 0.5): self.sim.sColl = self.sim.initCollisionSphere( self.sim.robot.robotModel, False, Point3(0, (COLLISION_DIST / 30) - i + 1, 1), j) base.cTrav.addCollider(self.sim.sColl[0], self.sim.collHandEvent) self.sim.accept('into-' + self.sim.sColl[1], self.sim.collide) j += 1 self.robot = self.sim.robot return self.robot else: # --- Import local libraries --- from easygopigo3 import EasyGoPiGo3 from RobotDexter import Dexter gpg = EasyGoPiGo3() self.robot = Dexter(gpg) return self.robot
def setup(self): if self.option == "a": # --- Import local libraries --- import direct.directbase.DirectStart from RobotModel3D import Robot from main_simulation3d import Simulation self.sim = Simulation() for i in numpy.arange(0, COLLISION_DIST / 30, 0.5): self.sim.sColl = self.sim.initCollisionSphere( self.sim.robot.robotModel, True, Point3(0, (COLLISION_DIST / 30) - i, 1)) base.cTrav.addCollider(self.sim.sColl[0], self.sim.collHandEvent) self.sim.accept('into-' + self.sim.sColl[1], self.sim.collide) self.robot = self.sim.robot return self.robot else: # --- Import local libraries --- from easygopigo3 import EasyGoPiGo3 from RobotDexter import Dexter #import main_robotA #Camera thread continious gpg = EasyGoPiGo3() self.robot = Dexter(gpg) return self.robot
def blinkers(): gpg = EasyGoPiGo3() global robot_operating while (robot_operating): if (ldr_right.light_detected and not ldr_left.light_detected): gpg.set_led(gpg.LED_BLINKER_RIGHT, 0) gpg.set_led(gpg.LED_BLINKER_LEFT, 255) gpg.set_led(gpg.LED_EYE_LEFT, 0) gpg.set_led(gpg.LED_EYE_RIGHT, 0) sleep(0.02) elif (ldr_left.light_detected and not ldr_right.light_detected): gpg.set_led(gpg.LED_BLINKER_RIGHT, 255) gpg.set_led(gpg.LED_BLINKER_LEFT, 0) gpg.set_led(gpg.LED_EYE_LEFT, 0) gpg.set_led(gpg.LED_EYE_RIGHT, 0) sleep(0.02) elif (ldr_right.light_detected and ldr_left.light_detected): gpg.set_led(gpg.LED_BLINKER_RIGHT, 0) gpg.set_led(gpg.LED_BLINKER_LEFT, 0) gpg.set_led(gpg.LED_EYE_LEFT, 255) gpg.set_led(gpg.LED_EYE_RIGHT, 255) sleep(0.02) else: gpg.set_led(gpg.LED_BLINKER_RIGHT, 0) gpg.set_led(gpg.LED_BLINKER_LEFT, 0) gpg.set_led(gpg.LED_EYE_LEFT, 0) gpg.set_led(gpg.LED_EYE_RIGHT, 0) sleep(0.02)
def main(): # start the camera in a separate thread stop_camera = Event() camera_thread = Thread(target=camera, args=(stop_camera,)) camera_thread.start() global gpg3, control_queue gpg3 = EasyGoPiGo3() gpg3.set_speed(180) control_queue = Queue() stop_robot = Event() robot_thread = Thread(target=robot, args=(stop_robot,)) robot_thread.start() prev_signal = ' ' while True: signal = key() # quit signal if signal == 'q': break elif control_queue.qsize() <= 0 or prev_signal != signal: prev_signal = signal control_queue.put(signal) # stop everything stop_camera.set() stop_robot.set() exit()
def obstacle(): # Initialize testbed motors gpg = EasyGoPiGo3() gpg.forward() [circle_detected, center, obj_distance] = detection_demo() gpg.stop() if (center[0] > 320): direction = 'right' rotate_dir = -1 else: direction = 'left' rotate_dir = 1 # Rotate camera gradually until object is no longer detected angle_rotated = 0 while (circle_detected): gpg.turn_degrees(rotate_dir * 10) [circle_detected, center, obj_distance] = detection_demo(infinite_loop=False) angle_rotated += 10 # Final turn to avoid grazing object after circle no longer in view gpg.turn_degrees(rotate_dir * 20) gpg.forward() time.sleep(5) gpg.stop()
def robot_drive(self, omf, uangle): gpg = EasyGoPiGo3() gpg.set_speed(180) #drive = gpg.forward() n = 0 rightAngle = False #print(n) if omf < 0: rightAngle = True while not rightAngle: orientationangle = self.imu.getHeadingDeg() print("orientationangle = ", orientationangle) if uangle - 9 <= orientationangle and uangle + 9 >= orientationangle: rightAngle = True #print("1 orientationangle = ", orientationangle, 'uangle = ', uangle) #print("same though") else: #print('current:',self.imu.getHeadingDeg()) #print('wanted :', uangle) temp = 0 temp2 = 0 for i in range(8, 359): #temp += 1 #tempCurrent #print('i', i) #print(int(orientationangle), int(uangle)) if int(orientationangle) + i > 360: if (int(orientationangle) + i) - 360 == int(uangle): temp = i #print('temp assigned: special', temp, i) if int(orientationangle) + i == int(uangle): temp = i #print('temp assigned:', temp, i) if int(orientationangle) - i < 0: if 360 + (int(orientationangle) - i) == int(uangle): temp2 = i if int(orientationangle) - i == int(uangle): temp2 = i #print('temp2 assigned:', temp2, i) if temp != 0 and temp2 != 0: #print('breaks at', i) break #print('1:', temp, '2:', temp2) if temp2 < temp: gpg.turn_degrees(-1 * temp2) print('turned') #print("2 orientationangle = ", orientationangle, 'uangle = ', uangle, 'diff cc=', temp2) else: gpg.turn_degrees(temp) print('turned') #print("2 orientationangle = ", orientationangle, 'uangle = ', uangle, 'diff cw=', temp) if omf > 0: gpg.forward() print('moved') if omf < 0: gpg.stop() print('stopped') """elif uangle - 10 < orientationangle:
def __init__(self, mock_controls=False): if mock_controls: self.gpg = MagicMock() else: self.gpg = EasyGoPiGo3() # instantiating a EasyGoPiGo3 object self.gpg = MagicMock() # instantiating a EasyGoPiGo3 object self.flashing = False self.executor = ThreadPoolExecutor(max_workers=1) self.executor.submit(self.flash_lights)
def __init__(self, dir_path, params=None): self.dir_path = dir_path self.calibrated = True self.gpg = EasyGoPiGo3() if params is None: self.rad = 200 self.h_spd = 400 self.m_spd = 200 self.l_spd = 30 else: self.rad = params['rad'] self.h_spd = params['h_spd'] self.m_spd = params['m_spd'] self.l_spd = params['l_spd']
def __init__(self, controler, fps=25, resolution=None, servoPort="SERVO1", motionPort="AD1"): """ Initialise le robot :controler: le controler du robot, muni d'une fonction update et d'une fonction stop qui rend in booleen (vrai a la fin du controle, faux sinon) :fps: nombre d'appel a controler.update() par seconde (approximatif!) :resolution: resolution de la camera :servoPort: port du servo (SERVO1 ou SERVO2) :motionPort: port pour l'accelerometre (AD1 ou AD2) """ self._gpg = EasyGoPiGo3() self.controler = controler self.fps = fps self.LED_LEFT_EYE = self._gpg.LED_LEFT_EYE self.LED_RIGHT_EYE = self._gpg.LED_RIGHT_EYE self.LED_LEFT_BLINKER = self._gpg.LED_LEFT_BLINKER self.LED_RIGHT_BLINKER = self._gpg.LED_RIGHT_BLINKER self.LED_WIFI = self._gpg.LED_WIFI self.MOTOR_LEFT = self._gpg.MOTOR_LEFT self.MOTOR_RIGHT = self._gpg.MOTOR_RIGHT try: self.camera = picamera.PiCamera() if resolution: self.camera.resolution = resolution except Exception as e: print("Camera not found", e) try: self.servo = Servo(servoPort, self._gpg) except Exception as e: print("Servo not found", e) try: self.distanceSensor = ds_sensor.DistanceSensor() except Exception as e: print("Distance Sensor not found", e) try: self.imu = imu.inertial_measurement_unit() except Exception as e: print("IMU sensor not found", e) self._gpg.set_motor_limits( self._gpg.MOTOR_LEFT + self._gpg.MOTOR_RIGHT, 0)
def move(bearing, leftMotorSpeed, rightMotorSpeed, pid): gpg = EasyGoPiGo3() motorSpeed = 50 output = pid(bearing) print(output) # calculate motor speedss leftMotorSpeed = int(motorSpeed + output) rightMotorSpeed = int(motorSpeed + output) if leftMotorSpeed == 0: leftMotorSpeed = 1 if rightMotorSpeed == 0: rightMotorSpeed = 1 if abs(bearing) <= 5: rightMotorSpeed = 100 leftMotorSpeed = 100 gpg.set_motor_dps(gpg.MOTOR_LEFT, dps=leftMotorSpeed) gpg.set_motor_dps(gpg.MOTOR_RIGHT, dps=rightMotorSpeed) return leftMotorSpeed, rightMotorSpeed, pid
def __init__(self): try: self.gopigo3_robot = EasyGoPiGo3() # init gopigo3 self.gopigo3_robot.set_speed(500) # adjust this accordingly except IOError: print("GoPiGo3 robot not detected") exit(0) try: self.distance_sensor = self.gopigo3_robot.init_distance_senseor() # init dist sensor except: print("GoPiGo3 distance sensor not detected") exit(0) try: self.servo = self.gopigo3_robot.init_servo() # init servo self.servo.reset_servo() # reset servo to straight ahead except: print("GoPiGo3 servo not detected") exit(0)
def movement(): gopigo3n = EasyGoPiGo3() while (True): if (ldr_right.light_detected and not ldr_left.light_detected): #print("Right LDR detected!") gopigo3n.left() sleep(0.02) elif (ldr_left.light_detected and not ldr_right.light_detected): #print("Left LDR detected!") gopigo3n.right() sleep(0.02) elif (ldr_right.light_detected and ldr_left.light_detected): #print("No LDRs detected!") gopigo3n.stop() sleep(0.02) else: #print("Both LDR detected!") gopigo3n.forward() sleep(0.02)
def objectAvoidance(): sleep(3) try: gopigo3 = EasyGoPiGo3() distance_sensor = gopigo3.init_distance_sensor() except IOError as msg: print("GoPiGo3 robot and sensors are not detected") sys.exit(1) gopigo3_stationary = True global robot_operating while robot_operating: current_distance = distance_sensor.read_mm() sleep(0.02) determined_speed = 0 if current_distance < MN_DISTANCE and current_distance > TOOCLOSE_DISTANCE: gopigo3_stationary = True gopigo3.stop() sleep(0.02) elif current_distance < TOOCLOSE_DISTANCE: gopigo3_stationary = False percent_speed = float(current_distance - MN_DISTANCE) / (MX_DISTANCE - MN_DISTANCE) determined_speed = MN_SPEED + (MX_SPEED - MN_SPEED) * percent_speed gopigo3.set_speed(determined_speed) gopigo3.drive_cm(-10, True) else: gopigo3_stationary = False if current_distance == NO_OBSTACLE: determined_speed = MX_SPEED else: percent_speed = float(current_distance - MN_DISTANCE) / ( MX_DISTANCE - MN_DISTANCE) determined_speed = MN_SPEED + (MX_SPEED - MN_SPEED) * percent_speed gopigo3.set_speed(determined_speed) #gopigo3.forward() gopigo3.stop()
def __init__(self, image_model_dir, cone_model_dir, image_dir, cone_image_dir, params, boundaries, log_dir='logs'): self.gpg = EasyGoPiGo3() self.dist_sensor = self.gpg.init_distance_sensor(port="AD1") self.servo = self.gpg.init_servo("SERVO1") self.servo.rotate_servo(100) self.params = params self.boundaries = boundaries self.image_dir = image_dir # Image Model self.image_model = ObjectClassificationModel(model_dir=image_model_dir, image_dir=image_dir, min_conf_threshold=0.3, use_TPU=True) # Cone Detection Model self.cone_detection_model = ConeClassificationModel( model_dir=cone_model_dir, image_dir=cone_image_dir, graph_name='cone_detect.tflite', min_conf_threshold=0.3, use_TPU=True) # Log File if not os.path.exists(log_dir): os.makedirs(log_dir) self.log_path = os.path.join(log_dir, 'log_' + str(date.today()) + '.txt')
def Main(): robotCar = EasyGoPiGo3() while True: command = speech.run() if command == 'go': print(command) robotCar.forward() elif command == 'back': print(command) robotCar.backward() elif command == 'left': print(command) robotCar.left() elif command == 'right': print(command) robotCar.right() elif command == 'stop': print(command) robotCar.stop() elif command == 'quit': print(command) robotCar.stop() break else: print("No command heard") continue
import time import math from easygopigo3 import EasyGoPiGo3 import easygopigo3 as easy gpg = easy.EasyGoPiGo3() gpg = EasyGoPiGo3() # Create object instance of the robot # Drawing a square: length = 30 # Side length for i in range(4): gpg.drive_cm(length) # Drive forward for length cm gpg.turn_degrees(90) # Rotate 90 degrees to the right time.sleep(5) # Time delay between figures # Drawing a circle: # uses the .orbit(D, R) command where D is the degrees you'll be # rotating and R the radius [cm] of said circle. gpg.orbit(360, 40) # Drawing a rectangle short_side = 30 long_side = 60 for i in range(2): gpg.drive_cm(short_side) gpg.turn_degrees(90) gpg.drive_cm(long_side) gpg.turn_degrees(90)
def squarepath(trigger): gopigo3_robot = EasyGoPiGo3() my_distance_sensor = gopigo3_robot.init_distance_sensor() #set NORTH before staring right = 1 i = 0 with open('problem1_pathtrace.csv', 'w') as csvfile: fieldnames = ['row_num', 'encoder_value','distance_value'] writer = csv.DictWriter(csvfile, fieldnames=fieldnames) writer.writeheader() def drive_and_turn(i,right,dist=999): gopigo3_robot.reset_encoders() #drive autonomously until Ctrl-C pressed #check if an obstacle faced within 250mm (25cm) while not (trigger.is_set() or dist<=250): gopigo3_robot.forward() dist = my_distance_sensor.read_mm() print("Distance Sensor Reading: {} mm ".format(dist)) #enocder average values to get distance in cm moved encoders_read = round(gopigo3_robot.read_encoders_average()) #write sensor values to a file row = [i, encoders_read, dist] with open('problem1_pathtrace.csv', 'a') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() i+=1 if(encoders_read) >= 50: #if 50 cm crossed, stop and take turn gopigo3_robot.stop() if(right): gopigo3_robot.turn_degrees(90) else: gopigo3_robot.turn_degrees(-90) #recursive drive square drive_and_turn(i,right) break if not trigger.is_set(): #when object within 25cm encountered gopigo3_robot.stop() gopigo3_robot.turn_degrees(180) #travel back rest of the distance (using enooder value which may not be robust value) #and take a turn(L/R) before continuing the square path again gopigo3_robot.drive_cm(encoders_read) if(right): gopigo3_robot.turn_degrees(-90) right = 0 else: gopigo3_robot.turn_degrees(90) right = 1 drive_and_turn(i,right) return if not trigger.is_set(): drive_and_turn(i,right) #returns when trigger is set return
def robotControl(trigger, simultaneous_launcher, motor_command_queue, sensor_queue): """ Thread-launched function for orientating the robot around. It gets commands from the keyboard as well as data from the sensor through the sensor_queue queue. :param trigger: CTRL-C event. When it's set, it means CTRL-C was pressed and the thread needs to stop. :param simultaneous_launcher: It's a barrier used for synchronizing all threads together. :param motor_command_queue: Queue containing commands from the keyboard. The commands are read from within main. :param sensor_queue: Processed data off of the IMU. The queue is intended to be read. :return: Nothing. """ time_to_wait_in_queue = 0.1 # measured in # try to connect to the GoPiGo3 try: gopigo3_robot = EasyGoPiGo3() #TODO CODE################ #my_distance_sensor = gopigo3_robot.init_distance_sensor() #TODO CODE################ except IOError: print("GoPiGo3 robot not detected") simultaneous_launcher.abort() except gopigo3.FirmwareVersionError: print("GoPiGo3 board needs to be updated") simultaneous_launcher.abort() except Exception: print("Unknown error occurred while instantiating GoPiGo3") simultaneous_launcher.abort() # synchronizing point between all threads # if abort method was called, then the synch will fail try: simultaneous_launcher.wait() except threading.BrokenBarrierError as msg: print("[robotControl] thread couldn't be launched") # if threads were successfully synchronized # then set the GoPiGo3 appropriately if not simultaneous_launcher.broken: gopigo3_robot.stop() gopigo3_robot.set_speed(MOTORS_SPEED) direction_degrees = None move = False acceptable_error_percent = 8 command = "stop" rotational_factor = 0.30 accepted_minimum_by_drivers = 6 # while CTRL-C is not pressed, the synchronization between threads didn't fail and while the batteries' voltage isn't too low while not (trigger.is_set() or simultaneous_launcher.broken or gopigo3_robot.volt() <= MINIMUM_VOLTAGE): # read from the queue of the keyboard try: command = motor_command_queue.get(timeout = time_to_wait_in_queue) motor_command_queue.task_done() except queue.Empty: pass # make some selection depending on what every command represents if command == "stop": move = False elif command == "move": move = True if command == "west": direction_degrees = -90.0 elif command == "east": direction_degrees = 90.0 elif command == "north": direction_degrees = 0.0 elif command == "south": direction_degrees = 180.0 # if a valid orientation was selected if direction_degrees is not None: # read data and calculate orientation heading = sensor_queue.get() if direction_degrees == 180.0: heading_diff = (direction_degrees - abs(heading)) * (-1 if heading < 0 else 1) error = abs(heading_diff / direction_degrees) * 100 else: heading_diff = direction_degrees - heading error = abs(heading_diff / 180) * 100 how_much_to_rotate = int(heading_diff * rotational_factor) if DEBUG is True: print("direction_degrees {} heading {} error {} heading_diff {}".format(direction_degrees, heading, error, heading_diff)) # check if the heading isn't so far from the desired orientation # if it needs correction, then rotate the robot if error >= acceptable_error_percent and abs(how_much_to_rotate) >= accepted_minimum_by_drivers: gopigo3_robot.turn_degrees(how_much_to_rotate, blocking = True) # command for making the robot move of stop if move is False: gopigo3_robot.stop() else: #TODOCODE########### #set NORTH when staring squarepath(trigger) #TODOCODE########### sleep(0.001) # if the synchronization wasn't broken # then stop the motors in case they were running if not simultaneous_launcher.broken: gopigo3_robot.stop()
import cv2 as cv import numpy as np import time import select import sys from time import sleep from picamera import PiCamera import picamera.array from easygopigo3 import EasyGoPiGo3 GPG = EasyGoPiGo3() cam = PiCamera() #-----CONFIG----- # Camera WIDTH = 128 HEIGHT = 80 # Canny CANNY_THRESHOLD_1 = 50 CANNY_THRESHOLD_2 = 150 # HoughLines RHO_RESOLUTION = 1 THETA_RESOLUTION = np.pi / 180 VOTE_THRESHOLD = 5 MIN_LINE_LENGTH = 15 MAX_LINE_GAP = 20 # Motion control FORWARD_POWER = 50 #-----FUNCTIONS-----
def runner(event_stopper): try: with open('config.json', 'r') as f: config = json.load(f) except Exception as err: logger.exception(err) event_stopper.set() return # start another process to render the graphical representation # of what the camera sees queue = MQueue(5) video_process = Process(target=video_worker, args=(queue, event_stopper)) video_process.start() try: # initialize devices pixy = pixy2.Pixy2I2C(address=0x54) gopigo3 = EasyGoPiGo3() logger.info('initialized pixy2 and gopigo3') # turn on the built-in LED for better visibility pixy.set_lamp(1) logger.info('turn on pixy2 lamp') # set it to follow white lanes pixy.set_mode(pixy2.LINE_MODE_WHITE_LINE) logger.info('set pixy2 line mode on white') # get the number of frames per second fps = pixy.get_fps() period = 1 / fps logger.info('pixy2 running at {} fps'.format(fps)) # camera resolution width, height = pixy.get_resolution() logger.info('pixy2 camera width={} height={}'.format(width, height)) # keep previous measurements for smoothing out the data previous_angle = 0 previous_magnitude = 0 previous_ema_angle = 0 previous_ema_magnitude = 0 frame_index = 0 alfa = config['alfa'] yotta = config['yotta'] # magnitude importance logger.info('alfa (exp. moving average factor) = {:3.2f}'.format(alfa)) logger.info('yotta (magnitude importance) = {:3.2f}'.format(yotta)) # the 2 PIDs c1 = PID(Kp=config['pid1']['Kp'], Ki=config['pid1']['Ki'], Kd=config['pid1']['Kd'], previous_error=0.0, integral_area=0.0) c2 = PID(Kp=config['pid2']['Kp'], Ki=config['pid2']['Ki'], Kd=config['pid2']['Kd'], previous_error=0.0, integral_area=0.0) pid_switch_point = config['pid-switch-point'] logger.info('PID 1 = {}'.format(str(c1))) logger.info('PID 2 = {}'.format(str(c2))) logger.info('PID switchpoint = {}'.format(pid_switch_point)) set_point = 0 max_motor_speed = config['max-motor-speed'] logger.info('max motor speed = {}'.format(max_motor_speed)) while not event_stopper.is_set(): # track each step's time start = time() # get the main features from the pixy2 features = pixy.get_main_features(features=1) frame = None if features: if 'vectors' in features: frame = features['vectors'] else: frame = [] frame_index += 1 else: gopigo3.stop() sleep(period) continue # calculate the heading of the lane and detect how long the lane is lanes, win_size, roi = detect_lanes_from_vector_frame( frame, height, width) angle, magnitude = calculate_direction_and_magnitude( lanes, win_size, previous_angle, previous_magnitude) # run the determined heading and magnitude through an exponential moving average function # this has the effect of smoothing the results and of ignoring the short term noise ema_angle = exponential_moving_average(angle, previous_ema_angle, alfa) ema_magnitude = exponential_moving_average(magnitude, previous_ema_magnitude, alfa) # save the measurements for the next iteration previous_angle = angle previous_magnitude = magnitude previous_ema_angle = ema_angle previous_ema_magnitude = ema_magnitude angle = ema_angle magnitude = ema_magnitude # push the processed information to the graphical renderer (on a separate process) if not queue.full(): queue.put({ 'frame_idx': frame_index, 'frame': frame, 'resolution': (height, width), 'fps': fps, 'angle': angle, 'magnitude': magnitude, 'lanes': lanes, 'win_size': win_size }) # run the pid controller error = angle - set_point c1.integral_area += error c2.integral_area += error pid1 = True if magnitude < pid_switch_point: c = c1 c2.integral_area = 0.0 else: c = c2 c1.integral_area = 0.0 pid1 = False # calculate the correction correction = c.Kp * error + c.Ki * c.integral_area + c.Kd * ( error - c.previous_error) c1.previous_error = error c2.previous_error = error # determine motor speeds left_motor_speed = int(( (1 - yotta) + yotta * magnitude / 100) * max_motor_speed + correction) right_motor_speed = int(( (1 - yotta) + yotta * magnitude / 100) * max_motor_speed - correction) logger.debug( 'using PID {} | angle: {:3d} magnitude: {:3d} | left speed: {:3d} right speed: {:3d}' .format(1 if pid1 else 0, int(angle), int(magnitude), left_motor_speed, right_motor_speed)) # actuate the motors gopigo3.set_motor_dps(gopigo3.MOTOR_LEFT, dps=left_motor_speed) gopigo3.set_motor_dps(gopigo3.MOTOR_RIGHT, dps=right_motor_speed) # make it run at a specific loop frequency # equal to camera's fps end = time() diff = end - start remaining_time = period - diff if remaining_time > 0: sleep(remaining_time) # and turn the built-in LED off when the program ends # and stop the gopigo3 pixy.set_lamp(0) gopigo3.stop() except Exception as e: event_stopper.set() logger.exception(e) finally: video_process.join()
def runner(event_stopper): try: pixy = pixy2.Pixy2I2C(address=0x54) gopigo3 = EasyGoPiGo3() target_signature = 1 tracked_index = None # call it once and discard the data # this is meant to automatically switch to block detection get_signature_blocks(pixy, sigmap=target_signature) # pid controller & exponential moving average settings pid = PID(Kp=400.0, Ki=0.0, Kd=0.0, previous_error=0.0, integral_area=0.0) width, height = pixy.get_resolution() loop_frequency = 60 period = 1 / loop_frequency alfa = 0.15 # 20% previous_ema_speed = 0.0 after_how_many_cycles_to_stop = 30 cycle = 0 logger.info('set ' + str(pid)) logger.info('exponential moving average alfa={}'.format(alfa)) logger.info('stop motors after {} cycles of inactivity'.format( after_how_many_cycles_to_stop)) logger.info('control loop running at {}Hz'.format(loop_frequency)) # ratio-to-speed interpolation ratio_to_speed = interpolate.interp1d(x=[1, 30], y=[100, 350], fill_value=(0, 350), bounds_error=False) while not event_stopper.is_set(): start = time() # get the detected blocks blocks = get_signature_blocks(pixy, sigmap=target_signature) # sleep if nothing is detected if not blocks: sleep(period) if cycle % after_how_many_cycles_to_stop == 0: cycle = 1 gopigo3.stop() else: cycle += 1 continue # initialize the tracked_index if it's the case if not tracked_index: tracked_index = blocks[0].track_index logger.info('change tracked index to {}'.format(tracked_index)) # select the best block based on our tracked index # if it ain't there, then pick a new one block = None for _block in blocks: if _block.track_index == tracked_index: block = _block if not block: tracked_index = blocks[0].track_index block = blocks[0] logger.info('change tracked index to {}'.format(tracked_index)) # gives us the error error = block.x_center / width - 0.5 # calculate motor speeds based on how far away the object is block_to_image_ratio = height * width / (block.height * block.width) unaveraged_motor_speed = ratio_to_speed(block_to_image_ratio) base_motor_speed = exponential_moving_average( unaveraged_motor_speed, previous_ema_speed, alfa) previous_ema_speed = base_motor_speed # run the pid controller pid.integral_area += error correction = pid.Kp * error + pid.Ki * pid.integral_area + pid.Kd * pid.previous_error pid.previous_error = error # calculate motor speeds based on pid's correction left_motor_speed = int(base_motor_speed + correction) right_motor_speed = int(base_motor_speed - correction) logger.debug('motor speed left={} right={}'.format( left_motor_speed, right_motor_speed)) # actuate the motors gopigo3.set_motor_dps(gopigo3.MOTOR_LEFT, dps=left_motor_speed) gopigo3.set_motor_dps(gopigo3.MOTOR_RIGHT, dps=right_motor_speed) end = time() remaining_time = period - (end - start) if remaining_time > 0: sleep(remaining_time) gopigo3.stop() except Exception: event_stopper.set()
def obstacleFinder(trigger, put_on_hold, simultaneous_launcher, sensor_queue): leftmost_degrees = 30 rightmost_degrees = 150 current_servo_position = leftmost_degrees middle = 90 step = 10 servo_delay = 0.035 wait_before_it_starts = 0 distance_trigger = 25 how_much_to_rotate = 150 to_the_right = True # try to connect to the GoPiGo3 try: gopigo3_robot = EasyGoPiGo3() except IOError: print("GoPiGo3 robot not detected") simultaneous_launcher.abort() # initialize a Servo object servo = gopigo3_robot.init_servo("SERVO1") # try to connect to the distance sensor try: distance_sensor = gopigo3_robot.init_distance_sensor() except IOError: print("DistanceSensor couldn't be found") simultaneous_launcher.abort() except gopigo3.FirmwareVersionError: print("GoPiGo3 board needs to be updated") simultaneous_launcher.abort() except Exception: print("Unknown error occurred while instantiating GoPiGo3") simultaneous_launcher.abort() # rotate the servo to the desired start-up position servo.rotate_servo(leftmost_degrees) sleep(wait_before_it_starts) # check if an error has occurred during all the above processes try: simultaneous_launcher.wait() except threading.BrokenBarrierError: print("[obstacleFinder] thread couldn't be launched") if not simultaneous_launcher.broken: print("[obstacleFinder] thread fully active") # and if everything is okay # start collecting, interpreting and sending messages # to the thread-launched [robotController] function while not trigger.is_set() and not simultaneous_launcher.broken: possible_routes = 0 deadends = 0 sonar_samples = [] # if the thread is put on hold by [robotController] # then wait until it's allowed to proceed if not put_on_hold.is_set() and sensor_queue.empty(): # when the servo is rotating to the right if to_the_right is True: # rotate to the leftmost_degrees position in case the head is in the middle servo.rotate_servo(leftmost_degrees) sleep(servo_delay) # read the distance to the target at every specific orientation of the servo # and go with the best greedy-like solution while current_servo_position <= rightmost_degrees: servo.rotate_servo(current_servo_position) sleep(servo_delay) distance = distance_sensor.read() current_servo_position += step possible_routes += 1 # print("left", distance, 90 - current_servo_position) if distance > distance_trigger: sonar_samples.append([distance, 90 - current_servo_position]) else: deadends += 1 to_the_right = False current_servo_position = rightmost_degrees # when the servo is rotating to the left elif to_the_right is False: # rotate to the leftmost_degrees position in case the head is in the middle servo.rotate_servo(rightmost_degrees) sleep(servo_delay) # read the distance to the target at every specific orientation of the servo # and go with the best greedy-like solution while current_servo_position >= leftmost_degrees: servo.rotate_servo(current_servo_position) sleep(servo_delay) distance = distance_sensor.read() current_servo_position -= step possible_routes += 1 # print("right", distance, 90 - current_servo_position) if distance > distance_trigger: sonar_samples.append([distance, 90 - current_servo_position]) else: deadends += 1 to_the_right = True current_servo_position = leftmost_degrees # remove the following line to make the servo rotational behavior more time-efficient # by removing it, the time needed to take the measurements is halved servo.rotate_servo(middle) # if the sonar (servo + distance sensor) couldn't find a distance >= threshold distance if deadends == possible_routes: # then rotate a lot in order to find a new spot sensor_queue.put([0, how_much_to_rotate]) else: # do k-means clustering on samples taken from the rotating servo sample_size = len(sonar_samples) if sample_size < 3: kmeans = KMeans(n_clusters = sample_size) else: kmeans = KMeans(n_clusters = 3) kmeans.fit(sonar_samples) params = kmeans.cluster_centers_ idx = getIndexOfHighestValueInList(params, 0) # push the orientation which leads to the farthest object detected # aka takes the "freest" route sensor_queue.put(params[idx]) sleep(0.01) # move the servo to the middle when the thread is being stopped servo.rotate_servo(middle)
def robotController(trigger, put_on_hold, simultaneous_launcher, sensor_queue): # try to connect to the GoPiGo3 try: gopigo3_robot = EasyGoPiGo3() except IOError: print("GoPiGo3 robot not detected") simultaneous_launcher.abort() except gopigo3.FirmwareVersionError: print("GoPiGo3 board needs to be updated") simultaneous_launcher.abort() except Exception: print("Unknown error occurred while instantiating GoPiGo3") simultaneous_launcher.abort() # set a lower speed of the GoPiGo3 gopigo3_robot.set_speed(300) gopigo3_robot.stop() # in case the GoPiGo3 is moving, stop it previous = 0 # see the following instructions how_much_of_distance = 0.30 # measured in percentage # if the robot is unplugged from the battery pack # or if the batteries are low # then abort everything if gopigo3_robot.volt() <= MINIMUM_VOLTAGE: print("Voltage too low") simultaneous_launcher.abort() # check if an error has occurred during all the above processes try: simultaneous_launcher.wait() except threading.BrokenBarrierError: print("[robotController] thread couldn't be launched") if not simultaneous_launcher.broken: print("[robotController] thread fully active") # if everything is fine # start polling messages from [obstacleFinder] function (which is thread-launched) while not trigger.is_set() and not simultaneous_launcher.broken: try: (distance_to_walk, rotation) = sensor_queue.get_nowait() sensor_queue.task_done() except queue.Empty: sleep(0.001) continue print("Rotating at {} degrees and driving for {} cm".format(rotation, distance_to_walk * how_much_of_distance)) put_on_hold.set() gopigo3_robot.turn_degrees(rotation, blocking = True) gopigo3_robot.drive_cm(distance_to_walk * how_much_of_distance, blocking = False) # give some time for the robot to start moving # before taking measurements of its speed sleep(0.3) # while the robot is moving and CTRL-C hasn't been pressed while gopigo3_robot.get_motor_status(gopigo3_robot.MOTOR_LEFT)[3] != 0 and \ gopigo3_robot.get_motor_status(gopigo3_robot.MOTOR_RIGHT)[3] != 0 and \ not trigger.is_set(): sleep(0.001) put_on_hold.clear() sleep(0.001) # stop the motors when the thread is being stopped gopigo3_robot.stop()
#!/usr/bin/env python import time from easygopigo3 import EasyGoPiGo3 from di_sensors import line_follower car = EasyGoPiGo3() car.stop() car.drive_cm(150) car.stop() car.turn_degrees(270)
def Main(): print(" _____ _____ _ _____ ____ ") print(" / ____| | __ (_)/ ____| |___ \ ") print(" | | __ ___ | |__) || | __ ___ __) |") print(" | | |_ |/ _ \| ___/ | | |_ |/ _ \ |__ < ") print(" | |__| | (_) | | | | |__| | (_) | ___) |") print(" \_____|\___/|_| |_|\_____|\___/ |____/ ") print(" ") # initializing an EasyGoPiGo3 object and a DistanceSensor object # used for interfacing with the GoPiGo3 and with the distance sensor try: gopigo3 = EasyGoPiGo3() distance_sensor = gopigo3.init_distance_sensor() print("intialised objects succesfully") except IOError as msg: print("GoPiGo3 robot not detected or DistanceSensor not installed.") debug(msg) sys.exit(1) except FirmwareVersionError as msg: print("GoPiGo3 firmware needs to updated.") debug(msg) sys.exit(1) except Exception as msg: print("Error occurred. Set debug = True to see more.") debug(msg) sys.exit(1) if DEBUG is True: distance_sensor.enableDebug() # variable that says whether the GoPiGo3 moves or is stationary # used during the runtime gopigo3_stationary = True global robot_operating # while the script is running while robot_operating: # read the distance from the distance sensor current_distance = distance_sensor.read_mm() determined_speed = 0 # if the sensor can't be detected if current_distance == ERROR: print("Cannot reach DistanceSensor. Stopping the process.") robot_operating = False # if the robot is closer to the target elif current_distance < MIN_DISTANCE: # then stop the GoPiGo3 gopigo3_stationary = True gopigo3.stop() # if the robot is far away from the target else: gopigo3_stationary = False # if the distance sensor can't detect any target if current_distance == NO_OBSTACLE: # then set the speed to maximum determined_speed = MAX_SPEED else: # otherwise, calculate the speed with respect to the distance from the target percent_speed = float(current_distance - MIN_DISTANCE) / ( MAX_DISTANCE - MIN_DISTANCE) determined_speed = MIN_SPEED + (MAX_SPEED - MIN_SPEED) * percent_speed # apply the changes gopigo3.set_speed(determined_speed) gopigo3.forward() # and last, print some stats print("Current distance : {:4} mm Current speed: {:4} Stopped: {}". format(current_distance, int(determined_speed), gopigo3_stationary is True)) # give it some time, # otherwise you'll have a hard time exiting the script sleep(0.08) # and finally stop the GoPiGo3 from moving gopigo3.stop()
def Main(): print(" _____ _____ _ _____ ____ ") print(" / ____| | __ (_)/ ____| |___ \ ") print(" | | __ ___ | |__) || | __ ___ __) |") print(" | | |_ |/ _ \| ___/ | | |_ |/ _ \ |__ < ") print(" | |__| | (_) | | | | |__| | (_) | ___) |") print(" \_____|\___/|_| |_|\_____|\___/ |____/ ") print(" ") print( "To move the robot around using the mouse buttons press 1 and enter.") print( "To move the robot around using the movements of the mouse press 2 and enter." ) # read data from the keyboard # if it fails reading the right values, then the script exits try: choice = int(input("choice (1/2) = ")) except ValueError: print("Invalid number read") sys.exit(1) if not (choice == 1 or choice == 2): print("Invalid number entered") sys.exit(1) # now the choice var can either be 1 or 2 # show different menus depending on the choice var print( "\nWith this script you can control your GoPiGo3 robot with a wireless mouse." ) if choice == 1: print( "1. Left + Right buttons of the mouse - move the GoPiGo3 forward") print("2. Left button of the mouse - move the GoPiGo3 to the left") print("3. Right button of the mouse - move the GoPiGo3 to the right") print("4. Middle button of the mouse - move the GoPiGo3 backward") else: print("1. Move the mouse forward - for moving the GoPiGo3 forward") print("2. Move the mouse backward - for moving the GoPiGo3 backward") print( "3. Move the mouse to the left - for rotating the GoPiGo3 to the left" ) print( "4. Move the mouse to the right - for rotating the GoPiGo3 to the right" ) # Wait for an input to start input("\nPress Enter to start") # create an instance of the EasyGoPiGo3 class # if it fails instantiating the object, then the scripts exits try: robot = EasyGoPiGo3() except IOError: print("GoPiGo3 not detected") sys.exit(1) except gopigo3.FirmwareVersionError: print("Please update your GoPiGo3 firmware") sys.exit(1) except Exception: print("Something went wrong") sys.exit(1) # stops the robot from moving when exiting the script # the cleanup_func is called after the signal_handler function atexit.register(cleanup_func, gopigo3=robot) print( "\nIn order to stop the script, press CTRL-C and move your mouse a little bit" ) # open file for reading the continuous stream of data from the mouse with open("/dev/input/mice", "rb") as mouse_input: left_button = 0 middle_button = 0 right_button = 0 x_axis = 0 y_axis = 0 # as long as CTRL-C hasn't been pressed while signal_not_called: # read the mouse's values # this is a blocking function (left_button, middle_button, right_button, x_axis, y_axis) = getMouseValues(mouse_input) # if we have the first choice (see the menu) if choice == 1: # when both the mouse's buttons are pressed # move forward if left_button == True and right_button == True: robot.forward() # when just the left button is pressed # move to the left elif left_button == True and right_button == False: robot.left() # when just the right button is pressed # move to the right elif left_button == False and right_button == True: robot.right() # when the middle button is pressed # move backward elif middle_button == True: robot.backward() # when no button is pressed # stop the robot from moving elif middle_button == False or (left_button == False and right_button == False): robot.stop() # if we have the second choice (see the menu) else: # if the mouse is moved to the left # then move the robot to the left if x_axis < -MOUSE_THRESH: robot.left() # if the mouse is moved to the right # then move the robot to the right elif x_axis > MOUSE_THRESH: robot.right() # if the mouse is moved backward # then move the robot backward elif y_axis < -MOUSE_THRESH: robot.backward() # if the mouse is moved forward # then move the robot forward elif y_axis > MOUSE_THRESH: robot.forward() # if the mouse is not moving in any direction # then stop the robot from moving else: robot.stop() sleep(0.10)
from easygopigo3 import EasyGoPiGo3 import math import time from picamera.array import PiRGBArray from picamera import PiCamera import argparse from fanshim import FanShim fanshim = FanShim() fanshim.set_fan(True) utils = raspberry_utils.Utils() cap = None easy = EasyGoPiGo3() easy.set_left_eye_color((10,20,30)) # def center_cone(): # b_box_rec = cones_data['cone-0']['bouding_box_center'] # print(f"Center of the rectangle-{b_box_rec}") # print(f"Center of the screen-{center_of_the_screen}") # cv2.line(frame_back, b_box_rec, center_of_the_screen, [0, 255, 0], 3) # if center_of_the_screen[0] != b_box_rec[0]: # if the angle is not equal to 90 only do thistohis else we are getting divde by 0 error # angle = int(math.atan((b_box_rec[1] - center_of_the_screen[1])/(b_box_rec[0] - center_of_the_screen[0])) * 180/math.pi ) # else: # angle = 90 # print(F"Angle is{angle}") # cv2.putText(frame_back, f"Angle: {angle}", (center_of_the_screen[0]-10, center_of_the_screen[1]-10), cv2.FONT_HERSHEY_SIMPLEX, .5, (0,255, 255), 1, cv2.LINE_AA) # # #if b_box_rec[0] > center_of_the_screen[0]:
####################### ### Web Server Stuff ## ####################### # Directory Path can change depending on where you install this file. Non-standard installations # may require you to change this directory. directory_path = '/home/pi/Dexter/GoPiGo3/Projects/RemoteCameraRobot/static' training_path = '/home/pi/test/training' data_file = training_path + '/' + 'training_data.csv' MAX_FORCE = 5.0 MIN_SPEED = 100 MAX_SPEED = 300 try: gopigo3_robot = EasyGoPiGo3() except IOError: logging.critical('GoPiGo3 is not detected.') sys.exit(1) except FirmwareVersionError: logging.critical('GoPiGo3 firmware needs to be updated') sys.exit(2) except Exception: logging.critical("Unexpected error when initializing GoPiGo3 object") sys.exit(3) HOST = "0.0.0.0" WEB_PORT = 5000 app = Flask(__name__, static_url_path='') state = 'stop'
#record while drive from scavear_main import Listener from scavear_main import Scavear #import multiprocessing as mp from threading import Thread import time from easygopigo3 import EasyGoPiGo3 from datetime import date gpg = EasyGoPiGo3() def drive(seconds = 150): now = time.time() while time.time() - now < seconds: drive_rectangle() def drive_rectangle(side=5): # gpg.set_speed(h_spd) gpg.forward() time.sleep(side) gpg.stop() gpg.turn_degrees(95) gpg.forward() time.sleep(side) gpg.stop() gpg.turn_degrees(95) gpg.forward()
def stop(): gpg = EasyGoPiGo3() gpg.stop()