def frames(self):
raw_capture = PiYUVArray(self.camera, size=RESOLUTION)
stream = self.camera.capture_continuous(raw_capture,
format="yuv",
use_video_port=True)
try:
for frame in stream:
raw_capture.truncate(0) # Reset the buffer for the next image
yield frame
except KeyboardInterrupt:
raw_capture.close()
stream.close()
print('Closed streams')
class PiVideoStream:
def __init__(self, resolution=(640, 480), framerate=120):
# initialize the camera and stream
self.camera = PiCamera()
self.camera.resolution = resolution
self.camera.framerate = framerate
self.rawCapture = PiYUVArray(self.camera, size=resolution)
self.stream = self.camera.capture_continuous(self.rawCapture,
format="yuv",
use_video_port=True)
# initialize the frame and the variable used to indicate
# if the thread should be stopped
self.frame = None
self.stopped = False
def start(self):
# start the thread to read frames from the video stream
Thread(target=self.update, args=()).start()
return self
def update(self):
# keep looping infinitely until the thread is stopped
for f in self.stream:
# grab the frame from the stream and clear the stream in
# preparation for the next frame
self.frame = f.array
self.rawCapture.truncate(0)
# if the thread indicator variable is set, stop the thread
# and resource camera resources
if self.stopped:
self.stream.close()
self.rawCapture.close()
self.camera.close()
return
def read(self):
# return the frame most recently read
return self.frame
def stop(self):
# indicate that the thread should be stopped
self.stopped = True
peaks: {0}
closest_peak_at: {1}
deviation: {2}
threshold_deviation: {3}
result:{4}
""".format(str(p[0]), closest_peak_at, deviation,
threshold_deviation, move))
continue
if move == 'left':
rot = min(zeroRot + maxRot, rot + 0.10 * rot)
elif move == 'right':
rot = max(zeroRot - maxRot, rot - 0.10 * rot)
else:
rot = zeroRot
pwm1.duty_cycle = int(rot)
except (KeyboardInterrupt, SystemExit):
# bring the car on rest in any system failure
pwm0.duty_cycle = minSpeed
pwm1.duty_cycle = zeroRot
stream.close()
rawCapture.close()
camera.close()
except Exception as e:
pwm0.duty_cycle = minSpeed
pwm1.duty_cycle = zeroRot
stream.close()
rawCapture.close()
camera.close()
def main_loop():
raw_capture = PiYUVArray(camera, size=res)
stream = camera.capture_continuous(raw_capture, format="yuv", use_video_port=True)
# for pwm1
# duty_cycle
# (array([482, 542], dtype=int32), {'peak_heights': array([186.831036 , 185.01286929])})
num_frames = 0
# if line_pos is 1 then the line is to the right of the car
# if line_pos is -1 then the line is to the left of the car
# if line_pos is 0 either uhoh we're doomed
# if lost is true then the line is lost
line_pos = STRAIGHT_
lost = False
prev_steer_dc = pwm1.duty_cycle
prev_steer_diff_arr = []
prev_peak_avg_arr = []
for i in range(NUM_PREV_VAL_):
prev_steer_diff_arr.append(-1)
prev_peak_avg_arr.append(-1)
for f in stream:
t = time.time()
# This was intended change speeds as necesary
# But not anymore since its too volatile, can be manually updated
# speeds = update_speeds()
# UCLA_ = speeds[0]
# SONIC_ = speeds[1]
# If killSwitch is toggled, breaks out of forever loop and kills the motor
if killSwitch.value:
break
num_frames += 1
# Get the intensity component of the image (a trick to get black and white images)
I = f.array[:, :, 0]
# Reset the buffer for the next image
raw_capture.truncate(0)
# Select a horizontal line in the middle of the image
U = I[UPPER_LINE_, :]
C = I[CENTER_LINE_, :]
L = I[LOWER_LINE_, :]
# Smooth the transitions so we can detect the peaks
Lf = filtfilt(b, a, C)
# Find peaks which are higher than 0.5
p = find_peaks(Lf, height=128)
total_peak_amt = 0
avg_peak_loc = NO_PEAKS_
num_peaks = 0
for peak in p[0]:
num_peaks += 1
total_peak_amt += peak
print("Num Peaks: " + num_peaks)
if num_peaks != 0:
avg_peak_loc = total_peak_amt / num_peaks
if num_peaks == 3:
print("THIS IS THE BEGINNING OF THE END")
if avg_peak_loc != NO_PEAKS_:
if avg_peak_loc < MEDIAN_:
if lost and line_pos == RIGHT_:
# If this happens, that means that the center line is to the right
# But we encountered the bordering track to the left
# Break to stop going over and f*****g shit up
# TODO: Make clause that handles this, so in case of race it doesn't stop without violating shit
break
line_pos = LEFT_
else:
if lost and line_pos == LEFT_:
# If this happens, that means that the center line is to the left
# But we encountered the bordering track to the right
# Break to stop going over and f*****g shit up
# TODO: Make clause that handles this, so in case of race it doesn't stop without violating shit
break
line_pos = RIGHT_
# Sets the steering position based on the peak of the Center line
pwm1.duty_cycle = int(STRAIGHT_ - ((avg_peak_loc - MEDIAN_)/MEDIAN_)*500000)
lost = False
else:
if line_pos == LEFT_:
pwm1.duty_cycle = LEFT_
elif line_pos == RIGHT_:
pwm1.duty_cycle = RIGHT_
else:
pwm1.duty_cycle = STRAIGHT_
lost = True
# pwm0.duty_cycle = SONIC_
if abs(prev_steer_dc - STRAIGHT_) < abs(pwm1.duty_cycle - STRAIGHT_) and prev_steer_diff_arr[0] > 0 and prev_steer_diff_arr[1] > 0 and abs(pwm1.duty_cycle - STRAIGHT_) > 400000:
pwm0.duty_cycle = SONIC_
elif abs(avg_peak_loc - MEDIAN_) <= CENTER_THRESHOLD_:
i = 0
while i < NUM_PREV_VAL_:
if abs(prev_peak_avg_arr[i] - MEDIAN_) > CENTER_THRESHOLD_:
break
i += 1
# print("STRAIGHT")
if i == NUM_PREV_VAL_:
pwm0.duty_cycle = USC_
else:
pwm0.duty_cycle = UCLA_
if abs(pwm1.duty_cycle - STRAIGHT_) > 300000:
pwm0.duty_cycle = UCLA_ + 4000
i = NUM_PREV_VAL_ - 1
while i > 0:
prev_steer_diff_arr[i] = prev_steer_diff_arr[i - 1]
prev_peak_avg_arr[i] = prev_peak_avg_arr[i - 1]
i -= 1
prev_steer_diff_arr[0] = abs(pwm1.duty_cycle - STRAIGHT_) - abs(prev_steer_dc - STRAIGHT_)
prev_peak_avg_arr[0] = avg_peak_loc
prev_steer_dc = pwm1.duty_cycle
time_elapsed = time.time() - t
print(prev_peak_avg_arr)
print(prev_steer_diff_arr)
print(sum(prev_steer_diff_arr))
print(sum(prev_steer_diff_arr) / len(prev_steer_diff_arr))
print(pwm1.duty_cycle)
print("Elapsed {:0.4f} seconds, estimated FPS {:0.2f}".format(time_elapsed, 1 / time_elapsed))
time.sleep(INTERVAL_)
pwm0.duty_cycle = 1000000
# Release resources
stream.close()
raw_capture.close()
camera.close()