Fix motor, remove idea
This commit is contained in:
Konstantin Lampalzer
parent
71a3decedf
commit
8db1b94ad5
14 changed files with 33 additions and 386 deletions
3
.idea/.gitignore
generated
vendored
3
.idea/.gitignore
generated
vendored
|
|
@ -1,3 +0,0 @@
|
||||||
# Default ignored files
|
|
||||||
/shelf/
|
|
||||||
/workspace.xml
|
|
||||||
11
.idea/compLIB.iml
generated
11
.idea/compLIB.iml
generated
|
|
@ -1,11 +0,0 @@
|
||||||
<?xml version="1.0" encoding="UTF-8"?>
|
|
||||||
<module type="PYTHON_MODULE" version="4">
|
|
||||||
<component name="NewModuleRootManager">
|
|
||||||
<content url="file://$MODULE_DIR$">
|
|
||||||
<sourceFolder url="file://$MODULE_DIR$/compLIB" isTestSource="false" />
|
|
||||||
<excludeFolder url="file://$MODULE_DIR$/venv" />
|
|
||||||
</content>
|
|
||||||
<orderEntry type="jdk" jdkName="Python 3.8 (compLIB)" jdkType="Python SDK" />
|
|
||||||
<orderEntry type="sourceFolder" forTests="false" />
|
|
||||||
</component>
|
|
||||||
</module>
|
|
||||||
6
.idea/inspectionProfiles/profiles_settings.xml
generated
6
.idea/inspectionProfiles/profiles_settings.xml
generated
|
|
@ -1,6 +0,0 @@
|
||||||
<component name="InspectionProjectProfileManager">
|
|
||||||
<settings>
|
|
||||||
<option name="USE_PROJECT_PROFILE" value="false" />
|
|
||||||
<version value="1.0" />
|
|
||||||
</settings>
|
|
||||||
</component>
|
|
||||||
4
.idea/misc.xml
generated
4
.idea/misc.xml
generated
|
|
@ -1,4 +0,0 @@
|
||||||
<?xml version="1.0" encoding="UTF-8"?>
|
|
||||||
<project version="4">
|
|
||||||
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.8 (compLIB)" project-jdk-type="Python SDK" />
|
|
||||||
</project>
|
|
||||||
8
.idea/modules.xml
generated
8
.idea/modules.xml
generated
|
|
@ -1,8 +0,0 @@
|
||||||
<?xml version="1.0" encoding="UTF-8"?>
|
|
||||||
<project version="4">
|
|
||||||
<component name="ProjectModuleManager">
|
|
||||||
<modules>
|
|
||||||
<module fileurl="file://$PROJECT_DIR$/.idea/compLIB.iml" filepath="$PROJECT_DIR$/.idea/compLIB.iml" />
|
|
||||||
</modules>
|
|
||||||
</component>
|
|
||||||
</project>
|
|
||||||
6
.idea/vcs.xml
generated
6
.idea/vcs.xml
generated
|
|
@ -1,6 +0,0 @@
|
||||||
<?xml version="1.0" encoding="UTF-8"?>
|
|
||||||
<project version="4">
|
|
||||||
<component name="VcsDirectoryMappings">
|
|
||||||
<mapping directory="$PROJECT_DIR$" vcs="Git" />
|
|
||||||
</component>
|
|
||||||
</project>
|
|
||||||
|
|
@ -1,3 +1,5 @@
|
||||||
|
import atexit
|
||||||
|
|
||||||
from compLIB.PCA9685 import PCA9685
|
from compLIB.PCA9685 import PCA9685
|
||||||
|
|
||||||
pwm = PCA9685(0x40, debug=True)
|
pwm = PCA9685(0x40, debug=True)
|
||||||
|
|
@ -44,3 +46,6 @@ class Motor(object):
|
||||||
"""
|
"""
|
||||||
for i in range(0, MOTOR_COUNT):
|
for i in range(0, MOTOR_COUNT):
|
||||||
Motor.power(i, 0)
|
Motor.power(i, 0)
|
||||||
|
|
||||||
|
|
||||||
|
atexit.register(Motor.all_off())
|
||||||
|
|
|
||||||
28
compLIB/Servo.py
Normal file
28
compLIB/Servo.py
Normal file
|
|
@ -0,0 +1,28 @@
|
||||||
|
from compLIB.PCA9685 import PCA9685
|
||||||
|
|
||||||
|
pwm = PCA9685(0x40, debug=True)
|
||||||
|
pwm.setPWMFreq(50)
|
||||||
|
|
||||||
|
|
||||||
|
class Servo:
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def set_position(channel: int, angle: int):
|
||||||
|
"""
|
||||||
|
Set position of servo connected to port
|
||||||
|
:param channel: channel between 0 and 7
|
||||||
|
:param angle: Angle of servo
|
||||||
|
:return: None
|
||||||
|
|
||||||
|
"""
|
||||||
|
angle = abs(angle)
|
||||||
|
|
||||||
|
if channel == 0:
|
||||||
|
pwm.setServoPulse(8 + channel, 2500 - int(angle / 0.09))
|
||||||
|
elif channel < 8:
|
||||||
|
pwm.setServoPulse(8 + channel, 500 - int(angle / 0.09))
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def setup_position():
|
||||||
|
pwm.setServoPulse(8, 1500)
|
||||||
|
pwm.setServoPulse(9, 1500)
|
||||||
|
|
@ -1,95 +0,0 @@
|
||||||
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()
|
|
||||||
|
|
@ -1,23 +0,0 @@
|
||||||
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')
|
|
||||||
|
|
@ -1,13 +0,0 @@
|
||||||
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
oldLib/Motor.py
107
oldLib/Motor.py
|
|
@ -1,107 +0,0 @@
|
||||||
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()
|
|
||||||
|
|
@ -1,106 +0,0 @@
|
||||||
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,4 +0,0 @@
|
||||||
__version__ = "0.0.1"
|
|
||||||
|
|
||||||
pwm = PCA9685(0x40, debug=True)
|
|
||||||
pwm.setPWMFreq(50)
|
|
||||||
Reference in a new issue