def main():
dist = []
inc = 10
disa = []
a = -90
while a <= 90:
disa.append(fc.get_distance_at(a))
a += inc
print(disa)
disb = []
b = 90
while b >= -90:
disb.append(fc.get_distance_at(b))
b -= inc
print(disb)
for c in range(len(disa)):
dist.append(min(disa[c], disb[len(disa) - 1 - c]))
print(dist)
fc.servo.set_angle(0)
np.savetxt("maps/map.csv", getMap(dist, inc), fmt='%i', delimiter=",")
def getDist(inc):
disa = []
a = -90
while a <= 90:
disa.append(fc.get_distance_at(a))
#time.sleep(0.1)
a += inc
print("DistanceA: ", disa)
disb = []
b = 90
while b >= -90:
disb.append(fc.get_distance_at(b))
#time.sleep(0.1)
b -= inc
print("DistanceB: ", disb)
dist = []
for c in range(len(disa)):
dist.append(min(disa[c],disb[len(disa)-1-c]))
print("Distances: ", dist)
fc.servo.set_angle(0)
return dist
def lettura_media(angolo):
res = 0
for i in range(1, 3):
res = res + fc.get_distance_at(angolo)
time.sleep(0.1)
logging.warning('score media: ' + str(res / 3))
return res / 3
def turn(self, angle: int, power: int = 10):
print(angle)
if angle < 0:
self.turn_right(abs(angle))
elif angle > 1:
self.turn_left(angle)
else:
while(fc.get_distance_at(0) < turn_dist):
fc.backward(2)
def getDist():
disa = []
for a in range(-90, 100, INC):
disa.append(fc.get_distance_at(a))
#print("DistanceA: ", disa)
disb = []
for b in range(90, -100, -INC):
disb.append(fc.get_distance_at(b))
#print("DistanceB: ", disb)
dist = []
for c in range(len(disa)):
dist.append(min(disa[c],disb[len(disa)-1-c]))
print("Distances: ", dist)
fc.servo.set_angle(0)
return dist
def map_environment():
environment_size = 100
x_offset = environment_size / 2
environment = np.full((environment_size, environment_size), 255)
detected_points = []
distance = 0
for angle in range(70, -70, -5):
distance_1 = fc.get_distance_at(angle)
distance_2 = fc.get_distance_at(angle)
if (distance_1 < 0 and distance_2 < 0):
continue
elif (distance_1 < 0):
distance = distance_2
elif (distance_2 < 0):
distance = distance_1
if (distance != -2 and distance <= 100):
theta = math.radians(angle)
# offset by one to fit into the environment. Revert the coordinates as top is 0 in numpy
x = environment_size - (int(x_offset +
(distance * math.sin(theta))) - 1)
y = environment_size - (int(distance * math.cos(theta)) - 1)
# make sure there is no point outside the environment
if (x > 0 and x < environment_size and y > 0
and y < environment_size):
detected_points.append((y, x))
# add padding so that car can navigate
pad_points(environment, detected_points, 5)
# Reset servo
fc.get_distance_at(0)
img = cv2.merge((environment, environment, environment))
cv2.imwrite('detected_points.bmp', img)
return environment
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 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 mapping_scan(min_angle=MIN_ANGLE, max_angle=MAX_ANGLE + 1, step=5):
# set to min_angle
time.sleep(1)
scan_dist = []
for angle in range(min_angle, max_angle, step):
# give time for settling
time.sleep(0.2)
dist = fc.get_distance_at(angle)
# ignore values that are -2 likely too far away
if dist != -2:
scan_dist.append([angle * -1, dist])
return np.array(scan_dist)
async def send_server_func(websocket):
global send_dict, recv_dict, gs_list
while 1:
send_dict = {}
send_dict['MS'] = [round(fc.speed_val() / 2.0), time.time()]
if recv_dict['ST'] == 'on':
send_dict['ST'] = pi_read()
if recv_dict['US'][0] == 'on':
send_dict['US'] = [
int(recv_dict['US'][1]),
fc.get_distance_at(int(recv_dict['US'][1]))
]
else:
send_dict['US'] = fc.angle_distance
if recv_dict['GS'] == 'on':
send_dict['GS'] = gs_list
await websocket.send(json.dumps(send_dict))
await asyncio.sleep(0.01)
def scan_step_dist():
global scan_dist, 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
dist = fc.get_distance_at(current_angle)
scan_dist.append(dist)
if current_angle == MIN_ANGLE or current_angle == MAX_ANGLE:
if us_step < 0:
scan_dist.reverse()
tmp = scan_dist.copy()
scan_dist = []
return tmp
else:
return False
def get_distance_from_ultrasonic(angle):
print("detecting distance at ", angle, " degree")
distance = fc.get_distance_at(angle)
print("Object is ", distance, "cm away")
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)
if __name__ == "__main__":
try:
roomba()
finally:
fc.get_distance_at(0)
fc.stop()