Моделирование и программирование робототехнических комплексов PDF

..
KPEMfIEB,
.. , .. fl
.r:~I
2013

. 5
1. LEGO MINDSTORMS . . 7
1.1 LEGO MINDSTORMS NXT...... . 9
1.2 EMBEDDED CODER ROBOT NXT
nxtOSEK. 11
1.2.1 ECRobot. 12
1.2.2 ECRobot.. 15
1.2.3 ECRobot NXT. 17
1.3
LEGO MINDSTORMS.
26
1.3.1 .
30
1.3.1.1 30
1.3.1.2
32
1.3.1.3 .
33
1.3.1.4 . 34
1.3.1.5 Motobot..
35
1.3.1.6 39
1.3.2 Segway.. 40
1.3.2.1 .. 40
1.3.2.2 44
1.3.2.3 . 45
1.3.2.4 . 46
1.3.2.5 Segway 47
1.3.2.6 55
1.3.3 .. 56
1.3.3.1 .. 56
1.3.3.2 . 59
3
1.3.3.3 . 61
1.3.3.4 Ballbot..
61
1.3.3.5 67
2 BIOLOID PREMIUM KIT.. 69
2.1 Dynamixel 71
2.1.1 Dynamixel.. 74
2.1.2 Dynamixel. 75
2.2 .. 76
2.3 BIOLOID
.. 83
2.3.1 CM-510..
84
2.3.2 Dynamixel.
85
2.3.3 . 88
2.3.4 88
2.3.5 . 89
2.3.6 .. 91
2.3.7 . 92
2.3.8 92
2.3.9 Dynamixel 94
2.3.10 95
2.3.11 Zigbee . 98
2.3.12 Bioloid
.. 101
. 113
. 115

.
, ;
.
:
, . ,
,
, , , ,
, .
,
.
, , ,
, ,
[2-3, 5].
,
.

. , ,
.

[1, 3, 4, 7].
,
. , ,

.
,
, , . ,
,

,
.
,
: VexRobotis, Mechatronics Control Kit,
FestoDidactic, LEGO Mindstorms, OLLO, fischertechnik, Bioloid .

, : LEGO
Mindstorms Bioloid.
5
,
,
,
.. , ,
,
Bioloid, ,
Lego [19-21].

(Lego Mindstorms 2.0 Bioloid Premium Kit),
. ,

.
,
221000.62
.
,
, ,
,
,
,
.
,
,
, .
1 LEGO MINDSTORMS
LEGO Mindstorms (
) .
LEGO 1998 . 8 (2006)
LEGO Mindstorms NXT, 2009 LEGO Mindstorms NXT 2.0.
LEGO Mindstorms NXT 2.0:
NXT
( 1.1).
1.1 NXT LEGO

4 ( 1.2 1.4):

;
;
: 6
(, , , , ),
.
1.2
1.3
1.4
3
( 1.5).
1.5
USB .
7 .
.
Windows Mac OS.
.
613 Lego ( 1.6).
1.6 - Lego LEGO Mindstorms NXT 2.0

, ,
LEGO,
(HiTechnic, Mindsensors, Dexter Industries).

1.6:
1.6 ) - HiTechnic NXT; )

Lego 9749; ) - HiTechnic NXT; )
NXTCam-v4 Mindsensors
Lego Mindstorms
, , ,
, ..

.
1.1
LEGO
MINDSTORMS NXT
LEGO MINDSTORMS NXT
Software [9] NXT-G
,
.

NXT,
( 1.1).
(, ,
,
.) .
1.1 NXT,
NXC
www.bricxcc.sourceforge.net
NXT-G
www.mindstorms.lego.com
RobotC
www.robotc.net
RoboLab
www.mindstorms.lego.com
Microsoft Robotics Developer Studio
www.mrds.com
LabVIEW Education Edition
www.ni.com
NBC
www.bricxcc.sourceforge.net
pbLua
www.hempeldesigngroup.com
ICON
www.teamhassenplug.org
leJOS NXJ
www.lejos.sourceforge.net
LEJOS OSEK
www.lejos-osek.sourceforge.net
, LabView MATLAB/Simulink
NXT - , ,
. ,
, ,
, MATLAB/Simulink
LabView
, - .
1.2
LEGO Mindstorms NXT
.
1.2 LEGO Mindstorms
NXT.
Robot C
NXC
leJOS NXJ
nxtOSEK
leJOS
LabView
Toolkit
Java
/++
, Simulink
leJOS
MATLAB/
Simulink
leJOS
10
LabView
MATLAB/
Simulink
LEGO MINDSTORM
Toolkit LabView Embedded Coder Robot NXT MATLAB/Simulink.
, LabView,
LEGO. LabView ,

, .
MATLAB
leJOS.
,
, NXC leJOS.
NXC (Not Exactly C) C-
NXT ,
LEGO.
IDE, ,
,
.
LeJOS NXT
leJOS
(Java).
[9].
leJOS
, , ..
1.2 EMBEDDED CODER ROBOT
NXT
nxtOSEK
NxtOSEK ( 2008 LEJOS OSEK)
: leJOS NXJ TOPPERS OSEK [11, 12]. LeJOS NXJ
ANSI C/C++ NXT
API-. ,
11
NXT ,
TOPPERS
OSEK
.
nxtOSEK
Embedded Coder Robot NXT ( ECRobot)
MATLAB/Simulink,
.
Simulink-
. nxtOSEK
.
1.2.1 ECRobot
ECRobot

