From 66c0f8fb7ee0c8c9141898ed8f96d5bf664124e3 Mon Sep 17 00:00:00 2001 From: "L. Bogner" Date: Thu, 16 May 2019 19:22:49 +0200 Subject: [PATCH] changed large parts of terminal Control to them using robotlib therefore also added functions to robotlib --- RobotLib.py | 20 ++- terminalControl.py | 348 ++++++++++++++++++++------------------------- 2 files changed, 173 insertions(+), 195 deletions(-) diff --git a/RobotLib.py b/RobotLib.py index 7aedfeb..d74531e 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 @@ -59,12 +60,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 + class Arm: def __init__(self, attatched_to, length, height=0.0): self.attachted_to = attatched_to diff --git a/terminalControl.py b/terminalControl.py index 0e3fc1c..11324eb 100644 --- a/terminalControl.py +++ b/terminalControl.py @@ -1,12 +1,12 @@ from __future__ import division import Adafruit_PCA9685 import sys -import math import time import csv from RobotLib import * pwm = Adafruit_PCA9685.PCA9685(address=0x41) +pwm.set_pwm_freq(50) 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)), @@ -16,43 +16,154 @@ joy_it.init_depending(Robot.Arm(joy_it.servo1, 10.26, 0.84), Robot.Arm(joy_it.se Robot.Arm(joy_it.servo3, 12, 9), 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 -def summe(i): - """ - Returns the sum of a list - :param i: list with float, int - :return: int sum - """ - r = 0 - for element in i: - r += element - return r +class argvReader: + def __init__(self, argv): + self.argv = argv + + def home(self): + 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)) + + def servo(self): + 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 servo_int <= 3: + if not servo.geometry.is_inside(pos): + print("Position to big or to small") + sys.exit() + elif 0 < servo < 5: + print("Servo is not on robotarm") + else: + servo.set_ms(pos) + set_servo_pulse(servo_int, servo.ms) + elif servo_int <= 5: + set_servo_pulse(servo_int, pos) + else: + sys.exit() + print(str(servo_int) + ": " + str(pos)) + + def list(self): + 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() + elif 0 < servo < 5: + print("Servo is not on robotarm") + 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) + else: + sys.exit() + print(str(servo_int) + ": " + str(pos)) + + def angle(self): + 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() + elif 0 < servo < 5: + print("Servo is not on robotarm") + 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) + else: + sys.exit() + + def csv(self): + file = self.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: + for servo_int, pos in enumerate(row): + 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() + elif 0 < servo < 5: + print("Servo is not on robotarm") + 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) + else: + sys.exit() + + +def get_ms_servo4(deg): + change = 1 / 90 + if servo4min > servo4max: + change *= -1 + ms = (deg * change + servo4mid) + return ms + + +def get_ms_servo0(deg): + change = 1 / 90 + if servo0min > servo0max: + change *= -1 + ms = (deg * change + servo0mid) + return ms def set_servo_pulse(channel, pulse): @@ -72,91 +183,6 @@ def set_servo_pulse(channel, pulse): pwm.set_pwm(channel, 0, pulse) -pwm.set_pwm_freq(50) - - -def get_pos(anglesdeg): - """ - 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 - """ - 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 - - def read_argv(): """ Reades and processes the start arguments. The main core of the program @@ -167,83 +193,17 @@ def read_argv(): :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() +argv_reader = argvReader(sys.argv) read_argv() +