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()
