changed large parts of terminal Control to them using robotlib
therefore also added functions to robotlib
This commit is contained in:
+19
-1
@@ -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
|
||||
|
||||
+154
-194
@@ -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()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user