NXT. [13]:
1. , MATLAB
, . :
. C:\MATLAB
. C:\MATLAB R2010a
2. , MATLAB ( R2008b),
Simulink, Real-Time Workshop and Real-Time Workshop Embedded
Coder. MATLAB
ver, MATLAB
.
3. Cygwin / GNU Make
. Cygwin 1.5.x - Cygwin:
http://www.cygwin.com/
. Cygwin C:\cygwin ( 1.6)
. make Devel .
12
1.6 Cygwin
4. GNU ARM
. GCC-4.0.2 FILES GNU ARM: http://www.gnuarm.com/
. GNU ARM
C:/cygwin/GNUARM
. 1.7
. Install Cygwin DLLs, Cygwin
( 1.8)
.
.
1.7 GNU
1.8 GNU
ARM(1)
ARM(2)
5. NXT USB:
. ( LEGO
)
MINSTORMS NXT Driver v1.02 -
LEGO:
http://mindstorms.lego.com/Support/Updates/ - . . 1.9
13
1.9 Mindstorms
6. NeXTTool:
. NeXTTool http://bricxcc.sourceforge.net/utilities.html
c:\cygwin/nexttool
7. NXT:
. NXT
, : http://bricxcc.sourceforge.net/lms_arm_jch.zip
( 1.06 )
. C:\cygwin\nexttool
lms_arm_nbcnxc_X, X=106
1.06
. Windows =>=>
cmd ok
. c:
. cd cygwin\NeXTTool
. NXT USB .
, ( )
, , lms_arm_nbcnxc_106 ,
b.
nexttool /COM=usb firmware=lms_arm_nbcnxc_106.rfw
8. Embedded Coder Robot NXT:
. ECROBOT NXT
http://www.mathworks.fr/matlabcentral/fileexchange/13399
MATLAB.
9. nxtOSEK ecrobotnxtsetup.m:
14
. nxtOSEK - nxtOSEK( http://lejososek.sourceforge.net/download.htm?group_id=196690 )

ecrobotNXT/environment ( 1.10).
1.10 nxtOSEK ecrobotNXT/environment

. m- ecrobotmxtsetup.m
1.11. .
1.11 ecrobotnxtsetup
1.2.2 ECRobot

MATLAB/Simulink ECRobot NXT Blockset,
Simulink Library Browser ECRobot.
NXT
(, , Bluetooth- ..),
.
1.2.3 ECRobot NXT
, ,
-.
MATLAB Real-Time Workshop Embedded Coder Real-Time
Workshop.
15

.rxe. GNU ARM toolchain 4.0.2.
Cygwin ( 1.5) Linux-
Windows, Linux-
Windows. Cygwin GCC (GNU Compiler Collection),
NXT.
NXT.
NeXTTool ,
NXT ( .rxe)
( .rfw).
NeXTTool LEGO Mindstorms NXT Driver
Windows NXT USB-.
64 .
NXT BIOS ( 224 ).
-
NXT
( 1.12). Generate code and built the generated code
. nxtprj
.
.rxe,
NXT, ***_app_ert_rtw ( *** ) ***_app.c ,
. Download
(NXT enhanced firmware) -
NXT (
USB-). NXT.
MATLAB
.
16
1.12 Simulink- ,
- NXT
, NXT
nxtOSEK, NXT
LEGO MINDSTORMS NXT Sofware
, (.rxe-).
, nxtOSEK
NXT,
( nxtOSEK 10
).
nxtOSEK
ECRobot .
,

.
1.2.3 ECRobot NXT
ECRobot NXT , ECRobot
NXT Simulink ( 1.13).
17
1.13 - ECRobot Simulink

, ECRobot NXT.
Revolution Sensor :
1.14 Revolution Sensor

Revolution Sensor
. Revolution Sensor Interface - ,
NXT
. Revolution Sensor Read
. ;
API
(
In\Out).
1.3 Revolution Sensor

int32
[1 1]

int32 []
ID
A/B/C
Servo Motor :
18
1.15 Servo Motor

Servo Motor . Servo
Motor . Servo Motor Interface
NXT . Servo Motor
Write .
In\Out.
1.4 Servo Motor

int8
[1 1]

-100 100
ID
A/B/C
/
Battery Voltage :
1.16 Battery Voltage

Battery Voltage
NXT. Battery Voltage Interface ,
NXT. Battery Voltage Read
.
1.5 Battery Voltage

Int16
[1 1]

0
[] (, 9000=9.000)
ID
System Clock :
19
1.17 System Clock

System Clock
NXT. System Clock Interface /,
. ,
System Clock Read.
System Clock Read
. NXT 0
.
1.5 System Clock

uint32
[1 1]

0 uint32
ID
Sound Volume Tone :
1.18 Sound Volume Tone

Sound Volume Tone ,
, .
1.6 Sound Volume Tone

: uint32, : uint32
[1 1]

: 31 [] 14080 []
: 10 2560 []
: 0 100 (0 - )
ID
Ultrasonic Sensor :
1.19 - Ultrasonic Sensor

Ultrasonic Sensor
. Ultrasonic
Sensor Interface NXT. Ultrasonic
20
Sensor Read
.
1.7 Ultrasonic Sensor

Int32
[1 1]

0 255 [], -1 ( )
ID
S1/S2/S3/S4
Light Sensor :
1.20 - Light Sensor

Light Sensor ,
. ,
( ). .
Light Sensor Interface NXT. Light
Sensor Read .
1.8 Light Sensor

Unt16
[1 1]

0 1023
ID
S1/S2/S3/S4
Sound Sensor :
1.21 - Sound Sensor

Sound Sensor .
, . Sound Sensor Interface
NXT. Sound Sensor Read
.
1.9 Sound Sensor

Unt16
[1 1]

0 1023
ID
S1/S2/S3/S4
21
Touch Sensor :
1.22 - Touch Sensor

Touch Sensor
. ,
1. Touch Sensor Interface NXT.
Touch Sensor Read .
1.10 Touch Sensor

Unt8
[1 1]

0 ( ), 1 ( )
ID
S1/S2/S3/S4
Bluetooth Rx :
1.23 - Bluetooth Rx
Bluetooth Rx Bluetooth
Bluetooth-.
. Bluetooth Rx Interface
NXT. Bluetooth Rx Read
. Bluetooth Rx
( ),
-NXT ( NXT-NXT).
1.11 Bluetooth Rx

Uint8
32

0 255
ID
Bluetooth Tx :
22
1.24 - Bluetooth Tx
Bluetooth Tx Bluetooth
Bluetooth-.
. Bluetooth Tx Interface ,
NXT. Bluetooth Tx Write .
Bluetooth Tx
( ) -NXT NXTNXT.
1.12 Bluetooth Tx

Uint8
32

0 255
ID
NXT GamePad Data Logger :
1.25 - NXT GamePad Data Logger

NxtOSEK , NXT GamePad.
NXT,
nxtOSEK Bluetooth,
(). 1.01

Analog Stick Control NXT Data
Acquisition, .
, NXT GamePad

[15].

NXTGamePad.exe COM-
. Data1 Data2
Analog GamePad.
23
CSV
NXT MATLAB. NXT GamePad Data Logger
Bluetooth Tx Write .
1.13 NXT GamePad Data Logger

int8
[1 1]

-128 127
ID
1.26 - NXT GamePad

NXT GamePad ADC Data Logger :
1.27 - NXT GamePad ADC Data Logger

NXT GamePad ADC Data Logger NXT
GamePad Data Logger, 4
. NXT GamePad ADC Data Logger
Bluetooth Tx Write .
1.11 NXT GamePad ADC Data Logger

Data1/Data2
Int8
-128 127
1
ADC1/ADC2/ADC3/ADC4 Int16
-32768 32767 1
24
Gyro Sensor :
1.28 - Gyro Sensor

Gyro Sensor ()
, . Gyro
Sensor Interface - NXT. Gyro
Sensor Read .
1.12 Gyro Sensor

Uint16
[1 1]

0 1023
ID
S1/S2/S3/S4
Exported Function-Calls Scheduler :
1.29 - Exported Function-Calls Scheduler

(,
,
). Exported Function-Calls Scheduler
Function-Call
, Block Parameters.
OSEK Resource :
1.30 - OSEK Resource

OSEK Resource
OSEK GetResource/ReleaseResource API /
, ,
. Simulink,
Atomic
Subsystem.
25
Exported Function-Call Scheduler. OSEK Resource

.
1.31 - OSEK Resource

1.3

LEGO MINDSTORMS

LEGO Mindstorms,
, ECRobot NXT.
.

( 1.32). [14] 1,
4.
, .
1.32
26
Simulink- Test.mdl
( 1.33), .
,
. .
1.33 Simulink- test.mdl

ExpFcnCalls Scheduler ()
.
Fcn_100ms 0,1 .
Explorer_app .
.
.
(0,01 ),
Function-Call Subsystem.
27
Function-Call Subsystem
( 1.34). 20
( ), 0,
1. 1
( Touch Sensor Read) 0 .
1.34 - Function-Call Subsystem

, , 0,
.
If Action Subsystem TURN (
), If Action Subsystem FORWARD (
). ,
.
, 70,
( 1.35).
28
1.35 -
,
.
nxtconfig (gsc, 'float') -
.
nxtbuild ('Explorer_app', 'cgen') -
(
).
nxtprj
ecrobot_main.c Fcn_10ms,
Explorer_app_ert_rtw Explorer_app.c ,
Explorer_app
.
nxtbuild ('Explorer_app', 'build') -
. nxtprj
Explorer_app.rxe
NXT.
nxtbuild ('Explorer_app', 'rxeflash') -
(Explorer_app.rxe) NXT (
, USB-).
NXT. ,
. [14]
1.3.1-1.3.3
, , ,
29
nxtOSEK.
www.nxtprograms.com/NXT2/explorer/index.html [14].
1.3.1

, ( 1.36).
1.36 - ,

,
.
.
,
.

.
1.3.1.1

, ,
, ,
.
: , .
7 , 3
30
.
: ,
,
. [5]
1.37 -

O
x , y z ( 1.37). R,
B. B
: x B 0 , y B 0 , z B 0 .
(3 3) ,
x z .
H x H 0 , y H 0 ,
z H 0 . :
I Bxx
I B 0
I Bxz
0
I Byy
0
I Bxz
I Hxx
0 , I H 0
I Hxz
I Bzz
0
I Hyy
0
I Hxz
0 .
I Hzz
:
P Fi i T T T R ,
R
T , T , T
(1)
F .

, v R rR .
, ,

. ,
( ) ,
x z , :
[rR2 mT I Ryy (rR / rF ) 2 I Fyy ] R TR ,
(2)
,

31
.
, ,
C q K q f .
Mq
v C1 q [ g K 0 v 2 K 2 ]q f ,
Mq
T
q [, ]T f [T , T ] , M ,
. C v C1
v ,
- .
C1 ,
. K :
g K 0
,
, v 2 K 2 ,
.
, :
det(M 2 v C1 g K 0 v 2 K 2 ) 0 .
(3)
(3) .
,
, .
,
.
:
q S(q, t )u , u [M(q, t )]1 Q(q, u, t ) ,
(4)
q , u , S
,
, M ,
Q .
1.3.1.2

: R , B
, H
, F , T , A
.
(
P):
mT mR mB mH mF , xT ( xB mB xH mH wmF ) / mT ,
32
zT (rR mR zB mB zH mH rF mF ) / mT .
z

P ,
:
I Tzz I Rzz I Bzz I Hzz I Fzz mB x B2 mH x H2 mF w2
A.
sin , 0, cos T , ,
x z . ,
u A ( xA w c) cos z A sin .
:
, ,
, .
, - .
(
)
(c / w) cos .
y,
,
:
SR I Ryy / rR , S F I Fyy / rF , ST SR SF .
.
M
K 0
, K0
M
K 0
M
M
M
K 0
K 2
, K2
K 0
K 2
K 2
C1
, C1
K 2
C1
C1
,
C1
(5)
M ITxx , M I Ax ITxz , M M , M I A 2I Az 2 ITzz
M;
K 0 mT zT ,
K 0 S A ,
K 0 K 0 ,
K0 S A sin (
g );
K 2 0 ,
K 2 0 ,
K 2 ((ST mT zT ) / w) cos , K 2 ((S A S F sin ) / w) cos
C1 ST SF cos ( ITxz / w) cos mT zT ,
v 2 );
C1 0 ,
C1 ( I Az / w) cos (S A ( ITxz / w) cos ) ,
C1 (ST S F cos ) C .
1.3.1.3
.
:
I Ax
vST vSF cos TB gmT zT gS A .
mT yP zT ITxx I Txz
33
P:
I Az vST vSF sin wFFy .
mT yP xT I Txz I Tzz
:
I A vSF ( cos
sin ) TH cFFy cos g ( sin )S A .
mA yPu A I Az
,

