forked from petroolg/robo-spline
/
robotBosch.py
120 lines (93 loc) · 4.45 KB
/
robotBosch.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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
# Coordinated Spline Motion and Robot Control Project
#
# Copyright (c) 2017 Olga Petrova <olga.petrova@cvut.cz>
# Advisor: Pavel Pisa <pisa@cmp.felk.cvut.cz>
# 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/
import numpy as np
from robotBoschikt import robotBoschikt
# based on BlueBot and Bosch Toolbox
# by O. Certik, V. Smutny, P. Krsek, M. Matousek
class robotBosch:
"""Robot specification: Bosch"""
def __init__(self):
# Robot link dimensions (these constants are read from documentation)
self.L1 = 250
self.L2 = 200
# Pavel Krsek measured 27th of September 2003
# self.degtoirc = [947.4865, 611.9475, ? 395.9611]
self.irc = [-1080, -1080, 1080, 1080]
# Joint motors' gearing
# self.gearing = [80 50 1 (3*11)] # original numbers
# self.gearing = [78.9572, 50.9956, 1 32.9967]
self.gearing = [78.9572, 50.9956, 37, 32.9967]
# TODO joint 3 must be precisely calibrated
# 11200 (134428) = 300mm
# ------------------------------------------------------------------------------
# General parameters (required)
self.description = 'Bosch SR450s'
self.DOF = 4
self.activemotors = 'ABCD'
self.hh_axes_list = 'CABD'
self.control_axes_list = 'ABCD'
self.coord_axes = 'ABCD'
# Softhome position in IRC and degrees/mm.
self.hhirc = [0, 0, 0, 0]
self.hhdeg = [0, 0, 0, 0]
# 1 degree/mm in IRC units
self.degtoirc = np.multiply(self.irc, self.gearing) * 4.0 / 360.0
# Robot bounds in deg/mm converted to IRC
self.bound = np.array([[ -80536, -73434, -146520, -7919],
[ 80536, 73434, 2220, 427637]])
# 2010-01-29, Martin Matousek
# this are the realy hard bounds. Its no possible to
# to reach them with full speed.
# robot.bound should be chosen with some reserve
truebound = np.array([[-86200, -79000, -147000, -9000],[86900, 75400, 2500, 428000]])
if not (np.all(self.bound[0] > truebound[0]) and np.all(self.bound[1] < truebound[1])):
raise ValueError("Internal error in robot parameters.")
# Speeds (IRC/256/msec)
self.minspeed = [1000, 1000, 1000, 1000]
self.maxspeed = [10000, 10000, 5000, 5000]
self.defaultspeed = [10000, 10000, 5000, 5000]
# Accelerations
self.minacceleration = [0, 0, 1, 1]
self.maxacceleration = [100, 100, 100, 100]
self.defaultacceleration = [30, 30, 20, 30]
self.controller = 'MARS8'
# ------------------------------------------------------------------------------
# General parameters (optional)
self.ikt = robotBoschikt
self.portname = 'COM2' # corrected from COM4 26.3.2013 by Smutny
self.BaudRate = 9600 # Comport baud rate (default 19200 baud)
self.REGPWRON = 1 # user must press arm power button
self.REGPWRFLG = 5
self.REGCFG = [1498, 1498, 1370, 1498]
# Set energies and PID
self.REGME = [30000, 16000, 16000, 16000]
self.REGP = [50, 50, 50, 50]
self.REGI = [5, 5, 5, 5]
self.REGD = [50, 50, 50, 50]
# Bosch robot shall not release after some time
self.IDLEREL = 0
# This variable use bbmovex, for help see this fucntion.
self.pose = 0 # TODO