initial commit
This commit is contained in:
joel
commit
5942c13721
10 changed files with 462 additions and 0 deletions
1
MANIFEST.in
Normal file
1
MANIFEST.in
Normal file
|
|
@ -0,0 +1 @@
|
|||
include readme.md
|
||||
8
README.md
Normal file
8
README.md
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
# compLIB
|
||||
|
||||
## TODOs
|
||||
|
||||
### LED control is not implemented
|
||||
|
||||
Need dependencies:
|
||||
https://github.com/Freenove/Freenove_RPI_WS281x_Python
|
||||
94
compLIB/ADC.py
Normal file
94
compLIB/ADC.py
Normal file
|
|
@ -0,0 +1,94 @@
|
|||
import smbus
|
||||
import time
|
||||
|
||||
|
||||
class Adc:
|
||||
def __init__(self):
|
||||
# Get I2C bus
|
||||
self.bus = smbus.SMBus(1)
|
||||
|
||||
# I2C address of the device
|
||||
self.ADDRESS = 0x48
|
||||
|
||||
# PCF8591 Command
|
||||
self.PCF8591_CMD = 0x40 # Command
|
||||
|
||||
# ADS7830 Command
|
||||
self.ADS7830_CMD = 0x84 # Single-Ended Inputs
|
||||
|
||||
for i in range(3):
|
||||
aa = self.bus.read_byte_data(self.ADDRESS, 0xf4)
|
||||
if aa < 150:
|
||||
self.Index = "PCF8591"
|
||||
else:
|
||||
self.Index = "ADS7830"
|
||||
|
||||
def analogReadPCF8591(self, chn): # PCF8591 read ADC value,chn:0,1,2,3
|
||||
value = [0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
for i in range(9):
|
||||
value[i] = self.bus.read_byte_data(self.ADDRESS, self.PCF8591_CMD + chn)
|
||||
value = sorted(value)
|
||||
return value[4]
|
||||
|
||||
# TODO: bug in original code???
|
||||
#def analogWritePCF8591(self, value): # PCF8591 write DAC value
|
||||
# self.bus.write_byte_data(self.ADDRESS, cmd, value)
|
||||
|
||||
def recvPCF8591(self, channel): # PCF8591 write DAC value
|
||||
while (1):
|
||||
value1 = self.analogReadPCF8591(channel) # read the ADC value of channel 0,1,2,
|
||||
value2 = self.analogReadPCF8591(channel)
|
||||
if value1 == value2:
|
||||
break;
|
||||
voltage = value1 / 256.0 * 3.3 # calculate the voltage value
|
||||
voltage = round(voltage, 2)
|
||||
return voltage
|
||||
|
||||
def recvADS7830(self, channel):
|
||||
"""Select the Command data from the given provided value above"""
|
||||
COMMAND_SET = self.ADS7830_CMD | ((((channel << 2) | (channel >> 1)) & 0x07) << 4)
|
||||
self.bus.write_byte(self.ADDRESS, COMMAND_SET)
|
||||
while (1):
|
||||
value1 = self.bus.read_byte(self.ADDRESS)
|
||||
value2 = self.bus.read_byte(self.ADDRESS)
|
||||
if value1 == value2:
|
||||
break;
|
||||
voltage = value1 / 255.0 * 3.3 # calculate the voltage value
|
||||
voltage = round(voltage, 2)
|
||||
return voltage
|
||||
|
||||
def recvADC(self, channel):
|
||||
if self.Index == "PCF8591":
|
||||
data = self.recvPCF8591(channel)
|
||||
elif self.Index == "ADS7830":
|
||||
data = self.recvADS7830(channel)
|
||||
return data
|
||||
|
||||
def i2cClose(self):
|
||||
self.bus.close()
|
||||
|
||||
|
||||
def loop():
|
||||
adc = Adc()
|
||||
while True:
|
||||
Left_IDR = adc.recvADC(0)
|
||||
print(Left_IDR)
|
||||
Right_IDR = adc.recvADC(1)
|
||||
print(Right_IDR)
|
||||
Power = adc.recvADC(2) * 3
|
||||
print(Power)
|
||||
time.sleep(1)
|
||||
print('----')
|
||||
|
||||
|
||||
def destroy():
|
||||
pass
|
||||
|
||||
|
||||
# Main program logic follows:
|
||||
if __name__ == '__main__':
|
||||
print('Program is starting ... ')
|
||||
try:
|
||||
loop()
|
||||
except KeyboardInterrupt: # When 'Ctrl+C' is pressed, the child program destroy() will be executed.
|
||||
destroy()
|
||||
23
compLIB/Buzzer.py
Normal file
23
compLIB/Buzzer.py
Normal file
|
|
@ -0,0 +1,23 @@
|
|||
import time
|
||||
import RPi.GPIO as GPIO
|
||||
from Command import COMMAND as cmd
|
||||
|
||||
GPIO.setwarnings(False)
|
||||
Buzzer_Pin = 17
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(Buzzer_Pin, GPIO.OUT)
|
||||
|
||||
|
||||
class Buzzer:
|
||||
def run(self, command):
|
||||
if command != "0":
|
||||
GPIO.output(Buzzer_Pin, True)
|
||||
else:
|
||||
GPIO.output(Buzzer_Pin, False)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
B = Buzzer()
|
||||
B.run('1')
|
||||
time.sleep(3)
|
||||
B.run('0')
|
||||
13
compLIB/Command.py
Normal file
13
compLIB/Command.py
Normal file
|
|
@ -0,0 +1,13 @@
|
|||
class COMMAND:
|
||||
CMD_MOTOR = "CMD_MOTOR"
|
||||
CMD_LED = "CMD_LED"
|
||||
CMD_LED_MOD = "CMD_LED_MOD"
|
||||
CMD_SERVO = "CMD_SERVO"
|
||||
CMD_BUZZER = "CMD_BUZZER"
|
||||
CMD_SONIC = "CMD_SONIC"
|
||||
CMD_LIGHT = "CMD_LIGHT"
|
||||
CMD_POWER = "CMD_POWER"
|
||||
CMD_MODE = "CMD_MODE"
|
||||
|
||||
def __init__(self):
|
||||
pass
|
||||
107
compLIB/Motor.py
Normal file
107
compLIB/Motor.py
Normal file
|
|
@ -0,0 +1,107 @@
|
|||
import time
|
||||
from PCA9685 import PCA9685
|
||||
|
||||
|
||||
class Motor:
|
||||
def __init__(self):
|
||||
self.pwm = PCA9685(0x40, debug=True)
|
||||
self.pwm.setPWMFreq(50)
|
||||
|
||||
def duty_range(self, duty1, duty2, duty3, duty4):
|
||||
if duty1 > 4095:
|
||||
duty1 = 4095
|
||||
elif duty1 < -4095:
|
||||
duty1 = -4095
|
||||
|
||||
if duty2 > 4095:
|
||||
duty2 = 4095
|
||||
elif duty2 < -4095:
|
||||
duty2 = -4095
|
||||
|
||||
if duty3 > 4095:
|
||||
duty3 = 4095
|
||||
elif duty3 < -4095:
|
||||
duty3 = -4095
|
||||
|
||||
if duty4 > 4095:
|
||||
duty4 = 4095
|
||||
elif duty4 < -4095:
|
||||
duty4 = -4095
|
||||
return duty1, duty2, duty3, duty4
|
||||
|
||||
def left_Upper_Wheel(self, duty):
|
||||
if duty > 0:
|
||||
self.pwm.setMotorPwm(0, 0)
|
||||
self.pwm.setMotorPwm(1, duty)
|
||||
elif duty < 0:
|
||||
self.pwm.setMotorPwm(1, 0)
|
||||
self.pwm.setMotorPwm(0, abs(duty))
|
||||
else:
|
||||
self.pwm.setMotorPwm(0, 4095)
|
||||
self.pwm.setMotorPwm(1, 4095)
|
||||
|
||||
def left_Lower_Wheel(self, duty):
|
||||
if duty > 0:
|
||||
self.pwm.setMotorPwm(3, 0)
|
||||
self.pwm.setMotorPwm(2, duty)
|
||||
elif duty < 0:
|
||||
self.pwm.setMotorPwm(2, 0)
|
||||
self.pwm.setMotorPwm(3, abs(duty))
|
||||
else:
|
||||
self.pwm.setMotorPwm(2, 4095)
|
||||
self.pwm.setMotorPwm(3, 4095)
|
||||
|
||||
def right_Upper_Wheel(self, duty):
|
||||
if duty > 0:
|
||||
self.pwm.setMotorPwm(6, 0)
|
||||
self.pwm.setMotorPwm(7, duty)
|
||||
elif duty < 0:
|
||||
self.pwm.setMotorPwm(7, 0)
|
||||
self.pwm.setMotorPwm(6, abs(duty))
|
||||
else:
|
||||
self.pwm.setMotorPwm(6, 4095)
|
||||
self.pwm.setMotorPwm(7, 4095)
|
||||
|
||||
def right_Lower_Wheel(self, duty):
|
||||
if duty > 0:
|
||||
self.pwm.setMotorPwm(4, 0)
|
||||
self.pwm.setMotorPwm(5, duty)
|
||||
elif duty < 0:
|
||||
self.pwm.setMotorPwm(5, 0)
|
||||
self.pwm.setMotorPwm(4, abs(duty))
|
||||
else:
|
||||
self.pwm.setMotorPwm(4, 4095)
|
||||
self.pwm.setMotorPwm(5, 4095)
|
||||
|
||||
def setMotorModel(self, duty1, duty2, duty3, duty4):
|
||||
duty1, duty2, duty3, duty4 = self.duty_range(duty1, duty2, duty3, duty4)
|
||||
self.left_Upper_Wheel(duty1)
|
||||
self.left_Lower_Wheel(duty2)
|
||||
self.right_Upper_Wheel(duty3)
|
||||
self.right_Lower_Wheel(duty4)
|
||||
|
||||
|
||||
PWM = Motor()
|
||||
|
||||
|
||||
def loop():
|
||||
PWM.setMotorModel(2000, 2000, 2000, 2000) # Forward
|
||||
time.sleep(3)
|
||||
PWM.setMotorModel(-2000, -2000, -2000, -2000) # Back
|
||||
time.sleep(3)
|
||||
PWM.setMotorModel(-500, -500, 2000, 2000) # Left
|
||||
time.sleep(3)
|
||||
PWM.setMotorModel(2000, 2000, -500, -500) # Right
|
||||
time.sleep(3)
|
||||
PWM.setMotorModel(0, 0, 0, 0) # Stop
|
||||
|
||||
|
||||
def destroy():
|
||||
PWM.setMotorModel(0, 0, 0, 0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
loop()
|
||||
except KeyboardInterrupt: # When 'Ctrl+C' is pressed, the child program destroy() will be executed.
|
||||
destroy()
|
||||
76
compLIB/PCA9685.py
Normal file
76
compLIB/PCA9685.py
Normal file
|
|
@ -0,0 +1,76 @@
|
|||
#!/usr/bin/python
|
||||
|
||||
import time
|
||||
import math
|
||||
import smbus
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# Raspi PCA9685 16-Channel PWM Servo Driver
|
||||
# ============================================================================
|
||||
|
||||
class PCA9685:
|
||||
# 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
|
||||
self.write(self.__MODE1, 0x00)
|
||||
|
||||
def write(self, reg, value):
|
||||
"Writes an 8-bit value to the specified register/address"
|
||||
self.bus.write_byte_data(self.address, reg, value)
|
||||
|
||||
def read(self, reg):
|
||||
"Read an unsigned byte from the I2C device"
|
||||
result = self.bus.read_byte_data(self.address, 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
|
||||
prescale = math.floor(prescaleval + 0.5)
|
||||
|
||||
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)
|
||||
|
||||
def setMotorPwm(self, channel, duty):
|
||||
self.setPWM(channel, 0, duty)
|
||||
|
||||
def setServoPulse(self, channel, pulse):
|
||||
"Sets the Servo Pulse,The PWM frequency must be 50HZ"
|
||||
pulse = pulse * 4096 / 20000 # PWM frequency is 50HZ,the period is 20000us
|
||||
self.setPWM(channel, 0, int(pulse))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
pass
|
||||
106
compLIB/Ultrasonic.py
Normal file
106
compLIB/Ultrasonic.py
Normal file
|
|
@ -0,0 +1,106 @@
|
|||
import time
|
||||
from Motor import *
|
||||
import RPi.GPIO as GPIO
|
||||
from servo import *
|
||||
from PCA9685 import PCA9685
|
||||
|
||||
|
||||
class Ultrasonic:
|
||||
def __init__(self):
|
||||
GPIO.setwarnings(False)
|
||||
self.trigger_pin = 27
|
||||
self.echo_pin = 22
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(self.trigger_pin, GPIO.OUT)
|
||||
GPIO.setup(self.echo_pin, GPIO.IN)
|
||||
|
||||
def send_trigger_pulse(self):
|
||||
GPIO.output(self.trigger_pin, True)
|
||||
time.sleep(0.00015)
|
||||
GPIO.output(self.trigger_pin, False)
|
||||
|
||||
def wait_for_echo(self, value, timeout):
|
||||
count = timeout
|
||||
while GPIO.input(self.echo_pin) != value and count > 0:
|
||||
count = count - 1
|
||||
|
||||
def get_distance(self):
|
||||
distance_cm = [0, 0, 0, 0, 0]
|
||||
for i in range(3):
|
||||
self.send_trigger_pulse()
|
||||
self.wait_for_echo(True, 10000)
|
||||
start = time.time()
|
||||
self.wait_for_echo(False, 10000)
|
||||
finish = time.time()
|
||||
pulse_len = finish - start
|
||||
distance_cm[i] = pulse_len / 0.000058
|
||||
distance_cm = sorted(distance_cm)
|
||||
return int(distance_cm[2])
|
||||
|
||||
def run_motor(self, L, M, R):
|
||||
if (L < 30 and M < 30 and R < 30) or M < 30:
|
||||
self.PWM.setMotorModel(-1450, -1450, -1450, -1450)
|
||||
time.sleep(0.1)
|
||||
if L < R:
|
||||
self.PWM.setMotorModel(1450, 1450, -1450, -1450)
|
||||
else:
|
||||
self.PWM.setMotorModel(-1450, -1450, 1450, 1450)
|
||||
elif L < 30 and M < 30:
|
||||
PWM.setMotorModel(1500, 1500, -1500, -1500)
|
||||
elif R < 30 and M < 30:
|
||||
PWM.setMotorModel(-1500, -1500, 1500, 1500)
|
||||
elif L < 20:
|
||||
PWM.setMotorModel(2000, 2000, -500, -500)
|
||||
if L < 10:
|
||||
PWM.setMotorModel(1500, 1500, -1000, -1000)
|
||||
elif R < 20:
|
||||
PWM.setMotorModel(-500, -500, 2000, 2000)
|
||||
if R < 10:
|
||||
PWM.setMotorModel(-1500, -1500, 1500, 1500)
|
||||
else:
|
||||
self.PWM.setMotorModel(600, 600, 600, 600)
|
||||
|
||||
def run(self):
|
||||
self.PWM = Motor()
|
||||
self.pwm_S = Servo()
|
||||
for i in range(30, 151, 60):
|
||||
self.pwm_S.setServoPwm('0', i)
|
||||
time.sleep(0.2)
|
||||
if i == 30:
|
||||
L = self.get_distance()
|
||||
elif i == 90:
|
||||
M = self.get_distance()
|
||||
else:
|
||||
R = self.get_distance()
|
||||
while True:
|
||||
for i in range(90, 30, -60):
|
||||
self.pwm_S.setServoPwm('0', i)
|
||||
time.sleep(0.2)
|
||||
if i == 30:
|
||||
L = self.get_distance()
|
||||
elif i == 90:
|
||||
M = self.get_distance()
|
||||
else:
|
||||
R = self.get_distance()
|
||||
self.run_motor(L, M, R)
|
||||
for i in range(30, 151, 60):
|
||||
self.pwm_S.setServoPwm('0', i)
|
||||
time.sleep(0.2)
|
||||
if i == 30:
|
||||
L = self.get_distance()
|
||||
elif i == 90:
|
||||
M = self.get_distance()
|
||||
else:
|
||||
R = self.get_distance()
|
||||
self.run_motor(L, M, R)
|
||||
|
||||
|
||||
ultrasonic = Ultrasonic()
|
||||
# Main program logic follows:
|
||||
if __name__ == '__main__':
|
||||
print('Program is starting ... ')
|
||||
try:
|
||||
ultrasonic.run()
|
||||
except KeyboardInterrupt: # When 'Ctrl+C' is pressed, the child program destroy() will be executed.
|
||||
PWM.setMotorModel(0, 0, 0, 0)
|
||||
ultrasonic.pwm_S.setServoPwm('0', 90)
|
||||
1
compLIB/__init__.py
Normal file
1
compLIB/__init__.py
Normal file
|
|
@ -0,0 +1 @@
|
|||
__version__ = "0.0.1"
|
||||
33
setup.py
Normal file
33
setup.py
Normal file
|
|
@ -0,0 +1,33 @@
|
|||
#!/usr/bin/env python3
|
||||
import setuptools
|
||||
import os
|
||||
|
||||
base_dir = os.path.dirname(__file__)
|
||||
readme_path = os.path.join(base_dir, "README.md")
|
||||
if os.path.exists(readme_path):
|
||||
with open(readme_path) as stream:
|
||||
long_description = stream.read()
|
||||
else:
|
||||
long_description = ""
|
||||
|
||||
setuptools.setup(
|
||||
name="complib",
|
||||
version="0.0.1",
|
||||
author="F-WuTs",
|
||||
author_email="--",
|
||||
description="",
|
||||
long_description=long_description,
|
||||
long_description_content_type="text/markdown",
|
||||
url="https://github.com/F-WuTS/compLIB",
|
||||
packages=setuptools.find_packages(),
|
||||
include_package_data=True,
|
||||
classifiers=[
|
||||
"Programming Language :: Python :: 3.7",
|
||||
"License :: Other/Proprietary License",
|
||||
"Operating System :: Unix"
|
||||
],
|
||||
license="proprietary",
|
||||
install_requires=[
|
||||
"smbus",
|
||||
]
|
||||
)
|
||||
Reference in a new issue