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 __init__(self, trig, echo, timeout=0.01):
self.timeout = timeout
self.trig = trig
self.echo = echo
# Init Servo
self.servo = Servo(PWM("P0"), offset=10)
self.angle_distance = [0, 0]
self.current_angle = 0
self.max_angle = self.ANGLE_RANGE / 2
self.min_angle = -self.ANGLE_RANGE / 2
self.scan_list = []
def reset_servo(angle1):
ser = Servo(PWM("P0"))
ser.set_angle(angle1)
turn_time_right =6
turn_time =1
go_time =0.8
ANGLE_RANGE = 180
STEP = 18
us_step = STEP
angle_distance = [0,0]
current_angle = 0
max_angle = ANGLE_RANGE/2
min_angle = -ANGLE_RANGE/2
scan_list = []
us = Ultrasonic(Pin('D8'), Pin('D9'))
config = FileDB()
ultrasonic_servo_offset = int(config.get('ultrasonic_servo_offset', default_value = 0))
servo = Servo(PWM("P0"), offset=ultrasonic_servo_offset)
def scan_step1(ref):
global scan_list, current_angle, us_step
current_angle += us_step
if current_angle >= max_angle:
current_angle = max_angle
us_step = -STEP
elif current_angle <= min_angle:
current_angle = min_angle
us_step = STEP
status = get_status_at(current_angle, ref1=ref)#ref1
from picar_4wd.servo import Servo
from picar_4wd.pwm import PWM
import sys
try:
ser = Servo(PWM("P0"))
# print("offset angle:",Servo(PWM("P0")) )
# print(ser.set_angle(0) )
ser.set_angle(-65)
# print(ser.offset)
except KeyboardInterrupt:
print("stopped by User")
ser = Servo(PWM("P0"))
ser.set_angle(-35)
class LightningMcqueen:
ser = Servo(PWM("P0"))
us = Ultrasonic(Pin("D8"), Pin("D9"))
angle_increment = 5
length_per_position = 2.5 #5 cm / position in numpy array
houser_boon = HouserBoon()
target = (150, 150)
direction = 0
def __init__(self, target):
self.target = target
self.map_to_fill = np.zeros((150, 150))
self.position = (self.map_to_fill.shape[0]*.5, 0)
self.rescan_limit = 10
def findMove(current, prev):
if current[1] == prev[1] + 1:
return "forward"
if current[0] == prev[0] + 1:
return "down"
if current[0] == prev[0] - 1:
return "up"
def lets_do_this_thing(self, map_to_fill):
map_to_fill = scan_and_build_map(60, map_to_fill)
np.set_printoptions(threshold=np.inf)
printmat(map_to_fill.astype(int))
start = (map_to_fill.shape[0]*.5, 0)
rec = a_star_recommendation(map_to_fill, self.target)
start = self.position
starting_coordinate = self.position
direction = self.direction
prev = starting_coordinate
# def turnRight():
# print("right")
# def turnLeft():
# print("left")
# def move25():
# print("forward")
# direction 0 = east
# direction 1 = south
# direction -1 = north
for i in range(min(len(rec), self.rescan_limit)):
coordinate = rec[i]
while (houser_boon.show_us_the_bolt()):
time.sleep(3)
print("STOP")
if direction == 0 and findMove(coordinate, prev) == "forward":
move25()
self.position[0] += self.length_per_position
elif direction == 1 and findMove(coordinate, prev) == "forward":
turnLeft()
move25()
direction = 0
self.position[0] += self.length_per_position
elif direction == -1 and findMove(coordinate, prev) == "forward":
turnRight()
move25()
direction = 0
self.position[0] += self.length_per_position
if direction == 0 and findMove(coordinate, prev) == "down":
turnRight()
move25()
direction = 1
self.position[1] -= self.length_per_position
elif direction == 1 and findMove(coordinate, prev) == "down":
move25()
self.position[1] -= self.length_per_position
elif direction == -1 and findMove(coordinate, prev) == "down":
turnRight()
turnRight()
move25()
direction = 1
self.position[1] -= self.length_per_position
elif direction == 0 and findMove(coordinate, prev) == "up":
turnLeft()
move25()
direction = -1
self.position[1] += self.length_per_position
elif direction == 1 and findMove(coordinate, prev) == "up":
turnLeft()
turnLeft()
move25()
direction = -1
self.position[1] += self.length_per_position
elif direction == -1 and findMove(coordinate, prev) == "up":
move25()
self.position[1] += self.length_per_position
map_to_fill[int(coordinate[0]), coordinate[1]] = 6
prev = coordinate
self.target[0] -= position[0]
self.target[1] -= position[1]
self.direction = direction
def kachow(self):
while(lets_do_this_thing(self.map_to_fill)):
print("Scanned")
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 turnLeft():
speed4 = Speed(25)
speed4.start()
# time.sleep(2)
fc.turn_right(80)
x = 0
for i in range(6):
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_left(80)
x = 0
for i in range(6):
time.sleep(0.1)
speed = speed4()
x += speed * 0.1
print("%smm/s"%speed)
print("%smm"%x)
speed4.deinit()
fc.stop()
def get_distance(angle):
self.ser.set_angle(angle)
time.sleep(0.5)
return self.us.get_distance()
def scan_and_build_map(angle, map_to_fill):
last_position = [0,0]
map_to_fill[0,0] = 7
map_to_fill[int(len(map_to_fill)*.5), 0] = 5
for current_angle in range(-1*angle, angle, self.angle_increment):
current_distance = get_distance(current_angle)
print(current_distance)
if current_angle == -1*angle:
last_position = [(len(map_to_fill)*.5 - current_distance*math.sin(math.radians(current_angle)))/self.length_per_position, current_distance*math.cos(math.radians(current_angle))/self.length_per_position]
if (last_position[0] < len(map_to_fill) and last_position[1] <= len(map_to_fill)):
map_to_fill[int(last_position[0]), int(last_position[1])] = 1
else:
current_position = [len(map_to_fill)*.5 - current_distance*math.sin(math.radians(current_angle))/self.length_per_position, current_distance*math.cos(math.radians(current_angle))/self.length_per_position]
if (current_position[0] < len(map_to_fill) and current_position[1] <= len(map_to_fill)):
map_to_fill[int(current_position[0]), int(current_position[1])] = 1
slope = (current_position[1] - last_position[0])/(current_position[0] - last_position[0])
for i in range(0, int(current_position[0] - last_position[0])):
if (last_position[0] + i < len(map_to_fill) and last_position[1] + i*slope < len(map_to_fill)):
map_to_fill[int(last_position[0] + i), int(last_position[1] + i*slope)] = 1
map_to_fill[int(len(map_to_fill)*.5), 0] = 5
return map_to_fill
def a_star_recommendation(map_to_fill, target_coordinates):
open = []
heapq.heappush(open, (0, start))
traversal = {}
cost = {}
traversal[target_coordinates] = None
cost[start] = 0
while True:
print
curr = heapq.heappop(open)[1]
if curr == target_coordinates:
break
for neighbor in get_possible_moves(map_to_fill, curr):
neighbor_cost = cost[curr] + 1
if neighbor not in cost or cost[neighbor] > neighbor_cost:
cost[neighbor] = neighbor_cost
heapq.heappush(open, (neighbor_cost + h(neighbor, target_coordinates),neighbor))
traversal[neighbor] = curr
back = target_coordinates
output = []
while back != None and back in traversal:
output.append(back)
back = traversal[back]
output.reverse()
return output
def h(a, b):
return abs(b[1] - a[1]) + abs(a[0] - b[0])
def get_possible_moves(map_to_fill, current_coordinate):
possible_moves = []
if current_coordinate[0] - 1 >= 0 and map_to_fill[int(current_coordinate[0]) - 1, current_coordinate[1]] != 1:
possible_moves.append((current_coordinate[0] - 1, current_coordinate[1]))
if (current_coordinate[0] + 1 < len(map_to_fill) and map_to_fill[int(current_coordinate[0]) + 1, current_coordinate[1]] != 1):
possible_moves.append((current_coordinate[0] + 1, current_coordinate[1]))
if (current_coordinate[1] - 1 >= 0 and map_to_fill[int(current_coordinate[0]), current_coordinate[1] - 1] != 1):
possible_moves.append((current_coordinate[0], current_coordinate[1] - 1))
if (current_coordinate[1] + 1 < len(map_to_fill[0]) and map_to_fill[int(current_coordinate[0]), current_coordinate[1] + 1] != 1 ):
possible_moves.append((current_coordinate[0], current_coordinate[1] + 1))
return possible_moves
def format__1(digits,num):
if digits<len(str(num)):
raise Exception("digits<len(str(num))")
return ' '*(digits-len(str(num))) + str(num)
def printmat(arr,row_labels=[], col_labels=[]): #print a 2d numpy array (maybe) or nested list
max_chars = 2
if row_labels==[] and col_labels==[]:
for row in arr:
print('[%s]' %(' '.join(format__1(max_chars,i) for i in row)))
elif row_labels!=[] and col_labels!=[]:
rw = max([len(str(item)) for item in row_labels]) #max char width of row__labels
print('%s %s' % (' '*(rw+1), ' '.join(format__1(max_chars,i) for i in col_labels)))
for row_label, row in zip(row_labels, arr):
print('%s [%s]' % (format__1(rw,row_label), ' '.join(format__1(max_chars,i) for i in row)))
else:
raise Exception("This case is not implemented...either both row_labels and col_labels must be given or neither.")
def add_clearance(map_to_fill):
copied_result = np.copy(map_to_fill)
for i in range(len(map_to_fill)):
for j in range(len(map_to_fill[0])):
if (map_to_fill[i, j] == 1):
for move in get_possible_moves(map_to_fill, [i, j]):
if (copied_result[move[0], move[1]] == 0):
copied_result[move[0], move[1]] = 1
return copied_result