379 lines
12 KiB
Python
379 lines
12 KiB
Python
from __future__ import division
|
|
|
|
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)
|
|
servo0min = 0.5
|
|
servo0max = 2.5
|
|
servo0mid = 1.85
|
|
servo4min = 0.5
|
|
servo4max = 2.5
|
|
servo4mid = 1.5
|
|
servo0actual = 0
|
|
|
|
|
|
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):
|
|
"""
|
|
Calculates the ms value for servo 0 at a given angle
|
|
:param deg: Angle in deg
|
|
:return: ms value which corresponds to the angle
|
|
"""
|
|
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):
|
|
"""
|
|
Sending the position to the servos
|
|
:param channel: servo number
|
|
:param pulse: position in ms
|
|
:return:
|
|
"""
|
|
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)
|
|
|
|
|
|
def converter_2d_to3d(hypotenuse, s0_angle):
|
|
"""
|
|
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
|
|
"""
|
|
rad = s0_angle * math.pi / 180
|
|
x = npy.cos(rad) * hypotenuse
|
|
y = npy.sin(rad) * hypotenuse
|
|
return x, y
|
|
|
|
|
|
def read_argv():
|
|
"""
|
|
Reades and processes the start arguments. The main core of the program
|
|
:param sys.argv: styles: python3 terminalControl.py -servo x -pos y(ms)
|
|
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":
|
|
argv_reader.home()
|
|
elif sys.argv[1] == "-servo":
|
|
argv_reader.servo()
|
|
elif sys.argv[1] == "-list": # -list 0,1.5 2,2 3,1.75
|
|
argv_reader.list()
|
|
elif sys.argv[1] == "-angle":
|
|
argv_reader.angle()
|
|
elif sys.argv[1] == "-csv":
|
|
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)
|