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)
