class Controller(object): def __init__(self, goal_vel=0.0): self.goal_ticks = self.convert_to_ticks(goal_vel) self.plant = PLANT() self.wheel_controllers = { Sensor.KEY_WENC_RIGHT: Wheel_PID_Controller(True, 0.01), Sensor.KEY_WENC_LEFT: Wheel_PID_Controller(False, 0.01) } self.wheel_inputs = [0.0, 0.0] self.jetbot = Robot() def input_actions(self, inputs): for i in range(len(inputs)): self.jetbot.set_motors(inputs[i][0], inputs[i][1]) time.sleep(inputs[i][2]) self.jetbot.stop() pass def handle_callback(self, callback): ticks_per_second = self.calculate_velocity(callback.data) if callback.key == Sensor.KEY_WENC_RIGHT: output = self.right_wheel_controller.calculate_output() # elif callback.key == Sensor.KEY_WENC_LEFT: def convert_to_ticks(self, goal_vel): return (goal_vel / (2 * math.pi) * 8)
class JetbotController: def __init__(self): self.robot = Robot() #self.robot = JetbotDriver().robot self.left_v = 0.0 self.right_v = 0.0 self.loop = True self.controll_thread = threading.Thread(target=self._controll_loop) rospy.init_node('jetbot_cmd_vel_controller') rospy.Subscriber("/cmd_vel", Twist, self._callback) def start(self): self.controll_thread = threading.Thread(target=self._controll_loop) self.controll_thread.start() rospy.spin() def _callback(self, msg): speed = msg.linear.x radius = msg.angular.z self.right_v = (speed + radius) * 0.5 self.left_v = (speed - radius) * 0.5 def _controll_loop(self): while self.loop: self.robot.set_motors(self.left_v, self.right_v) time.sleep(0.1) def stop(self): self.loop = False self.robot.set_motors(0.0, 0.0)
def listener(): rospy.init_node('twist_to_motor') rospy.Subscriber('dtw_robot/diff_drive_controller/cmd_vel', Twist, twist_cb) rate = rospy.Rate(10) robot = Robot() global init_flag init_flag = 1 while not rospy.is_shutdown(): robot.set_motors(v_l, v_r) print(v_l) rate.sleep()
class PLANT(object): def __init__(self): self.jetbot = Robot( ) # Robot() is jetbot API for inputting motor controls, no need to manage PWM inputs ourselves. # Moves jetbot by controlling individual wheels. # inputs: 2D tuple with list of motor inputs and time for each motor input # inputs[i][0]: Left motor input for ith command # inputs[i][1]: Right motor input for ith command # inputs[i][2]: DeltaT for ith command def move(self, inputs): for i in range(len(inputs)): self.jetbot.set_motors(inputs[i][0], inputs[i][1]) time.sleep(inputs[i][2]) self.jetbot.stop() pass
def main(): parser = argparse.ArgumentParser(description='Khan') parser.add_argument("-p", type=str, help='Port of the serial', default='/dev/ttyACM0') parser.add_argument("-b", type=int, help="Baudrate of the serial", default=9600) parser.add_argument("-speed", type=float, help="speed in cm/s", default=10) args = parser.parse_args() # khan = Khan(args.p, args.b) robot = Robot() imageHandler = ImageHandler(width=640, height=480) laneDetector = LaneDetector(imageHandler.camera) print("ready to run, wait for 1 second...") sleep(1) robot.set_motors(0.2,0.3) t1 = threading.Thread(target=markROI(), args=(laneDetector,)) t1.start() sleep(10) robot.stop()
# In[105]: count = 0 seta = 0 while True: img = camera.value try: img_result, img_op, seta = search_road(img, seta) print(seta, end='\t') result1.value = bgr8_to_jpeg(img_result) result2.value = bgr8_to_jpeg(img_op) pw = 1.2 w = (seta / 90) left_power = pw * (0.1 + w * 0.1) right_power = pw * (0.1 - w * 0.1) robot.set_motors(left_power, right_power) # time.sleep(0.5) # robot.stop() except: print('not Found', end='\r') robot.stop() input_image.value = bgr8_to_jpeg(img) if count == 120: break else: count = count + 1 print(count, end='\r') time.sleep(0.1) robot.stop() # In[37]:
class SpeedController(Node): def __init__(self, port=8765): super().__init__('ws_server') self.cmd_sub_ = self.create_subscription(Twist, 'cmd_vel', self.cmd_callback, 10) self.odom_sub_ = self.create_subscription(Odometry, 'rs_t265/odom', self.odom_callback, 10) self.vel_cmd_ = 0.0 self.yaw_rate_cmd_ = 0.0 self.yaw_rate_error_int_ = 0.0 self.vel_error_int_ = 0.0 if not IS_LOCAL: self.robot = Robot() def inverse_diff_kinematics(self, vel, yaw_rate): vel = vel #/30.0 if (self.vel_cmd_ == 0.0): yaw_rate *= 4.5 #/-15.0 vel = 0.0 self.vel_error_int_ = 0.0 if (self.yaw_rate_cmd_ == 0.0): self.yaw_rate_error_int_ = 0.0 yaw_rate = 0.0 if vel == 0.0: yaw_rate = yaw_rate wheel_speed_l = (2 * vel - wheelbase * yaw_rate) / (2 * wheel_radius) wheel_speed_r = (2 * vel + wheelbase * yaw_rate) / (2 * wheel_radius) return wheel_speed_l / 10.0, wheel_speed_r / 10.0 def odom_callback(self, msg): vel = msg.twist.twist.linear.x yaw_rate = msg.twist.twist.angular.z # Long. speed control error = (self.vel_cmd_ - vel) if (abs(self.vel_error_int_) < 100.0): self.vel_error_int_ += error vel_out = KF_FWD * self.vel_cmd_ + KP_FWD * ( error + KI_FWD * self.vel_error_int_) # Yaw rate control error = self.yaw_rate_cmd_ - yaw_rate if (abs(self.yaw_rate_error_int_) < 100.0): self.yaw_rate_error_int_ += error yaw_rate_out = KF_TURN * self.yaw_rate_cmd_ + KP_TURN * ( error + KI_TURN * self.yaw_rate_error_int_) wheel_speed_l, wheel_speed_r = self.inverse_diff_kinematics( vel_out, -yaw_rate_out) # print('vel: {}, vel_cmd: {} , vel_out: {}'.format(vel,self.vel_cmd_,vel_out)) # print('w: {}, w_cmd: {} , w_out: {}'.format(yaw_rate,self.yaw_rate_cmd_,yaw_rate_out)) if not IS_LOCAL: self.robot.set_motors(wheel_speed_r, -wheel_speed_l) def cmd_callback(self, msg): vel_ref = msg.linear.x yaw_rate_ref = msg.angular.z if (vel_ref < 0.0): yaw_rate_ref = -yaw_rate_ref self.yaw_rate_cmd_ = yaw_rate_ref self.vel_cmd_ = vel_ref
print('Race start after 3seconds') time.sleep(3) while (True): try: # leftline, rightline image = camera.value print('Load image success') leftarr, rightarr = mainfunc(image) print('Get left, right line', leftarr, rightarr) presentx, presenty = CenterCalculation(leftarr, rightarr) print('Center x, Center y : ', presentx, presenty) diff = presentx - staticx if abs(diff) < 5: print('Preseont dicrect is correct just straight') robot.forward(speed) robot.set_motors(speed, speed) # Right motor up if diff < 0: print('Go left!!!! Right motor up') robot.set_motors(speed, speed + 0.05) else: print('Go right!!!! left motor up') robot.set_motors(speed + 0.05, speed) except KeyboardInterrupt: print('Ctrl + C press Exit Paragon Program') camera.stop() print( '----------------------------------------------------------------------------------------------' ) print('Race start after 3seconds') sys.exit()
input_tensor = tf.convert_to_tensor(frame) input_tensor = input_tensor[tf.newaxis, ...] detections = detect_fn(input_tensor) scores = detections['detection_scores'][0] boxes = detections['detection_boxes'][0] matching_detections = [box for score, box in zip(scores, boxes) if score > 0.5] box = closest_detection(matching_detections) print("box : ", box) if box is None: robot.forward(speed) print("speed : ", speed) else: center = detection_center(box) center = np.float16(center)[0] print("center : ", center) robot.set_motors(speed + 0.8 * center, speed - 0.8 * center) print("left : ", speed + 0.8 * center, "right : ", speed - 0.8 * center) except Exception as e: print(e.args[0]) cap.release() robot.stop() sock.close()
# period_mean = None # period_count = 0 try: while True: im = cv2.resize(cam.value, input_shape[1:3], interpolation=cv2.INTER_CUBIC) im = np.expand_dims(im, axis=0) im = im / 255.0 t1 = time.time() preds = model.predict(im) t2 = time.time() # if period_mean is None: # period_mean = t2-t1 # period_count = 1 # else: # period_count += 1 # period_mean = period_mean * ((period_count-1) / period_count) + (t2-t1) / period_count print(1. / (t2 - t1), ",") if np.argmax(preds, axis=1) == 0: robot.set_motors(0.35, 0.35) else: robot.stop() except KeyboardInterrupt: cam.stop()
mode="bilinear", align_corners=False).cuda() disp_resized_np = disp_resized.squeeze().cpu().numpy() # Algorithm for evaluating depth perceptions and pushing onto motor values A = [] B = [] C = [] edge1 = round(camwidth / 3) edge2 = round(2 * camwidth / 3) for w in range(camheight): for j in range(edge1): A.append(disp_resized_np[w][j]) for j in range(edge1, edge2): B.append(disp_resized_np[w][j]) for j in range(edge2, camwidth): C.append(disp_resized_np[w][j]) B_scale = 1 - geometric_average(B) / np.max(disp_resized_np) right_motor = 2 * (0.5 - geometric_average(A) * B_scale) left_motor = 2 * (0.5 - geometric_average(C) * B_scale) robot.set_motors(left_motor, right_motor) except KeyboardInterrupt: # Set up a keyboard interrupt so that if we abort from keyboard it will dismantle properly and not SIGKILL. cam.unobserve_all() robot.stop() print('Loop successfully ended') break # We end up here if we successfully find our object, therefore we cant dismantle all camera observables and for safety also stop robot. cam.unobserve_all() robot.stop()
if distance_x is not None: distance_x_pre = distance_x output = pid.update(distance_x) turn = output / 100.0 value_left = left_default - turn / 2.0 value_right = right_default + turn / 2.0 value_left_pre = value_left value_right_pre = value_right ratio = -0.005 * abs(distance_x) + 1 value_left *= ratio value_right *= ratio frame = showOperationInfo(frame, value_left, value_right, ratio, distance_x, output, turn) robot.set_motors(value_left, value_right) else: value_left = 0 value_right = 0 if direction: # 向右轉馬達設定 value_left = value_left_pre * 0.8 else: # 向左轉馬達設定 value_right = value_right_pre * 0.8 robot.set_motors(value_left, value_right) frame = showOperationInfo(frame, value_left, value_right)
class VideoCamera(object): def __init__(self): # jetbot setting self.robot = Robot() self.avoidance_status = False self.cruise_status = False self.move_arrow = 'stop' self.n = 0.0 self.direction = "" self.pw = 1 self.pw_c = 1.5 self.left_power = (0.15) self.right_power = (0.145) # deep learning model setting self.set_detect_model() self.set_seg_model() self.roi = [(0, 120),(80, 60),(160, 120),] # camera setting self.cap = cv2.VideoCapture(1) self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) # width self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240) # height self.cap.set(cv2.CAP_PROP_FPS, 10) (self.grabbed, self.frame) = self.cap.read() # thread threading.Thread(target=self.update, args=()).start() def __del__(self): self.cap.release() self.robot.stop() def set_seg_model(self): self.segNet = jetson.inference.segNet("fcn-resnet18-sun") self.segNet.SetOverlayAlpha(150) self.buffers = segmentationBuffers(self.segNet) def set_detect_model(self): self.detectNet = jetson.inference.detectNet("ssd-mobilenet-v2") def get_seg(self): img = jetson.utils.cudaFromNumpy(self.frame) self.buffers.Alloc(img.shape, img.format) self.segNet.Process(img, ignore_class="toilet") self.segNet.Overlay(self.buffers.overlay, filter_mode="linear") self.segNet.Mask(self.buffers.mask, filter_mode="linear") self.img_rander = jetson.utils.cudaToNumpy(self.buffers.overlay) self.mask_rander = jetson.utils.cudaToNumpy(self.buffers.mask) self.floor_rander = self.mask_rander[:,:,1].copy() self.floor_rander[self.floor_rander[:,:]!=128] = 0 self.floor_rander[self.floor_rander[:,:]==128] = 1 self.floor_rander = self.floor_rander.reshape((120,320)) self.floor_rander[:,:160] = mask_roi(self.floor_rander[:,:160], self.roi) self.floor_rander[:,160:] = mask_roi(self.floor_rander[:,160:], self.roi) self.get_score() def get_detect(self): img = jetson.utils.cudaFromNumpy(self.frame) detections = self.detectNet.Detect(img, overlay="box,labels,conf") self.img_detect_rander = jetson.utils.cudaToNumpy(img) def get_score(self): self.n = np.sum(self.floor_rander) self.n_left = np.sum(self.floor_rander[:,:80]) self.n_right = np.sum(self.floor_rander[:,240:]) def update_jetbot(self): if self.avoidance_status and self.cruise_status: if self.n > 6000: self.direction = "Straight" self.robot.set_motors(self.pw*self.left_power, self.pw*self.right_power) elif self.n < 2000: self.direction = "back" self.robot.set_motors(-self.pw*self.left_power, -self.pw*self.right_power) elif self.n_right >= self.n_left + 100: self.direction = "right" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) elif self.n_left > self.n_right + 100: self.direction = "left" self.robot.set_motors(-0.75*self.left_power, 0.75*self.right_power) else: self.direction = "Unknown" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) elif self.cruise_status: if self.move_arrow == "stop": self.direction = "stop" self.robot.stop() elif self.move_arrow == "up": self.direction = "Straight" self.robot.set_motors(self.pw_c*self.left_power, self.pw_c*self.right_power) elif self.move_arrow == "down": self.direction = "back" self.robot.set_motors(-self.pw*self.left_power, -self.pw*self.right_power) elif self.move_arrow == "left": self.direction = "left" self.robot.set_motors(-0.75*self.left_power, 0.75*self.right_power) elif self.move_arrow == "right": self.direction = "right" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) else: self.direction = "Unknown" self.robot.stop() elif self.avoidance_status: if self.move_arrow == "stop": self.direction = "stop" self.robot.stop() elif self.move_arrow == "up": if self.n > 6000: self.direction = "Straight" self.robot.set_motors(self.pw_c*self.left_power, self.pw_c*self.right_power) else: self.direction = "stop with avoidance" self.robot.stop() self.move_arrow = "stop" elif self.move_arrow == "down": self.direction = "back" self.robot.set_motors(-self.pw*self.left_power, -self.pw*self.right_power) elif self.move_arrow == "left": self.direction = "left" self.robot.set_motors(-0.75*self.left_power, 0.75*self.right_power) elif self.move_arrow == "right": self.direction = "right" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) else: self.direction = "Unknown" self.robot.stop() else: if self.move_arrow == "stop": self.direction = "stop" self.robot.stop() elif self.move_arrow == "up": self.direction = "Straight" self.robot.set_motors(self.pw*self.left_power, self.pw*self.right_power) elif self.move_arrow == "down": self.direction = "back" self.robot.set_motors(-self.pw*self.left_power, -self.pw*self.right_power) elif self.move_arrow == "left": self.direction = "left" self.robot.set_motors(-0.75*self.left_power, 0.75*self.right_power) elif self.move_arrow == "right": self.direction = "right" self.robot.set_motors(0.75*self.left_power, -0.75*self.right_power) else: self.direction = "Unknown" self.robot.stop() self.move_arrow = "stop" def get_frame(self): image = self.img_rander detect_img = self.img_detect_rander text = f'floor: {self.n:0.2f}, left_n: {self.n_left}, right_n: {self.n_right}, direction: {self.direction}' image = cv2.putText(image, text, (5,15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA) ret, jpeg = cv2.imencode('.jpg', image) # roi visualization tmp = cv2.resize(self.floor_rander, dsize=(0, 0), fx=2, fy=2, interpolation=cv2.INTER_LINEAR) tmp = np.stack((255*tmp,)*3,axis = 2) result = np.concatenate((detect_img, image, tmp, ),axis=0) self.result = result ret, jpeg = cv2.imencode('.jpg', result) return jpeg.tobytes() def update(self): while True: start = time.time() (self.grabbed, self.frame) = self.cap.read() # segmentation self.get_seg() # detection self.get_detect() # update jetbot self.update_jetbot() end = time.time() - start if end < 0.1: time.sleep(0.1-end) print("time :", end, ",\ttotal: ", time.time() - start) # check time
# # The ``BasicJetbot`` class also has the methods ``right``, ``forward``, and ``backwards``. Try creating your own cell to make # the robot move forward at 50% speed for one second. # # Create a new cell by highlighting an existing cell and pressing ``b`` or the ``+`` icon above. Once you've done that, type in the code that you think will make the robot move forward at 50% speed for one second. # ### Controlling motors individually # # Above we saw how we can control the robot using commands like ``left``, ``right``, etc. But what if we want to set each motor speed # individually? Well, there are two ways you can do this # # The first way is to call the ``set_motors`` method. For example, to turn along a left arch for a second we could set the left motor to 30% and the right motor to 60% like follows. # In[8]: robot.set_motors(0.2, 0.2) time.sleep(3.0) robot.stop() # Great! You should see the robot move along a left arch. But actually, there's another way that we could accomplish the same thing. # # The ``Robot`` class has two attributes named ``left_motor`` and ``right_motor`` that represent each motor individually. # These attributes are ``Motor`` class instances, each which contains a ``value`` attribute. This ``value`` attribute # is a [traitlet](https://github.com/ipython/traitlets) which generates ``events`` when assigned a new value. In the motor # class, we attach a function that updates the motor commands whenever the value changes. # # So, to accomplish the exact same thing we did above, we could execute the following. # In[9]: robot.left_motor.value = 0.2
# 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()
def run(remotecontrol=True, cameracapture=False): global dualshock global robot global axis global continuouscap global continuouscaptime global neuralnet_latched global nnproc prevShutter = False meaningful_input_time = None neuralnet_input_time = None cam_cap_time = None last_speed_debug_time = datetime.datetime.now() last_tick_debug_time = datetime.datetime.now() print("Remote Control script running! ", end=" ") if remotecontrol: print("in RC mode") elif cameracapture: print("in CAMERA mode") else: print("unknown mode, quitting") quit() if remotecontrol: try: robot = Robot() except Exception as ex: sys.stderr.write("Failed to initialize motor drivers, error: %s" % (str(ex))) robot = None while True: dualshock = get_dualshock4() if dualshock != None: print("DualShock4 found, %s" % str(dualshock)) else: time.sleep(2) now = datetime.datetime.now() delta_time = now - last_tick_debug_time if delta_time.total_seconds() > 5: last_tick_debug_time = now sys.stderr.write("tick %s" % (str(now))) while dualshock != None: now = datetime.datetime.now() delta_time = now - last_tick_debug_time if delta_time.total_seconds() > 5: last_tick_debug_time = now sys.stderr.write("tick %s" % (str(now))) try: event = dualshock.read_one() if event != None: gamepad_event_handler(event, is_remotecontrol=remotecontrol, is_cameracapture=cameracapture) else: time.sleep(0) all_btns = dualshock.active_keys() if remotecontrol: meaningful_input = False # meaningful input means any buttons pressed or the stick has been moved if TRIANGLE in all_btns: neuralnet_latched = False mag_dpad, ang_dpad = axis_vector( axis[DPAD_X], axis[DPAD_Y]) mag_left, ang_left = axis_vector( axis_normalize(axis[LEFT_X], curve=0), axis_normalize(axis[LEFT_Y], curve=0)) mag_right, ang_right = axis_vector( axis_normalize(axis[RIGHT_X], curve=0), axis_normalize(axis[RIGHT_Y], curve=0)) now = datetime.datetime.now() if mag_dpad != 0 or mag_left > 0.1 or mag_right > 0.1: meaningful_input = True if meaningful_input_time == None: print("meaningful input!") meaningful_input_time = now elif meaningful_input_time != None: # user may have let go, check for timeout delta_time = now - meaningful_input_time if delta_time.total_seconds() > 2: print( "No meaningful input, stopping robot motors" ) meaningful_input = False meaningful_input_time = None if robot != None: robot.stop() else: meaningful_input = True use_nn = False if SQUARE in all_btns: neuralnet_latched = True if TRIANGLE in all_btns: neuralnet_latched = False if neuralnet_latched or CROSS in all_btns: use_nn = True if meaningful_input == False and nnproc is not None: # remote control inputs always override neural net inputs while sys.stdin in select.select([sys.stdin], [], [], 0)[0]: line = sys.stdin.readline() if line: try: axis[NN_THROTTLE] = int(line[0:3]) axis[NN_STEERING] = int(line[3:]) neuralnet_input_time = now except: pass if neuralnet_input_time != None and use_nn: delta_time = now - neuralnet_input_time if delta_time.total_seconds() < 5: meaningful_input = True meaningful_input_time = now if meaningful_input: left_speed = 0 right_speed = 0 ignore_dpad = False #ignore_rightstick = True if use_nn: start_nn_proc( ) # this will check if process has already started left_speed, right_speed = axis_mix( axis_normalize(axis[NN_STEERING]), axis_normalize(255 - axis[NN_THROTTLE])) elif mag_dpad != 0 and ignore_dpad == False: left_speed, right_speed = axis_mix( float(axis[DPAD_X]) / 3.0, axis[DPAD_Y]) #elif mag_left > mag_right or ignore_rightstick == True: # left_speed, right_speed = axis_mix(axis_normalize(axis[LEFT_X]), axis_normalize(axis[LEFT_Y])) # if ignore_rightstick == False: # left_speed /= 2 # right_speed /= 2 else: # left_speed, right_speed = axis_mix(axis_normalize(axis[RIGHT_X]), axis_normalize(axis[RIGHT_Y])) left_speed, right_speed = axis_mix( axis_normalize(axis[RIGHT_X]), axis_normalize(axis[LEFT_Y])) if robot != None: robot.set_motors(left_speed, right_speed) delta_time = now - last_speed_debug_time if delta_time.total_seconds() >= 1: if use_nn: print("nn -> ", end="") print("leftmotor: %.2f rightmotor: %.2f" % (left_speed, right_speed)) last_speed_debug_time = now elif cameracapture: now = datetime.datetime.now() need_cap = False if L1 in all_btns: if prevShutter == False: if continuouscaptime != None: timedelta = now - continuouscaptime if timedelta.total_seconds() > 0.5: continuouscap = not continuouscap else: continuouscap = not continuouscap prevShutter = True else: prevShutter = False if continuouscap: cam_cap_time = now need_cap = L1 else: if cam_cap_time != None: timedelta = now - cam_cap_time if timedelta.total_seconds() < 1: #need_cap = OTHERCODE pass else: cam_cap_time = None if need_cap != False: snapname = get_snap_name(need_cap) print("saving running pic: " + snapname) cam_capture(snapname) cam_frame_time = now while True: now = datetime.datetime.now() cam_frame_timedelta = now - cam_frame_time if cam_frame_timedelta.total_seconds() >= 0.01: break event = dualshock.read_one() if event != None: gamepad_event_handler( event, is_remotecontrol=False, is_cameracapture=True) except OSError: print("DualShock4 disconnected") dualshock = None if remotecontrol: end_cam_proc() if robot != None: robot.stop()
if barrier_center[0] <= 0: robot.right(0.4) else: robot.left(0.4) #robot.set_motors( # float(speed - turn_gain * barrier_center[0]), # float(speed + turn_gain * barrier_center[0]) #) time.sleep(0.2) robot.stop() continue if bypass_number >= 0: print("Bypassing barrier.", bypass_number) #robot.forward(0.3) robot.set_motors(0.42, 0.4) time.sleep(0.35) robot.stop() bypass_number -= 1 continue if len(target_boxes) == 0 and center != None and found_number == 0: print("barrier_center:", barrier_center) if barrier_center == -1: print("In tracing modei, center: ", center[0]) light_on(LED_GREEN) if center[0] >= 0: print("Turn right") robot.right(0.4) else:
if "exit" in joystic_command: if joystic_command["exit"]: break # get out of the loop with button 3 if "axis_0" in joystic_command: motor_coeff_left = motor_coeff_left_compute( joystic_command["axis_0"]) motor_coeff_right = motor_coeff_right_compute( joystic_command["axis_0"]) if "axis_1" in joystic_command: motor_coeff_forward = motor_coeff_forward_compute( joystic_command["axis_1"]) motor_left = motor_coeff_left * motor_coeff_forward * max_velocity motor_right = motor_coeff_right * motor_coeff_forward * max_velocity # set speeds to robot wheels robot.set_motors(motor_right, motor_left) # robot.left_motor.value = motor_left # robot.right_motor.value = motor_right sys.stdout.write("\rL={0:+1.3f} R={1:+1.3f} ".format( motor_left, motor_right)) sys.stdout.flush() print("\nExiting...") robot.stop() pygame.joystick.quit() pygame.quit()