diff --git a/.gitignore b/.gitignore index 36948be..abef767 100644 --- a/.gitignore +++ b/.gitignore @@ -105,3 +105,6 @@ venv.bak/ # tests for fast debugging tests/ + +# last position of robot thru terminalControl +.robotpos.file diff --git a/RobotLib.py b/RobotLib.py index f61571c..db5b5d8 100644 --- a/RobotLib.py +++ b/RobotLib.py @@ -3,7 +3,7 @@ import math class Robot: class Servo: - def __init__(self, angle, geometry, previous_servos=None): + def __init__(self, angle, geometry): self.deg = angle self.rad = self._get_rad(self.deg) self.geometry = geometry @@ -12,6 +12,7 @@ class Robot: self.total_angle_deg = None self.total_angle_rad = None self.delta = None + self.ms = None def add_previous_servos(self, previous_servos=None): self.previous_servos = previous_servos @@ -32,6 +33,7 @@ class Robot: def _calc_previous(self): if self.previous_servos: + self.total_angle_deg = 0 for servo in self.previous_servos: self.total_angle_deg += servo.deg self.delta = self.deg + self.previous_servos[-1].deg @@ -39,9 +41,9 @@ class Robot: self.delta *= -1 else: self.delta = self.deg + self.total_angle_deg = self.deg if self.delta < 0: self.delta *= -1 - self.total_angle_rad = self._get_rad(self.total_angle_deg) def set_angle(self, angle): self.deg = angle @@ -61,12 +63,29 @@ class Robot: def _get_max_min(self): return self._get_angle(self.geometry.max), self._get_angle(self.geometry.min) + def set_ms(self, ms): + self.ms = ms + self.deg = self._get_angle(self.ms) + self.rad = self._get_rad(self.deg) + self._calc_previous() + + def get_ms(self, calc=False): + if calc: + change = 1 / 90 + if self.geometry.min > self.geometry.max: + change *= -1 + self.ms = (self.deg * change + self.geometry.mid) + return self.ms + class Geometry: def __init__(self, _max, _min, mid): self.max = _max self.min = _min self.mid = mid + def is_inside(self, ms): + return self.min <= ms <= self.max or self.max <= ms <= self.min + class Arm: def __init__(self, attatched_to, length, height=0.0): self.attachted_to = attatched_to diff --git a/csv_analyse.py b/csv_analyse.py index 69512a3..986c441 100644 --- a/csv_analyse.py +++ b/csv_analyse.py @@ -1,7 +1,7 @@ import time import csv -file = "/home/lars/Schreibtisch/sequence.csv" +file = "sequence.csv" with open(file) as f: csv_reader = csv.reader(f, delimiter=',') for row in csv_reader: @@ -9,8 +9,6 @@ with open(file) as f: print("sleep " + row[0].split(" ")[1]) time.sleep(int(row[0].split(" ")[1])) else: - f = 0 - for i in row: + for count, i in enumerate(row): print(i) - print(str(f)) - f += 1 + print(count) diff --git a/databaseCreatorv2.py b/databaseCreatorv2.py index 8b1b57a..c38b312 100644 --- a/databaseCreatorv2.py +++ b/databaseCreatorv2.py @@ -34,7 +34,7 @@ test = Robot(Robot.Servo(1, Robot.Servo.Geometry(0.55, 2.3, 1.4)), Robot.Servo(2, Robot.Servo.Geometry(2.5, 0.55, 1.55)), Robot.Servo(3, Robot.Servo.Geometry(0.7, 2.25, 2.25)), Robot.CoordinateSystem([-100, 100], [-100, 100])) -test.init_depending(Robot.Arm(test.servo1, 10.26, 0.84), Robot.Arm(test.servo2, 9.85), Robot.Arm(test.servo3, 12, 9), +test.init_depending(Robot.Arm(test.servo1, 10.26, 0.84), Robot.Arm(test.servo2, 9.85), Robot.Arm(test.servo3, 12, -3), None, [test.servo1], [test.servo1, test.servo2]) looper = Looper(test) diff --git a/sequence.csv b/sequence.csv new file mode 100644 index 0000000..ba9d7d5 --- /dev/null +++ b/sequence.csv @@ -0,0 +1,7 @@ +0,0,0,0,0 +delay 10,,,, +10,10,10,10,10 +delay 5,,,, +20,0,70,70,20 +delay 3,,,, +0,0,0,0,0 \ No newline at end of file diff --git a/terminalControl.py b/terminalControl.py index 92a8a90..62b5f1f 100644 --- a/terminalControl.py +++ b/terminalControl.py @@ -1,51 +1,304 @@ from __future__ import division -import Adafruit_PCA9685 -import sys -import math -import time + import csv +import sys +import time +import Adafruit_PCA9685 +import numpy as npy + +from RobotLib import * + +# setting up the Servo Controller pwm = Adafruit_PCA9685.PCA9685(address=0x41) +pwm.set_pwm_freq(50) +# file with latest given servo pos +file = ".robotpos.file" + +# defining robot model of joy_it +joy_it = Robot(Robot.Servo(1, Robot.Servo.Geometry(0.55, 2.3, 1.4)), + Robot.Servo(2, Robot.Servo.Geometry(2.5, 0.55, 1.55)), + Robot.Servo(3, Robot.Servo.Geometry(0.7, 2.25, 2.25)), + Robot.CoordinateSystem([-100, 100], [-100, 100])) +joy_it.init_depending(Robot.Arm(joy_it.servo1, 10.26, 0.84), Robot.Arm(joy_it.servo2, 9.85), + Robot.Arm(joy_it.servo3, 12, -3), None, [joy_it.servo1], [joy_it.servo1, joy_it.servo2]) # the values for all the servos (are different for each robot) -geometry = { - "servo0min": 0.5, - "servo0max": 2.5, - "servo0mid": 1.85, - "servo1min": 2.3, - "servo1max": 0.55, - "servo1mid": 1.4, - "servo2min": 0.55, - "servo2max": 2.5, - "servo2mid": 1.55, - "servo3min": 2.25, - "servo3max": 0.7, - "servo3mid": 2.25, - "servo4min": 0.5, - "servo4max": 2.5, - "servo4mid": 1.5 - } -# the length of the arms (same for each joy it robot) -arms = { - 0: 0.84, - 1: 10.26, - 2: 9.85, - "claw": 12, - "height": 9 -} +servo0min = 0.5 +servo0max = 2.5 +servo0mid = 1.85 +servo4min = 0.5 +servo4max = 2.5 +servo4mid = 1.5 +servo0actual = 0 -def summe(i): +class argvReader: + def __init__(self, argv): + """ + Takes the values from a list of arguments and controls the servos accordingly + :param argv: The arguments, that should be analyzed + """ + self.argv = argv + + def set_argument(self, argv): + """ + sets the argument if it change since the init + :param argv: The arguments, that should be analyzed + :return: + """ + self.argv = argv + + def output(self, s0_angle): + """ + Calculates the position of the claw and outputs it with the angles of the servos + :param s0_angle: Angle of servo 0 for conversion from 2d to 3d + :return: Console output + """ + joy_it.calculate() + x = joy_it.x + z = joy_it.y + x, y = converter_2d_to3d(x, s0_angle) + print("x:{},y:{},z:{}".format(x, y, z)) + print("servo0:{}, servo1:{}, servo2:{}, servo3:{}".format(servo0actual, joy_it.servo1.deg, joy_it.servo2.deg, + joy_it.servo3.deg)) + + def home(self): + """ + Controls all servos to correspond the home position + :return: sets all servos to 0deg + """ + global servo0actual + for i, servo in enumerate([joy_it.servo1, joy_it.servo2, joy_it.servo3]): + servo.set_angle(0) + set_servo_pulse(i+1, servo.get_ms(True)) + ms = get_ms_servo0(0) + set_servo_pulse(0, ms) + ms = get_ms_servo4(0) + set_servo_pulse(4, ms) + servo0actual = 0 + self.output(0) + + def servo(self): + """ + Controls one servo to according to the given ms value + :return: sets one servo to the angle that correspond the ms value + """ + global servo0actual + servo_int = int(self.argv[2]) + pos = float(self.argv[4]) + if servo_int == 1: + servo = joy_it.servo1 + elif servo_int == 2: + servo = joy_it.servo2 + elif servo_int == 3: + servo = joy_it.servo3 + if 0 < servo_int <= 3: + if not servo.geometry.is_inside(pos): + print("Position to big or to small") + sys.exit() + else: + servo.set_ms(pos) + set_servo_pulse(servo_int, servo.ms) + elif servo_int <= 5: + set_servo_pulse(servo_int, pos) + if servo_int == 0: + servo0actual = get_angle_servo0(pos) + else: + sys.exit() + self.output(servo0actual) + + def list(self): + """ + Controls multiple servos according to their given ms values + :return: sets multiple servos to their ms values + """ + global servo0actual + commands = self.argv[2:] + for entry in commands: + servo_int = int(entry.split(',')[0]) + pos = float(entry.split(',')[1]) + if servo_int == 1: + servo = joy_it.servo1 + elif servo_int == 2: + servo = joy_it.servo2 + elif servo_int == 3: + servo = joy_it.servo3 + if 0 < servo_int <= 3: + if not servo.geometry.is_inside(pos): + print("Position to big or to small") + sys.exit() + else: + servo.set_ms(pos) + set_servo_pulse(servo_int, servo.ms) + elif servo_int <= 5 or servo_int == 0: + set_servo_pulse(servo_int, pos) + if servo_int == 0: + servo0actual = get_angle_servo0(pos) + else: + sys.exit() + self.output(servo0actual) + + def angle(self): + """ + Controls multiple servos according to their given angle + :return: sets multiple servos to the angle that is given + """ + global servo0actual + commands = self.argv[2:] + for entry in commands: + servo_int = int(entry.split(',')[0]) + pos = float(entry.split(',')[1]) + if servo_int == 1: + servo = joy_it.servo1 + elif servo_int == 2: + servo = joy_it.servo2 + elif servo_int == 3: + servo = joy_it.servo3 + if 0 < servo_int <= 3: + servo.set_angle(pos) + ms = servo.get_ms(True) + if not servo.geometry.is_inside(ms): + print("Position to big or to small") + sys.exit() + else: + set_servo_pulse(servo_int, servo.ms) + elif servo_int == 4: + ms = get_ms_servo4(pos) + set_servo_pulse(servo_int, ms) + elif servo_int == 0: + ms = get_ms_servo0(pos) + set_servo_pulse(servo_int, ms) + servo0actual = pos + else: + sys.exit() + self.output(servo0actual) + + def csv(self): + """ + Follows an procedure given by an csv file + :return:sets all the servos to the angles from the csv file + """ + csv_file = self.argv[2] + global servo0actual + + with open(csv_file) as f: + csv_reader = csv.reader(f, delimiter=',') + for row in csv_reader: + if "delay" in row[0]: + print("sleep " + row[0].split(" ")[1]) + time.sleep(int(row[0].split(" ")[1])) + else: + for servo_int, pos in enumerate(row): + pos = int(pos) + if servo_int == 1: + servo = joy_it.servo1 + elif servo_int == 2: + servo = joy_it.servo2 + elif servo_int == 3: + servo = joy_it.servo3 + if 0 < servo_int <= 3: + servo.set_angle(pos) + ms = servo.get_ms(True) + if not servo.geometry.is_inside(ms): + print("Position to big or to small") + sys.exit() + else: + set_servo_pulse(servo_int, servo.ms) + elif servo_int == 4: + ms = get_ms_servo4(pos) + set_servo_pulse(servo_int, ms) + elif servo_int == 0: + ms = get_ms_servo0(pos) + set_servo_pulse(servo_int, ms) + servo0actual = pos + else: + sys.exit() + argv_reader.output(servo0actual) + + def serialize(self, filepath): + """ + Prints the current position to an file + :param filepath: filepath of the file to which the position should be written to + :return: The angles of servo0 to servo3 seperated by a ',' + """ + with open(filepath, 'w') as file: + string = "{},{},{},{}".format(servo0actual, joy_it.servo1.deg, joy_it.servo2.deg, joy_it.servo3.deg) + file.write(string) + + def deserialize(self, filepath): + """ + Sets all position variables to the values from the file + :param filepath: filepath of the file from which should be read + :return: The variables are set accordingly + """ + global servo0actual + try: + with open(filepath, 'r') as file: + string = file.readline() + string = string.split(",") + servo0actual = float(string[0]) + joy_it.servo1.set_angle(float(string[1])) + joy_it.servo2.set_angle(float(string[2])) + joy_it.servo3.set_angle(float(string[3])) + except FileNotFoundError: + servo0actual = 0 + joy_it.servo1.set_angle(0) + joy_it.servo2.set_angle(0) + joy_it.servo3.set_angle(0) + + +def get_ms_servo0(deg): """ - Returns the sum of a list - :param i: list with float, int - :return: int sum + Calculates the ms value for servo 0 at a given angle + :param deg: Angle in deg + :return: ms value which corresponds to the angle """ - r = 0 - for element in i: - r += element - return r + change = 1 / 90 + if servo0min > servo0max: + change *= -1 + ms = (deg * change + servo0mid) + return ms + + +def get_angle_servo0(ms): + """ + Calculates the angle for servo0 at a given ms-value + :param ms: ms value + :return: angle which corresponds to the ms value + """ + change = 1 / 90 + if servo0min > servo0max: + change *= -1 + angle = (ms - servo0mid) / change + return angle + + +def get_ms_servo4(deg): + """ + see 'get_ms_servo0' + :param deg: + :return: + """ + change = 1 / 90 + if servo4min > servo4max: + change *= -1 + ms = (deg * change + servo4mid) + return ms + + +def get_angle_servo4(ms): + """ + see 'get_angle_servo0' + :param ms: + :return: + """ + change = 1 / 90 + if servo4min > servo4max: + change *= -1 + angle = (ms - servo4mid) / change + return angle def set_servo_pulse(channel, pulse): @@ -65,89 +318,17 @@ def set_servo_pulse(channel, pulse): pwm.set_pwm(channel, 0, pulse) -pwm.set_pwm_freq(50) - - -def get_pos(anglesdeg): +def converter_2d_to3d(hypotenuse, s0_angle): """ - Calculates the position of the claw relative to the center of the baseplate - Further details in documentation - :param anglesdeg: dict with the servo number and position in degree (deg) - :return: list: [x, y, z] in cm from the center of the baseplate of the robot + Converts the 2d model to an 3d model by using the angle of s0 + :param hypotenuse: The distance from the base to the claw in x direction + :param s0_angle: Angle in deg of servo0 + :return: The calculated x and y coordinates """ - factor = math.pi / 180 - angles = anglesdeg.copy() - for i in angles: - # print(angles[i]) - angles[i] = angles[i] * factor - # print(angles[i]) - x = 0 - y = 0 - z = 0 - for angle in range(1, 3): - x += (math.sin(summe([angles[i] for i in range(1, angle + 1)])) * arms[angle]) - z += (math.cos(summe([angles[i] for i in range(1, angle + 1)])) * arms[angle]) - x += arms[0] - z += arms["height"] - angle = 3 - x += (math.sin(summe([angles[i] for i in range(1, angle + 1)]) - 15 * factor) * arms["claw"]) - z += (math.cos(summe([angles[i] for i in range(1, angle + 1)]) - 15 * factor) * arms["claw"]) - y += (math.sin(angles[0]) * x) - x = (math.cos(angles[0]) * x) - coordinate = [x, y, z] - # print(str(x)) - # print(str(y)) - # print(str(z)) - return coordinate - - -def get_angles(ms): - """ - Calculates the theoretical angles relative to a vertical position based on the ms values for each of the servos - :param ms: dict with the servo number and ms - :return: dict with the servo number and position in degree (deg) - """ - angles = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0} - for servo in angles: - if ms[servo]: - minimum = "servo" + str(servo) + "min" - maximum = "servo" + str(servo) + "max" - middle = "servo" + str(servo) + "mid" - val_min = geometry[minimum] - val_max = geometry[maximum] - val_mid = geometry[middle] - change = 1 / 90 - if val_min > val_max: - change *= -1 - angles[servo] = (ms[servo] - val_mid) / change - return angles - - -def get_ms(servo, angle): - """ - Calculates the ms value based on the given angle (deg) - :param servo: servo number - :param angle: angle in degree based of the vertical position - :return: ms value for the servo - """ - minimum = "servo" + str(servo) + "min" - maximum = "servo" + str(servo) + "max" - middle = "servo" + str(servo) + "mid" - val_min = geometry[minimum] - val_max = geometry[maximum] - val_mid = geometry[middle] - change = 1/90 - if val_min > val_max: - change *= -1 - ms = (angle * change + val_mid) - if change > 0: - if ms < val_min or ms > val_max: - return 0 - else: - if ms > val_min or ms < val_max: - return 0 - # ms = round(ms, 2) - return ms + rad = s0_angle * math.pi / 180 + x = npy.cos(rad) * hypotenuse + y = npy.sin(rad) * hypotenuse + return x, y def read_argv(): @@ -157,86 +338,41 @@ def read_argv(): python3 terminalControl.py -list servo,pos(ms) servo,pos(ms) python3 terminalControl.py -angle servo,angle servo,angle python3 terminalControl.py -csv file + python3 terminalControl.py -loop + > -servo x -pos y(ms) + > ... :return: """ if sys.argv[1] == "-home": - for sv in range(0, 5): - set_servo_pulse(sv, 1.5) + argv_reader.home() elif sys.argv[1] == "-servo": - servo = int(sys.argv[2]) - pos = float(sys.argv[4]) - if pos < 1 or pos > 2.5: - print("Position to big or to small") - sys.exit() - elif servo < 0 or servo > 5: - print("Servo is not on robotarm") - print(str(servo) + ": " + str(pos)) - - set_servo_pulse(servo, pos) + argv_reader.servo() elif sys.argv[1] == "-list": # -list 0,1.5 2,2 3,1.75 - commands = sys.argv[2:] - varTime = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0} - for entry in commands: - servo = int(entry.split(',')[0]) - pos = float(entry.split(',')[1]) - if servo != 4 or servo != 5: - varTime[servo] = pos - print(str(servo) + ": " + str(pos)) - set_servo_pulse(servo, pos) - calcangles = get_angles(varTime) - calcpos = get_pos(calcangles) - for sentry in calcangles: - print(str(sentry) + ": " + str(round(calcangles[sentry]))) - for sentry in calcpos: - print(str(calcpos.index(sentry)) + ": " + str(sentry)) + argv_reader.list() elif sys.argv[1] == "-angle": - commands = sys.argv[2:] - # calcangles = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0} - varTime = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0} - for entry in commands: - servo = int(entry.split(',')[0]) - pos = float(entry.split(',')[1]) - # calcangles[servo] = pos - pos = get_ms(servo, pos) - if servo != 4: - varTime[servo] = pos - print(str(servo) + ": " + str(pos)) - set_servo_pulse(servo, pos) - calcangles = get_angles(varTime) - calcpos = get_pos(calcangles) - for entry in calcangles: - print(str(entry) + ": " + str(round(calcangles[entry]))) - for entry in calcpos: - print(str(calcpos.index(entry)) + ": " + str(entry)) + argv_reader.angle() elif sys.argv[1] == "-csv": - file = sys.argv[2] - - with open(file) as f: - csv_reader = csv.reader(f, delimiter=',') - for row in csv_reader: - if "delay" in row[0]: - print("sleep " + row[0].split(" ")[1]) - time.sleep(int(row[0].split(" ")[1])) - else: - print(row) - varTime = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0} - f = 0 - for i in row: - servo = f - print(str(servo)) - pos = int(i) - pos = get_ms(servo, pos) - if servo != 4: - varTime[servo] = pos - print(str(servo) + ": " + str(pos)) - set_servo_pulse(servo, pos) - f += 1 - calcangles = get_angles(varTime) - calcpos = get_pos(calcangles) - for entry in calcangles: - print(str(entry) + ": " + str(round(calcangles[entry]))) - for entry in calcpos: - print(str(calcpos.index(entry)) + ": " + str(entry)) + argv_reader.csv() + elif sys.argv[1] == "-loop": + while True: + _input = input(">").split(" ") + _input.insert(0, "loop") + argv_reader.set_argument(_input) + if _input[1] == "-home": + argv_reader.home() + elif _input[1] == "-servo": + argv_reader.servo() + elif _input[1] == "-list": # -list 0,1.5 2,2 3,1.75 + argv_reader.list() + elif _input[1] == "-angle": + argv_reader.angle() + elif _input[1] == "-csv": + argv_reader.csv() + elif _input[1] == "-exit": + break +argv_reader = argvReader(sys.argv) +argv_reader.deserialize(file) read_argv() +argv_reader.serialize(file)