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)