Terminal control with robot lib (#5)
* imports and model described * changed large parts of terminal Control to them using robotlib therefore also added functions to robotlib * bug fix * debug * debug * bug fix * bug fix * bug fix * sequence.csv for testing of -csv * bug fix * debug * bug fix * bug fix * extreme positions weren't possible * edited sequence.csv * added function 'output' * debug * debug * debug * bug fix * cleaned up * added loop * bug fix * serialize and deserialize added * bug fix * bug fix * bug fix * Update .gitignore terminalControl creates file to save last position, which will now not be uploaded * Comment * csv output added
This commit was merged in pull request #5.
This commit is contained in:
lade043
@@ -105,3 +105,6 @@ venv.bak/
|
||||
|
||||
# tests for fast debugging
|
||||
tests/
|
||||
|
||||
# last position of robot thru terminalControl
|
||||
.robotpos.file
|
||||
|
||||
+21
-2
@@ -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
|
||||
|
||||
+3
-5
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
+327
-191
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user