def Calc_SNR(noise_var,start_task,end_task,start_signal,end_signal,segment_size,segment_shift):
Seg_nums = int((end_signal - start_signal-segment_size)/segment_shift) +1
task_snr=np.empty((end_task,Seg_nums+1))
for taskN in range(start_task,end_task):
eeg_raw=np.loadtxt('Experiments/Task'+str(taskN+1)+'.csv')
FilterMains = IIR2Filter(10,[45,55],'bandstop',design='cheby1',rp=2,fs = fsampl)
eeg_cl_raw = np.zeros(len(eeg_raw)) #Initialize array to store the filtered samples
for i in range(len(eeg_raw)):
eeg_cl_raw[i] = FilterMains.filter(eeg_raw[i])
eeg_cl_alpha = np.zeros(len(eeg_cl_raw)) #Initialize array to store the filtered samples
FilterAlpha = IIR2Filter(10,[7,13],'bandpass',design='butter',rp=2,fs = fsampl)
for i in range(len(eeg_cl_alpha)):
eeg_cl_alpha[i] = FilterAlpha.filter(eeg_cl_raw[i])
# The following 3 lines are used when the SNR is computed using the noise for each experiment
#eeg_noise= eeg_cl_alpha[1500:2500]
#noise_var=np.var(eeg_noise)
#print("SNR var: ",noise_var,"\n")
eeg_signal = eeg_cl_alpha[start_signal:end_signal]
all_var = np.var(eeg_signal)
SNR = 10 * math.log10(all_var/noise_var)
task_snr[taskN,0]=SNR
for segmnt in range(0,Seg_nums):
seg_start = start_signal + segmnt*segment_size
seg_end = seg_start + segment_size
eeg_signal = eeg_cl_alpha[seg_start:seg_end]
seg_var = np.var(eeg_signal) #eeg_cl_alpha[seg_start:seg_end])
SNR = 10 * math.log10(seg_var/noise_var)
task_snr[taskN,segmnt+1]= SNR
return task_snr
def __init__(self, sos: np.ndarray):
self.sos_filters = []
# get 2nd order filters
for index in range(sos.shape[0]):
ba = sos[index]
filter_tmp = IIR2Filter(ba[0], ba[1], ba[2], ba[3], ba[4], ba[5])
self.sos_filters.append(filter_tmp)
def bandPass(self, order, ripple, cut_off1, cut_off2, sampling_frequency):
python_freq1 = cut_off1 / sampling_frequency
python_freq2 = cut_off2 / sampling_frequency
self.sos = signal.cheby1(order, ripple, [python_freq1*2, python_freq2*2], 'bandpass', output='sos')
self.filter_list = []
for filt in self.sos:
self.filter_list.append(IIR2Filter(filt))
def __init__(self,
sos=signal.butter(2, Fc * 2 / samplingRate, output='sos')):
sos = sos.T
self.b = sos[:3]
self.a = sos[-3:]
self.IIR2 = IIR2Filter(self.b[0], self.b[1], self.b[2], self.a[0],
self.a[1], self.a[2])
def bandStop(self, order, cut_off1, cut_off2, sampling_frequency):
python_freq1 = cut_off1 / sampling_frequency
python_freq2 = cut_off2 / sampling_frequency
self.sos = signal.bessel(order, [python_freq1*2, python_freq2*2], 'bandstop', output='sos')
self.filter_list = []
for filt in self.sos:
self.filter_list.append(IIR2Filter(filt))
def Noise_var_all(start_task,end_task,start_noise,end_noise):
task_noiseVar=np.zeros(end_task)
for taskN in range(start_task,end_task):
eeg_raw=np.loadtxt('Experiments/Task'+str(taskN+1)+'.csv')
FilterMains = IIR2Filter(10,[45,55],'bandstop',design='cheby1',rp=2,fs = fsampl)
eeg_cl_raw = np.zeros(len(eeg_raw)) #Initialize array to store the filtered samples
for i in range(len(eeg_raw)):
eeg_cl_raw[i] = FilterMains.filter(eeg_raw[i])
eeg_cl_alpha = np.zeros(len(eeg_cl_raw)) #Initialize array to store the filtered samples
FilterAlpha = IIR2Filter(10,[7,13],'bandpass',design='butter',rp=2,fs = fsampl)
for i in range(len(eeg_cl_alpha)):
eeg_cl_alpha[i] = FilterAlpha.filter(eeg_cl_raw[i])
eeg_noise=eeg_cl_alpha[start_noise:end_noise]
Nvar=np.var(eeg_noise)
task_noiseVar[taskN]=Nvar
print("Task", taskN+1, "Noise Variance = ", Nvar,"\n")
return task_noiseVar
def highPass(self, order, cut_off, sampling_frequency):
python_freq = cut_off / sampling_frequency
self.sos = signal.butter(order,
python_freq * 2,
'highpass',
output='sos')
self.filter_list = []
for filt in self.sos:
self.filter_list.append(IIR2Filter(filt))
def lowPass(self, order, attenuation, cut_off, sampling_frequency):
python_freq = cut_off / sampling_frequency
self.sos = signal.cheby2(order,
attenuation,
python_freq * 2,
'lowpass',
output='sos')
self.filter_list = []
for filt in self.sos:
self.filter_list.append(IIR2Filter(filt))
def __init__(self):
global pid
continuous_threading.PausableThread.__init__(self)
#---------------------------------Inicializacion-----------------------------------------------------
# Create the I2C bus
self.i2c = busio.I2C(board.SCL, board.SDA)
# Create the ADC object using the I2C bus
self.ads = ADS.ADS1115(self.i2c)
# Create single-ended input on channel 0
self.ch0 = AnalogIn(self.ads, ADS.P0)
self.ch3 = AnalogIn(self.ads, ADS.P3)
self.ch2 = AnalogIn(self.ads, ADS.P2)
self.ads.gain = 2 / 3
self.ads.data_rate = 860
# Create Current and Voltage Filters
self.VolFilter = IIR2Filter(2, [5], 'lowpass', 'butter', fs=1000)
self.CurFilter = IIR2Filter(2, [200], 'lowpass', 'butter', fs=1000)
#--------------------------------------------------------------------------------------------------
self.starttime = time.time()
#-----------------------------------------PID SETUP-----------------------------------------------
#pid = PID(0.55,0.9,0.005)
pid.SetPoint = 20
pid.setSampleTime(0.001)
self.pidmin = 0
self.pidmax = 5
# -----------------------------------------------------------------------------------------------
self.voltajedac = 0
DAC.set_voltage(self.voltajedac)
self.i = 0
self.stoptime = 0
def run(self):
global DAC, WPt, WPpw, mode
#---------------------------------Inicializacion-----------------------------------------------------
# Create the I2C bus
i2c = busio.I2C(board.SCL, board.SDA)
# Create the ADC object using the I2C bus
ads = ADS.ADS1115(i2c)
# Create single-ended input on channel 0
ch0 = AnalogIn(ads, ADS.P0)
ch3 = AnalogIn(ads, ADS.P3)
ch2 = AnalogIn(ads, ADS.P2)
# Create differential input between channel 0 and 1
dif01 = AnalogIn(ads, ADS.P0, ADS.P1)
dif23 = AnalogIn(ads, ADS.P2, ADS.P3)
ads.gain = 2 / 3
ads.data_rate = 860
# Create Current and Voltage Filters
VolFilter = IIR2Filter(2, [5], 'lowpass', 'butter', fs=1000)
CurFilter = IIR2Filter(2, [200], 'lowpass', 'butter', fs=1000)
#--------------------------------------------------------------------------------------------------
start = time.time()
#-----------------------------------------PID SETUP-----------------------------------------------
#pid = PID(0.55,0.9,0.005)
pid = PID(0.55, 1, 0.01)
pid.SetPoint = 20
pid.setSampleTime(0.001)
feedback = 0
feedback_list = []
time_list = []
pidmin = 0
pidmax = 5
# -----------------------------------------------------------------------------------------------
voltajedac = 0
DAC.set_voltage(voltajedac)
i = 0
#------------------------------------- MAIN LOOP--------------------------------------------------
for i in range(500):
try:
Current = ch0.voltage
Voltage = ch3.voltage
#-----------------------------------------IRR FILTER----------------------------------------------
DataVoltage.append(VolFilter.filter(Voltage))
DataCurrent.append(CurFilter.filter(Current))
#-------------------------------------------------------------------------------------------------
#-------------------------------------------------------------------------------------------------
timenow = (time.time() - start)
t.append(timenow)
mode = q.get()
# if mode == 'Test':
# if (timenow > 0 and timenow < 15):
# pid.SetPoint=20
# elif (timenow > 15 and timenow < 30):
# pid.SetPoint=30
# elif (timenow > 30 ):
# pid.SetPoint=10
# q.put('Test')
# elif mode == 'Perfil':
# for j in range(len(WPt)-1):
# if (timenow > WPt[j] and timenow < WPt[j+1]):
# pid.SetPoint=WPpw[j]
# q.put('Perfil')
pid.SetPoint = 10
#--------------------------------Para graficar la consigna-----------------------------------------
if i == 0:
consigna.append(0)
else:
consigna.append(pid.SetPoint)
#-------------------------------------------------------------------------------------------------
DataVoltage[i] = DataVoltage[i] * 9.5853 - 0.1082
DataCurrent[i] = DataCurrent[i] * 1.4089 + 0.1326
DataPower.append(DataVoltage[i] * DataCurrent[i])
# --------------------------------------- PID CONTROLLER------------------------------------------
pid.update(DataPower[i])
output = pid.output
if pid.SetPoint > 0:
voltajedac = voltajedac + (output - (1 / (i + 1)))
if voltajedac < pidmin:
voltajedac = pidmin
elif voltajedac > pidmax:
voltajedac = pidmax
# ---------------------------------------------DAC------------------------------------------------
voltbits = int((4096 / 5) * voltajedac)
DAC.set_voltage(voltbits)
# ------------------------------------------------------------------------------------------------
#i = i+1
except IOError:
print('IOError')
ecg = data[:,1]
x = data[:,0]
# reducing the signal to remove amplifier gain
ecg = ecg/ampGain #ecg amplitude in mVs
rangeEcg = max(ecg)-min(ecg)
ecg = ecg-(min(ecg)+rangeEcg/2)
ecg = ecg[0:10000]
x = x[0:10000]
#---------------------------------Filtering-----------------------------
# Frequencies are given as Hz, hence they need to be normalised (fs required)
FilterMains = IIR2Filter(3,[45,55],'bandstop',design='cheby1',rp=0.01,fs=fs)
# Creates a 10th order highpass Butterworth filter with normalised cutoff
# frequency 0.002 (=1 Hz with a 1000 Hz sampling frequency).
FilterDC = IIR2Filter(4,0.002,'highpass')
ecgFiltered = np.zeros(len(ecg))
for i in range(len(ecg)):
ecgFiltered[i] = FilterMains.filter(ecg[i])
ecgFiltered[i] = FilterDC.filter(ecgFiltered[i])
#-------------------------------Plotting---------------------------------
plt.figure(1)
plt.plot(x,ecg)
plt.xlabel("Time [ms]")
# https://github.com/poganyg/IIR-filter
import matplotlib.pyplot as plt
import numpy as np
from IIR2Filter import IIR2Filter
from getDataAndLabels1Subj1 import getDataAndLabels, channelsSamplesTrialKernels, getConfusionMatrixNames, getNumClasses
[data, labels] = getDataAndLabels()
fs = 200
FilterMains = IIR2Filter(3, [0.5, 40], 'bandpass', fs=231)
# impulse = np.zeros(1000)
# impulse[0] = 1
# impulseResponse = np.zeros(len(impulse))
impulseResponse = data[0]
for i in range(len(impulseResponse)):
for j in range(len(impulseResponse[i])):
impulseResponse[i][j] = FilterMains.filter(impulseResponse[i][j])
# To obtain the frequency response from the impulse response the Fourier
# transform of the impulse response has to be taken. As it produces
# a mirrored frequency spectrum, it is enough to plot the first half of it.
freqResponse = np.fft.fft(impulseResponse)
freqResponse = abs(freqResponse[0:int(len(freqResponse) / 2)])
xfF = np.linspace(0, fs / 2, len(freqResponse))
plt.figure("Frequency Response")
plt.plot(xfF, np.real(freqResponse))
plt.xlabel("Frequency [Hz]")
def run(self):
global DAC, WPt, WPpw, mode, actpow, qsetpoint
#---------------------------------Inicializacion-----------------------------------------------------
# Create the I2C bus
i2c = busio.I2C(board.SCL, board.SDA)
# Create the ADC object using the I2C bus
ads = ADS.ADS1115(i2c)
# Create single-ended input on channel 0
ch0 = AnalogIn(ads, ADS.P0)
ch3 = AnalogIn(ads, ADS.P3)
ch2 = AnalogIn(ads, ADS.P2)
# Create differential input between channel 0 and 1
dif01 = AnalogIn(ads, ADS.P0, ADS.P1)
dif23 = AnalogIn(ads, ADS.P2, ADS.P3)
ads.gain = 2/3
ads.data_rate = 860
# Create Current and Voltage Filters
VolFilter = IIR2Filter(1,[5],'lowpass','butter',fs=1000)
CurFilter = IIR2Filter(1,[200],'lowpass','butter',fs=1000)
#--------------------------------------------------------------------------------------------------
#-----------------------------------------PID SETUP-----------------------------------------------
#pid = PID(0.55,0.9,0.005)
#pid = PID(0.55,1,0.01)
if (qkp.empty()==False):
kp = qkp.get()
qkp.put(kp)
else:
kp = 0.55
if (qki.empty()==False):
ki = qki.get()
qki.put(ki)
else:
ki = 1
if (qkd.empty()==False):
kd = qkd.get()
qkd.put(kd)
else:
kd = 0.01
pid = PID(kp,ki,kd)
npid.set("PID = {0}, {1}, {2}".format(str(pid.getKp()),str(pid.getKi()),str(pid.getKd())))
print('PID = {0},{1},{2}'.format(pid.getKp(),pid.getKi(),pid.getKd()))
time.sleep(1)
#--------------------------------------------------------------------------------------------------
pid.SetPoint=20
pid.setSampleTime(0.001)
feedback = 0
feedback_list = []
time_list = []
pidmin = 0
pidmax = 5
# -----------------------------------------------------------------------------------------------
voltajedac = 0
DAC.set_voltage(voltajedac)
i=0;
c1 = 0
c2 = 0
c3 = 0
c4 = 0
iniciando.set('')
starttime = time.time()
self.powermeter.start()
#------------------------------------- MAIN LOOP--------------------------------------------------
#for i in range(2000):
while True:
try:
Current = ch0.voltage
Voltage = ch3.voltage
#-----------------------------------------IRR FILTER----------------------------------------------
DataVoltage.append(VolFilter.filter(Voltage))
DataCurrent.append(CurFilter.filter(Current))
#-------------------------------------------Tiempo------------------------------------------------
timenow=(time.time()-starttime)
t.append(timenow)
#-------------------------------------------------------------------------------------------------
mode=q.get()
if mode == 'Test':
if (timenow > 0 and timenow < 15):
pid.SetPoint=20
elif (timenow > 15 and timenow < 30):
pid.SetPoint=30
elif (timenow > 30 ):
pid.SetPoint=10
q.put('Test')
elif mode == 'Perfil':
for j in range(len(WPt)-1):
if (timenow > WPt[j] and timenow < WPt[j+1]):
pid.SetPoint=WPpw[j]
q.put('Perfil')
elif mode == 'Manual':
if(qsetpoint.empty()==False):
pid.SetPoint=float(qsetpoint.get())
q.put('Manual')
#------------------------------Prueba aleatoria---------------------------------------------------
# if (timenow > 0 and timenow < 10):
# c1=c1+1
# if(c1 == 1):
# rand = round(random.uniform(10,35),2)
# pid.SetPoint = rand
# elif (timenow > 10 and timenow < 20):
# c2=c2+1
# if(c2 == 1):
# rand = round(random.uniform(10,35),2)
# pid.SetPoint = rand
# elif (timenow > 20 and timenow < 30):
# c3=c3+1
# if(c3 == 1):
# rand = round(random.uniform(10,35),2)
# pid.SetPoint = rand
# elif (timenow > 30 and timenow < 40):
# c4=c4+1
# if(c4 == 1):
# rand = round(random.uniform(10,35),2)
# pid.SetPoint = rand
# elif (timenow > 40):
# t.pop()
# break
#--------------------------------Para graficar la consigna-----------------------------------------
consigna.append(pid.SetPoint)
#-------------------------------------------------------------------------------------------------
DataVoltage[i]=DataVoltage[i]*9.5853-0.1082
DataCurrent[i]=DataCurrent[i]*1.4089+0.1326
DataPower.append(DataVoltage[i]*DataCurrent[i])
qpower.put(DataPower[i])
# --------------------------------------- PID CONTROLLER------------------------------------------
pid.update(DataPower[i])
output = pid.output
if pid.SetPoint > 0:
voltajedac = voltajedac + (output - (1/(i+1)))
if voltajedac < pidmin:
voltajedac = pidmin
elif voltajedac > pidmax:
voltajedac = pidmax
# ---------------------------------------------DAC------------------------------------------------
voltbits=int((4096/5)*voltajedac)
DAC.set_voltage(voltbits)
# ------------------------------------------------------------------------------------------------
i = i+1
except IOError:
print('IOError')
def run(self):
global DAC, mode
#---------------------------------Inicializacion-----------------------------------------------------
# Create the I2C bus
i2c = busio.I2C(board.SCL, board.SDA)
# Create the ADC object using the I2C bus
ads = ADS.ADS1115(i2c)
# Create single-ended input on channel 0
ch0 = AnalogIn(ads, ADS.P0)
ch3 = AnalogIn(ads, ADS.P3)
ch2 = AnalogIn(ads, ADS.P2)
# Create differential input between channel 0 and 1
dif01 = AnalogIn(ads, ADS.P0, ADS.P1)
dif23 = AnalogIn(ads, ADS.P2, ADS.P3)
ads.gain = 2 / 3
ads.data_rate = 860
# Create Current and Voltage Filters
VolFilter = IIR2Filter(2, [5], 'lowpass', 'butter', fs=1000)
CurFilter = IIR2Filter(2, [200], 'lowpass', 'butter', fs=1000)
#--------------------------------------------------------------------------------------------------
starttime = time.time()
#-----------------------------------------PID SETUP-----------------------------------------------
#pid = PID(0.55,0.9,0.005)
pid = PID(0.55, 1, 0.01)
pid.SetPoint = 20
pid.setSampleTime(0.001)
feedback = 0
feedback_list = []
time_list = []
pidmin = 0
pidmax = 5
# -----------------------------------------------------------------------------------------------
voltajedac = 0
DAC.set_voltage(voltajedac)
#------------------------------------- MAIN LOOP--------------------------------------------------
while True:
try:
print('aqui')
Current = ch0.voltage
Voltage = ch3.voltage
#-----------------------------------------IRR FILTER----------------------------------------------
DataVoltage.append(VolFilter.filter(Voltage))
DataCurrent.append(CurFilter.filter(Current))
#-------------------------------------------------------------------------------------------------
timenow = (time.time() - starttime)
t.append(timenow)
if mode == 'Test':
if (timenow > 0 and timenow < 15):
pid.SetPoint = 20
elif (timenow > 15 and timenow < 30):
pid.SetPoint = 30
elif (timenow > 30):
pid.SetPoint = 10
consigna.append(pid.SetPoint)
DataVoltage[self.i] = DataVoltage[self.i] * 9.5853 - 0.1082
DataCurrent[self.i] = DataCurrent[self.i] * 1.4089 + 0.1326
DataPower.append(DataVoltage[self.i] * DataCurrent[self.i])
# --------------------------------------- PID CONTROLLER------------------------------------------
pid.update(DataPower[self.i])
output = pid.output
if pid.SetPoint > 0:
voltajedac = voltajedac + (output - (1 / (self.i + 1)))
if voltajedac < pidmin:
voltajedac = pidmin
elif voltajedac > pidmax:
voltajedac = pidmax
# ---------------------------------------------DAC------------------------------------------------
voltbits = int((4096 / 5) * voltajedac)
DAC.set_voltage(voltbits)
# ------------------------------------------------------------------------------------------------
#print("| {0:^5.3f} | {1:^5.3f} | {2:^5.3f} |".format(DataCurrent[i],DataVoltage[i],DataPower[i]))
self.i = self.i + 1
except IOError:
print('IOError')
to increase as this accumulated error is unwound
(offset by errors in the other direction).
The specific problem is the excess overshooting.
"""
self.min = PIDmin
self.max = PIDmax
def setSampleTime(self, sample_time):
"""PID that should be updated at a regular interval.
Based on a pre-determined sampe time, the PID decides if it should compute or return immediately.
"""
self.sample_time = sample_time
#----------------------------------------FILTER SETUP----------------------------------------------
VolFilter = IIR2Filter(2, [5], 'lowpass', 'butter', fs=1000)
CurFilter = IIR2Filter(2, [200], 'lowpass', 'butter', fs=1000)
#--------------------------------------------------------------------------------------------------
filteredVol = []
filteredCur = []
DataVoltage = []
DataCurrent = []
DataPower = []
DataOut = []
t = []
start = time.time()
#-----------------------------------------PID SETUP-----------------------------------------------
#pid = PID(0.55,0.9,0.005)
mask2 = np.fft.fft(data)
plt.figure(2)
plt.title('Raw EEG Signal Spectrum')
plt.xlabel('Frequency (Hz)')
plt.ylabel('Signal Power')
plt.grid(which='both', axis='both')
kf = np.arange(s_size) * fsampl / s_size #Array for spectrum x-axis
plt.plot(kf, abs(mask2) / s_size)
plt.show()
'''
Filter the signal with a 50Hz notch filter
'''
f_signal = np.zeros(s_size) #Initialize array to store the filtered samples
FilterMains = IIR2Filter(10, [45, 55],
'bandstop',
design='cheby1',
rp=2,
fs=fsampl)
for i in range(len(data)):
f_signal[i] = FilterMains.filter(data[i])
"""
Plot the 50Hz filtered EEG signal and its spectrum
"""
plt.figure(3)
plt.title('50Hz Filtered EEG Signal')
plt.xlabel('Time (ms)')
plt.ylabel('Amplitude (μV)')
plt.grid(which='both', axis='both')
plt.plot(ks, f_signal)
plt.show()
mask2 = np.fft.fft(f_signal)
def __init__(self, sos=signal.butter(2, Fc * 2 / samplingRate, 'lowpass')):
self.b = sos[0]
self.a = sos[1]
self.IIR2 = IIR2Filter(self.b[0], self.b[1], self.b[2], self.a[0],
self.a[1], self.a[2])
def __init__(self):
self.start = True
# subscribe to kinect image messages
# qHD = quaterHD(960x540), sd = 512x424,
self.qhd_resolution = [960, 540]
self.sd_resolution = [512, 424]
self.image_type = 1
if self.image_type == 1:
self.sub = rospy.Subscriber("kinect2/qhd/image_mono_rect", Image, self.image_callback, queue_size=1)
elif self.image_type == 2:
self.sub = rospy.Subscriber("kinect2/sd/image_color_rect", Image, self.image_callback, queue_size=1)
self.target_pub = rospy.Publisher("target_point", PointStamped, queue_size = 1)
self.g_pt_arr_pub = rospy.Subscriber("hrvl_gbot/gaze_pt_arr", Image, self.g_pt_arr_callback, queue_size=1)
self.suppress_pts_sub = rospy.Subscriber("hrvl_gbot/g_suppress",Point,self.suppress_pt_callback, queue_size=1 )
self.suppress_pt = Point()
self.suppress_pt.x = 0
self.suppress_pt.y = 0
self.suppress_pt.z = 0
# self.sub_jstate = rospy.Subscriber("hrvl_gbot/joint_states", JointState, self.jstate_callback, queue_size=1)
# TODO: Temporary hard coding (have to read from kinect2 camera info (subscribe once))
self.fxy = [ 515.2761784227258, 515.9218548201856]
self.cxy = [ 478.23045348775975, 275.136887532946]
self.bridge = CvBridge()
self.orb = cv2.ORB_create()
self.cur_image = np.zeros((self.qhd_resolution[1], self.qhd_resolution[0],1), np.uint8)
self.cur_h_image = np.zeros((self.qhd_resolution[1]/2, self.qhd_resolution[0]/2,1), np.uint8)
self.cur_q_image = np.zeros((self.qhd_resolution[1]/4, self.qhd_resolution[0]/4,1), np.uint8)
self.callback_img = np.zeros_like(self.cur_image)
self.prev_h_image = None #= np.zeros((self.qhd_resolution[1]/4, self.qhd_resolution[0]/2,3), np.uint8)
self.cur_g_pt_image = np.zeros((61, 181),np.uint8)
# gaze_control
self.gaze_ang_img = np.zeros((180,60), dtype=np.uint8)
self.IoR_mask = np.zeros_like(self.gaze_ang_img) # inhibition of return.
''' Optical flow '''
self.color = np.random.randint(0,255,(2000,3))
# self.lines = None
self.termcriteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)
self.prevPt = None
self.curPt = None
self.curPt_filtered = None
self.curMv = None
# Image Matching
self.drawing_offset = 10 # drawing offset
self.filling_mask = None
self.key_bmargin = 5
# Blur detection
self.blurry_max = 2500
self.pause_subscribing = False
self.debug_window = 'MouseCallback'
cv2.imshow(self.debug_window, self.cur_image)
cv2.setMouseCallback(self.debug_window, self.onMouse)
## IIR Filter
self.fs = 1000
# self.x_filter = IIR2Filter(3, e[50.0],'lowpass', fs=self.fs)
# self.y_filter = IIR2Filter(3, [50.0],'lowpass', fs=self.fs)
self.x_filter = IIR2Filter(3, [20.0],'lowpass', fs=self.fs)
self.y_filter = IIR2Filter(3, [20.0],'lowpass', fs=self.fs)
# PanTilt control
self.c_index = 0
self.c_index_max = 6
self.init_count = 0
self.init_count_max = 1000
ch3 = AnalogIn(ads, ADS.P3) ch2 = AnalogIn(ads, ADS.P2) # Create differential input between channel 0 and 1 dif01 = AnalogIn(ads, ADS.P0, ADS.P1) dif23 = AnalogIn(ads, ADS.P2, ADS.P3) ads.gain = 2 / 3 ads.data_rate = 860 # Create a DAC instance. DAC = Adafruit_MCP4725.MCP4725(address=0x60, busnum=1) #----------------------------------------FILTER SETUP---------------------------------------------- VolFilter = IIR2Filter(1, [100], 'lowpass', 'butter', fs=1000) CurFilter = IIR2Filter(1, [100], 'lowpass', 'butter', fs=1000) #-------------------------------------------------------------------------------------------------- filteredVol = [] filteredCur = [] DataVoltage = [] DataCurrent = [] DataPower = [] t = [] start = time.time() #-----------------------------------------PID SETUP-----------------------------------------------
def StartEmulator(self):
#---------------------------------Inicializacion-----------------------------------------------------
# Create the I2C bus
i2c = busio.I2C(board.SCL, board.SDA)
# Create the ADC object using the I2C bus
ads = ADS.ADS1115(i2c)
# Create single-ended input on channel 0
ch0 = AnalogIn(ads, ADS.P0)
ch3 = AnalogIn(ads, ADS.P3)
ch2 = AnalogIn(ads, ADS.P2)
# Create differential input between channel 0 and 1
dif01 = AnalogIn(ads, ADS.P0, ADS.P1)
dif23 = AnalogIn(ads, ADS.P2, ADS.P3)
ads.gain = 2/3
ads.data_rate = 860
# Create a DAC instance.
DAC = Adafruit_MCP4725.MCP4725(address=0x60, busnum=1)
# Create Current and Voltage Filters
VolFilter = IIR2Filter(2,[5],'lowpass','butter',fs=1000)
CurFilter = IIR2Filter(2,[200],'lowpass','butter',fs=1000)
#PIDFilter = IIR2Filter(1,[20],'lowpass','butter',fs=1000)
#--------------------------------------------------------------------------------------------------
start = time.time()
#-----------------------------------------PID SETUP-----------------------------------------------
#pid = PID(0.55,0.9,0.005)
pid = PID(0.55,1,0.005)
pid.SetPoint=20
pid.setSampleTime(0.001)
feedback = 0
feedback_list = []
time_list = []
pidmin = 0
pidmax = 5
# -----------------------------------------------------------------------------------------------
voltajedac = 0
DAC.set_voltage(voltajedac)
i=0;
#------------------------------------- MAIN LOOP--------------------------------------------------
while True:
Current = ch0.voltage
Voltage = ch3.voltage
#-----------------------------------------IRR FILTER----------------------------------------------
DataVoltage.append(VolFilter.filter(Voltage))
DataCurrent.append(CurFilter.filter(Current))
#-------------------------------------------------------------------------------------------------
timenow=(time.time()-start)
t.append(timenow)
if (timenow > 0 and timenow < 15):
pid.SetPoint=20
elif (timenow > 15 and timenow < 30):
pid.SetPoint=30
elif (timenow > 30 ):
pid.SetPoint=10
DataVoltage[i]=DataVoltage[i]*9.5853-0.1082
DataCurrent[i]=DataCurrent[i]*1.4089+0.1326
DataPower.append(DataVoltage[i]*DataCurrent[i])
# --------------------------------------- PID CONTROLLER------------------------------------------
pid.update(DataPower[i])
output = pid.output
if pid.SetPoint > 0:
voltajedac = voltajedac + (output - (1/(i+1)))
if voltajedac < pidmin:
voltajedac = pidmin
elif voltajedac > pidmax:
voltajedac = pidmax
# ---------------------------------------------DAC------------------------------------------------
voltbits=int((4096/5)*voltajedac)
DAC.set_voltage(voltbits)
# ------------------------------------------------------------------------------------------------
#print("| {0:^5.3f} | {1:^5.3f} | {2:^5.3f} |".format(DataCurrent[i],DataVoltage[i],DataPower[i]))
i = i+1
plt.figure("gaze x")
plt.plot(t, gaze_xs)
plt.figure("gaze y")
plt.plot(t, gaze_ys)
fs = 200
# freqx = np.linspace(0, 1.0/dt, N)
# Fx = np.fft.fft(gaze_xs)
# Fx = Fx / (N/2)
# Fx[0] = Fx[0] / 2
# plt.figure("fft x")
# plt.plot(freqx, np.abs(Fx))
# FilterMains = IIR2Filter(3, [20,80], 'bandpass', design='butter', fs=200)
FilterX = IIR2Filter(3, [40], 'lowpass', design='butter', fs=200)
FilterY = IIR2Filter(3, [40], 'lowpass', design='butter', fs=200)
for i, gaze_x in enumerate(gaze_xs):
gaze_xs[i] = FilterX.filter(gaze_x)
for i, gaze_y in enumerate(gaze_ys):
gaze_ys[i] = FilterY.filter(gaze_y)
for i, gaze_x in enumerate(gaze_xs):
with open("smoothed_multi_tool_resnet.csv", "a") as f:
np.savetxt(f, np.array([[gaze_x, gaze_ys[i]]]), delimiter=",")
# Fx = np.fft.fft(gaze_xs)
# Fx = Fx / (N/2)
# Fx[0] = Fx[0] / 2
# plt.figure("fft x_after")
# plt.plot(freqx, np.abs(Fx))