:
yP ((v 2 vc ) / w) cos

:
x P v cos , y P v sin .
--, x , , , ,
u T .
T
.[27]
-
J ( x T Q u x u T R u u )dt , Q u R u
0
. Qu diag{0, 0,1, 0} ,
,
, .
R u 1 .

-
(-), k p 8,23 , ki 1,48
k d 2,76 .
1.3.1.4
1.38
34
1.
1.13
Physical
g=9.81
m=0.03
R=0.04
M=0.678
M_front=0.2
M_rear=0.478
L_base =0.225
/2
L_trail = 0.011
Head_angle =67
V=0.6
(w)
,

(c)

1.3.1.5 Motobot
( 1.39). ,
, .
, - . ,
k-, .
, :
, ,
Bluetooth, , ,
, ,
,
.
nxtway_app.
35
1.39 -
1.40
/. , ,
Bluetooth. A B
,

.
1.40 -
36
1.41 .
:
nxtway_app/Balance & Drive control/Balance & Drive Control/Controller
k_thetadot thetadot_ref, .
(x1_ref-x1), x1=[phi
phidot phidotdot] , motobota
.
1.41 -
.
phi_cmd motobota,
0. , (
)
.
thetadot_cmd.

.
Blutooth. [15]
37
1.42 -
.
( 1.43), theta_in ( )
thetadot (/).
0.004 .

x1, phi_dotdot
phi. 0.004 .
38
1.43
, .

( 1.44).
.
1.44 ,
1.3.1.6
NXT GamePad

. 1.45 ,
.
39
1.45 :
) [./c.]; )
[.]
1.3.2 Segway
1.3.2.1
Segway ( 1.46)

.
.
- ,
.
1.46 - Segway

, ,
40
, ,
.
1.47 , .
M ;
m ;
b ;
L ;
J ;
u(t) , ;
x(t) ;
(t) ;
H(t) ;
V(t) ;
g .
:
d2
xt L sin t H t
dt 2
(6)
d2
m 2 L cos t V t mg
dt
(7)
d 2 t
L H t cos t L V t sin t
dt 2
d 2 xt
dxt
u t H t b
2
dt
dt
(8)
(9)
41
(6) (7), :
mx mL 2 sin mL cos H t
(10)
mL 2 cos mLsin V t mg
(11)
H(t) (10) (9),

:
M mx bx mLcos mL 2 sin ut
(12)
H(t) (10) V(t)

(11) (8), :
J mL mLxcos mgLsin 0
2
(12)
(13)
(13)
.

(12) (13)
.
t 0 , xt 0 , ut 0 ,
sin t cos t (12) (13)
:
M mx bx mL ut
(14)
J mL mLx mgL 0
(15)

, ,
u(t), (t).

(12)
(13)
(s) U(s)
(t) u(t) :
J mL2 g 2
J mL2 g
s M m
2 s b
2 s mLs2 U s
s
s
mL
mL
(16)
:
42
Wp s
s
mLs
2
2 2 3
U s M m J mL m L s b J mL2 s 2 M mmgLs bmgL
(17)
:
.
:
x1 t xt , x2 t x1 t xt , x4 t t , x4 t x3 t t .
(18)
(12) (13) :
M mx2 bx2 mLx4 ut 0,
(19)
J mL x
(20)
mLx2 mgLx3 0.
x2
b J mL2
m 2 gL2 M m
J mL2
x2
x3
u t 0, (21)
J M m MmL2
J M m MmL2
J M m MmL2
x4
bmL
mgLM m
mL
x
x
u t 0. (22)
2 2
2 3
J M m MmL
J M m MmL
J M m MmL2
, :
x Ax Bu t ,
(23)
1
0
x1 t
b J mL2
x t
0 J M m MmL2
2
x
, A
0
x3 t
0
bmL
0
x4 t
J M m MmL2
0
m 2 gL2 M m
J M m MmL2
0
mgLM m
J M m MmL2
0
,
1
0
J mL2
J
M
MmL
.
B
mL
2
J M m MmL
( t xt )
:
y Cx Dut ,
(24)
43
y t
1 0 0 0
y 1 , C
, D 0.
0 0 1 0

y2 t
1.3.2.2
1.
,

