def update(self):
now = rospy.Time.now()
if True:#(now > self.t_next):
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
try:
self.lValueA = GPIO.input(self.leftPinA)
if self.prev_lpinA != self.lValueA:
self.lValueB = GPIO.input(self.leftPinB)
if self.lValueB != self.lValueA:
self.lticks += 1
else:
self.lticks -= 1
self.prev_lpinA = self.lValueA
self.rValueA = GPIO.input(self.rightPinA)
if self.prev_rpinA != self.rValueA:
self.rValueB = GPIO.input(self.rightPinB)
if self.rValueB != self.rValueA:
self.rticks += 1
else:
self.rticks -= 1
self.prev_rpinA = self.rValueA
except:
def read_Encoder(self, e):
print('Read Encoding starting')
while True:
if e.is_set():
print("Shutting down encoder")
break
a_val = GPIO.input(self.a_pin)
b_val = GPIO.input(self.b_pin)
# print("vals:", a_val, b_val)
if self.b_prev != b_val:
# if self.angle_store_time is None or (time.time() - self.angle_store_time > 0.001):
if a_val != b_val:
self.count = self.count + 1
#print('ccw')
else:
#print('cw')
self.count = self.count - 1
angle = self.count * (360 / 512)
print(angle, time.time())
self.store_angle(angle)
time.sleep(0.001)
self.a_prev = a_val
self.b_prev = b_val
def readEncoder():
global a_prev
global b_prev
global count
while True:
a_val = GPIO.input(a_pin)
b_val = GPIO.input(b_pin)
# print("vals:", a_val, b_val)
if b_prev != b_val:
if (a_val != b_val):
count = count + 1
print('ccw')
else:
print('cw')
count = count - 1
print(count * (360 / 1024), "deg time:", time.time())
degrees.append( (count * (360 / 1024), time.time()) )
a_prev = a_val
b_prev = b_val
def get_measurement(self):
"""
Needs refactoring, ideally should support interrupts.
"""
GPIO.output(self.trigger_pin, True)
time.sleep(0.00001)
GPIO.output(self.trigger_pin, False)
timeout_start = time.time()
StartTime = time.time()
StopTime = time.time()
while GPIO.input(self.echo_pin) == 0:
StartTime = time.time()
if StartTime - timeout_start > 0.5:
break
while GPIO.input(self.echo_pin) == 1:
StopTime = time.time()
if StartTime - timeout_start > 0.5:
break
TimeElapsed = StopTime - StartTime
return (TimeElapsed * 34300) / 2
def otp_check():
GPIO.setmode(GPIO.BOARD)
GPIO.setup(33, GPIO.IN)
GPIO.setup(35, GPIO.IN)
GPIO.setup(37, GPIO.OUT)
count = 0
result = 0
GPIO.output(37, GPIO.HIGH)
time.sleep(0.1)
GPIO.output(37, GPIO.LOW)
while (1):
if GPIO.input(33) == 1:
result = "1"
time.sleep(0.5)
break
elif GPIO.input(35) == 1:
result = "0"
time.sleep(0.5)
break
return result
def distance(GPIO_TRIGGER, GPIO_ECHO, sensor_number):
# set Trigger to HIGH
GPIO.output(GPIO_TRIGGER, True)
# set Trigger after 0.01ms to LOW
time.sleep(0.00001)
GPIO.output(GPIO_TRIGGER, False)
StartTime = time.time()
StopTime = time.time()
# save StartTime
while GPIO.input(GPIO_ECHO) == 0:
StartTime = time.time()
# save time of arrival
while GPIO.input(GPIO_ECHO) == 1:
StopTime = time.time()
# time difference between start and arrival
TimeElapsed = StopTime - StartTime
# multiply with the sonic speed (34300 cm/s)
# and divide by 2, because there and back
distance = (TimeElapsed * 34300) / 2
# print ("Measured Distance = {} cm, Sensor {}".format(distance, sensor_number))
return distance
def wait(pins, counter):
print("wait")
while True:
if GPIO.input(pins[0]) == GPIO.HIGH and GPIO.input(
pins[1]) == GPIO.HIGH:
case = 2
break
def main():
# Pin Setup:
# Board pin-numbering scheme
GPIO.setmode(GPIO.BOARD)
# set pin as an output pin with optional initial state of LOW
GPIO.setup(trig_output_pin, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(echo_input_pin, GPIO.IN)
#value = GPIO.input(echo_input_pin)
#print("Value read from pin {} : {}".format(echo_input_pin,value_str))
print("Starting Measure now! Press CTRL+C to exit")
try:
while True:
# Toggle the output every second
GPIO.output(trig_output_pin, GPIO.HIGH)
time.sleep(0.00001)
GPIO.output(trig_output_pin, GPIO.LOW)
pulse_start = time.time()
while GPIO.input(echo_input_pin)==0:
pulse_start = time.time()
pulse_end = time.time()
while GPIO.input(echo_input_pin)==1:
pulse_end = time.time()
pulse_duration = pulse_end - pulse_start
distance = pulse_duration * 17150
distance = round(distance, 2)
print ("Distance" , distance)
finally:
GPIO.cleanup()
def call_sonar(TRIG, ECHO):
GPIO.setmode(GPIO.BOARD)
GPIO.setup(TRIG,GPIO.OUT)
GPIO.setup(ECHO,GPIO.IN)
GPIO.output(TRIG, GPIO.LOW)
GPIO.output(TRIG,GPIO.HIGH)
time.sleep(0.00001)
GPIO.output(TRIG, GPIO.LOW)
pulse_start = time.time()
pulse_end = time.time()
count = 0
flag = 0
while GPIO.input(ECHO)==0:
pulse_start = time.time()
count = count + 1
if count >= 5:
print ("I break")
flag = 1
break
while GPIO.input(ECHO)==1:
pulse_end = time.time()
pulse_duration = pulse_end - pulse_start
distance = pulse_duration * 17150
distance = round(distance,2)
if flag == 1 or distance < 0:
distance = 0
return distance
def cote():
if (GPIO.input(11) == 1): # TODO: Validé
print("Jaune")
return True
if (GPIO.input(11) == 0): # TODO: Validé
print("Violet")
return False
def _echo_ready(self, echo):
if echo in self._pairs_list_pin:
if self._invert_echo_pin:
return GPIO.input(self._pairs_list_pin[echo][1])
return not GPIO.input(self._pairs_list_pin[echo][1])
return False
def main():
GPIO.setmode(GPIO.BOARD)
inputPin1 = 11
inputPin2 = 12
GPIO.setup(inputPin1, GPIO.IN)
GPIO.setup(inputPin2, GPIO.IN)
while True:
x1 = GPIO.input(inputPin1)
x2 = GPIO.input(inputPin2)
print(x1, " ", x2)
time.sleep(0.1)
def rot_state(self):
""" Update the current state and count state transitions """
pin_1_state = GPIO.input(self.PIN_1)
pin_2_state = GPIO.input(self.PIN_2)
next_state = (pin_1_state, pin_2_state)
prev_state = self.curr_state
if self.curr_state != next_state:
self.count += 1
self.curr_state = next_state
return prev_state
def high_high(pins, counter):
print(counter)
while True:
if GPIO.input(pins[0]) == GPIO.LOW and GPIO.input(
pins[1]) == GPIO.HIGH:
counter = counter + 1
break
elif GPIO.input(pins[0]) == GPIO.HIGH and GPIO.input(
pins[1]) == GPIO.LOW:
counter = counter - 1
break
case = 1
return case, counter
def rot_state(self):
""" Update the current state and count state transitions """
pin_1_state = GPIO.input(self.PIN_1)
pin_2_state = GPIO.input(self.PIN_2)
next_state = (pin_1_state, pin_2_state)
prev_state = self.curr_state
if self.curr_state != next_state:
self.count += 1
self.curr_state = next_state
if clockwise_state_transitions[prev_state] == self.curr_state:
self.dir = CLOCKWISE
elif counterclockwise_state_transitions[prev_state] == self.curr_state:
self.dir = COUNTERCLOCKWISE
def setup(self):
""" Initialize GPIO pins and rotary encoder state """
GPIO.setmode(GPIO.BOARD)
# These pins will need external pullups set up
GPIO.setup(self.PIN_1, GPIO.IN)
GPIO.setup(self.PIN_2, GPIO.IN)
GPIO.setup(self.BUTTON_PIN, GPIO.IN)
pin_1_state = GPIO.input(self.PIN_1)
pin_2_state = GPIO.input(self.PIN_2)
self.curr_state = (pin_1_state, pin_2_state)
self.count = 0
def read(self):
while not self.is_ready():
# print("WAITING")
pass
dataBits = [
self.createBoolList(),
self.createBoolList(),
self.createBoolList()
]
dataBytes = [0x0] * 4
for j in range(self.byte_range_values[0], self.byte_range_values[1],
self.byte_range_values[2]):
for i in range(self.bit_range_values[0], self.bit_range_values[1],
self.bit_range_values[2]):
GPIO.output(self.PD_SCK, True)
dataBits[j][i] = GPIO.input(self.DOUT)
GPIO.output(self.PD_SCK, False)
dataBytes[j] = numpy.packbits(numpy.uint8(dataBits[j]))
# set channel and gain factor for next reading
for i in range(self.GAIN):
GPIO.output(self.PD_SCK, True)
GPIO.output(self.PD_SCK, False)
# check for all 1
# if all(item is True for item in dataBits[0]):
# return long(self.lastVal)
dataBytes[2] ^= 0x80
return dataBytes
def read_angle(channel):
timestamps.append(time.time())
if (GPIO.input(DIR_PIN)):
steps.append(steps[-1] - 1)
else:
steps.append(steps[-1] + 1)
def __init__(self, is_right, period = 0.20, sensor_state_manager = None, sensor_key = ''):
global RIGHT_CHANNEL
global LEFT_CHANNEL
if GPIO.getmode() != GPIO.TEGRA_SOC:
GPIO.setmode(GPIO.TEGRA_SOC) # Initialization of GPIO pin identification mode
self.period = period # Sampling time period (each measurement is taken at every <period>). Default set to 1 millisecond
self.current_time = 0.0 # Used to store current time in UTC ms from epoch (1/1/1970 00:00)
self.channel = RIGHT_CHANNEL # Left or right channel identification for left/right wheel
self.he_value = 0.0
#Measured value is instantaneous angular velocity (approximation)
self.measured_value = [0.0, 0.0, 0.0]
if is_right:
GPIO.setup(self.channel, GPIO.IN)
else:
self.channel = LEFT_CHANNEL
GPIO.setup(self.channel, GPIO.IN)
self.he_value = GPIO.input(self.channel) #Initialize hall effect sensor value
self.is_ready = True
self.sensor_key = sensor_key
self.sensor_state_manager = sensor_state_manager
def sense(self, is_running):
tick_count = 0 # Initialize tick_count to 0
deltaT = 0.0
total_time = 0
self.current_time = time.time()
while True: # Infinitely loops and measures angular velocity
if not is_running(): # is_running() is a boolean but is passed as a lambda when the thread is created
break
input = GPIO.input(self.channel) # Read new input from GPIO
if self.he_value != input: # If the input is different (which means that the he sensor picked up a different magnet)
self.he_value = input # Change state of he sensor value
tick_count += 1 # Add 1 to tick count
#print(self.channel, ' tick: ', tick_count)
now = time.time()
deltaT += now - self.current_time # Calculate time from previous loop and add to deltaT
if deltaT >= self.period: # If enough time passed by for one period
total_time += deltaT
self.measured_value = Measured_Value(self.sensor_key, [(tick_count)/ (total_time), tick_count], deltaT) # [key, [rad/s, ticks], deltaT]
#tick_count = 0 # Reset tick_count
deltaT = 0 # Reset deltaT
self.callback()
self.current_time = now
def main():
# Pin Setup:
GPIO.setmode(GPIO.BOARD) # BOARD pin-numbering scheme
# GPIO.setup([led_pin_1, led_pin_2], GPIO.IN) # LED pins set as output
GPIO.setup(12, GPIO.IN) # button pin set as input
#GPIO.setup(led_pin_2, GPIO.IN)
# Initial state for LEDs:
# GPIO.output(led_pin_1, GPIO.LOW)
# GPIO.output(led_pin_2, GPIO.LOW)
GPIO.add_event_detect(12, GPIO.RISING, callback=blink1, bouncetime=10)
#GPIO.add_event_detect(led_pin_2, GPIO.RISING, callback=blink2, bouncetime=10)
print("Starting demo now! Press CTRL+C to exit")
try:
while True:
input1 = GPIO.input(led_pin_1)
#input2 = GPIO.input(led_pin_2)
# blink LED 1 slowly
print("Value read from pin {} : {}\n".format(input1, ' pin_1 low'))
time.sleep(2)
#print("Value read from pin {} : {}\n".format(input2, '2down'))
# time.sleep(1)
finally:
GPIO.cleanup() # cleanup all GPIOs
def disMeasure():
dis = None
pulse_start = None
pulse_end = None
GPIO.output(WAVE_TRIG, True)
sleep(0.00001)
GPIO.output(WAVE_TRIG, False)
timeout = time()
while GPIO.input(WAVE_ECHO) == 0:
pulse_start = time()
sleep(0.01)
if pulse_start != None:
if ((pulse_start - timeout) * 1000000) >= WAVE_MAX_DURATION:
dis = -1
break
else:
dis = -1
break
if dis == -1:
return dis
timeout = time()
while GPIO.input(WAVE_ECHO) == 1:
pulse_end = time()
sleep(0.01)
if pulse_end != None:
if ((pulse_end - pulse_start) * 1000000) >= WAVE_MAX_DURATION:
dis = -1
break
else:
dis = -1
break
if dis == -1:
return dis
if pulse_start != None and pulse_end != None:
sleep(0.01)
pulse_duration = (pulse_end - pulse_start) * 1000000
dis = distanceCM(pulse_duration)
dis = round(dis, 2)
else:
dis = -1
return dis
def getDistance():
# set TriggerA to HIGH
GPIO.output(GPIO_TRIGGER_A, True)
# set TriggerA after 0.01ms to LOW
time.sleep(0.00001)
GPIO.output(GPIO_TRIGGER_A, False)
start_time = time.time()
stop_time = time.time()
# save start_time
while GPIO.input(GPIO_ECHO_A) == 0:
start_time = time.time()
# save time of arrival
while GPIO.input(GPIO_ECHO_A) == 1:
stop_time = time.time()
# time difference between start and arrival
time_elapsed = stop_time - start_time
# multiply with the sonic speed (34300 cm/s)
# and divide by 2, because there and back
distance_a = (time_elapsed * 34300) / 2
# set TriggerB to HIGH
GPIO.output(GPIO_TRIGGER_B, True)
# set TriggerB after 0.01ms to LOW
time.sleep(0.00001)
GPIO.output(GPIO_TRIGGER_B, False)
# save start_time
while GPIO.input(GPIO_ECHO_B) == 0:
start_time = time.time()
# save time of arrival
while GPIO.input(GPIO_ECHO_B) == 1:
stop_time = time.time()
# time difference between start and arrival
time_elapsed = stop_time - start_time
# multiply with the sonic speed (34300 cm/s)
# and divide by 2, because there and back
distance_b = (time_elapsed * 34300) / 2
return distance_a, distance_b
def start(self) -> None:
"""Starts infinate loop listening to button click. When clicked, it changes the active artwork."""
while True:
input_state = GPIO.input(self.GPIO_pinout)
if (input_state
== False) and (not self._is_false_negative_click()):
self._change_active_artwork()
time.sleep(self.loop_sleep_sec)
def face_identification(models):
cap = cv2.VideoCapture(gstreamer_pipeline(), cv2.CAP_GSTREAMER)
if cap.isOpened():
cv2.namedWindow("Face Identification", cv2.WINDOW_AUTOSIZE)
while cv2.getWindowProperty("Face Identification", 0) >= 0:
ret, img = cap.read()
# gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# blur = cv2.GaussianBlur(gray, (5,5), 0)
# faces = face_cascade.detectMultiScale(blur, 1.3, 5)
# for(x, y, w, h) in faces:
# cv2.rectangle(img, (x, y), (x+w, y+h), (0, 255, 255), 1)
# roi_gray = gray[y : y+h, x : x+w]
# roi_color = img[y : y+h, x : x+w]
# PIR 센서를 통해서 생물체를 인식 및 카메라를 통해서 읽어온 이미지를 얼굴로 인식할 경우
face = face_detector(img)
if GPIO.input(PIR) == GPIO.HIGH and face is not None:
face = cv2.cvtColor(face, cv2.COLOR_BGR2GRAY) # crop한 이미지를 회색 배경으로 바꿔서 저장
# 학습한 모델로 예측 시도
min_score = 999
min_score_name = ""
for key, model in models.items():
result = model.predict(face)
if min_score > result[1]:
min_score = result[1]
min_score_name = key
# result[1]는 신뢰도를 뜻하며, 0에 가까울수록 본인과 일치함을 의미
if min_score < 500:
confidence = int(100*(1-(result[1])/300))
display_string = str(confidence)+'% Confidence it is '+min_score_name
cv2.putText(img, display_string, (175, 100), cv2.FONT_HERSHEY_COMPLEX, 1, (255, 0, 0), 2)
# confidence에 따라서 등록된 사용자인지 외부인인지 판별하여 이미지를 저장
current = datetime.datetime.now()
current_datetime = current.strftime('%Y-%m-%d_%H:%M:%S_')
if confidence >= 80:
cv2.putText(img, 'User:'******'User:'******'.jpg', img) # 등록된 사용자로 인식될 경우에 대한 이미지 파일 저장
cv2.imwrite(face_dirs+min_score_name+'/'+current_datetime+'User:'******'.jpg', face) # 촬영된 이미지를 재활용하여 학습 데이터 생성
print("User:"******"Unknown User")
cv2.imshow("Face Identification", img)
keyCode = cv2.waitKey(1) & 0xFF
if keyCode == 27:
break
cap.release()
cv2.destroyAllWindows()
else:
print("Unable to open camera")
def distance(measure='cm'):
det1 = False
det2 = False
try:
gpio.setmode(gpio.BOARD)
gpio.setup(11, gpio.OUT)
gpio.setup(13, gpio.IN)
gpio.output(11, gpio.HIGH)
time.sleep(0.00001)
gpio.output(11, gpio.LOW)
while gpio.input(13) == False:
if det1 == False:
t1 = time.time()
det1 = True
# print(t1)
nosig = time.time()
if ((nosig - t1) > 0.1):
print("break 당했습니다.")
break
while gpio.input(13) == True:
if ~det2:
t2 = time.time()
det2 = True
sig = time.time()
# if(sig-t2>2):
# break
# print(sig - t2)
tl = sig - nosig
if measure == 'cm':
distance = round((tl / 0.000058), 2)
else:
print('improper choice of measurement: cm')
distance = None
gpio.cleanup()
return distance
except:
distance = 100
gpio.cleanup()
return distance
def obstacle_dis(Trig_OP, Echo_IP):
GPIO.output(Trig_OP, GPIO.HIGH)
time.sleep(0.00001)
# Set trigger to Low - 0
GPIO.output(Trig_OP, GPIO.LOW)
# Check for the Echo pulse and get the Start and End Time stamps
Start = 0
Stop = 0
while GPIO.input(Echo_IP) != 1:
Start = time.time()
while GPIO.input(Echo_IP) != 0:
Stop = time.time()
# Calculate pulse length
Tot_Time = Stop - Start
# Distance pulse travelled = (Total time * speed of sound )/2 (cm/s)
distance = int(Tot_Time * 17150) # distance in one direction
#print ("Distance =", distance)
return distance
def check(self):
"""
checks the input of all the pins
Parameters
----------
parameter: variable type
parameter description
Returns
-------
return variable:variable type
parameter description
"""
red = GPIO.input(self.red)
green = GPIO.input(self.green)
blue = GPIO.input(self.blue)
return (red, green, blue)
def distance_to_spitball():
distance = 0
GPIO.output(self.trig_output_pin, GPIO.HIGH)
time.sleep(0.00001)
GPIO.output(self.trig_output_pin, GPIO.LOW)
pulse_start = time.time()
while GPIO.input(self.echo_input_pin) == 0:
pulse_start = time.time()
pulse_end = time.time()
while GPIO.input(self.echo_input_pin) == 1:
pulse_end = time.time()
pulse_duration = pulse_end - pulse_start
distance = pulse_duration * 17150
distance = round(distance, 2)
return distance
def Get_Dist_Vel(previous_distance, elapsed_time):
# Send Trigger
GPIO.output(TRIG, True)
time.sleep(0.00001)
GPIO.output(TRIG, False)
# Read Echo
while GPIO.input(ECHO) == 0:
pulse_start = time.time()
while GPIO.input(ECHO) ==1:
pulse_end = time.time()
# Calculate Distance from Pulse Duration
pulse_duration = pulse_end - pulse_start
distance = (pulse_duration * 17150)/100 # m