#!/usr/bin/env python2
# -*- coding: utf-8 -*-
"""
* @par Copyright (C)
* @file basic_writeAndrRead
* @version V1.0
* @details
* @par History
* @author lc
"""
from __future__ import division
import cv2
import time
import math
import numpy as np
import matplotlib.pyplot as plt
cap=cv2.VideoCapture(0)
#设置摄像头分辨率为（640，480）
#如果感觉图像卡顿严重，可以降低为（320，240）
cap.set(3,480)
cap.set(4,320)
#设置黄色的阈值
yellow_lower=np.array([125,130,104])
yellow_upper=np.array([197,223,255])
time.sleep(1)
kalmanflag=1
#————————————————————————
#参数————————————————————
x_pre=0
y_pre=0
basex=0
basey=0
T=0.00 #周期
#标志
over_base_num=0
obn_allow=False
max_deta_x=0
#masks=np.zeros([320,480],dtype=np.uint8)
#masks[0:320,0:480]=255
time_start=time.time()
while 1:
#ret为是否找到图像， frame是帧本身
ret,frame=cap.read()
frame=cv2.convertScaleAbs(frame,1,1)
#frame=cv2.illuminationChange(frame, img, alpha=0, beta=1)
#cv2.imshow('convertScaleAbs',frame)
'''SSR
size = 3
b_gray, g_gray, r_gray = cv2.split(frame)
b_gray = SSR(b_gray, size)
g_gray = SSR(g_gray, size)
r_gray = SSR(r_gray, size)
result = cv2.merge([b_gray, g_gray, r_gray])
cv2.imshow('SSSR',frame)
'''
frame=cv2.GaussianBlur(frame,(5,5),0) #高斯模糊
#cv2.imshow('GaussianBlur',frame)
#frame = cv2.medianBlur(frame,5)
#cv2.imshow('medianBlur',frame)
hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV) #转hsv
#cv2.imshow('hsv1',hsv)
'''
hsv=cv2.cvtColor(hsv,cv2.COLOR_BGR2HSV) #转hsv
cv2.imshow('hsv2',hsv)
hsv=cv2.GaussianBlur(hsv,(5,5),0) #高斯模糊
cv2.imshow('GaussianBlur_hsv',hsv)
'''
mask=cv2.inRange(hsv,yellow_lower,yellow_upper) #生成掩膜
#cv2.imshow('mask',mask)
#形态学操作
mask=cv2.erode(mask,None,iterations=2)
#cv2.imshow('mask2',mask)
mask=cv2.dilate(mask,None,iterations=2)
#cv2.imshow('mas3k',mask)
mask=cv2.GaussianBlur(mask,(3,3),0)
res=cv2.bitwise_and(frame,frame,mask=mask) #与运算
cnts=cv2.findContours(mask.copy(),cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2] #检测颜色的轮廓
if len(cnts)>0:
cnt = max (cnts,key=cv2.contourArea) #找最大
#print('AREA:::::',)
""" 找合适范围内
"""
x, y, w, h =cv2.boundingRect(cnt)
if kalmanflag!=1:
"""
pos = np.array([x_pre, x], np.float32)
mes = np.reshape(pos,(2,1))
x = kalman.correct(mes)
pos = np.array([y_pre, y], np.float32)
mes = np.reshape(pos,(2,1))
y = kalman.correct(mes)
y_pre=y
x_pre=x
#y = kalman.predict()
"""
#过中线判定
if x<basex+10 and x>basex-10:
if obn_allow:
#print('tttttttt')
over_base_num=over_base_num+1
if over_base_num == 1:
time_start = time.time()
obn_allow=False
if x>basex+20 or x<basex-20:
obn_allow=True
if over_base_num == 1:
deta_x = abs(x - basex)
if deta_x>max_deta_x:
max_deta_x = deta_x
print("max_deta_x----->",max_deta_x)
#if deta_x<=max_deta_x:
# 发送max_deta_x
#5次计算一次周期
if over_base_num==5:
#print('22222222222222')
time_end=time.time()
T=(time_end-time_start)/2
over_base_num=10
print('T_over_base_num',T,over_base_num)
L = math.pow(T/(2*math.pi),2)*9.8
print("LLLLLLLLLL--->",L)
print("max_deta_x----->",max_deta_x)
filename = 'data.txt'
with open(filename, 'w') as data:
L_str = '%f' % L
max_deta_x = '%d' % max_deta_x
data.write('L'+L_str+"\n")
data.write('max_deta_x'+max_deta_x+"\n")
else :
if kalmanflag==1:
kalmanflag=kalmanflag+1
x_pre=x
y_pre=y
#第一次基准线
basex=x
basey=y
cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 0,255), 2)
#print('x:',x,'y:',y)
cv2.line(frame, (basex, 0), (basex, 300), (0, 255, 0))
cv2.imshow('capture',frame)
#cv2.resizeWindow('capture',1600,900)
#time_end=time.time()
#print('totally cost',time_end-time_start)
#print('1111111111111x:',basex,'1111y:',basex)
#print('TTTTTTTTTTTTTTTT',T)
if cv2.waitKey(1)==119:
break
cap.release()
cv2.destroyAllWindows()