, :

,
,
.
2.

xref x.
1.48
,
1.48, :
(
)
, ,

-.
, .
,
, :
Control System Toolbox
ctrb.

.
1. .

-.
. Control System Toolbox
place.
44
, ,
A=[0, 1; -2; -3], B=[0;1]
: [-5, -6]
2.

J,
:

Q R,
. Control System Toolbox
lqr.
:
A=[0, 1; -2, -3], B=[0; 1], Q=[100, 0; 0, 1]; R=1
1.3.2.3
,
.
- - ,

. - ,
,
, ,
.
- x0
x u.
x0 , e = (xref x)
.
u :
45

p, i, d ,
, .
1.49 -
1.3.2.4
1.50
1.14
2
g = 9.81 (/ )

m = 0.03 ()

R = 0.035 ()

2
2
Jw = mR /2 ( )

M = 0.6 ()

W = 0.14 ()

D = 0.04 ()

H = 0.144 ()

L = H/2 ()

46
J = ML2/3 (2)
J = M(W2+D2)/12 (2)
Jm = 10-5 (2)
Kb = 0.468 (/)
Kt = 0.317 (/)
1.3.2.5 Segway
. , ,
.
, Motobot.
nxtway_app.
1.51

/. , ,
Bluetooth.
.
Reference Generator ,
.
47
1.52
1.51 Controller ,
NXT. ,
, , Bluetooth, ,
Bluetooth
,
,
.
1.53
1.54 NXTway-GS - , ,
, .
, ,
.
.
48
1.54

1.54 Actuator ,
, .
1.55
Plant - ,
, .
49
1.56
Sensor ,
.
, .

.
50
1.57
1.58 - nxtway_gs_controller.mdl
Data Store Memory ,
.
1.59
task_ts2: .
51
1.60 task_ts2
task_ts3: .
1.61 - task_ts3
task_ts1: , ;
.
. time_start.
52
1.62 - task_ts1
.
1.63
, 1.64 Discrete Derivative
Block,
Discrete Integrator Block,
.
53
1.64 - Discrete Derivative Discrete Integrator Block

1.64 ,
,
,
.
1.65
,
.
,
.
54
1.66
,
.
1.67
1.3.2.6
NXT GamePad

.
1.68 1.69 ,
.
55
1.68
1.69
1.3.3
1.3.3.1
( 1.70),
,
56
.

[1], 1.65.
1.70

. , . m
. Rwm Rss ( 1.71).
1.71

, ,
. =0 t=0,
:
( xs , z s ) ( Rs , zb ) , ( x s , z s ) ( Rs ,0) ,
cos ,L
sin ) .
( xb , zb ) ( x L sin , z L cos ) , ( xb , zb ) ( Rs L
T1,
T2, U :
57
T1
1
1
M s ( x s2 z s2 ) M b ( xb2 zb2 )
2
2
T2
R2
1 2 1
1
1
1
1
1
2 J w 2m J m 2m J s 2s J
2 ( J m J w ) s (
),
J s s J
2
2
2
2
2
2
2
Rw2
U M s gz s M b gzb
.
L :
L T1 T2 U .
.
:
d L L
F ,

dt
d L L
F .
dt
,
:
F Kt i f m m f s , F K t i f m m ,
i ; m k ( ) .
,
[4]:
K
K K
, t , k( t b fm ) .
, F
F ( f s )
Rm
Rm
2
0(sin , cos 1) ,
:

E F G H ,

( M M s ) Rs2 J s J m
E b
M b LRs k 2 J m
0
0
G
M
b gL
M b LRs k 2 J m
M b L2 J k 2 J m
f s
F

H .

x ; u .
T
; u ;
x
, :
x A(t ) x(t ) B(t )u(t )
0
0
A
0
0
0
A(3,2)
A(4,2)
1
0
A(3,3)
A(4,3)
0
0
0
1
,
B (3)
A(3,4)
A(4,4)
B ( 4)
A(3,2) Mb gLE(1,2) / det( E)

A(4,2) M b gLE(1,1) / det( E)
58
A(3,3) [( f s ) E(2,2) E(1,2)] / det( E)
A(4,3) [( f s ) E(1,2) E(1,1)] / det( E)

A(3,4) [ E(2,2) E(1,2)] / det( E)
A(4,4) [ E(1,1) E(1,2)] / det( E)

B(3) [ E(2,2) E(1,2)] / det( E)
B(4) [ E(1,1) E(1,2)] / det( E)

det( E ) E (1,1) E (2,2) E (1,2)2 .
1.3.3.2

.
m
.
.

,
.
.
,
, .
- .

- ( 1.72). C
x.
1.72 - -

- .
-
,
[28]. Q R [29]:
59
0
1
0 6 *10 5
Q 0
0
0
0
0
0
0
0
1
0
0
0 0
0 0
180 2
0 0 , R 6 *103 * (
)
1 0
0 10 3
Q(2,2) ,
, Q(5,5)
,
.

, Matlab
. lqr
[30].
:
kf = [0,015 1,5698 0,027 0,2325], ki = 0,0071.
. ,
: t 0,36 c; 0 .

:
J x, u
T
T
x t Qt xt u t Rt ut dt .
x 0 ,

V xt xT t Pt xt ,
P PT n n ,
: 0 ,
t 0 :
V xt 2V xt .
, :
V xt 2V xt xT t Qt xt uT t Rt ut .
,
:
At xt Bt ut xt T Pt xt xT t Pt At xt Bt ut xt xT t P t xt
xT t Qt xt uT t Rt ut 0 ,
, [31],
:
60
ut R 1t BT t Pt xt .
[31]:
P t At Bt K t I T Pt Pt At Bt K t I K T t Rt K t Qt 0 ,
K t R 1t BT t Pt .
A, B, Q, R ,
,
P ( P 0 ):
A BK I T P PA BK I K T RK Q 0 , K R1BT P .
,
K : K f [0,0117 1,4993 0,0284 0,2432] .
1.3.3.3
1.15
g
9,81
/2
Ms
0,013
Rs
0,026
Js
2 Ms Ms2/3
Mw
0,015
Rw
0,021
Lw
0,022
Jw
Mw Lw2/2
Mb
0,682
0,135
0,135
0,17
Mb L2/3
Jm
0,00001
Rm
6,69
Kb
0,468
1.3.3.4 Ballbot

61

, ,
.
.
1.73 NXT Ballbot,
, , .
,
.
. ,
, , .
, ,
.
1.73 NXT Ballbot

X Y.
1.74
62
1.75 ,
, .
1.75
1.76 ,
,
.
1.76
Sensor ,
.
63
1.77
ExpFcnCalls ,
, .
1.78
Data Store Memory ,
.
.
(
)
64
1.79 task_init
task_ts2-4 , Segway.
.
1.80

.
65
1.81
,
.
,
.
1.82
,
.
66
1.83
1.3.3.5
NXT GamePad

.
,
. 1.84 1.85
,
. [4]
1.84 - -
:
(); ()
67

1.85 -
:
(); ()
, nxtOSEK
,
. nxtOSEK

MATLAB/Simulink.
ECRobot
Simulink- .
nxtOSEK
.
-,
/++
- .
68
2 BIOLOID PREMIUM KIT

Bioloid
.
, Dynamixel
,
, [18].
Bioloid LEGO Mindstorms LEGO Vex
Robotics Design System VEX Robotics.
-
[16]. Bioloid
RoboCup [22].
BIOLOID Premium Kit ( 2.1),
.
, ,
,
. BIOLOID
bio+ all+ oid, ,
( 2.1).
69
2.1 BIOLOID Premium Kit
2.1 Bioloid

( 2.2), ,
,
. Bioloid Dynamixel,
, , 3D
70
- .
,
.
2.2 Bioloid Premium Kit

CM-510 ATmega2561
DYNAMIXEL ( 2.3):
2.3 -510
2.1 DYNAMIXEL
Robotis Dynamixel Servos
, .
,
71
(,
) .

BIOLOID -510
( 2.4).
2.4 - -510
Dynamixel

,
,

.
Dynamixel
.
16 * 106 *
.
, ,
.
Dynamixel,
72
,
( 2.5).
2.5 Dynamixel
Dynamixel ,
,
. Dynamixel
Maxon
RE-max,

0.35-0.07
( 2.6).
73
2.6 Dynamixel
2.1.1 Dynamixel
,
.
AVR ATmega.
,
Dynamixel
, .
2.7 Dynamixel
-510

, ROBOTIS (
74
. ).
Dynamixel
RS-232 RS-485 (
2.8).
2.8 RS-232 RS-485

Dynamixel
2.1.2 Dynamixel
: 54.6;
: 32 * 50 * 40;
: 0,29;
: 254: 1;
: 1.5 (12, 1.5);
: 59rpm ( 12);
: 1) 0 ~ 300; 2) ;
: -5 ~ +70;
: 9 ~ 12 ( 11,1);
: ;
:
(8, 1-, );
ID: 254 ID (0 ~ 253);
: 7343bps ~ 1Mbps;
75
, ..;
: .
2.2

