def move(direction): robot = Robot() if direction == "forward": robot.forward(0.4) time.sleep(0.5) robot.stop() return render_template('index.html') elif direction == "left": robot.left(0.3) time.sleep(0.5) robot.stop() return render_template('index.html') elif direction == "right": robot.right(0.3) time.sleep(0.5) robot.stop() return render_template('index.html') elif direction == "backward": robot.backward(0.4) time.sleep(0.5) robot.stop() return render_template('index.html') else: return render_template('index.html') return render_template('index.html')
class Wheels(object): def __init__(self): self.robot = Robot() self.stop() def cmd_vel_process(self, linear_speed, angular_speed): if angular_speed != 0: # We turn if angular_speed > 0: self.go_left(abs(angular_speed)) else: self.go_right(abs(angular_speed)) else: if linear_speed > 0: self.go_forward(abs(linear_speed)) elif linear_speed < 0: self.go_backward(abs(linear_speed)) else: # We have to stop self.stop() def stop(self): self.robot.stop() def go_forward(self, speed=0.4): self.robot.forward(speed) def go_backward(self, speed=0.4): self.robot.backward(speed) def go_left(self, speed=0.3): self.robot.left(speed) def go_right(self, speed=0.3): self.robot.right(speed)
bypass_number = 0 found_number = 0 barrier_center = -1 continue_found = 0 player = MusicPlayer() try: while (True): print("bypass_number: ", bypass_number) distance = get_distance() print("Distance detected by supersonic:", distance) if distance != None and distance < 20: robot.backward(0.35) time.sleep(0.5) robot.stop() continue # Capture frame-by-frame frame = cap.read( ) # ret = 1 if the video is captured; frame is the image # Our operations on the frame come here img = cv2.flip(frame, 1) # flip left-right img = cv2.flip(img, 0) # flip up-down boxes, confs, clss = trt_yolo.detect(img, 0.8) print("boxes:", boxes) print("confs:", confs)
class FormantJetBotAdapter: def __init__(self): print("INFO: Starting Formant JetBot Adapter") # Set global params self.max_speed = DEFAULT_MAX_SPEED self.min_speed = DEFAULT_MIN_SPEED self.speed_deadzone = DEFAULT_SPEED_DEADZONE self.speed_increment = DEFAULT_SPEED_INCREMENT self.angular_reduction = DEFAULT_ANGULAR_REDUCTION self.latitude = DEFAULT_LATITUDE self.longitude = DEFAULT_LONGITUDE self.gst_string = DEFAULT_GST_STRING self.start_speed = DEFAULT_START_SPEED self.speed = self.start_speed self.is_shutdown = False # Store frame rate information to publish self.camera_width = 0 self.camera_height = 0 self.camera_frame_timestamps = collections.deque([], maxlen=100) self.camera_frame_sizes = collections.deque([], maxlen=100) # Create clients self.robot = Robot() self.ina219 = INA219(addr=0x41) self.fclient = FormantClient(ignore_throttled=True, ignore_unavailable=True) self.update_from_app_config() self.publish_online_event() self.fclient.register_command_request_callback( self.handle_command_request) self.fclient.register_teleop_callback(self.handle_teleop, ["Joystick", "Buttons"]) print("INFO: Starting speed thread") threading.Thread(target=self.publish_speed, daemon=True).start() print("INFO: Starting motor states thread") threading.Thread(target=self.publish_motor_states, daemon=True).start() print("INFO: Starting location thread") threading.Thread(target=self.publish_location, daemon=True).start() print("INFO: Starting battery state thread") threading.Thread(target=self.publish_battery_state, daemon=True).start() print("INFO: Starting camera stats thread") threading.Thread(target=self.publish_camera_stats, daemon=True).start() # Start the camera feed self.publish_camera_feed() def __del__(self): self.is_shutdown = True def publish_speed(self): while not self.is_shutdown: # self.fclient.post_numeric("speed", self.speed) self.fclient.post_numericset( "Speed", {"speed": (self.speed + self.speed_deadzone, "m/s")}, ) time.sleep(1.0) def publish_motor_states(self): while not self.is_shutdown: self.fclient.post_numeric("Motor Speed", self.robot.left_motor.value, {"value": "left"}) self.fclient.post_numeric("Motor Speed", self.robot.right_motor.value, {"value": "right"}) time.sleep(0.5) def publish_location(self): while not self.is_shutdown: self.fclient.post_geolocation("Location", self.latitude, self.longitude) time.sleep(10.0) def publish_battery_state(self): while not self.is_shutdown: bus_voltage = self.ina219.getBusVoltage_V() shunt_voltage = self.ina219.getShuntVoltage_mV() / 1000 current = self.ina219.getCurrent_mA() / 1000 charging = False if shunt_voltage > 0.01 and current > 0.01: charging = True # approximate battery charge percentage calibration now = bus_voltage - MIN_DISCHARGING_VOLTAGE full = MAX_DISCHARGING_VOLTAGE - MIN_DISCHARGING_VOLTAGE charge_percentage = now / full * 100 if charging: now = bus_voltage - MIN_CHARGING_VOLTAGE full = MAX_CHARGING_VOLTAGE - MIN_CHARGING_VOLTAGE charge_percentage = now / full * 100 if charge_percentage >= 100: charge_percentage = 100 self.fclient.post_battery("Battery State", charge_percentage, voltage=bus_voltage, current=current) self.fclient.post_bitset("Battery Charging", { "charging": charging, "discharging": not charging }) time.sleep(1.0) def publish_camera_stats(self): while not self.is_shutdown: try: timestamps = list(self.camera_frame_timestamps) sizes = list(self.camera_frame_sizes) except: print("ERROR: deque mutated while iterating") pass length = len(timestamps) if length > 2: size_mean = mean(sizes) size_stdev = stdev(sizes) jitter = self.calculate_jitter(timestamps) oldest = timestamps[0] newest = timestamps[-1] diff = newest - oldest if diff > 0: hz = length / diff self.fclient.post_numericset( "Camera Statistics", { "Rate": (hz, "Hz"), "Mean Size": (size_mean, "bytes"), "Std Dev": (size_stdev, "bytes"), "Mean Jitter": (jitter, "ms"), "Width": (self.camera_width, "pixels"), "Height": (self.camera_height, "pixels"), }, ) time.sleep(5.0) def publish_camera_feed(self): cap = cv2.VideoCapture(self.gst_string, cv2.CAP_GSTREAMER) if cap is None: print("ERROR: Could not read from video capture source.") sys.exit() while not self.is_shutdown: _, image = cap.read() try: encoded = cv2.imencode(".jpg", image)[1].tostring() self.fclient.post_image("Camera", encoded) # Track stats for publishing self.camera_frame_timestamps.append(time.time()) self.camera_frame_sizes.append(len(encoded) * 3 / 4) self.camera_width = image.shape[1] self.camera_height = image.shape[0] except: print("ERROR: encoding failed") def publish_online_event(self): try: commit_hash_file = ( "/home/jetbot/formant-jetbot-adapter/.git/refs/heads/main") with open(commit_hash_file) as f: commit_hash = f.read() except Exception: print( "ERROR: formant-jetbot-adapter repo must be installed at " "/home/jetbot/formant-jetbot-adapter to receive online event") self.fclient.create_event( "Formant JetBot adapter online", notify=False, tags={"hash": commit_hash.strip()}, ) def update_from_app_config(self): print("INFO: Updating configuration ...") self.max_speed = float( self.fclient.get_app_config("max_speed", DEFAULT_MAX_SPEED)) self.min_speed = float( self.fclient.get_app_config("min_speed", DEFAULT_MIN_SPEED)) self.speed_deadzone = float( self.fclient.get_app_config("speed_deadzone", DEFAULT_SPEED_DEADZONE)) self.speed_increment = float( self.fclient.get_app_config("speed_increment", DEFAULT_SPEED_INCREMENT)) self.angular_reduction = float( self.fclient.get_app_config("angular_reduction", DEFAULT_ANGULAR_REDUCTION)) self.latitude = float( self.fclient.get_app_config("latitude", DEFAULT_LATITUDE)) self.longitude = float( self.fclient.get_app_config("longitude", DEFAULT_LONGITUDE)) self.gst_string = self.fclient.get_app_config("gst_string", DEFAULT_GST_STRING) self.start_speed = float( self.fclient.get_app_config("start_speed", DEFAULT_START_SPEED)) def handle_command_request(self, request): if request.command == "jetbot.nudge_forward": self._handle_nudge_forward() self.fclient.send_command_response(request.id, success=True) elif request.command == "jetbot.nudge_backward": self._handle_nudge_backward() self.fclient.send_command_response(request.id, success=True) elif request.command == "jetbot.update_config": self.update_from_app_config() self.fclient.send_command_response(request.id, success=True) else: self.fclient.send_command_response(request.id, success=False) def handle_teleop(self, control_datapoint): if control_datapoint.stream == "Joystick": self.handle_joystick(control_datapoint) elif control_datapoint.stream == "Buttons": self.handle_buttons(control_datapoint) def handle_joystick(self, joystick): left_motor_value = 0.0 right_motor_value = 0.0 # Add contributions from the joysticks # TODO: turn this into a circle, not a square left_motor_value += (self.speed * joystick.twist.angular.z * self.angular_reduction) right_motor_value += (-self.speed * joystick.twist.angular.z * self.angular_reduction) left_motor_value += self.speed * joystick.twist.linear.x right_motor_value += self.speed * joystick.twist.linear.x # Improve the deadzone if left_motor_value > 0: left_motor_value += self.speed_deadzone elif left_motor_value < 0: left_motor_value -= self.speed_deadzone if right_motor_value > 0: right_motor_value += self.speed_deadzone elif right_motor_value < 0: right_motor_value -= self.speed_deadzone # Set the motor values self.robot.left_motor.value = left_motor_value self.robot.right_motor.value = right_motor_value def handle_buttons(self, _): if _.bitset.bits[0].key == "nudge forward": self._handle_nudge_forward() elif _.bitset.bits[0].key == "nudge backward": self._handle_nudge_backward() elif _.bitset.bits[0].key == "speed +": self._handle_increase_speed() elif _.bitset.bits[0].key == "speed -": self._handle_decrease_speed() def _handle_nudge_forward(self): self.fclient.post_text("Commands", "nudge forward") self.robot.forward(self.speed + self.speed_deadzone) time.sleep(0.5) self.robot.stop() def _handle_nudge_backward(self): self.fclient.post_text("Commands", "nudge backward") self.robot.backward(self.speed + self.speed_deadzone) time.sleep(0.5) self.robot.stop() def _handle_increase_speed(self): self.fclient.post_text("Commands", "increase speed") if self.speed + self.speed_increment <= self.max_speed: self.speed += self.speed_increment else: self.speed = self.max_speed def _handle_decrease_speed(self): self.fclient.post_text("Commands", "decrease speed") if self.speed - self.speed_increment >= self.min_speed: self.speed -= self.speed_increment else: self.speed = self.min_speed def calculate_jitter(self, timestamps): length = len(self.camera_frame_timestamps) oldest = self.camera_frame_timestamps[0] newest = self.camera_frame_timestamps[-1] step_value = (newest - oldest) / length # Make a list of the difference between the expected and actual step sizes jitters = [] for n in range(length - 1): if n > 0: jitter = abs((timestamps[n] - timestamps[n - 1]) - step_value) jitters.append(jitter) return mean(jitters)
if '1' in tasks: print('[Task 1] in 1s') time.sleep(1) robot.left(speed=0.3) time.sleep(1) robot.stop() time.sleep(1) robot.right(speed=0.3) time.sleep(1) robot.stop() time.sleep(1) robot.forward(speed=0.3) time.sleep(1) robot.stop() time.sleep(1) robot.backward(speed=0.3) time.sleep(1) robot.stop() print('[Task 1] done') if '2' in tasks: print('[Task 2] in 1s') time.sleep(1) robot.left_motor.value = 0.5 robot.right_motor.value = 0 time.sleep(1) robot.stop() time.sleep(1) robot.left_motor.value = 0 robot.right_motor.value = 0.5 time.sleep(1)
axis = axis_map[number] if axis: #print("{}".format(axis)) if axis == "x": fvalue = value / 32767 print("%s: %.3f" % (axis, fvalue)) if (fvalue < 0): robot.left(speed=math.fabs(fvalue / 3)) elif (fvalue > 0): robot.right(speed=math.fabs(fvalue / 3)) else: robot.stop() if axis == "y": fvalue = value / 32767 print("%s: %.3f" % (axis, fvalue)) if (fvalue < 0): robot.forward(speed=math.fabs(fvalue / 3)) elif (fvalue > 0): robot.backward(speed=math.fabs(fvalue / 3)) else: robot.stop() if axis == "rx": fvalue = value / 32767 print("%s: %.3f" % (axis, fvalue)) if axis == "ry": fvalue = value / 32767 print("%s: %.3f" % (axis, fvalue))
import time move = Robot() try: while True: mode = input( "Enter mode: f forward, b backward, l left, r right, s stop, c controller " ) if mode == 'f': move.forward(0.5) print("Jetbot is moving foward!") elif mode == 'b': move.backward(0.5) print("Jetbot is moving backward!") elif mode == 'l': move.left(0.5) print("Jetbot is turning left!") elif mode == 'r': move.right(0.5) print("Jetbot is turning right!") elif mode == 's': move.stop() elif mode == 'c': joy = xbox.Joystick()
# it will make exit the while loop robot.stop print("quit") run = False keys = pygame.key.get_pressed() if keys[pygame.K_LEFT]: print("left") robot.left(maxSpeed) if keys[pygame.K_RIGHT]: print("right") robot.right(maxSpeed) if keys[pygame.K_UP]: print("forward") robot.forward(maxSpeed) if keys[pygame.K_DOWN]: print("backward") robot.backward(maxSpeed) if keys[pygame.K_SPACE]: print("stop") robot.set_motors(0, 0) #pygame.display.update() pygame.quit()
from jetbot import Robot import smbus import time robot = Robot() bus = smbus.SMBus(1) address = 8 # I2C address for Ultrasonic bus def distance(): """Returns distance measured by ultrasonics in cm""" return bus.read_byte_data(address, 0) while True: if (distance() < 20): robot.backward(0.4) time.sleep(0.6) robot.left(0.4) time.sleep(0.5) else: robot.forward(0.4)