add new exemple

4 years ago · bdf4eb9580
parent 2ccd7f2d94
commit bdf4eb9580
3 changed files with 549 additions and 0 deletions
--- a/example/pycache/bsp.cpython-37.pyc
+++ b/example/pycache/bsp.cpython-37.pyc
--- a/example/bsp.py
+++ b/example/bsp.py
@ -0,0 +1,423 @@
+import RPi.GPIO as GPIO
+from rpi_ws281x import Adafruit_NeoPixel, Color
+import time
+import math
+import smbus
+
+
+class motor ():
+    def __init__(self, ain1=12, ain2=13, ena=6, bin1=20, bin2=21, enb=26):
+        self.AIN1 = ain1
+        self.AIN2 = ain2
+        self.BIN1 = bin1
+        self.BIN2 = bin2
+        self.ENA = ena
+        self.ENB = enb
+        self.PA = 50
+        self.PB = 50
+
+        GPIO.setmode(GPIO.BCM)
+        GPIO.setwarnings(False)
+        GPIO.setup(self.AIN1, GPIO.OUT)
+        GPIO.setup(self.AIN2, GPIO.OUT)
+        GPIO.setup(self.BIN1, GPIO.OUT)
+        GPIO.setup(self.BIN2, GPIO.OUT)
+        GPIO.setup(self.ENA, GPIO.OUT)
+        GPIO.setup(self.ENB, GPIO.OUT)
+        self.PWMA = GPIO.PWM(self.ENA, 500)
+        self.PWMB = GPIO.PWM(self.ENB, 500)
+        self.PWMA.start(self.PA)
+        self.PWMB.start(self.PB)
+        self.stop()
+
+    def forward(self, PWM=30):
+        self.PWMA.ChangeDutyCycle(PWM)
+        self.PWMB.ChangeDutyCycle(PWM)
+        GPIO.output(self.AIN1, GPIO.LOW)
+        GPIO.output(self.AIN2, GPIO.HIGH)
+        GPIO.output(self.BIN1, GPIO.LOW)
+        GPIO.output(self.BIN2, GPIO.HIGH)
+
+    def stop(self):
+        self.PWMA.ChangeDutyCycle(0)
+        self.PWMB.ChangeDutyCycle(0)
+        GPIO.output(self.AIN1, GPIO.LOW)
+        GPIO.output(self.AIN2, GPIO.LOW)
+        GPIO.output(self.BIN1, GPIO.LOW)
+        GPIO.output(self.BIN2, GPIO.LOW)
+
+    def backward(self, PWM=30):
+        self.PWMA.ChangeDutyCycle(PWM)
+        self.PWMB.ChangeDutyCycle(PWM)
+        GPIO.output(self.AIN1, GPIO.HIGH)
+        GPIO.output(self.AIN2, GPIO.LOW)
+        GPIO.output(self.BIN1, GPIO.HIGH)
+        GPIO.output(self.BIN2, GPIO.LOW)
+
+    def left(self, PWM=30):
+        self.PWMA.ChangeDutyCycle(PWM)
+        self.PWMB.ChangeDutyCycle(PWM)
+        GPIO.output(self.AIN1, GPIO.HIGH)
+        GPIO.output(self.AIN2, GPIO.LOW)
+        GPIO.output(self.BIN1, GPIO.LOW)
+        GPIO.output(self.BIN2, GPIO.HIGH)
+
+    def right(self, PWM=30):
+        self.PWMA.ChangeDutyCycle(PWM)
+        self.PWMB.ChangeDutyCycle(PWM)
+        GPIO.output(self.AIN1, GPIO.LOW)
+        GPIO.output(self.AIN2, GPIO.HIGH)
+        GPIO.output(self.BIN1, GPIO.HIGH)
+        GPIO.output(self.BIN2, GPIO.LOW)
+
+    def setMotor(self, left, right):
+        left = -left
+        right = -right
+        if((right >= 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)
--- a/example/find_circul.py
+++ 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