.
.
Robotis RoboPlus
( 2.9), : RoboPlus Terminal,
RoboPlus Task, RoboPlus Manager, RoboPlus Motion, RoboPlus Help & eManual.
Robotis OLLO, Bioloid, Expert
kit [23].
2.9 RoboPlus
76
RoboPlus Manager ( 2.10)

, .
RoboPlus Manager
, .. RoboPlus Manager
,
Robotis.
2.10 RoboPlus Manager

RoboPlus Manager ID
, .
Dynamixel :
, , ,
.. , RoboPlus Manager
Dynamixel, 5,-510,-700 Zigbee.
RoboPlus Terminal ( 2.11) ,
-510
.
,
-510.
Boot Loader.
77
2.11 RoboPlus Terminal

RoboPlus Motion ( 2.12) ,

.
2.12 - RoboPlus Motion

.
-510
,
, .
78
.
Basic Pose Editor
, ,
. Pose Utility
,
.

, .

[24].
RoboPlus Task ( 2.13)
,
,
, .
2.13 RoboPlus Task

RoboPlus Motion RoboPlus Task

.
79
Robotis
.

[17].
, - ATmega,
Atmel Studio, WinAVR AVRStudio
. Atmel Studio 6,
AVR Studio 5,
. Microsoft Visual Studio Shell 10
Visual Assist,
C/C++.
Atmel Studio 6 Atmel Software
Framework,
,
, Atmel Studio 6.0 1000
, . Atmel Studio 6
Atmel Software
Framework (ASF Explorer) Qtouch composer,
[25].
: ,
"", , ..
"",
Framework, ,
..
Atmel Studio:
1. Atmel ;
2. ;
3. ;
4.
;
80
5. Atmel Studio .
2.14 Atmel Studio 6.0

. Atmel Studio .hex
.
RoboPlus Terminal:
1) c
COM
57600bps.
:
: ;
-: 1
: 8
2)
shift+3
,
( 2.15):
81
2.15 CM-510 (ATmega2561)
3) L ( 2.16), "Transmit File"

RoboPlus Terminal, .hex ;
2.16 .hex
4) ,
"GO".

APP/SYS. ,
.
ATmega2561 2.17.
82
2.17 - ATmega2561
RoboPlus Terminal ,
.txt.
2.3
BIOLOID

ATmega2561 Atmel Studio.
Dynamixel
: serial.h dynamixel.h.
Zigbee libzigbee.h
[17].
CM-510
Dynamixel.
83
2.3.1 CM-510
2.18 - CM-510
2.1 -510

PORTF1 ~ PORTF6
PORTD0
PORTD1 ~ PORTD2
PORTA2 ~ PORTA7
PORTC0 ~ PORTC6
PORTB5
PORTE4 ~ PORTE6
PORTD4 ~ PORTD6

(Tx) (Rx)
( 5
( 2.19))
(, )

(U, D, L, R)

2.19 -510
84
:
1.
5 ;
2.
VCC (5 );
3.
4.
GND;
5.
NC: .
2.3.2 Dynamixel
( 2.2) Dynamixel,
Dynamixel
.
EEPROM RAM. (RAM)
,
EEPROM ,
.

.
.
. Dynamixel :
(R), ,
(RW), .
. EEPROM
,

.
/ . ,
16 ,
8 .
.
85
2.2 Dynamixel
0 (0x00)
1 (0x01)
2 (0x02)
3 (0x03)
4 (0x04)
5 (0x05)
6 (0x06)
7 (0x07)
8 (0x08)
E
E
P
R
O
M
9 (0x09)
11
(0x0B)
12
(0x0C)
13
(0x0D)
14
(0x0E)
15
(0x0F)
16
(0x10)
17
(0x11)
18
(0x12)
24
(0x18)
25
(0x19)
Model Number(H)

Version of Firmware
ID
ID
Baud Rate

Return Delay Time
CW Angle Limit(L)

( )
CW Angle Limit(H)

( )
CCW Angle

Limit(L)

( )
CCW Angle

Limit(H)

( )
the Highest Limit

Temperature
the Lowest Limit

Voltage
the Highest Limit

Voltage
Max Torque(L)

Max Torque(H)

Status Return Level

Alarm LED

Alarm Shutdown
Torque Enable
/

LED
/
Model Number(L)
12 (0x0C)
0 (0x00)
RW
RW
1 (0x01)
1 (0x01)
RW
RW
250 (0xFA)
0 (0x00)
RW
0 (0x00)
RW
255 (0xFF)
RW
3 (0x03)
RW
70 (0x46)
RW
60 (0x3C)
RW
140 (0xBE)
RW
255 (0xFF)
RW
3 (0x03)
RW
2 (0x02)
RW
36 (0x24)
RW
36 (0x24)
RW
0 (0x00)
RW
0 (0x00)
86
R
A
M
RW
1 (0x01)
RW
1 (0x01)
RW
32 (0x20)
RW
32 (0x20)
RW
RW
RW
RW
RW
ADD14
RW
ADD15
Present Voltage
Present Temperature
Registered
,
-
0 (0x00)
0 (0x00)
26
(0x1A)
CW Compliance
Margin
27
(0x1B)
CCW Compliance
Margin
28
(0x1C)
CW Compliance
Slope
29
(0x1D)
CCW Compliance
Slope
30
(0x1E)
31
(0x1F)
32
(0x20)
33
(0x21)
34
(0x22)
35
(0x23)
36
(0x24)
37
(0x25)
38
(0x26)
39
(0x27)
40
(0x28)
41
(0x29)
42
(0x2A)
43
(0x2B)
44
(0x2C)
Goal Position(L)
46
(0x2E)
Goal Position(H)
Moving Speed(L)
Moving Speed(H)
Torque Limit(L)
Torque Limit(H)
Present Position(L)
Present Position(H)
Present Speed(L)
Present Speed(H)
Present Load(L)
Present Load(H)
Moving
87
47
(0x2F)
48
(0x30)
49
(0x31)
Lock
Punch(L)
Punch(H)
EEPROM

