First Commit, databaseCreator.py not working
This commit is contained in:
@@ -0,0 +1,143 @@
|
||||
import math
|
||||
import csv
|
||||
|
||||
|
||||
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
|
||||
}
|
||||
arms = {
|
||||
0: 0.84,
|
||||
1: 10.26,
|
||||
2: 9.85,
|
||||
"claw": 12,
|
||||
"height": 9
|
||||
}
|
||||
|
||||
|
||||
def summe(i):
|
||||
r = 0
|
||||
for element in i:
|
||||
r += element
|
||||
return r
|
||||
|
||||
|
||||
def get_pos(anglesdeg):
|
||||
factor = math.pi / 180
|
||||
angles = anglesdeg.copy()
|
||||
for i in angles:
|
||||
# print(angles[i])
|
||||
angles[i] = angles[i] * factor
|
||||
# print(angles[i])
|
||||
x = 0
|
||||
z = 0
|
||||
for angle in range(0, 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"])
|
||||
coordinateback = [x, z]
|
||||
# print(str(x))
|
||||
# print(str(y))
|
||||
# print(str(z))
|
||||
return coordinateback
|
||||
|
||||
|
||||
def get_angles(ms):
|
||||
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):
|
||||
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 get_max_min(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
|
||||
minimum = (val_min - val_mid) / change
|
||||
maximum = (val_max - val_mid) / change
|
||||
|
||||
return [minimum, maximum]
|
||||
|
||||
|
||||
def get_efficency(anglesdeg):
|
||||
eff = 0
|
||||
i = 1
|
||||
for servo in reversed(anglesdeg):
|
||||
eff += i * servo
|
||||
i += 1
|
||||
return eff
|
||||
|
||||
|
||||
pos = []
|
||||
for servoR in range(5):
|
||||
pos[servoR] = get_max_min(servoR)
|
||||
|
||||
"""
|
||||
pos = [x, y, z, eff]
|
||||
if eff < replace
|
||||
"""
|
||||
db = {}
|
||||
for steps in range(1, 0, -0.2):
|
||||
for servo1 in range(pos[1][0], pos[1][1], steps):
|
||||
for servo2 in range(pos[2][0], pos[2][1], steps):
|
||||
for servo3 in range(pos[3][0], pos[3][1], steps):
|
||||
for servo4 in range(pos[4][0], pos[4][1], steps):
|
||||
coordinate = []
|
||||
coordinate.append(get_pos([servo1, servo2, servo3, servo4]))
|
||||
#coordinate.append(get_efficency([servo0, servo1, servo2, servo3, servo4]))
|
||||
if coordinate in db:
|
||||
if db[coordinate][]
|
||||
@@ -0,0 +1,207 @@
|
||||
from __future__ import division
|
||||
import Adafruit_PCA9685
|
||||
import sys
|
||||
import math
|
||||
import time
|
||||
import csv
|
||||
|
||||
pwm = Adafruit_PCA9685.PCA9685(address=0x41)
|
||||
|
||||
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
|
||||
}
|
||||
arms = {
|
||||
0: 0.84,
|
||||
1: 10.26,
|
||||
2: 9.85,
|
||||
"claw": 12,
|
||||
"height": 9
|
||||
}
|
||||
|
||||
|
||||
def summe(i):
|
||||
r = 0
|
||||
for element in i:
|
||||
r += element
|
||||
return r
|
||||
|
||||
|
||||
def set_servo_pulse(channel, pulse):
|
||||
pulse_length = 1000000
|
||||
pulse_length /= 50
|
||||
pulse_length /= 4096
|
||||
pulse *= 1000
|
||||
pulse /= pulse_length
|
||||
pulse = round(pulse)
|
||||
pulse = int(pulse)
|
||||
pwm.set_pwm(channel, 0, pulse)
|
||||
|
||||
|
||||
pwm.set_pwm_freq(50)
|
||||
|
||||
|
||||
def get_pos(anglesdeg):
|
||||
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):
|
||||
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):
|
||||
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():
|
||||
"""
|
||||
:param TerminalParameter: style: python3 terminalControl.py -servo x -pos y
|
||||
:return:
|
||||
"""
|
||||
if sys.argv[1] == "-home":
|
||||
for sv in range(0, 5):
|
||||
set_servo_pulse(sv, 1.5)
|
||||
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)
|
||||
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))
|
||||
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))
|
||||
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))
|
||||
|
||||
|
||||
read_argv()
|
||||
Reference in New Issue
Block a user