You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
422 lines
12 KiB
422 lines
12 KiB
4 years ago
|
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.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(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)
|