forked from petroolg/robo-spline
-
Notifications
You must be signed in to change notification settings - Fork 1
/
robCRSgripper.py
86 lines (75 loc) · 3.73 KB
/
robCRSgripper.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
# Coordinated Spline Motion and Robot Control Project
#
# Copyright (c) 2017 Olga Petrova <[email protected]>
# Advisor: Pavel Pisa <[email protected]>
# FEE CTU Prague, Czech Republic
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# In 2017, project funded by PiKRON s.r.o. http://www.pikron.com/
def robCRSgripper(commander, power):
"""
Clench/open CRS gripper.
:param commander: Robot controller. Instance of Commander.
:param power: Power to apply in gripper.
"""
b = commander.robot.gripper_bounds
f = commander.robot.gripper_bounds_force
if abs(power) <= 0.1: # 'position' mode
pos = ((0.1 - power) * b[0] + (power - 0.1) * b[1] + 0.1) / 0.2
else:
# 'force' mode
# the force is simulated such that we set the position outside
# the physical range and the force is generated by P component of
# PID regulator(the I component must be definitely set to 0!)
if power > 0: # set pos between b[1] and b[1]
pos = ((1 - power) * b[1] + (power - 0.1) * f[1]) / 0.9
else: # set pos between b[0] and b[0]
power = -power
pos = ((1 - power) * b[0] + (power - 0.1) * f[0]) / 0.9
# Release errors and reset controller
commander.send_cmd('RELEASE%s:\n' % commander.robot.gripper_ax)
# Set position on gripper
commander.send_cmd('G%s:%d\n' % (commander.robot.gripper_ax, pos))
# Release motor of open gripper
if power == 0:
commander.wait_gripper_ready()
commander.send_cmd('RELEASE%s:\n' % commander.robot.gripper_ax)
def robCRSgripperinit(controller):
"""
Initialization of CRS robot gripper.
:param controller: Robot controller. Instance of Commander.
"""
m = controller.robot.gripper_ax
# Set analog mode of controller
controller.send_cmd('ANAXSETUP%s:%i,%i\n' %(m, controller.robot.gripper_ADC, controller.robot.gripper_current))
# Maximal curent limit(0 - 255)
controller.send_cmd('REGS1%s:%i\n' % (m, controller.robot.gripper_current))
# Limitation constant(feedback from overcurrent)
controller.send_cmd('REGS2%s:%i\n' % (m, controller.robot.gripper_feedback))
# Maximal energy limits voltage on motor
controller.send_cmd('REGME%s:%i\n' % (m, controller.robot.gripper_REGME))
# Maximal speed
controller.send_cmd('REGMS%s:%i\n' % (m, controller.robot.gripper_REGMS))
# Axis configuration word
controller.send_cmd('REGCFG%s:%i\n' % (m, controller.robot.gripper_REGCFG))
# PID parameters of controller
controller.send_cmd('REGP%s:%i\n' % (m, controller.robot.gripper_REGP))
controller.send_cmd('REGI%s:%i\n' % (m, controller.robot.gripper_REGI))
controller.send_cmd('REGD%s:%i\n' % (m, controller.robot.gripper_REGD))