parent
d604b45ff2
commit
b7d6bf738b
@ -0,0 +1,422 @@ |
||||
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) |
||||
Binary file not shown.
Loading…
Reference in new issue