From bdf4eb95800b3e20bf427fe871a1d877b8d2bd79 Mon Sep 17 00:00:00 2001 From: zenls Date: Fri, 28 May 2021 06:14:03 +0100 Subject: [PATCH] add new exemple --- example/__pycache__/bsp.cpython-37.pyc | Bin 0 -> 12539 bytes example/bsp.py | 423 +++++++++++++++++++++++++ example/find_circul.py | 126 ++++++++ 3 files changed, 549 insertions(+) create mode 100644 example/__pycache__/bsp.cpython-37.pyc create mode 100644 example/bsp.py create mode 100644 example/find_circul.py diff --git a/example/__pycache__/bsp.cpython-37.pyc b/example/__pycache__/bsp.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..dc7094b212f798d7364e08d6a9515b74a385a0ec GIT binary patch literal 12539 zcmd5?TW}lKdEOhYB*BX)N|a;=zBWj0UFg_WoYdl7R7!y?C0Wik4h-TfDUcvQ9V{#g zp;{-B+nK~^l_s4|Tel^9$uut+KXj&jNMAcnU+TBEJALTw1=E?flgU$hQNQo(0$4zx zB#+Z+f!Tl0f9~f$|M_ocrw0a-8h$^#`2L%JdPdX!iAeLul1V((PwAQ_G@+NYE#202 zrj3%ZW!mPJWm^i@OA$Mw>O}3RddKY8c}6j%aSOvHi3fhB<@ zL=spMSRb%H(GRR2SU<1aXW=R zhsDSpO^l$=L12f(D6mmrL%T%^Pxa-!mx7!t!s#l&$jA`T%H z7o*}ZY9+)N=5oS|Y*pNfZ1kh=#sC3_Ym5>cCO8IAhyn1&w}od0PxW5`cJ*B?r|lYm z`mP0N>_!01T@%pSjRHml>qHfdx$(V(8{bHhBuMuOBS`h{Y8%Me7}z&d330s(H@a}M z3%9!PNC;1%KTDYVoarD&CLRg!A*2WQh5@6T4cE7nZzO)p55f!tu=MnTuW@up4<< zE|%A;UIa*O+cTH0T=vZ6s~0^BU`oMh6uNoodREn~Y-eJ4JEb+-$`{LL6vTi|Id5C5 zDg#hutbSLMCotO|KXK|>Wy?9WT|AX9ZC}f;R@{kG&Q5-7yX2f&t!|&(zUjqtxndc7 zm`iYHKLw!a(ayiouP61Pu765T>GhOip_9!n8jquO!C)Ww@!evXF9HF1wLN`T4@hk^ z)2_J(2HJ?YCbof0;Nvmi@erQqDo-}c)duO=OrJc0QM|#KYx(lJGh1_S&fF}N9C;ME za)?FIO3mG_xsp8ZnTtzTB^lqd7A~A$kSSIh1jrcjIMZVUq@xcWL&}S-RU~#!JOQN2 zZZy3gf1f(_0iPa0odQ@z{rK)d2@#@%pMC%(!jy@3QKCf|MF}G-IbbHK$RX26w@Dy5 zakYN=BvV{}Xyv-U^3Fv|N`efY`xl(_7fkN_zuxzQ7y49dp=!yGw9oO?d|{It4c*sc z-2vk_EB&voJ$wep^NJHXdHyH6cB|y9xu5B|?qI)J;s5WN+l;Mt{*m@OB8%(S+^L?6 z?(>;k)hL*#!XMwQGd+X@x2yMQxAa{D(AYJEz8R69$J^M{<>y_>J#E8qwGAjDNH`z2 zfJcaulRVuMOsak9DYj~UJdkKrHESHzlx)@2=pXFrYV;3+`th#O z6QR+Q7+p!D@+?~Eo3L7BKT;TeB$e67$7!>OR#qs&bm@ ztJyf4iOL9jry(S1@yIvo6+Sb4tyo4O3V2zCztb*xg3;zt;BSHE_c0_FZm^s z8d7qCN~p?bm})PJLN>HEwMwcTNlxQjsw7NYPaaLijRuu%fRS8LG~LuBZ?!N^3e*h z+$q$PWVznsdq$-!P+BGZG`${ELs!Z_fk7#e4j9y|tgWqwMveQUuC0+9X^zRCCU}lO zG0$nHsFiv$%JV4r5*|gvxE_zhP07684DvaYJe)!3jlrW(ei-}-<$=MQ7y#p>y!b}t zX4NegHap0~V>H8Qya(Vjk5I0HpnG5lIt-?u!(h3Qy{H@6P$oChaiL6cq!N&88cLFQ z`u0t*oS&!n$TNE6nLY9_TDw2RBe86QC%wVb)!<2Q@T52T0Om9Oo;h=Q#nY!eecID! zJbkvCrw^f?Vr~4dR8f|q zw8{1Rl-UOMqiOq@YMx%CSf;sq7FB!C$x9TjIfcz!!IfV|r6%i-wC2z%?%3XsG6{JO z9ZQ-M2TUR6kCFd{&;*9MCXoM_!@u&-;a9)%fZ@iU>+qDDm!VZ*Cg$4r)YEgYWM4uQPge3l{AyQ$Wt} zzi!>KcE0Z-z$0R}jaluD@7+SW$GrXy{l(EQ-`)Mbulm(@CPHh$T(HJb)J`ZHl&S7^ z|E$)N+_X^h(+E8o5p5jt#9i}MZ3$-i=d@i@Bvn6)oHMOxP{g?Y9pp1*UjGhyu!KHN zXpU<~5LHz5-v!>UatH3u1?5tB7z`TpJ?zE@*r^6KNn0;IcIDzwZSb6YiCN18X#$25 z8lMD^FEDkFfZW|M0lcVmww=5SMNeVw3(E5>u-xL7gV;|A#*Y_~3}>JcZ6+^LsMN}? zXBNedXBLpMMBdGNdNFk9RHP!h?zo%I&2OPmbrL|+M|4X+rpFPui6a%H4W<~tXycEw zj*)~yN+LID^c&CVaicyG=3Kw?Y$noEFxVI_G@rH*m_xzW@l<&_Ap~NQIlT}6(EV>F?A+E7!y2ge&A4oGCNR!ssL)a36z}Sf4i7T7K z&(p(siU$x7 zN?>q8p~KY6B=DJx7Gm#dc!4&Se}O7Zb6~hd-jlwr?j-X^AD>qKgU2uZ;6dY;4l<74 zL@Svm=%s>w9tGdW(=?6~FpinmoBtj``6O!9T-7O85x3_?@W*$H?-qeP3g2yX@SU)} zVYaxDoR?sXKyh6p+hA0(K~XTD(Y+XBx5a!Zlk}|1D;Lgt*4)g((!oqf@5>MTJw$PR zh>BgDIHuP}TI;=^6UhZFom&Rdz_oDDRFEN5qJmjFL&%BY=*9Q->MPfuyQ-nC zwGTBGE|Hh7&$mhtVqvgB7^4vV$NdO>AcgSRuGX z@Fanb)%>S5fd+c$xw-A( zCYLW{?LH>vSLS{$w_y9ZvlplA6mzF>OcpL@=Pq5clf*A%Uz=OG?6;U%n964779~|Q zuU=#k>UY&yFP3{tD`n&T2$icml_8go7bu1Sy*i@ThXV4oj||tqAKxuHei{g@9mL+` zX}pb13vbYhw%^5}U3h-s;7MRlntc{Xu(bggm& z3R{^Gde+n7*q)d4j6$5K_&ibIRb)=)Z$R!e!!biT1i)}}Ev*^__RSyPv~(3yQ1Atd zSqK)h&|ZuSql_loIIenB%4($|<63BukO6_e3qcC9n^whRAIp}E|yE+$sUENR3GSA?moJ*w^JB<9mRh%y+_qw#jC1AzbxEUB2_Cno&f}XazHB_0iowT)kww z^btq+8(0f{;8{PQ*rtYB-SwxH8)*pb9mpa$bur(m^W_Saj$9_7d|rQk=ET4J<4ecR zWd`I|n0c9?ZCKnQ`Xs?G5Ky;VqB|jaX>;V0US}B=II;q3oP#vNXe;kzEoLX zaq{9xauNf;kFr&R4R>24wa_cRv!!VRJ*H#tdVy*XV8{iH&Kw{ z_+&%4YlkFrIwThkqc6GYNVMHGTr?QCWH8~PS%@7+5I2q@W*o!UCviI|5_X?R+Wn#r z-|h6`2GyqLl8nY0bFhl{N$N>2rgC(CuWW?ZCK)agqC}aHGL}771CKK55ir4BxD-N_Z zJ_SiH&rE&hrL!;k`mGP6z^{x{w^nOaJ96pbbgk-{NPDq-AnhW;S*@*mk!@K-ARux> z!s_!53EX4aDm{k;g_3{s$-_&rj~r8e(na)z2K<^aroNUEj9r`cm#=aK)pWj` zKKuM?(M_u}l6EU;_nMQgZaanITG0_{35%@iI`ULNi2BnPCT3nqtJdk$JI|b+c%6-! z)q*T?|F2+D_MjqEZuO??HRR<;APQ#97y*Im!(qax z{#*Cs9aNb>i!fF84cr!!CMOk5phS6RJPG4z&Gc!r>(eGfSwPCv%GKg}8C2l>(`&M_ zrKkf6q=oZVvEbAv6g6fQ(jXXOiIrMZA}w3C@LvK!>`u|*F?FBrf+DwdKP}9&%!2_U zUv+txr2CXYIju4Yw>r{o<4O&Tv}01y4jd zqS0Mn;RSA~i!W?&-H9mCZ9O>2^kbsCmNI{m_?%6B*w$)oiN0x#WuhOPJy0LJXxjUeLE_)f)gMO_Un+!KkguN+>4kdz$Q-WSwrI?x zx25xZ6~>M83d({W{{CG3(0mbEq_ec4G}IR{?E05Szjyv0>h8J5+rYB8>x!(EDtXtJ z2|Ng%h3>DZ?244XfgJfQ0`k8%)+uI|SLQCwOfAkKFo{_F3ain-4x*N}#UpA~#XTY6czcemuM&?)MEUgvyvYiYE#67v{3I zPzEI@O;#!3Teh-OQeLn+q)Khop9=E1;psHp z*rlvWtK|GV)7ebS->M^>TZJm8Q9ZZl9Z;~~9qrX(ejZH@Dp_c=P1C|bB+;MyX;8TP z##*^uQGai~%B}53B@OnC|A#~!xqlNo)NyMA$rKO|*doM&jN?!!6b^Ko5sFYsM_3?l zr2c{;FICAUN4`}_FV{*{=ef(kI}T-ft5$W>t4{jG(+hS{gPB)MXR!jzlHcyi-^E-0 zAwW};{sGci)nIC@uXCYfJ`KiOeY|Ud?Ru?sa*eF76lw_x88+R5CQp6vBCsZ^yB!{`UtK*P%n(bT-gK7AlQ8K1RrAiB25`6 z(QM;SFsAQyeHa5B`0=YUS`ZHTBNPSE1bg5uN8%fRd`TH!_TaUOEY@);7i4(GTX{U! zx~Y@G{)dS0O_GNmpN7NDL*9CzK!$L{>&HU1Pj)uu&b*7J^O+-ER}%K&PHw(*WqN9M z%8;Fo66$f<1zLg5L+Q$2*6ZT3qzcWNz``q6Pa{?^Zl$4=7tu7MeQR zzEGxkR1B&1M1GfO+on?~9yj?*0CS`%aAz{Qclu=7=3%SuURB*UicmvXgSVn7dJ(ID2MC1<+J1##aTu&Eda6KyFqaq^Mh8 zb^D`2@P3%{21^+nQGSLRT5YDMmn1cke20LFrPl(z_~-nuu3p6=wJPm= 0) and (right <= 100)): + GPIO.output(self.AIN1, GPIO.HIGH) + GPIO.output(self.AIN2, GPIO.LOW) + self.PWMA.ChangeDutyCycle(right) + elif((right < 0) and (right >= -100)): + GPIO.output(self.AIN1, GPIO.LOW) + GPIO.output(self.AIN2, GPIO.HIGH) + self.PWMA.ChangeDutyCycle(0 - right) + if((left >= 0) and (left <= 100)): + GPIO.output(self.BIN1, GPIO.HIGH) + GPIO.output(self.BIN2, GPIO.LOW) + self.PWMB.ChangeDutyCycle(left) + elif((left < 0) and (left >= -100)): + GPIO.output(self.BIN1, GPIO.LOW) + GPIO.output(self.BIN2, GPIO.HIGH) + self.PWMB.ChangeDutyCycle(0 - left) + + +class beep(): + def __init__(self): + self.BUZ = 4 + GPIO.setmode(GPIO.BCM) + GPIO.setup(self.BUZ, GPIO.OUT) + + def on(self): + GPIO.output(self.BUZ, GPIO.HIGH) + + def off(self): + GPIO.output(self.BUZ, GPIO.LOW) + + +class joystick (): + def __init__(self): + self.CTR = 7 + self.A = 8 + self.B = 9 + self.C = 10 + self.D = 11 + + GPIO.setmode(GPIO.BCM) + GPIO.setwarnings(False) + GPIO.setup(self.CTR, GPIO.IN, GPIO.PUD_UP) + GPIO.setup(self.A, GPIO.IN, GPIO.PUD_UP) + GPIO.setup(self.B, GPIO.IN, GPIO.PUD_UP) + GPIO.setup(self.C, GPIO.IN, GPIO.PUD_UP) + GPIO.setup(self.D, GPIO.IN, GPIO.PUD_UP) + + def check_ctr(self): + return (GPIO.input(self.CTR) == 0) + + def check_a(self): + return (GPIO.input(self.A) == 0) + + def check_b(self): + return (GPIO.input(self.B) == 0) + + def check_c(self): + return (GPIO.input(self.C) == 0) + + def check_d(self): + return (GPIO.input(self.D) == 0) + + +class ir_controll (): + def __init__(self): + self.IR = 17 + + GPIO.setmode(GPIO.BCM) + GPIO.setwarnings(False) + GPIO.setup(self.IR, GPIO.IN) + + def getkey(self): + if GPIO.input(self.IR) == 0: + count = 0 + while GPIO.input(self.IR) == 0 and count < 200: # 9ms + count += 1 + time.sleep(0.00006) + if(count < 10): + return + count = 0 + while GPIO.input(self.IR) == 1 and count < 80: # 4.5ms + count += 1 + time.sleep(0.00006) + + idx = 0 + cnt = 0 + data = [0, 0, 0, 0] + for i in range(0, 32): + count = 0 + while GPIO.input(self.IR) == 0 and count < 15: # 0.56ms + count += 1 + time.sleep(0.00006) + + count = 0 + while GPIO.input(self.IR) == 1 and count < 40: # 0: 0.56mx + count += 1 # 1: 1.69ms + time.sleep(0.00006) + + if count > 7: + data[idx] |= 1 << cnt + if cnt == 7: + cnt = 0 + idx += 1 + else: + cnt += 1 + # print data + if data[0]+data[1] == 0xFF and data[2]+data[3] == 0xFF: # check + return data[2] + else: + return "repeat" + + +class ir_bumper (): + def __init__(self): + self.DR = 16 + self.DL = 19 + + GPIO.setmode(GPIO.BCM) + GPIO.setwarnings(False) + GPIO.setup(self.DR, GPIO.IN, GPIO.PUD_UP) + GPIO.setup(self.DL, GPIO.IN, GPIO.PUD_UP) + + def check_r(self): + return (GPIO.input(self.DR) == 0) + + def check_l(self): + return (GPIO.input(self.DL) == 0) + +# Ultrasonic distance measurement + + +class us_sensor (): + def __init__(self): + self.TRIG = 22 + self.ECHO = 27 + + GPIO.setmode(GPIO.BCM) + GPIO.setwarnings(False) + GPIO.setup(self.TRIG, GPIO.OUT, initial=GPIO.LOW) + GPIO.setup(self.ECHO, GPIO.IN) + + def dist(self): + GPIO.output(self.TRIG, GPIO.HIGH) + time.sleep(0.000015) + GPIO.output(self.TRIG, GPIO.LOW) + while not GPIO.input(self.ECHO): + pass + t1 = time.time() + while GPIO.input(self.ECHO): + pass + t2 = time.time() + return (t2-t1)*34000/2 + + +COLOR_ENUM = { + "Aqua": [000, 255, 255], + "Black": [000, 000, 000], + "Blue": [000, 000, 255], + "Purple": [255, 000, 255], + "Green": [000, 255, 000], + "Red": [255, 000, 000], + "White": [255, 255, 255], + "Yellow": [255, 255, 000] +} + + +class rgb_led(): + + def __init__(self): + self.LED_COUNT = 4 # Number of LED pixels. + self.LED_PIN = 18 + self.LED_FREQ_HZ = 800000 + self.LED_DMA = 5 + self.LED_BRIGHTNESS = 255 # Set to 0 for darkest and 255 for brightest + self.LED_INVERT = False + self.LED_CHANNEL = 0 + + self.strip = Adafruit_NeoPixel( + self.LED_COUNT, self.LED_PIN, self.LED_FREQ_HZ, + self.LED_DMA, self.LED_INVERT, self.LED_BRIGHTNESS) + self.strip.begin() + + def color_raw(self, N, R, G, B): + if (N < 4 and N > -1): + self.strip.setPixelColor(N, Color(R, G, B)) + self.strip.show() + + def color(self, N, C): + if (N < 4 and N > -1): + self.strip.setPixelColor(N, Color(COLOR_ENUM[C][0], + COLOR_ENUM[C][1], COLOR_ENUM[C][2])) + self.strip.show() + + +class line_sensor(): + def __init__(self): + self.CS = 5 + self.Clock = 25 + self.Address = 24 + self.DataOut = 23 + self.Button = 7 + + self.numSensors = 5 + self.calibratedMin = [0] * self.numSensors + self.calibratedMax = [1023] * self.numSensors + self.last_value = 0 + GPIO.setmode(GPIO.BCM) + GPIO.setwarnings(False) + GPIO.setup(self.Clock, GPIO.OUT) + GPIO.setup(self.Address, GPIO.OUT) + GPIO.setup(self.CS, GPIO.OUT) + GPIO.setup(self.DataOut, GPIO.IN, GPIO.PUD_UP) + GPIO.setup(self.Button, GPIO.IN, GPIO.PUD_UP) + + def AnalogRead(self): + value = [0]*(self.numSensors+1) + # Read Channel0~channel6 AD value + for j in range(0, self.numSensors+1): + GPIO.output(self.CS, GPIO.LOW) + for i in range(0, 4): + + # sent 4-bit self.Address + if(((j) >> (3 - i)) & 0x01): + GPIO.output(self.Address, GPIO.HIGH) + else: + GPIO.output(self.Address, GPIO.LOW) + + # read MSB 4-bit data + value[j] <<= 1 + if(GPIO.input(self.DataOut)): + value[j] |= 0x01 + + GPIO.output(self.Clock, GPIO.HIGH) + GPIO.output(self.Clock, GPIO.LOW) + + for i in range(0, 6): + + # read LSB 8-bit data + value[j] <<= 1 + if(GPIO.input(self.DataOut)): + value[j] |= 0x01 + + GPIO.output(self.Clock, GPIO.HIGH) + GPIO.output(self.Clock, GPIO.LOW) + + time.sleep(0.0001) + GPIO.output(self.CS, GPIO.HIGH) + return value[1:] + +class CameraAngle: + + # Registers/etc. + __SUBADR1 = 0x02 + __SUBADR2 = 0x03 + __SUBADR3 = 0x04 + __MODE1 = 0x00 + __PRESCALE = 0xFE + __LED0_ON_L = 0x06 + __LED0_ON_H = 0x07 + __LED0_OFF_L = 0x08 + __LED0_OFF_H = 0x09 + __ALLLED_ON_L = 0xFA + __ALLLED_ON_H = 0xFB + __ALLLED_OFF_L = 0xFC + __ALLLED_OFF_H = 0xFD + + def __init__(self, address=0x40, debug=False): + self.bus = smbus.SMBus(1) + self.address = address + self.debug = debug + if (self.debug): + print("Reseting PCA9685") + self.write(self.__MODE1, 0x00) + self.setPWMFreq(50) + + def write(self, reg, value): + "Writes an 8-bit value to the specified register/address" + self.bus.write_byte_data(self.address, reg, value) + if (self.debug): + print("I2C: Write 0x%02X to register 0x%02X" % (value, reg)) + + def read(self, reg): + "Read an unsigned byte from the I2C device" + result = self.bus.read_byte_data(self.address, reg) + if (self.debug): + print("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % + (self.address, result & 0xFF, reg)) + return result + + def setPWMFreq(self, freq): + "Sets the PWM frequency" + prescaleval = 25000000.0 # 25MHz + prescaleval /= 4096.0 # 12-bit + prescaleval /= float(freq) + prescaleval -= 1.0 + if (self.debug): + print("Setting PWM frequency to %d Hz" % freq) + print("Estimated pre-scale: %d" % prescaleval) + prescale = math.floor(prescaleval + 0.5) + if (self.debug): + print("Final pre-scale: %d" % prescale) + + oldmode = self.read(self.__MODE1) + newmode = (oldmode & 0x7F) | 0x10 # sleep + self.write(self.__MODE1, newmode) # go to sleep + self.write(self.__PRESCALE, int(math.floor(prescale))) + self.write(self.__MODE1, oldmode) + time.sleep(0.005) + self.write(self.__MODE1, oldmode | 0x80) + + def setPWM(self, channel, on, off): + "Sets a single PWM channel" + self.write(self.__LED0_ON_L+4*channel, on & 0xFF) + self.write(self.__LED0_ON_H+4*channel, on >> 8) + self.write(self.__LED0_OFF_L+4*channel, off & 0xFF) + self.write(self.__LED0_OFF_H+4*channel, off >> 8) + if (self.debug): + print("channel: %d LED_ON: %d LED_OFF: %d" % (channel, on, off)) + + def setServoPulse(self, channel, pulse): + "Sets the Servo Pulse,The PWM frequency must be 50HZ" + # PWM frequency is 50HZ,the period is 20000us + pulse = int(pulse*4096/20000) + self.setPWM(channel, 0, pulse) + + def setCameraAngle(self, verticalAngle, horizontalAngle): + if (verticalAngle > 180): + verticalAngle = 180 + if (verticalAngle < 0): + verticalAngle = 0 + if (horizontalAngle > 180): + horizontalAngle = 180 + if (horizontalAngle < 0): + horizontalAngle = 0 + + va = verticalAngle * 2000 / 180 + 500 + vh = horizontalAngle * 2000 / 180 + 500 + + self.setServoPulse(0, va) + self.setServoPulse(1, vh) + + +if __name__ == '__main__': + + l = line_sensor() + while True: + print(l.AnalogRead()) + time.sleep(0.2) diff --git a/example/find_circul.py b/example/find_circul.py new file mode 100644 index 0000000..7cc5777 --- /dev/null +++ b/example/find_circul.py @@ -0,0 +1,126 @@ +import cv2 +import numpy as np +from bsp import * + +# Подключаемся к камере +cam = cv2.VideoCapture(0) + +# Мотор +m = motor() +speed = 10 + +while True: + _, frame = cam.read() + # Перевод изображения в HSV + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + # Немного размываем изображение + hsv = cv2.blur(hsv,(5,5)) + + # Задаем параметр из прошлого примера + mask = cv2.inRange(hsv, (0,190,129),(21,255,255)) + + # С помощью специального алгоритма ищем контур обьекта + (contours, hierarchy) = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + # Отрисовывем найденый контур чтобы его было видно на экране + cv2.drawContours(frame, contours, -1, (255,0,0), 3, cv2.LINE_AA, hierarchy, 1 ) + + max_radius = 0 + center = (0,0) + + # Находим контур с максимальным радиусом + x = 0 + y = 0 + + for contour in contours: + (x,y),radius = cv2.minEnclosingCircle(contour) + if max_radius < int(radius): + max_radius = int(radius) + center = (int(x),int(y)) + + # Описываем максимальный контур кругом + frame = cv2.circle(frame,center,max_radius,(0,255,0),2) + + S = 3.1415 * max_radius * max_radius + # Выводим на эран площадь круга + cv2.putText(frame, "S: " + str(S) + " X: " + str(x) + " Y: " + str(y), (30, 30),cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 10, 10), 2) + + # # Выводим сообщение если круг слишком близко или далеко + # if S > 100: + # if S > 80000: + # cv2.putText(frame, "backward", (30, 60),cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 10, 10), 2) + # m.backward(speed) + # elif S < 40000: + # cv2.putText(frame, "forward", (30, 60),cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 10, 10), 2) + # m.forward(speed) + # else: + # m.stop() + + + + delta = 100 + max_x = 640 + if (x < max_x/2 - delta): + m.left(speed) + time.sleep(0.3) + m.stop() + time.sleep(0.1) + elif (x > max_x/2 + delta): + m.right(speed) + time.sleep(0.3) + m.stop() + time.sleep(0.1) + else: + if S > 100 and S < 40000: + m.forward(speed) + else: + m.stop() + + # # конечный автомат: + # # 0 - центрирование относительно метки + # # 1 - Подбезжаем к метке + # # 2 - ожидаем выхода + # state = 0 + # delta = 100 + # max_x = 640 + + # if state == 0: + # if (x < max_x/2 - delta): + # m.left(speed) + # elif (x > max_x/2 + delta): + # m.right(speed) + # else: + # state = 1 + + # elif state == 1: + # if (S < 7000): + # m.forward(speed) + # else: + # state = 2 + + # if (x < max_x/2 - delta or x > max_x/2 - delta): + # state = 0 + # else: + # if (x < max_x/2 - delta or x > max_x/2 - delta): + # state = 0 + # elif (S < 7000): + # state = 1 + # else: + # time.sleep(1) + + # cv2.putText(frame, "State: " + str(state), (30, 60),cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 10, 10), 2) + + + # Промежуточные изображения + #cv2.imshow("Image with opening", mask) + #cv2.imshow("Image with closing", hsv) + + # выводим то что получилось вместе с текстом и контуром + cv2.imshow("Image", frame) + + # m.stop() + # time.sleep(1) + + k = cv2.waitKey(2) + if k == 27: + cv2.destroyAllWindows() + break