def Keyborad_control():
while True:
global power_val
key = readkey()
if key == '6':
if power_val <= 90:
power_val += 10
print("power_val:", power_val)
elif key == '4':
if power_val >= 10:
power_val -= 10
print("power_val:", power_val)
if key == 'w':
fc.forward(power_val)
elif key == 'a':
fc.turn_left(power_val)
elif key == 's':
fc.backward(power_val)
elif key == 'd':
fc.turn_right(power_val)
else:
fc.stop()
if key == 'q':
print("quit")
break
def __init__(self):
""" Init camera and wheels"""
logging.info('Creating a PiCar-4WD...')
fc.stop()
logging.debug('Set up camera')
self.camera = cv2.VideoCapture(-1)
self.camera.set(3, self.__SCREEN_WIDTH)
self.camera.set(4, self.__SCREEN_HEIGHT)
ser = Servo(PWM("P0"))
ser.set_angle(-65)
self.lane_follower = LaneFollower(self)
#self.traffic_sign_processor = ObjectsOnRoadProcessor(self)
# lane_follower = DeepLearningLaneFollower()
self.fourcc = cv2.VideoWriter_fourcc(*'XVID')
datestr = datetime.datetime.now().strftime("%y%m%d_%H%M%S")
self.video_orig = self.create_video_recorder(
'../data/temp/car_video%s.avi' % datestr)
self.video_lane = self.create_video_recorder(
'../data/temp/car_video_lane%s.avi' % datestr)
# self.video_objs = self.create_video_recorder('../data/temp/car_video_objs%s.avi' % datestr)
logging.info('Created a PiCar-4wd')
def drive_forward_cam(self, distance: int, power: int = 2):
self.trip_meter.reset()
coast_clear = True
while(self.trip_meter.distance < distance):
# wait for obstacles to clear
if not self.obstacle_queue.empty():
fc.stop()
with self.obstacle_queue.mutex:
self.obstacle_queue.queue.clear()
time.sleep(2)
else:
fc.forward(power)
fc.stop()
actually_traveled = self.trip_meter.distance
print(self.orientation, distance, actually_traveled)
self.nav.update_postion(distance, self.orientation)
return coast_clear
def main():
while True:
scan_list = fc.scan_step(23)
# print(scan_list)
if not scan_list:
continue
scan_list = [str(i) for i in scan_list]
scan_list = "".join(scan_list)
paths = scan_list.split("2")
length_list = []
for path in paths:
length_list.append(len(path))
# print(length_list)
if max(length_list) == 0:
fc.stop()
else:
i = length_list.index(max(length_list))
pos = scan_list.index(paths[i])
pos += (len(paths[i]) - 1) / 2
# pos = int(pos)
delta = len(scan_list) / 3
# delta *= us_step/abs(us_step)
if pos < delta:
fc.turn_left(speed)
elif pos > 2 * delta:
fc.turn_right(speed)
else:
if scan_list[int(len(scan_list)/2-1)] == "0":
fc.backward(speed)
else:
fc.forward(speed)
def main():
while True:
scan_list = scan_step1(33)
if not scan_list:
continue
tmp = scan_list[0][3:7]
# print(angle_distance_list)
# if tmp == [0,0,0,0]:
# fc.backward(speed)
print(tmp)
img = np.zeros((480, 480, 3), np.uint8)
cv.rectangle(img, (190, 10), (290, 100), yellow, 2)
cv.namedWindow("image")
cv.imshow('image', img)
cv.waitKey(500)
draw_dot(scan_list[0], scan_list[1], img)
print("$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$")
if tmp != [2, 2, 2, 2]:
fc.stop()
# time.sleep(0.3)
# fc.backward(speed)
# time.sleep(0.3)
# fc.turn_right(speed)
else:
fc.stop()
async def main_func():
global recv_dict, send_dict, gs_list
while 1:
gs_list = fc.get_grayscale_list()
if recv_dict['CD'][0] == 'on':
if fc.is_on_edge(recv_dict['CD'][1], gs_list):
fc.backward(20)
time.sleep(0.5)
fc.stop()
if recv_dict['TL'][0] == 'on':
if fc.get_line_status(recv_dict['TL'][1], gs_list) == 0:
fc.forward(recv_dict['PW'])
elif fc.get_line_status(recv_dict['TL'][1], gs_list) == -1:
fc.turn_left(recv_dict['PW'])
elif fc.get_line_status(recv_dict['TL'][1], gs_list) == 1:
fc.turn_right(recv_dict['PW'])
if recv_dict['OA'] == 'on':
scan_list = fc.scan_step(35)
if scan_list:
tmp = scan_list[3:7]
if tmp != [2, 2, 2, 2]:
fc.turn_right(recv_dict['PW'])
else:
fc.forward(recv_dict['PW'])
elif recv_dict['OF'] == 'on':
scan_list = fc.scan_step(23)
if scan_list != False:
scan_list = [str(i) for i in scan_list]
scan_list = "".join(scan_list)
paths = scan_list.split("2")
length_list = []
for path in paths:
length_list.append(len(path))
if max(length_list) == 0:
fc.stop()
else:
i = length_list.index(max(length_list))
pos = scan_list.index(paths[i])
pos += (len(paths[i]) - 1) / 2
delta = len(scan_list) / 3
if pos < delta:
fc.turn_left(recv_dict['PW'])
elif pos > 2 * delta:
fc.turn_right(recv_dict['PW'])
else:
if scan_list[int(len(scan_list) / 2 - 1)] == "0":
fc.backward(recv_dict['PW'])
else:
fc.forward(recv_dict['PW'])
elif recv_dict['RD'] == 'on':
fc.scan_step(35)
await asyncio.sleep(0.01)
def mright():
speed4 = fc.Speed(25)
speed4.start()
fc.turn_right(15)
x = 0
for i in range(3):
time.sleep(0.1)
speed4.deinit()
fc.stop()
def cleanup(self):
""" Reset the hardware"""
logging.info('Stopping the car, resetting hardware.')
fc.stop()
self.camera.release()
self.video_orig.release()
self.video_lane.release()
# self.video_objs.release()
cv2.destroyAllWindows()
def move25():
speed4 = Speed(25)
speed4.start()
# time.sleep(2)
fc.forward(35)
fc.stop()
time.sleep(0.5)
fc.turn_right(50)
print('distance:', fc.us.get_distance())
def mforward():
speed4 = fc.Speed(25)
speed4.start()
fc.forward(15)
x = 0
for i in range(3):
time.sleep(0.1)
speed4.deinit()
fc.stop()
def mbackward():
speed4 = fc.Speed(25)
speed4.start()
fc.backward(17)
x = 0
for i in range(10):
time.sleep(0.1)
speed4.deinit()
fc.stop()
def main():
import sys
if len(sys.argv) >= 2:
print("Welcome to SunFounder PiCar-4WD.")
command = sys.argv[1]
if command == "soft-reset":
print("soft-reset")
soft_reset()
elif command == "power-read":
print("power-read")
print("Power voltage: {}V".format(power_read()))
elif command == "web-example":
if len(sys.argv) >= 3:
opt = sys.argv[2]
if opt == "enable":
os.system(
"sudo update-rc.d picar-4wd-web-example defaults")
print("web-example start on boot is enabled")
elif opt == "disable":
os.system("sudo update-rc.d picar-4wd-web-example remove")
print("web-example start on boot is disabled")
else:
usage(command)
else:
print(
"Run: `picar-4wd web-example enable/disable` to enable/disable start on boot"
)
os.system(
"sudo python3 /home/pi/picar-4wd/examples/web/start.py")
elif command == "test":
from picar_4wd import forward, get_distance_at, get_grayscale_list, stop
if len(sys.argv) >= 3:
opt = sys.argv[2]
if opt == "motor":
print("Motor test start!, Ctrl+C to Stop")
forward(50)
try:
while True:
pass
except KeyboardInterrupt:
stop()
elif opt == "servo":
print(get_distance_at(0))
elif opt == "grayscale":
print(get_grayscale_list())
else:
usage(command)
else:
print('Command error, "%s" is not in list' % sys.argv[1])
usage()
else:
usage()
destroy()
def Remote_control(control_flag, speed=50):
speed = int(speed)
if control_flag == 'forward':
fc.forward(speed)
elif control_flag == 'backward':
fc.backward(speed)
elif control_flag == 'turn_left':
fc.turn_left(speed)
elif control_flag == 'turn_right':
fc.turn_right(speed)
else:
fc.stop()
def naviCar(turns):
dir = []
o = start
d = 'F'
for t in turns:
dy = t[0] - o[0]
dx = t[1] - o[1]
if dy == 0 and dx > 0:
dir.append('R')
if d == 'B' or d == 'F':
fc.turn_right(pow)
time.sleep(agR)
d = 'R'
if dy == 0 and dx < 0:
dir.append('L')
if d == 'F' or d == 'F':
fc.turn_left(pow)
time.sleep(agL)
d = 'L'
if dy > 0 and dx == 0:
dir.append('B')
if d == 'L':
fc.turn_right(pow)
time.sleep(agR)
if d == 'R':
fc.turn_left(pow)
time.sleep(agL)
d = 'B'
if dy < 0 and dx == 0:
dir.append('F')
if d == 'L':
fc.turn_right(pow)
time.sleep(agR)
if d == 'R':
fc.turn_left(pow)
time.sleep(agL)
d = 'F'
fc.stop()
fc.forward(pow)
time.sleep(fcm * abs(dy+dx))
o = t
fc.stop()
print("Direction: ", dir)
def drive_backward(self, distance = 10, power = 5):
self.trip_meter.reset()
fc.backward(power)
while(self.trip_meter.distance < distance):
continue
fc.stop()
actually_traveled = self.trip_meter.distance
return actually_traveled
def main():
time.sleep(5)
fc.forward(pow)
while True:
dist = fc.get_distance_at(ang)
print(dist)
if dist != -2:
if dist <= 60:
fc.forward(pow / 5)
if dist <= 20:
fc.stop()
break
fc.stop()
def drive_n_stop(speed: int = 10):
clear = True
fc.forward(speed)
while clear:
scan_list = fc.scan_step()
if not scan_list:
continue
ahead = scan_list[2:8]
# coast clear full speed ahead
if min(ahead) < 2:
#print("Coast Clear")
clear = False
fc.stop()
def drive_step(self, distance: int, power: int = 2):
self.trip_meter.reset()
while(self.cam.detect_traffic() == True):
fc.stop()
print("Obstacle detected")
time.sleep(2)
while(self.trip_meter.distance < distance):
fc.forward(2)
fc.stop()
print(self.trip_meter.distance, distance)
def move25():
speed4 = Speed(25)
speed4.start()
# time.sleep(2)
fc.backward(100)
x = 0
for i in range(1):
time.sleep(0.1)
speed = speed4()
x += speed * 0.1
print("%smm/s"%speed)
print("%smm"%x)
speed4.deinit()
fc.stop()
def test3():
speed4 = Speed(25)
speed4.start()
# time.sleep(2)
fc.forward(100)
x = 0
for i in range(20):
time.sleep(0.1)
speed = speed4()
x += speed * 0.1
print("%smm/s" % speed)
print("%smm" % x)
speed4.deinit()
fc.stop()
def turnRight():
speed4 = Speed(25)
speed4.start()
# time.sleep(2)
fc.turn_right(80)
x = 0
for i in range(15):
time.sleep(0.1)
speed = speed4()
x += speed * 0.1
print("%smm/s" % speed)
print("%smm" % x)
speed4.deinit()
fc.stop()
def main_loop():
if lettura_media(0) > MIN_DIST_FOLLOW:
fc.forward(20)
while lettura_media(0) > MIN_DIST_FOLLOW:
pass
fc.stop()
lval = scan_sx()
rval = scan_dx()
turn_time = 0.5 + random.random() * 1
if rval < lval:
fc.turn_right(20)
else:
fc.turn_left(20)
time.sleep(turn_time)
def drive_n_stop(speed: int = 5):
clear = True
fc.forward(speed)
while clear:
# scan list returns false until a full 180 deg scan is performed.
scan_list = fc.scan_step(35)
if not scan_list:
continue
ahead = scan_list[2:8]
if min(ahead) < 2:
clear = False
fc.stop()
def main():
time.sleep(5)
fc.forward(pow)
time.sleep(3)
fc.backward(pow)
time.sleep(2)
fc.turn_left(pow)
time.sleep(2)
fc.turn_right(pow)
time.sleep(2)
fc.stop()
def roomba(speed=10):
while True:
# get scan by distance
scan_list = scanner.scan_step_dist()
if not scan_list:
continue
# the preprocessing limits the max distance to 200cm
# since any -2 means no response set the value to 200cm
scan_list = [200 if d == -2 else d for d in scan_list]
# readings that are infront on the Picar
# -54 degrees to 54 degrees
ahead = scan_list[2:7]
# coast clear full speed ahead
if min(ahead) > 35:
fc.forward(speed)
continue
logging.info("Too close stopping")
fc.stop()
left = scan_list[:5]
right = scan_list[5:]
# -1 = turn left, 0 forward, 1 turn right
direction = 0
# evaluates which direction turn
# turns in the direction with the most open space
if (sum(right) > sum(left)):
direction = 1
else:
direction = -1
#print(sum(left), sum(right))
#print( direction)
if direction == 1:
logging.info("Turning right")
fc.turn_right(speed)
elif direction == -1:
logging.info("Turning left")
fc.turn_left(speed)
def turn_right(self, angle: int, power: int = 5):
# need to adjust slippage for turning
slippage = 2.2
dist = utils.angle_to_dist(angle) * slippage
self.trip_meter.reset()
fc.turn_right(power)
while(self.trip_meter.distance < dist):
continue
fc.stop()
self.orientation = update_angle(self.orientation, angle)
print("new car orientation", self.orientation)
return self.orientation
def drive_forward(self, distance: int, power: int = 5):
self.trip_meter.reset()
fc.forward(power)
coast_clear = True
while(self.trip_meter.distance < distance and coast_clear):
if (fc.get_status_at(0, 20) != 2):
coast_clear = False
break
continue
fc.stop()
print(self.trip_meter.distance, distance)
return coast_clear
def main():
while True:
scan_list = scan_step1(25)
if not scan_list:
continue
tmp = scan_list[3:7]
# detect()
# print(tmp)
print(detect())
if 1 not in tmp:
print(tmp)
# fc.forward(speed)
fc.stop()
else:
print(tmp)
fc.stop()
def main():
while True:
scan_list = scan_step1(40)
if not scan_list:
continue
tmp = scan_list[3:7]
# if tmp == [0,0,0,0]:
# fc.backward(speed)
if 1 not in tmp:
print(tmp)
fc.forward(speed)
else:
print(tmp)
fc.stop()
time.sleep(0.3)
fc.backward(speed)
time.sleep(0.3)
fc.turn_right(speed)
def test1():
# import fwd as nc
fc.forward(100)
speed3 = Speed(25)
speed4 = Speed(4)
speed3.start()
speed4.start()
try:
# nc.stop()
while 1:
# speed_counter
# = 0
print(speed3())
print(speed4())
print(" ")
time.sleep(0.5)
finally:
speed3.deinit()
speed4.deinit()
fc.stop()
