diff --git a/.idea/.gitignore b/.idea/.gitignore
deleted file mode 100644
index 26d3352..0000000
--- a/.idea/.gitignore
+++ /dev/null
@@ -1,3 +0,0 @@
-# Default ignored files
-/shelf/
-/workspace.xml
diff --git a/.idea/compLIB.iml b/.idea/compLIB.iml
deleted file mode 100644
index e5f3420..0000000
--- a/.idea/compLIB.iml
+++ /dev/null
@@ -1,11 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
deleted file mode 100644
index 105ce2d..0000000
--- a/.idea/inspectionProfiles/profiles_settings.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
deleted file mode 100644
index 1dae23d..0000000
--- a/.idea/misc.xml
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
deleted file mode 100644
index bfe6efe..0000000
--- a/.idea/modules.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
deleted file mode 100644
index 94a25f7..0000000
--- a/.idea/vcs.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/compLIB/Motor.py b/compLIB/Motor.py
index f5197dc..dd9177d 100644
--- a/compLIB/Motor.py
+++ b/compLIB/Motor.py
@@ -1,3 +1,5 @@
+import atexit
+
from compLIB.PCA9685 import PCA9685
pwm = PCA9685(0x40, debug=True)
@@ -44,3 +46,6 @@ class Motor(object):
"""
for i in range(0, MOTOR_COUNT):
Motor.power(i, 0)
+
+
+atexit.register(Motor.all_off())
diff --git a/compLIB/Servo.py b/compLIB/Servo.py
new file mode 100644
index 0000000..0a6ba68
--- /dev/null
+++ b/compLIB/Servo.py
@@ -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)
diff --git a/oldLib/ADC.py b/oldLib/ADC.py
deleted file mode 100644
index 73dcdb0..0000000
--- a/oldLib/ADC.py
+++ /dev/null
@@ -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()
diff --git a/oldLib/Buzzer.py b/oldLib/Buzzer.py
deleted file mode 100644
index b6d6965..0000000
--- a/oldLib/Buzzer.py
+++ /dev/null
@@ -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')
diff --git a/oldLib/Command.py b/oldLib/Command.py
deleted file mode 100644
index 9faddae..0000000
--- a/oldLib/Command.py
+++ /dev/null
@@ -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
diff --git a/oldLib/Motor.py b/oldLib/Motor.py
deleted file mode 100644
index 634c06a..0000000
--- a/oldLib/Motor.py
+++ /dev/null
@@ -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()
diff --git a/oldLib/Ultrasonic.py b/oldLib/Ultrasonic.py
deleted file mode 100644
index 12bff45..0000000
--- a/oldLib/Ultrasonic.py
+++ /dev/null
@@ -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)
diff --git a/oldLib/__init__.py b/oldLib/__init__.py
deleted file mode 100644
index f30a6a0..0000000
--- a/oldLib/__init__.py
+++ /dev/null
@@ -1,4 +0,0 @@
-__version__ = "0.0.1"
-
-pwm = PCA9685(0x40, debug=True)
-pwm.setPWMFreq(50)