diff --git a/example/__pycache__/bsp.cpython-37.pyc b/example/__pycache__/bsp.cpython-37.pyc new file mode 100644 index 0000000..dc7094b Binary files /dev/null and b/example/__pycache__/bsp.cpython-37.pyc differ diff --git a/example/bsp.py b/example/bsp.py new file mode 100644 index 0000000..a170f1c --- /dev/null +++ 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) 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