,

RW
0 (0x00)
RW
32 (0x20)
RW
0 (0x00)
2.3.3
.
C.
.hex
.
#include <avr/io.h>
#include <util/delay.h>
int main(void)
{
DDRC = 0x7F;
PORTC = 0x7E;
while (1)
{
int i;
for(i = 0; i <= 6; i++)
{
PORTC = ~(1<<i);
_delay_ms(250);
}
}
return 1;
}
2.3.4
, ,
/.
.
88
.
D E, PIND PINE
. PIND PINE
&.
if(~PINE & BTN_UP)

PORTC &= ~LED_MANAGE;
else if(~PINE & BTN_DOWN)
PORTC &= ~LED_AUX;
else if(~PINE & BTN_LEFT)
PORTC &= ~LED_PROGRAM;
else if(~PINE & BTN_RIGHT)
PORTC &= ~LED_PLAY;
else if(~PIND & BTN_START)
PORTC =
~(LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRA
M|LED_PLAY);
else PORTC =
LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|
LED_PLAY;
2.3.5
2.20
89

.
.
// USART ( -
-)
serial_initialize(57600);
sei(); //
serial_initialize
serial.h, sei() , .
unsigned char ReceivedData = getchar();

if(ReceivedData == 'u')
Value++;
else if(ReceivedData == 'd')
Value--;
printf("%d\r\n", Value);
Value,
1 u 1
d. getchar().
2.21 -
90
2.3.6

.
2.3.
2.3
-510
C
C#
D
D#
E
F
F#
G
G#
A
A#
G
32.7032
34.6478
36.7081
38.8909
41.2034
43.6535
46.2493
48.9994
51.9130
55.0000
58.2705
61.7354
65.4064
69.2957
73.4162
77.7817
82.4069
87.3071
92.4986
97.9989
103.8262
110.0000
116.5409
123.4708
130.8128
138.5913
146.8324
155.5635
164.8138
174.6141
184.9972
195.9977
207.6523
220.0000
233.0819
246.9417
261.6256
277.1826
293.6648
311.1270
329.6276
349.2282
369.9944
391.9954
415.3047
440.0000
466.1638
493.8833
523.2511
554.3653
587.3295
622.2540
659.2551
698.4565
739.9888
783.9909
830.6094
880.0000
932.3275
987.7666
1046.502
1108.731
1174.659
1244.508
1318.510
1396.913
1479.978
1567.982
1661.219
1760.000
1864.655
1975.533
2093.005
2217.461
2349.318
2489.016
2637.020
2793.826
2959.955
3135.963
3322.438
3520.000
3729.310
3951.066
4186.009
4434.922
4698.636
4978.032
5274.041
5587.652
5919.911
6271.927
6644.875
7040.000
7458.620
7902.133

. Start
.
if(~PIND & SW_START)

{
PORTC =
M|LED_PLAY);
_delay_ms(1);
PORTB |= 0x20;
_delay_ms(1);
PORTB &= ~0x20;
}
else
{
PORTC =
LED_PLAY;
PORTB &= ~0x20;
}
91
2.3.7
,
. , ,
, 1 .
if(~PIND & MIC_SIGNAL)

{
PORTC =
M|LED_PLAY);
_delay_ms(1000);
}
else PORTC =
LED_PLAY;
2.3.8

. ,
-,
- .. , ,
-, 1.
92
2.22 -
sei();
ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1); //
ADMUX = ADC_PORT_1; // 1
PORTA &= ~0x80; // -
_delay_us(12); //
ADCSRA |= (1 << ADIF); //
ADCSRA |= (1 << ADSC); //

while( !(ADCSRA & (1 << ADIF)) ); //
PORTA = 0xFC; // -
printf( "%d\r\n", ADC); //
_delay_ms(50);
93
-
, , ,
.
2.3.9 Dynamixel
Dynamixel
dinamixel.h.
ID 1.
unsigned short GoalPos[2] = {0, 1023};

dxl_initialize( 0, 1 );
sei(); //
dxl_initialize(int devIndex, int baudnum)
. devIndex
, baudnum
(
). : 1 0
.
2.4
4
1
3
4
7
9
16
34
103
207
, bps
1000000.0
500000.0
400000.0
250000.0
200000.0
117647.1
57142.9
19230.8
9615.4
bps
1000000.0
0.000 %
500000.0
0.000 %
400000.0
0.000 %
250000.0
0.000 %
200000.0
0.000 %
115200.0
-2.124 %
57600.0
0.794 %
19200.0
-0.160 %
9600.0
-0.160 %
94
//
bMoving = dxl_read_byte( id, P_MOVING );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
if( bMoving == 0 )
{
//
if( index == 0 )
index = 1;
else
index = 0;
//
dxl_write_word( id, P_GOAL_POSITION_L, GoalPos[index] );
}
PrintErrorCode();
//
wPresentPos = dxl_read_word( id, P_PRESENT_POSITION_L );
printf( "%d %d\n",GoalPos[index], wPresentPos );
}
else
PrintCommStatus(CommStatus);
dxl_write_word( int id, int address, value )
; id ID
; address ; value .
.
, ,
,
.
2.3.10

ID 1 3.

Dynamixel.
95
int AmpPos = 512;

dxl_initialize( 0, 1);
sei(); // Interrupt Enable
for( i=0; i<NUM_ACTUATOR; i++ )
{
id[i] = i+1;
phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
}
//
dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
//
dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos );
_delay_ms(1000);
.
.
,
,
. , SYNC WRITE ,

. , 4
, 26 Dynamixel
.
, 143 ,
Dynamixel 143 .
:
ID 0XFE
Length (L+1) X N + 4 (L: , N:
)
Instruction 0X83
Parameter1
96
Parameter2
Parameter3 ID
Parameter4
Parameter5
Parameter L+3 L-
Parameter L+4 ID
Parameter L+5
Parameter L+6
Parameter 2L+4 L-
2.5 :

:
ID 0:
0010 0150
ID 1:
:
0XFF 0XFF 0XFE 0X18 0X83 0X1E
0X04 0X00 0X10 0X00 0X50 0X01
0X01 0X20 0X02 0X60 0X03 0X02
0X30 0X00 0X70
0220 0360
ID 2:
0030 0170
ID 3:
0220 0380
:
//
dxl_set_txpacket_id(BROADCAST_ID);
dxl_set_txpacket_instruction(INST_SYNC_WRITE);
dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
dxl_set_txpacket_parameter(1, 2);
{
dxl_set_txpacket_parameter(2+3*i, id[i]);
97
GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos);

printf( "%d ", GoalPos );
dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos));
dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos));
}
dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4);
printf( "\n" );
dxl_txrx_packet();
PrintErrorCode();
else
theta += STEP_THETA;
if( theta > 2*PI )
theta -= 2*PI;
_delay_ms(CONTROL_PERIOD);
.

,
.
2.3.11 Zigbee
ZigBee (
(API)
(NWK)),

(MAC) (PHY),
IEEE
802.15.4.
ZigBee
IEEE
802.15.4
(WPAN).
ZigBee
98

[25].
2.23
ZigBee ,

(-, ),
(mesh)
. , ZigBee
,
, ,
, , ,
, .
ZigBee
2,405-2,485
(
07.05.2007 07-20-03-001).
ZigBee
, (
),
99
,
.

RC100 ZigBee .
C D. ZigBee
D:
PORTD &= ~0x80; //PORT_LINK_PLUGIN = 0;

