def listen(): key = event.char if (key.lower() == "w"): robot.forward() else: gpio.cleanup() time.sleep(0.025)
def control_robot(): global ts, old_id ts = time.time() direction = request.args.get('direction', 0) mousedown = request.args.get('mousedown', 0) user_age = int(request.args.get('user_age', 0)) user_id = request.args.get('user_id', 0) for robot in ROBOTS: if user_id == robot.user_id: if mousedown == '1': print('button pressed:', direction) if direction == 'up': robot.forward() elif direction == 'left': robot.left() elif direction == 'right': robot.right() else: robot.reverse() else: robot.stop() return jsonify(color=robot.color, endgame=0) elif user_id == old_id: return jsonify(color=robot.color, endgame=1) # if above didn't return, new user: oldest_robot = max(ROBOTS, key=lambda x: x.age()) old_id = oldest_robot.user_id oldest_robot.user_id = user_id oldest_robot.user_age = user_age oldest_robot.user_time = time.time() print("new user: {}, {}, assigned {}".format(user_id, user_age, robot.color)) return jsonify(color=robot.color, endgame=0)
async def handle_command(websocket, path): while True: message = await websocket.recv() message = literal_eval(message) print(message) if message['command'] == 'moveForward': robot.forward() answer = {'action': 'moveForward'} elif message['command'] == 'moveBackward': robot.backward() answer = {'action': 'moveBackward'} elif message['command'] == 'turnRight': robot.right() answer = {'action': 'turnRight'} elif message['command'] == 'turnLeft': robot.left() answer = {'action': 'turnLeft'} elif message['command'] == 'stop': robot.stop() answer = {'action': 'stop'} elif message['command'] == 'grabberGrab': # grabber action grab answer = {'action': 'grabberGrab', 'value': message['value']} elif message['command'] == 'grabberTilt': # grabber action tilt answer = {'action': 'grabberTilt', 'value': message['value']} elif message['command'] == 'grabberHeight': # grabber action height answer = {'action': 'grabberHeight', 'value': message['value']} await websocket.send(json.dumps(answer))
def execute(img): global width global height global isObject global isMoving global start image = cv2.resize(img, (300,300)) # compute all detected objects detections = model(image) # draw all detections on image for det in detections[0]: bbox = det['bbox'] text = str(category_map[det['label']]) cv2.rectangle(image, (int(width * bbox[0]), int(height * bbox[1])), (int(width * bbox[2]), int(height * bbox[3])), (255, 0, 0), 2) cv2.putText(image, text, (int(width * bbox[0]), int(height * bbox[3])), cv2.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 255), lineType=cv2.LINE_AA) # select detections that match selected class label matching_detections = [d for d in detections[0] if d['label'] == 53] # get detection closest to center of field of view and draw it det = closest_detection(matching_detections) if det is not None: bbox = det['bbox'] cv2.rectangle(image, (int(width * bbox[0]), int(height * bbox[1])), (int(width * bbox[2]), int(height * bbox[3])), (0, 255, 0), 5) if det is None: if isObject == True: start = time.time() isObject = False if (time.time() - start) > 4: robot.stop() # otherwsie steer towards target else: if isMoving == False: start = time.time() isMoving = True center = detection_center(det) print(center) # move robot forward and steer proportional target's x-distance from center if center[0] > 0.2: print('left') robot.left() elif center[0] < -0.2: print('right') robot.right() elif center[0] > -0.2 and center[0] < 0.2: print('forward') robot.forward() if (time.time() - start) > 2: isMoving = False return cv2.imencode('.jpg', image)[1].tobytes()
def do_POST(self): global f global b global r global l global robot self._set_headers() form = cgi.FieldStorage(fp=self.rfile, headers=self.headers, environ={'REQUEST_METHOD': 'POST'}) print form.getvalue("forward") print form.getvalue("backward") print form.getvalue("left") print form.getvalue("right") if form.getvalue("forward") == "Forward": if f == 0: robot.stop() robot.forward() f = 1 else: robot.stop() f = 0 if form.getvalue("backward") == "Backward": if b == 0: robot.stop() robot.backward() b = 1 else: robot.stop() b = 0 if form.getvalue("left") == "Left": if l == 0: robot.stop() robot.left() l = 1 else: robot.stop() l = 0 if form.getvalue("right") == "Right": if r == 0: robot.stop() robot.right() r = 1 else: robot.stop() r = 0 file = open("index.html", "r") page = file.read() file.close() self.wfile.write(page)
def move(direction): if direction == 'forward': robot.forward(0.5) if direction == 'backward': robot.backward(0.5) if direction == 'left': robot.left(0.5) if direction == 'right': robot.right(0.5) return '{}'
def action(client, userdata, message): data = message.payload.decode() data = json.loads(data) data = data['action'] print(data) if data == 'forward': print('forward') robot.forward() elif data == 'stop': print('stop') robot.stop() elif data == 'left': print('left') robot.left() elif data == 'right': print('right') robot.right() elif data == 'backward': print('backward') robot.backward() elif data == 'servo left': print('servo left') robot.servo_left() elif data == 'servo center': print('servo center') robot.servo_center() elif data == 'servo right': print('servo right') robot.servo_right() elif data == 'lights': print('lights') robot.lights() elif data == 'blinkers': print('blinkers') robot.blinkers() elif data == 'voltage': print('voltage') voltage = robot.voltage() voltage = send_json(voltage) myMQTT.publish('from_robot', voltage, 0) elif data == 'distance': print('distance') distance = robot.distance() distance = send_json(distance) myMQTT.publish('from_robot', distance, 0) else: pass
def move_div(): #if flask.request.method == 'POST': if flask.request.form['button'] == 'forward': print('forward!') robot.forward() elif flask.request.form['button'] == 'stop': print('stop!') robot.stop() elif flask.request.form['button'] == 'left': print('left') robot.left() elif flask.request.form['button'] == 'right': print('right') robot.right() elif flask.request.form['button'] == 'backward': print('backward') robot.backward() else: pass return flask.render_template('index.html')
def findLineCtrl(self, posInput, setCenter): #2 if not posInput: robot.robotCtrl.moveStart(speedMove, 'no', 'no') return if posInput > (setCenter + findLineError): #turnRight robot.right() self.CVCommand = 'Turning Right' print('Turning Right') elif posInput < (setCenter - findLineError): #turnLeft robot.left() self.CVCommand = 'Turning Left' print('Turning Left') else: #forward robot.forward() self.CVCommand = 'Forward' print('Forward')
def robot_code(): robot.forward(20) robot.turn_right(90) robot.forward(40) robot.turn_left(90) robot.forward(40) robot.turn_right(90) robot.backwards(40) robot.stop()
import robot robot.pause_for(2.00) # s for count in range(4): robot.forward(50.0) # cm robot.left(90.0) # degrees robot.send_command_list()
[1,1,1,1,1,1,1,1,1,1]] robot.create(world, direction, speed) # INSTRUCTIONS # robot.turn_on() Power on # robot.turn_off() Power off # robot.forward() Go forward # robot.left() Turn left # robot.right() Turn right # robot.has_finished() Returns True if game has finished # robot.sense() Sensor determines if there's a wall # # Start game and PRESS ENTER to active the robot # Let's go!! robot.turn_on() # YOUR CODE HERE robot.forward() # Power off the robot robot.turn_off()
def forward(): global currentSpeed, currentCommand robot.forward(currentSpeed) carLeds.execute("forward")
# robot to take to come to a stop. # # If you want the robot to move backwards (or it is moving # backwards and you want it to come to a stop), just put a # negative sign in front of every value v. # # Finally, if you want the robot to rotate, enter: # # robot.left(theta) # deg # # where theta is the number of degrees you want the robot # to rotate in the counter-clockwise direction. Use a # negative value if you want the robot to rotate clockwise. # # # Note that the Python interpreter ignores anything written # after a sharp character (#). In IDLE, text that the interpreter # will ignore (like this text) is red. # # You can start your list of commands on the line after # this line. We have written one command for you: robot.pause_for(1.00) # s robot.forward(10.00) # cm ############################################################# # If you want your program to work, don't change this part! # # # robot.send_command_list() # # # #############################################################
import robot ''' We import robot. This lets us call all of the functions in robot.py. We set robot_debug to True to test our code without sending it to the robot. Set robot_debug to False when you are ready to run your code on the robot. ''' robot.robot_debug = False robot.robot_speed = 200 robot.forward(5.0) robot.forward(1.0) robot.left(1.5) robot.right(1.0) robot.blink(1.0) robot.forward(4.5) # # robot.robot_speed = 125 # robot.forward(3) # robot.right_rot(.7) # robot.forward(2.5) # robot.left_rot(.7) # robot.forward(5) # robot.forward(4) # robot.left_rot(.7) # robot.forward(3) # # robot.forward(3) # robot.right_rot(.7)
def run_along_obstacle(robot, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar): log(INFO, 'Starting along obstacle mode') front_pan_tilt.reset() back_pan_tilt.reset () # ###################################### # # Find direction of the closest obstacle # # ###################################### # log(INFO, 'Looking for the best direction to start') closest_obstacle_distance, closest_obstacle_angle = find_closest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar) # ##################################### # # Get parallel to this closest obstacle # # ##################################### # if closest_obstacle_angle < 0: log(INFO, 'Closest obstacle is on the right') is_obstacle_on_the_right = True if -90 < closest_obstacle_angle < 0: robot.left_with_angle(90 + closest_obstacle_angle, True) else: robot.right_with_angle(-closest_obstacle_angle - 90, True) else: log(INFO, 'Closest obstacle is on the left') is_obstacle_on_the_right = False if 0 < closest_obstacle_angle < 90: robot.right_with_angle(90 - closest_obstacle_angle, True) else: robot.left_with_angle(closest_obstacle_angle - 90, True) log(INFO, 'Now parallel to the closest obstacle') # ############################## # # Now move along obstacle # # ############################## # if is_obstacle_on_the_right == True: back_pan_tilt.pan_go_left() else: back_pan_tilt.pan_go_right() reference_distance = back_lidar.get_distance() log(INFO, 'Reference distance: {:06.3f}'.format(reference_distance)) while (control.main_mode == MODE.ALONG_OBSTACLE) and (control.is_mode_aborted == False): robot.forward(TARGET_SPEED_STEP) front_distance = front_lidar.get_distance() side_distance = back_lidar.get_distance() delta_distance = side_distance - reference_distance if front_distance < AVOID_OBSTACLE_STOP_DISTANCE: log(INFO, 'Obstacle ahead: changing direction') robot.stop() if is_obstacle_on_the_right == True: robot.left_with_angle(45, True) else: robot.right_with_angle(45, True) log(INFO, 'Ready to restart along obstacle') elif side_distance > 2 * reference_distance: log(INFO, 'Lost obstacle: changing direction') robot.stop() if is_obstacle_on_the_right == True: robot.right_with_angle(45, True) else: robot.left_with_angle(45, True) robot.forward(TARGET_SPEED_STEP) # Wait a bit for a move to be done time.sleep(1) log(INFO, 'Ready to restart along obstacle') elif -ALONG_OBSTACLE_PRECISION < delta_distance < ALONG_OBSTACLE_PRECISION: log(DEBUG, 'Obstacle is at the expected distance: moving on...') robot.stop_turn() elif delta_distance > 0: log(DEBUG, 'Going away from obstacle: correcting...') if is_obstacle_on_the_right == True: robot.right_with_strength(LEFT_RIGHT_TURN_STRENGTH) else: robot.left_with_strength(LEFT_RIGHT_TURN_STRENGTH) else: log(DEBUG, 'Going away from obstacle: correcting...') if is_obstacle_on_the_right == True: robot.left_with_strength(LEFT_RIGHT_TURN_STRENGTH) else: robot.right_with_strength(LEFT_RIGHT_TURN_STRENGTH) robot.stop() front_pan_tilt.reset() back_pan_tilt.reset ()
import robot ''' We import robot. This lets us call all of the functions in robot.py. We set robot_debug to True to test our code without sending it to the robot. Set robot_debug to False when you are ready to run your code on the robot. ''' robot.robot_debug = False robot.robot_speed = 150 robot.forward(1.0) robot.forward(1.0) robot.left(1.0) robot.right(1.0) robot.blink(10) robot.forward(1.0) # # robot.robot_speed = 125 # robot.forward(3) # robot.right_rot(.7) # robot.forward(2.5) # robot.left_rot(.7) # robot.forward(5) # robot.forward(4) # robot.left_rot(.7) # robot.forward(3) # # robot.forward(3) # robot.right_rot(.7)
obv = int(obv) oba = oba - 100 obv = obv - 100 except: pass #oba=60 print("obstacle at: {}".format(oba)) print("visible at: {}".format(obv)) #thoorama irruku' if ((x <= 400) and (x >= 200)): #desti is straight but check for foreign objs if ((oba < 50) or (oba > 150)): #go straight robot.forward(0.5) elif (x > 290): robot.left(0.55) robot.forward(2.5) robot.right(0.55) elif (x <= 290): robot.right(0.55) robot.forward(2.5) robot.left(0.55) elif (x > 400): robot.right(0.1) elif (x < 200): robot.left(0.1) elif (h > 100): robot.stopp()
print('Connection Refused') time.sleep(5) else: print('Connected') state = True break print('Waiting for data...') data = s.recv(2048) if not data: s.close() state = False print(data.decode()) if data == 'forward': print('forward!') robot.forward() elif data == 'stop': print('stop!') robot.stop() elif data == 'left': print('left') robot.left() elif data == 'right': print('right') robot.right() elif data == 'backward': print('backward') robot.backward() elif data == 'servo left': print('servo left') robot.servo_left()
def console_thread(robot, left_motor, right_motor, imu_device, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar, speed_pid_controller): print_help() is_console_on = True selected_pid = 1 while is_console_on == True: print('> ', end = '') user_input = input() if len(user_input) == 1: command = user_input[0] if command == '1': print('') print('Speed PID selected') print('') selected_pid = 1 elif command == 'm': print('') left_motor.print_info ('LEFT DC MOTOR') right_motor.print_info('RIGHT DC MOTOR') print('') elif command == 'v': print('') front_pan_tilt.print_info('FRONT') print('') back_pan_tilt.print_info ('BACK' ) print('') elif command == 'c': print('') print('SPEED PID:') speed_pid_controller.print_info() print('') elif command == 'u': print('') imu_device.print_info() print('') elif command == 'r': print('') robot.print_info() print('') front_lidar.print_info('FRONT LIDAR') back_lidar.print_info ('BACK LIDAR') print('') print('Line following threshold: {}'.format(control.line_threshold)) print('') elif command == 'q': print('') print('***** GOING TO OPERATIONAL MODE *****') is_console_on = False elif command == 'x': print('***** EXITING GRACEFULLY *****') robot.stop() front_pan_tilt.stop() back_pan_tilt.stop () if status.is_rpi_gpio_used: RPi.GPIO.cleanup() os._exit(0) elif command == 'h': print_help() elif len(user_input) == 2: command = user_input[0:2] if command == 'sf': robot.forward_step() elif command == 'sb': robot.backward_step() elif command == 'sl': robot.left_step() elif command == 'sr': robot.right_step() elif len(user_input) > 2 and user_input[1] == '=': command = user_input[0] value = float(user_input[2:]) if command == 'l': if value == 0: log_set_level(NO_LOG ) elif value == 1: log_set_level(ERROR ) elif value == 2: log_set_level(WARNING) elif value == 3: log_set_level(INFO ) elif value == 4: log_set_level(DEBUG ) else: print('Invalid log level') elif command == 'p': if selected_pid == 1: speed_pid_controller.set_kp(value) elif command == 'i': if selected_pid == 1: speed_pid_controller.set_ki(value) elif command == 'd': if selected_pid == 1: speed_pid_controller.set_kd(value) elif command == 'w': if selected_pid == 1: speed_pid_controller.set_anti_wind_up(value) elif command == 's': if value > 0: robot.forward(value) elif value < 0: robot.backward(-value) else: robot.stop() elif command == 'a': if value < 0: robot.right_with_angle(-value, True) else: robot.left_with_angle(value, True) elif command == 'g': if value < 0: robot.right_with_strength(-value) else: robot.left_with_strength(value) elif command == 't': control.line_threshold = value elif len(user_input) > 2 and user_input[2] == '=': command = user_input[0:2] value = float(user_input[3:]) if command == 'fp': front_pan_tilt.set_pan(value) elif command == 'ft': front_pan_tilt.set_tilt(value) elif command == 'bp': back_pan_tilt.set_pan(value) elif command == 'bt': back_pan_tilt.set_tilt(value)
import robot print "forward" robot.forward(5) print "right" robot.right(2) print "left" robot.left(2) print "reverse" robot.reverse(2) print "stop"
def run_corridor_following(robot, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar): log(INFO, 'Starting corridor following mode') front_pan_tilt.reset() back_pan_tilt.reset () # ###################################### # # Find direction of the closest obstacle # # ###################################### # log(INFO, 'Looking for the best direction to start') closest_obstacle_distance, closest_obstacle_angle = find_closest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar) # ############################ # # Aim at this closest obstacle # # ############################ # if closest_obstacle_angle < 0: robot.right_with_angle(-closest_obstacle_angle, True) else: robot.left_with_angle(closest_obstacle_angle, True) log(INFO, 'Now aiming at the closest obstacle') # ############################ # # Get centered in the corridor # # ############################ # is_centered = False while is_centered == False: front_wall_distance = front_lidar.get_distance() back_wall_distance = back_lidar.get_distance () delta_distance = front_wall_distance - back_wall_distance if -CORRIDOR_FOLLOWING_PRECISION < delta_distance < CORRIDOR_FOLLOWING_PRECISION: is_centered = True elif delta_distance > 0: robot.forward_step () else: robot.backward_step() log(INFO, 'Now centered in corridor') # ############################## # # Get into the forward direction # # ############################## # robot.right_with_angle(90, True) log(INFO, 'Ready to move in corridor') # ######################################## # # Now move forward in corridor and # # keep the best possible centered position # # ######################################## # front_pan_tilt.pan_go_left() back_pan_tilt.pan_go_left () while (control.main_mode == MODE.FOLLOW_CORRIDOR) and (control.is_mode_aborted == False): robot.forward(TARGET_SPEED_STEP) left_wall_distance = front_lidar.get_distance() right_wall_distance = back_lidar.get_distance () delta_distance = left_wall_distance - right_wall_distance if -CORRIDOR_FOLLOWING_PRECISION < delta_distance < CORRIDOR_FOLLOWING_PRECISION: log(DEBUG, 'Correctly centered in corridor: moving on...') robot.stop_turn() # Allow u-turn only when robot is centered if control.is_u_turn_requested == True: log(DEBUG, 'Performing u-turn as per requested') robot.stop() robot.right_with_angle(180, True) control.is_u_turn_requested = False elif delta_distance > 0: log(DEBUG, 'Going away from center: correcting to the left...') robot.left_with_strength(LEFT_RIGHT_TURN_STRENGTH / 2) else: log(DEBUG, 'Going away from center: correcting to the right...') robot.right_with_strength(LEFT_RIGHT_TURN_STRENGTH / 2) robot.stop() front_pan_tilt.reset() back_pan_tilt.reset ()
import robot import time espera = 2 robot.init() for i in range(0, 4): robot.forward(robot.throttle_max) time.sleep(espera) robot.right(robot.throttle_max) time.sleep(espera / 2) robot.stop()
def run_obstacles_avoidance(robot, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar): log(INFO, 'Starting obstacles avoidance mode') front_pan_tilt.reset() back_pan_tilt.reset () # ####################################### # # Find direction of the farthest obstacle # # ####################################### # log(INFO, 'Looking for the best direction to start') farthest_obstacle_distance, farthest_obstacle_angle = find_farthest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar) # ############################# # # Aim at this farthest obstacle # # ############################# # if farthest_obstacle_angle < 0: robot.right_with_angle(-farthest_obstacle_angle, True) else: robot.left_with_angle(farthest_obstacle_angle, True) # ############################## # # Now start avoiding obstacles # # ############################## # search_direction = False while (control.main_mode == MODE.AVOID_OBSTACLES) and (control.is_mode_aborted == False): if search_direction == True: robot.stop() robot.forward_step() # ####################################### # # Find direction of the farthest obstacle # # ####################################### # log(INFO, 'Looking for the best direction to continue') back_pan_tilt.reset() farthest_obstacle_distance, farthest_obstacle_angle = find_farthest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, front_pan_tilt, front_lidar, None, None) if farthest_obstacle_distance < AVOID_OBSTACLE_STOP_DISTANCE: front_pan_tilt.reset() farthest_obstacle_distance, farthest_obstacle_angle = find_farthest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, None, None, back_pan_tilt, back_lidar) # ############################# # # Aim at this farthest obstacle # # ############################# # if farthest_obstacle_angle < 0: robot.right_with_angle(-farthest_obstacle_angle, True) else: robot.left_with_angle(farthest_obstacle_angle, True) log(INFO, 'Aiming at the farthest obstacle') search_direction = False else: # ############################# # # Get distance of next obstacle # # ############################# # next_obstacle_distance = front_lidar.get_distance() # ################################ # # Now move forward that direction # # ################################ # if next_obstacle_distance < AVOID_OBSTACLE_STOP_DISTANCE: log(INFO, 'Obstacle ahead: changing direction') search_direction = True else: log(DEBUG, 'Path is clear: moving on...') robot.forward(TARGET_SPEED_STEP / 2) robot.stop() front_pan_tilt.reset() back_pan_tilt.reset ()