PORTD &= ~0x20; //PORT_ENABLE_RXD_LINK_PC = 0;
PORTD |= 0x40; //PORT_ENABLE_RXD_LINK_ZIGBEE = 1;
zgb_initialize( 0 );
sei(); //
Zigbee . Zigbee
, .
0.
if(zgb_rx_check() == 1)
{
RcvData = zgb_rx_data();
if(RcvData & RC100_BTN_1)
PORTC &= ~LED_MANAGE;
else
PORTC |= LED_MANAGE;
PORTC &= ~LED_PROGRAM;
else
PORTC |= LED_PROGRAM;
PORTC &= ~LED_PLAY;
else
PORTC |= LED_PLAY;
}
, ZigBee ,
zgb_rx_data().
100
zgb_rx_check. , ,
2.6.
2.6 ,
1
2
4
8
16
32
64
128
256
512

RC100_BTN_U - U
RC100_BTN_D - D
RC100_BTN_L - L
RC100_BTN_R - R
RC100_BTN_1 - 1
RC100_BTN_2 - 2
RC100_BTN_3 - 3
RC100_BTN_4 - 4
RC100_BTN_5 - 5
RC100_BTN_6 - 6
2.23 - RC100

CM-510
.
2.3.12 Bioloid

: Bioloid
. (.
2.2):
(),
(),
().
101
2.24

Bioloid.
.
Bioloid Premium Kit.
2.24 -

. ,
, .
RoboPlus Motion,
dxl_read_word( int id, int address ),
int address=36. ,
.
.

:
#include <avr/io.h>
102
#include <avr/interrupt.h>
#include <stdio.h>
#include <math.h>
#include <util/delay.h>
#include "serial.h"
#include "dynamixel.h"
/// Control table address

#define P_GOAL_POSITION_L
30
#define P_GOAL_POSITION_H
31
#define P_GOAL_SPEED_L
32
#define P_GOAL_SPEED_H
33
#define P_PRESENT_POSITION_L 36
#define P_PRESENT_POSITION_H 37
#define DEFAULT_BAUDNUM
1 // 1Mbps
#define NUM_ACTUATOR
6 // Number of actuator
#define CONTROL_PERIOD
(500) // msec
void PrintCommStatus(int CommStatus);

void PrintErrorCode(void);
int main(void)
{
int id[NUM_ACTUATOR];
int AmpPos = 512;
int GoalPos, i, j, Error, l, CommStatus, wPresentPos;
int u[NUM_ACTUATOR];
103
const int k_hip = 57, k_kn=43, k_an=43;

const int MaxSpeed = 1023;
int buffer = 0;
// ID 11-16
int IDPositions[6][28] = {{363,372, 373, 361, 341, 319, 299, 287, 283,
286, 291, 293, 293, 297, 302, 308, 314, 321, 327, 332, 335,
337, 338, 339, 340, 343, 347, 354},
{721, 715, 709, 702, 696, 691, 688, 686, 685, 684, 683, 680,
676, 669, 660, 651, 650, 662, 682, 704, 724, 736, 740, 737,
732, 730, 730, 726},
{254, 262, 262, 245, 222, 203, 196, 203, 222, 245, 262, 262,
254, 248, 246, 248, 252, 257, 262, 266, 267, 266, 262, 257,
252, 248, 246, 248},
{777, 775, 771, 766, 761, 757, 756, 757, 761, 766, 771, 775,
777, 775, 769, 761, 761, 778, 801, 820, 827, 820, 801, 778,
761, 761, 769, 775},
{675, 675, 677, 682, 685, 681, 669, 649, 627, 607, 595, 596,
605, 614, 621, 625, 628, 629, 630, 631, 633, 636, 641, 647,
654, 660, 666, 671},
{402, 398, 395, 394, 393, 392, 390, 387, 382, 376, 369, 363,
357, 352, 348, 348, 346, 341, 338, 342, 354, 374, 396, 416,
428, 427, 418, 409}};
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
sei(); //
ID , 11 16, :
104

{id[i] = i+11;
//
Set goal position

dxl_write_word( 7, P_GOAL_POSITION_L, 358 );
_delay_ms(1000);

.
:
while(1)
{
for( j=0; j<28; j++ ) // Loop for walk position moving
{
for( i=0; i<6; i++ )
{
wPresentPos = dxl_read_word( id[i], 36 );

if (j==0) l=27;
else l=j-1;
}
//
for( i=0; i<2; i++ )
105
{
GoalPos = IDPositions[i][j];
wPresentPos = dxl_read_word( id[i], P_PRESENT_POSITION_L );
Error = wPresentPos - IDPositions[i][j];
buffer = k_hip * Error;
if(buffer < 0) buffer=-buffer;
if(buffer > MaxSpeed) buffer = MaxSpeed;
else if(buffer < 100) buffer = 100;
u[i] = buffer;
}
//
for( i=2; i<4; i++ )
{
buffer = k_kn * Error;
if(buffer > MaxSpeed) buffer = MaxSpeed
//
for( i=4; i<6; i++ )
{
buffer = k_an * Error;
106
if(buffer > MaxSpeed) buffer = MaxSpeed
//
dxl_set_txpacket_id(BROADCAST_ID);
dxl_set_txpacket_instruction(INST_SYNC_WRITE);
dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
dxl_set_txpacket_parameter(1, 4);
{
dxl_set_txpacket_parameter(2+5*i, id[i]);
dxl_set_txpacket_parameter(2+5*i+1, dxl_get_lowbyte(GoalPos));
dxl_set_txpacket_parameter(2+5*i+2, dxl_get_highbyte(GoalPos));
dxl_set_txpacket_parameter(2+5*i+3, dxl_get_lowbyte(u[i]));
dxl_set_txpacket_parameter(2+5*i+4, dxl_get_highbyte(u[i]));
}
dxl_set_txpacket_length((4+1)*NUM_ACTUATOR+4);
dxl_txrx_packet();
PrintErrorCode();
else
_delay_ms(CONTROL_PERIOD);
}
}
return 0;
}
107

PrintCommStatus(int CommStatus)
PrintErrorCode():
//
void PrintCommStatus(int CommStatus)
{
switch(CommStatus)
{
case COMM_TXFAIL:
printf("COMM_TXFAIL: Failed transmit instruction packet!\n");
break;
case COMM_TXERROR:
printf("COMM_TXERROR: Incorrect instruction packet!\n");
break;
case COMM_RXFAIL:
printf("COMM_RXFAIL: Failed get status packet from device!\n");
break;
case COMM_RXWAITING:
printf("COMM_RXWAITING: Now recieving status packet!\n");
break;
case COMM_RXTIMEOUT:
printf("COMM_RXTIMEOUT: There is no status packet!\n");
break;
case COMM_RXCORRUPT:
printf("COMM_RXCORRUPT: Incorrect status packet!\n");
break;
default:
printf("This is unknown error code!\n");
break;
108
}
}
//
void PrintErrorCode()
{
if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
printf("Input voltage error!\n");
if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
printf("Angle limit error!\n");
if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
printf("Overheat error!\n");
if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
printf("Out of range error!\n");
if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
printf("Checksum error!\n");
if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
printf("Overload error!\n");
if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
printf("Instruction code error!\n");
}

( )
2.25 2.30.
109
2.25
11
2.26
12
2.27
13
110
2.28
14
2.29
15
2.30
16

,
40, .
2.7

2.31.
.
111
2.31 : 1 0.12; 2 0.25; 3

0.37; 4 0.48; 5 0.6; 6 0.72; 7 0.84; 8 0.95; 9 1.09; 10 1.2; 11
1.31; 12 1.44; 13 1.56; 14 1.56; 15 1.68; 16 1.8; 17 1.90; 18
2.02

. , ,

,
, .
112

1. .., .., .., ..,
.., .., .. LEGO MINDSTORMS
NXT // . 2011. 1. .103-108.
2. Information Control Problems in Manufacturing, V. 1 -14.
3. Bobtsov A.A., Pyrkin A.A., Borgul A.S., Zimenko K.A. Control Approaches
for Complicated Self-Unstable Plants with Applications for Two-Wheel Mobile
Robot Motobot in Educational Purposes // Preprints of the 9th IFAC
Symposium on Advances in Control Education (ACE2012), Nizhny Novgorod,
Russia,2012, pp. 107-111;
4. Bobtsov A.A., Kolyubin S.A., Pyrkin A.A., Borgul A.S., Zimenko K.A.,
Rabysh E.Y. Mechatronic and Robotic Setups forModern Control Theory
Workshops // Preprints of the 9th IFAC Symposium on Advances in Control
Education (ACE2012), Nizhny Novgorod, Russia,2012, pp. 348-353;
5. .., .., .., ..
//
- , 2011, #75, . 6368;
6. .., .., .., ..
// -
, 2011, #75, . 5863;
7. .., .. NXTOSEK
EMBEDDED CODER ROBOT NXT LEGO
MINDSTORMS NXT //
PACO 2010, .
.1489-1496.
8. .. : //
.: , 2011. 184 .
9. LEGO firmware. http://mindstorms.lego.com
10.leJOS. http://lejos.sourceforge.net
11.nxtOSEK. http://lejos-osek.sourceforge.net
12.OSEK. http://portal.osek-vdx.org
13.ECRobotInstaller. www.mathworks.com/matlabcentral/fileexchange/25207
14.Explorer. www.nxtprograms.com/NXT2/explorer/index.html
15.NXT GamePad http://lejos-osek.sourceforge.net/nxtgamepad.htm.
16.US Navy Academy to Acquire 50 Robotis Bioloid Humanoid Robot Kits from
KumoTek http://www.azonano.com/news.aspx?NewsID=4498
17.
Bioloid
C
http://support.robotis.com/en/software/embeded_c/cm510_cm700.htm
18. ROBOTIS BIOLOID http://www.robotis.com/xe/BIOLOID_main_en
19. Benedettelli D., Casini M., Garulli A., Giannitrapani A., Vicino A.A. LEGO
Mindstorms experimental setup for multi-agent systems // Control Applications,
(CCA) & Intelligent Control, (ISIC), 2009 IEEE, pp. 1230-1235;
113
20. Brigandi S., Yunfeng Wang, Field J. A LEGO Mindstorms NXT based
multirobot system // Advanced Intelligent Mechatronics (AIM), 2010
IEEE/ASME International Conference, pp. 135-139;
21. Akhtaruzzaman M., Shafie A.A. Geometrical analysis on BIOLOID humanoid
system standing on single leg // Mechatronics (ICOM), 2011 4th International
Conference, pp. 1-5;
22. RoboCup. http://www.robocup.org;
23. RoboPlus - BIOLOID.
http://www.wertech.ru/Blog/23-roboplus-bioloid.aspx;
24. Roboforum. http://roboforum.ru/forum67/topic11643.html#p245158;
25. Atmel Studio 6. http://www.atmel.com/microsite/atmel_studio6;
26. ZigBee. http://ru.wikipedia.org/wiki/ZigBee
27. .., .., ..
. .: . , 1981. 448 .
28.Anderson B.D.O., Moore J.B. Optimal control: linear quadratic methods //
Prentice-Hall, 1989. 394 p.
29.Ha Y.-S., Yuta S. Trajectory tracking control for navigation of self-contained
mobile inverse pendulum // In Proc. IEEE/RSJ Intl. Conf. on Intelligent Robots
and Systems, pages 1875-1882, 1994.
30.Linear-Quadratic-Regulator (LQR) design MATLAB [
].
:
http://www.mathworks.com/help/toolbox/control/ref/lqr.html, . .
.
31. ..
[]: . ... - . : 05.13.01 / ..
. . .: [..], 1988. 483 .
114
115
116
117
118
119
120
Segway
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136

-
- My files
- NXTway-GS,
137

Моделирование и программирование робототехнических комплексов PDF

Загружено:

Сведения о документе

Оригинальное название

Авторское право

Доступные форматы

Поделиться этим документом

Поделиться или встроить документ

Параметры публикации

Этот документ был вам полезен?

Это неприемлемый материал?

Авторское право:

Доступные форматы

Моделирование и программирование робототехнических комплексов PDF

Загружено:

Авторское право:

Доступные форматы

..

1.1 NXT LEGO

1.6 - Lego LEGO Mindstorms NXT 2.0

(HiTechnic, Mindsensors, Dexter Industries).

1.6 ) - HiTechnic NXT; )

. nxtOSEK - nxtOSEK( http://lejososek.sourceforge.net/download.htm?group_id=196690 )

1.10 nxtOSEK ecrobotNXT/environment

1.13 - ECRobot Simulink

1.14 Revolution Sensor

1.15 Servo Motor

1.16 Battery Voltage

1.17 System Clock

Sound Volume Tone :

1.18 Sound Volume Tone

1.19 - Ultrasonic Sensor

1.20 - Light Sensor

1.21 - Sound Sensor

1.22 - Touch Sensor

NXT GamePad Data Logger :

1.25 - NXT GamePad Data Logger

1.26 - NXT GamePad

1.27 - NXT GamePad ADC Data Logger

1.28 - Gyro Sensor

1.29 - Exported Function-Calls Scheduler

1.30 - OSEK Resource

Exported Function-Call Scheduler. OSEK Resource

1.31 - OSEK Resource

1.33 Simulink- test.mdl

1.34 - Function-Call Subsystem

M ITxx , M I Ax ITxz , M M , M I A 2I Az 2 ITzz

K 2 ((ST mT zT ) / w) cos , K 2 ((S A S F sin ) / w) cos

C1 ST SF cos ( ITxz / w) cos mT zT ,

C1 ( I Az / w) cos (S A ( ITxz / w) cos ) ,

H(t) (10) (9),

H(t) (10) V(t)

M mx2 bx2 mLx4 ut 0,

1.64 - Discrete Derivative Discrete Integrator Block

A(3,2) Mb gLE(1,2) / det( E)

A(3,3) [( f s ) E(2,2) E(1,2)] / det( E)

A(4,3) [( f s ) E(1,2) E(1,1)] / det( E)

A(4,4) [ E(1,1) E(1,2)] / det( E)

B(4) [ E(1,1) E(1,2)] / det( E)

1.73 NXT Ballbot

2 BIOLOID PREMIUM KIT

2.1 BIOLOID Premium Kit

2.2 Bioloid Premium Kit

2.8 RS-232 RS-485

RoboPlus Manager ( 2.10)

2.10 RoboPlus Manager

2.11 RoboPlus Terminal

2.12 - RoboPlus Motion

2.13 RoboPlus Task

2.14 Atmel Studio 6.0

2.15 CM-510 (ATmega2561)

3) L ( 2.16), "Transmit File"

the Lowest Limit

the Highest Limit

Status Return Level

if(~PINE & BTN_UP)

unsigned char ReceivedData = getchar();

if(~PIND & SW_START)

if(~PIND & MIC_SIGNAL)

ADCSRA |= (1 << ADSC); //